diff --git a/agent_context/MAP.yaml b/agent_context/MAP.yaml index 6474bc5878..8fa29822f3 100644 --- a/agent_context/MAP.yaml +++ b/agent_context/MAP.yaml @@ -83,10 +83,11 @@ topics: - embodichain/lab/gym/utils/trajectory_state.py - embodichain/lab/gym/envs/managers/observation_types.py - embodichain/lab/gym/envs/demo.py + - embodichain/lab/gym/envs/wrapper/replay.py - embodichain/lab/gym/utils/profiler.py - extensions/embodichain_tasks/configs/tasks/manipulation/gaussian_pick_place/ watch_paths: [embodichain/lab/gym/utils/, embodichain/lab/gym/envs/task_program/, embodichain/cli/, extensions/embodichain_tasks/, - tests/gym/envs/, embodichain/lab/evaluation/, tests/lab/evaluation/, tests/lab/scripts/test_evaluate_task_objective.py] + tests/gym/envs/, embodichain/lab/gym/envs/wrapper/replay.py, embodichain/lab/evaluation/, tests/lab/evaluation/, tests/lab/scripts/test_evaluate_task_objective.py] related_topics: [simulation-system, manager-functor, sim-visualization, task-programs, data-pipeline] status: active - id: manager-functor @@ -206,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/ @@ -303,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 @@ -439,13 +441,17 @@ topics: - id: atomic-actions title: Atomic Skills aliases: [atomic skill, atomic skills, atomic action, 原子技能, 原子动作] - keywords: [PlanningBudget, PlanningRecovery, ExecutionPlanLifecycle, AtomicSkillEngine, run-atomic-skill, PlanningContext, ExecutionSession, ExecutionRunner, SkillInvocation, + keywords: [annotate-trajectory, trajectory_annotator, 轨迹切分, 轨迹标注, fit-trajectory, --skill-sequence, scene-skill-fitting-inputs, SkillFitCfg, PickUpMotionGoal, SlideMotionGoal, TwistMotionGoal, HandOverMotionGoal, 视频关节轨迹拟合, 轨迹回填, PlanningBudget, PlanningRecovery, ExecutionPlanLifecycle, AtomicSkillEngine, run-atomic-skill, PlanningContext, ExecutionSession, ExecutionRunner, SkillInvocation, hand_geometry_cfgs, GraspOpeningTarget, prepare_grasp_query, scoped grasp preparation, RecoveryPolicy, HeldObjectGuardRequest, PhaseEffectGateRequest, articulation_geometry, control-grid retiming, AffordanceSamplingContext, AffordancePoseCandidates, AffordanceSample, affordance sampling] paths: [topics/atomic-actions/atomic-actions.md] source_of_truth: - embodichain/lab/sim/atomic_skills/engine.py + - embodichain/lab/sim/atomic_skills/fitting.py + - embodichain/toolkits/skill_fitting/ + - embodichain/toolkits/trajectory_annotator/ + - embodichain/data_pipeline/skill_fitting.py - embodichain/lab/sim/atomic_skills/core.py - embodichain/lab/sim/atomic_skills/goals.py - embodichain/lab/sim/atomic_skills/execution.py @@ -458,7 +464,8 @@ topics: - embodichain/cli/run_example.py - examples/sim/atomic_skill/ watch_paths: [embodichain/lab/sim/atomic_skills/, embodichain/lab/sim/atomic_actions/, tests/sim/atomic_skills/, - scripts/tutorials/atomic_skill/, tests/examples/atomic_skill/] + scripts/tutorials/atomic_skill/, examples/toolkits/trajectory_annotation/, + tests/toolkits/trajectory_annotator/, tests/examples/atomic_skill/] related_topics: [simulation-system, motion-planning, robot-system, ik-solvers, task-programs] status: active - id: task-programs diff --git a/agent_context/topics/atomic-actions/atomic-actions.md b/agent_context/topics/atomic-actions/atomic-actions.md index f8927e9e9b..b98b23e192 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,29 @@ 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. + +## Trajectory annotation and fitting + +`embodichain annotate-trajectory --bundle` opens the portable input package's +video, joints, calibrated robot and reconstructed scene. `fit-trajectory` and +the website's whole-trajectory fitting panel share `toolkits/skill_fitting/`; +`atomic_skills/fitting.py` adapts the ordinary native planners. Explicit bindings +and observed closing motion select participating control parts; names infer none. +Accepted native spans replace only their selected cores. Remaining source frames +become DemoSkill spans with original DOF-by-frame tensors. + +`run-task --skill-sequence skills.yaml` is a direct engine host that plans native +object-bound goals from live state. It preserves closing preload on unbound +joints. A matching Expansion Profile transforms only native free phases; +DemoSkill commands remain unchanged. Execution completion is separate from +physical success. See [fitting contracts and resource ownership](trajectory-fitting.md) +and the [runnable guide](../../../examples/toolkits/skill_fitting/README.md) +for pose frames, grasp symmetry, boundary qualification, collision caches, +Viser comparison and portable input/output packages. ## Recommended change sites diff --git a/agent_context/topics/atomic-actions/trajectory-fitting.md b/agent_context/topics/atomic-actions/trajectory-fitting.md new file mode 100644 index 0000000000..07f6c358d3 --- /dev/null +++ b/agent_context/topics/atomic-actions/trajectory-fitting.md @@ -0,0 +1,110 @@ +# Trajectory annotation, fitting and native execution + +Video-selected reconstruction enters through `embodichain fit-trajectory` +(`toolkits/skill_fitting/cli.py`) or the annotation website's whole-trajectory +fitting panel (`toolkits/trajectory_annotator/_fitting.py`). Both hosts reuse +`toolkits/skill_fitting/_pipeline.py`; the web host retains one background job +and result, keeps decoding on its existing owner, and invalidates results after +annotation revisions change. Annotation choices cover all built-in skills without +importing simulation; only supported reconstruction types enter fitting. A saved +optional core interval separates loose annotation boundaries from reconstruction, +preserving surrounding motion. `_episode.py` fits each annotation independently +from the immutable recording, combines only its core, and partitions every +remaining source sample into DemoSkill spans. Unsupported authored skills fail +explicitly. The CLI selects this path with `--all-segments --annotations ...`; +its existing single-segment mode remains available. +Its bounded joint charts follow video time. `_viewer.py` owns a local Viser worker +with independent original-material and translucent-blue URDF models; playback +samples all recorded joints on the full clock, including explicit mimic columns. +Model bases use the profile calibration relative to each solver's URDF root, +without TCP offsets. One latest-time slot bounds playback work; replacing the fit +or closing the host stops its viewer. The downloadable NPZ retains full samples. +`_export.py` packages accepted episodes for `run-task --skill-sequence skills.yaml`. +`_native_export.py` writes object-bound Pickup goals, actual participant bindings, +native options/policy and a sealed local resource inventory. `_execution.py` is a +direct AtomicSkillEngine caller: each native invocation plans from measured robot +state and current object pose, then submits ControllerAction commands through Gym. +Only explicitly bound motion/grasp parts take new native-plan commands. Unbound +joints retain the previous commanded targets, not measured positions: a loaded +opposite gripper must keep its closing preload. Demo spans retain full source +command ownership, including deliberate releases. Grasp command/TCP calibration +belongs to the input robot profile; export copies solver TCPs into the Gym robot. +It is not a Task Program lowerer. Only DemoSkill gaps carry original DOF-by-frame +PT matrices and source timestamps; there is no atomic/full-episode trajectory PT. +The host resamples demo clocks and inserts derivative-bounded quintic connections +between adjacent postures. Its execution report separates command completion from +measured object heights and leaves physical success unqualified. +`_sequence_expansion.py` accepts a matching Combined Expansion Profile with +`source.kind=skill_sequence`. This B=1 fixed-scene host applies shared +`atomic_skills/_plan_expansion.py` transforms only to native spans; DemoSkill +source tensors and resampled command paths are excluded. The timeline indices +include every span. Native free-phase permissions, bound motion joints and +protected boundary stencils constrain perturbations. Each recipe resets physics, +and rejected proposals retry without nominal substitution. `_sequence_trace.py` +streams T submitted commands and T+1 measured robot/object states into bounded +HDF5 chunks. These artifacts record command completion with unknown physical +success, not verified Expansion Engine dataset receipts. Editable `expansion.yaml` +is separate from the sealed physical inventory. The +[fitting example](../../../examples/toolkits/skill_fitting/README.md) owns commands +and profile settings. +`_bundle.py` owns `scene-skill-fitting-inputs/v1`: local scene meshes/colliders and +explicit poses, calibrated robot model/profile, original joints, annotations and +frame presentation clock. `fit-trajectory --bundle` requires no image pixels or +original exporter directory; `--package-output` creates portable input resources. +Optional `frame_source` adds a content-bound local video for +`annotate-trajectory --bundle`. This entry resolves all fitting defaults from the +same inventory and persists mutable `annotations.edited.json` separately from +the sealed annotation seed. `--package-video` creates video-enabled resources; +the web fit and exported native runtime need no original recording directory. +A v2 bundle additionally pins a host-owned execution context and declares the +qualified design reference as its fitting trajectory. CLI fitting accepts saved +annotation edits for the same frame source via `--annotations`. +`export_task_program` uses that context to bind a scene-bound Pickup to one +named invocation. It separates the fitted core from the surrounding annotation +samples. Each retained interval needs an explicit named DemoSkill and measured +boundary contract. A retained slice on the same post-step clock can be built +from the pinned source asset with its original commands, shifted phases, events +and scoped evidence. The output links recorded event ticks and asset hashes to +compiled calls. The Expansion Engine owns physical qualification and writes a +new receipt before dataset conversion. +`_collision.py` prepares content-addressed scene convex parts; web import and +packaging supply precomputed files for decomposition-free runtime startup. +Scene-owned Z compensation precedes fitting; the existing pickup interpolation +factor becomes native duration scaling at an unchanged control cadence. +`RobotProfile.simulation` carries explicit physics overlays without changing +kinematic calibration or custom control names. See the +[web example](../../../examples/toolkits/trajectory_annotation/README.md). +`toolkits/skill_fitting/_scene.py` grounds reconstructed local meshes in explicit +world poses and obtains calibrated, finite-surface-qualified antipodal grasps. +Single-arm scene fitting ranks valid TCP poses against the recorded grasp, then +`atomic_skills/fitting.py` calls the ordinary native planner. PickUp uses an +object-bound `GraspGoal`; Slide/Twist retain their motion/axis contracts with the +selected grasp anchor. Parallel-hand alternatives apply an exact half turn in +local TCP Z; contact-frame calibration is conjugated into the physical surface +query so tilted/offset virtual TCPs do not change the rotation axis. Both variants +must pass target-surface checks before ranking. Fitting uses a wider configurable +approach angle and disables the inferred support-plane filter by default; +these host policies do not change native affordance defaults or certify table +clearance. PickUp approaches along the selected grasp's Z. Source motion supplies approach, phase clocks and motion +parameters rather than pointwise acceptance targets. Quintic free-phase bridges +preserve the outer derivative stencils and the contact core. Pickup may rebalance +its free-phase clock inside the unchanged outer interval; explicit phase hints +remain fixed and observed phases remain in provenance. Qualification covers +every changed joint/derivative stencil, while pre-existing source-limit violations +remain separately visible. Rejected results retain candidates without replacing +the original reference. The native fitter does not execute physics; the host +exports the native invocation deployment described above. + +`_participants.py` pairs configured motion/hand groups through URDF ancestry and +selects a unique closing hand; ambiguous evidence requires an explicit choice. +Names do not establish capabilities. A profile must supply explicit runtime TCP +calibration and actual finger collision assets; automatic scene geometry supports +opposing prismatic fingers, including two URDF followers of one aperture driver. +Without a scene, the effect-free world-TCP motion +fitter remains available for PickUp, Slide, Twist and explicitly bound HandOver. +Scene HandOver needs time-resolved object/attachment bindings and is not inferred +from an initial object pose. Both paths retain explicit source identity, inclusive +frame conversion, joint semantics and clock offsets. Automatic RGB contact +recognition and physical-success certification are absent. The +[runnable guide](../../../examples/toolkits/skill_fitting/README.md) owns scene +formats, calibration conventions, phase partitions and result arrays. diff --git a/agent_context/topics/data-pipeline/data-pipeline.md b/agent_context/topics/data-pipeline/data-pipeline.md index 44ff61c455..e31fde5d8c 100644 --- a/agent_context/topics/data-pipeline/data-pipeline.md +++ b/agent_context/topics/data-pipeline/data-pipeline.md @@ -35,6 +35,12 @@ tabletop Cartesian transfer needs source TCP poses. Prototype preparation and planning do not establish collision freedom or physical task success. See [Real2Sim motion](real2sim.md) for ownership and source binding. +`data_pipeline/skill_fitting.py` separately owns named joint recordings, inclusive +video-to-joint clock conversion and standalone numeric/JSON reconstruction +bundles. It does not relax Real2Sim's object-reference requirements. The +[Atomic Skills overview](../atomic-actions/atomic-actions.md) owns the fitting +entry point and the scene-grounded versus effect-free motion-only boundaries. + Read [online sampling](online-sampling.md) for worker states, refill, shared errors, action schemas, sample provenance, causal histories and example-runner boundaries. Read [persistence](persistence.md) diff --git a/agent_context/topics/env-framework/execution.md b/agent_context/topics/env-framework/execution.md index 32cf64b65e..e9feea83ab 100644 --- a/agent_context/topics/env-framework/execution.md +++ b/agent_context/topics/env-framework/execution.md @@ -355,6 +355,23 @@ not orchestrate multiple experiments or qualify kinematic playback. and control replay restore complete particle states at each recorded boundary; dynamic replay restores only the initial particle state and recomputes later fluid motion from recorded actions. + External expert recordings may declare `meta.state_joint_names` for the full + state columns. Replay verifies the exact robot and active joint sets, then + maps state qpos/qvel and explicit expert commands into native order. Missing, + duplicate or mismatched joints reject before stepping. Legacy files without + this field retain numeric-layout checks; raw policy actions cannot be remapped + as joint commands. Legacy fitted replay packages remain readable through this path. +- **Native fitted sequences**: `run-task --skill-sequence skills.yaml` is a separate + direct AtomicSkillEngine host in `toolkits/skill_fitting/_execution.py`. It uses + the plain physical Gym deployment, replans native goals from live state and + submits ControllerAction through `env.step`; only source DemoSkill gaps use PT. + It cannot combine with replay or Task Program ownership. A matching + `--expansion-profile` delegates fixed-scene B=1 spatial variants to + `toolkits/skill_fitting/_sequence_expansion.py`; DemoSkill paths stay unchanged. + Execution + completion/observed heights are recorded separately from physical success. + The portable input/output contract is owned by + [Atomic Skills](../atomic-actions/atomic-actions.md). - **Policy projection**: `rollout_buffer` retains actual policy observations, including sampled noise, and its existing obs/action/reward alignment for LeRobot/RL. It shares the acquisition boundary with raw history, rather than @@ -381,6 +398,12 @@ not orchestrate multiple experiments or qualify kinematic playback. policy action space; a controller/expert trajectory uses the private `ExpertActionSpec` layout. One trajectory cannot mix these schemas. - **CLI**: `run-task --replay --replay_trajectory --replay_mode {kinematic,dynamic,control}`. + Automatic `kinematic` / `dynamic` replay runs without wall-clock pacing and + closes the viewer on completion. For persistent inspection use `control`: + `--headless --viser` exposes a frame slider, and `--no-headless` without + `--viser` opens the native window even when the file sets `headless: true`. + Control replay starts paused; terminal `a` plays at the control interval, + `n` / `p` step, a number plus Enter seeks, `r` resets, and `q` exits. --- 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 bcb9f87ade..0e9602af61 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. + Articulated USDC admission allows absent `gen_sim:closedPosition` metadata; present declarations must identify a finite joint-limit endpoint. Missing endpoint semantics remain unknown. Operation-specific target qualification diff --git a/agent_context/topics/motion-planning/motion-planning.md b/agent_context/topics/motion-planning/motion-planning.md index df16f69a42..ecd5ba8106 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,20 @@ 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. + +Motion-only atomic reconstruction uses `compute.trajectory.phase_alignment` for +bounded monotone native-path clock fitting and `splicing` for fixed-clock joint +replacement. Callers qualify required anchors, TCP constraints and derivatives; +discrete boundary continuity is not collision evidence. An explicitly injected +planner/robot port bypasses simulator lookup; ordinary construction keeps the +simulator-owned robot. Scene-grounded fitting retains its outer clock and native +contact core; see [fitting ownership](../atomic-actions/trajectory-fitting.md). + 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 @@ -102,6 +132,10 @@ owns cross-domain collection; [scene expansion](../scene-expansion/scene-expansi owns layout proposals and host integration. [Real2Sim](../data-pipeline/real2sim.md) owns protected source mapping and retiming. +Combined Expansion Profiles select `source.kind=task_program` or +`skill_sequence`. The native sequence host owns supported factors and unchanged +DemoSkill spans; see [fitting execution](../atomic-actions/trajectory-fitting.md). + The installed benchmark host is `embodichain_benchmarks.motion_generation`. It owns scene lifetime, frozen cases, evaluation and artifacts; adapters return `PlanResult` and expose `MotionGenerator` for Atomic evaluation. Configuration 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 323ce2c047..2b93328bd9 100644 --- a/agent_context/topics/sensor-system/sensor-system.md +++ b/agent_context/topics/sensor-system/sensor-system.md @@ -208,6 +208,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 07ea4cfc9b..ddd2ffb6a6 100644 --- a/agent_context/topics/sim-visualization/sim-visualization.md +++ b/agent_context/topics/sim-visualization/sim-visualization.md @@ -29,6 +29,11 @@ Human-facing behavior and usage are documented in `docs/source/overview/sim/visualization/index.rst`. +Offline annotation's dual-robot Viser playback lives in +`toolkits/trajectory_annotator/_viewer.py`, outside `SimulationManager`. +See [video-selected skill fitting](../atomic-actions/atomic-actions.md) for its +calibration, full-recording clock, and session-owned lifecycle. + ## Ownership and Data Flow ``` @@ -82,6 +87,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/agent_context/topics/simulation-system/physics-configuration.md b/agent_context/topics/simulation-system/physics-configuration.md index f73881997b..a76e8fadbb 100644 --- a/agent_context/topics/simulation-system/physics-configuration.md +++ b/agent_context/topics/simulation-system/physics-configuration.md @@ -150,6 +150,15 @@ imported articulation geometry retains its source approximation. Parser compatibility and current algorithm defaults stay in `shapes.py`, `cfg/rigid_object.py` and `spawn/descriptors.py`, not duplicated here. +For `convex_decomposition`, optional `decomposition_fpath` supplies an OBJ with +one object/group per precomputed convex piece, in the unscaled source-mesh local +frame. `spawn/_convex_parts.py` validates the parts; the rigid compiler emits +separate convex-hull colliders with the declared physics and body scale while +retaining the original visual mesh. Missing/invalid files fail without recooking. +The deployment resource resolver treats this file as an owner-relative asset. +The fitting toolkit prepares and bundles these files before run-task starts; +its cache does not replace ordinary native convex-shape initialization. + Portable `contact_offset/rest_offset` becomes Newton margin/gap: `rest_offset → margin`, `contact_offset - effective_margin → gap`. Native margin/gap take precedence; an ambiguous standalone contact offset is rejected diff --git a/docs/architecture/README.md b/docs/architecture/README.md index 48c2870309..a02553208a 100644 --- a/docs/architecture/README.md +++ b/docs/architecture/README.md @@ -13,6 +13,7 @@ Local and CI Sphinx builds regenerate their own version-specific assets. - [Earlier frontend snapshot](preview.snapshot.json): retained as a reference, no longer consumed by the app. - [GenSim / TaskSpec design](gen-sim-taskspec-design.md): current implementation status, ownership boundaries, and staged target architecture. - [Task Program scene expansion design](task-program-scene-expansion-design.md): workspace integration, shared expert generation workflow, and three-developer implementation plan. +- [Teleoperation trajectory annotation design](teleop-trajectory-annotation-design.md): proposed local video annotation tool, frame interval semantics, keyboard controls, and persistence. `agent_context/MAP.yaml` remains the sole topic inventory. Groups are presentation choices. Neither import declarations nor adjacency establish runtime order. Every 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 a7bce18684..0cb0f66d19 100644 --- a/docs/architecture/generated/architecture.json +++ b/docs/architecture/generated/architecture.json @@ -3,8 +3,8 @@ "kind": "static-architecture", "coverage": "curated-sample", "repository": "DexForce/EmbodiChain", - "revision": "87d8d9454cd18f19d24434380bcd26bcd0dd376e", - "source_ref": "87d8d9454cd18f19d24434380bcd26bcd0dd376e", + "revision": "cfdc9f3cf510432ceb8c19af996c746f798a4eb3", + "source_ref": "HEAD", "package_version": "0.3.2", "topic_index": "agent_context/MAP.yaml", "limitations": [ @@ -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:" } ], @@ -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": 617, + "end_line": 617, "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:" } ], @@ -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", @@ -3116,8 +3116,8 @@ { "path": "embodichain/lab/sim/sim_manager.py", "symbol": "SimulationManager", - "start_line": 562, - "end_line": 562, + "start_line": 566, + "end_line": 566, "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\"," } ], @@ -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()," } ] @@ -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\"" }, { @@ -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": 4389, + "end_line": 4390, "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": 4429, + "end_line": 4432, "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": 1198, + "end_line": 1203, "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": 4390, + "end_line": 4395, "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": 4585, + "end_line": 4585, "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": 4051, + "end_line": 4051, "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": 4192, + "end_line": 4192, "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": 3472, + "end_line": 3472, "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": 4293, + "end_line": 4293, "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": 3851, + "end_line": 3851, "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": 2983, + "end_line": 2983, "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", @@ -6606,6 +6588,24 @@ } ] }, + { + "id": "static-imports-3d3109273aad", + "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": 170, + "end_line": 170, + "excerpt": "from embodichain.utils import configclass, logger" + } + ] + }, { "id": "static-imports-3d4c7a13ce4e", "source": "environment-adapter", @@ -7488,6 +7488,24 @@ } ] }, + { + "id": "static-imports-9ccfb0f3bdcf", + "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": 141, + "end_line": 141, + "excerpt": "from embodichain.lab.sim.physics import make_physics_backend" + } + ] + }, { "id": "static-imports-a225c2b26ae1", "source": "environment-adapter", @@ -7848,24 +7866,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", @@ -8604,8 +8604,8 @@ "static-imports-735fdeb87797", "static-imports-30e2933cfbeb", "static-imports-a38ea1f6aa90", - "static-imports-c84985d043b9", - "static-imports-01a9592b5f6f", + "static-imports-9ccfb0f3bdcf", + "static-imports-3d3109273aad", "static-imports-3d4c7a13ce4e", "static-imports-14722eceaa11", "static-imports-84f40c9ef00a", @@ -9060,8 +9060,8 @@ "static-imports-735fdeb87797", "static-imports-30e2933cfbeb", "static-imports-a38ea1f6aa90", - "static-imports-c84985d043b9", - "static-imports-01a9592b5f6f" + "static-imports-9ccfb0f3bdcf", + "static-imports-3d3109273aad" ], "groups": [ { @@ -9363,7 +9363,7 @@ "static-imports-a954b58f9edc", "static-imports-17df33c96f97", "static-imports-9bc131d4e099", - "static-imports-01a9592b5f6f" + "static-imports-3d3109273aad" ], "groups": [ { diff --git a/docs/architecture/generated/summary.md b/docs/architecture/generated/summary.md index 6336b00c5b..e15c9ac680 100644 --- a/docs/architecture/generated/summary.md +++ b/docs/architecture/generated/summary.md @@ -1,139 +1,139 @@ # Architecture overview -Source revision: `87d8d9454cd18f19d24434380bcd26bcd0dd376e` · package 0.3.2 +Source revision: `cfdc9f3cf510432ceb8c19af996c746f798a4eb3` · 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/87d8d9454cd18f19d24434380bcd26bcd0dd376e/embodichain/cli/main.py#L17-L17) · [CLI reference](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/docs/source/guides/cli.md) | -| Environment launcher | Runs task discovery, configuration loading, and Gym environment creation. | [Source](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/embodichain/lab/scripts/run_task.py#L2015-L2015) · [Environment launch guide](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/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/87d8d9454cd18f19d24434380bcd26bcd0dd376e/embodichain/lab/gym/utils/registration.py#L446-L446) · [Task discovery and supported tasks](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/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/87d8d9454cd18f19d24434380bcd26bcd0dd376e/extensions/embodichain_tasks/embodichain_tasks/__init__.py#L17-L23) · [Supported tasks](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/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/87d8d9454cd18f19d24434380bcd26bcd0dd376e/embodichain/lab/gym/utils/gym_utils.py#L1954-L1954) · [Configuration guide](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/docs/source/guides/configuration.md) | -| Component composition | Resolves environment and embodiment components into runnable Gym configurations. | [Source](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/embodichain/lab/gym/utils/_component_composition.py#L17-L17) · [Configuration guide](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/docs/source/guides/configuration.md) | -| EmbodiedEnv | Assembles scenes, managers, and demonstrations with optional Task Program integration. | [Source](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/embodichain/lab/gym/envs/embodied_env.py#L518-L518) · [EmbodiedEnv documentation](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/docs/source/overview/gym/env.md) | -| BaseEnv | Owns the Gym loop, simulation initialisation, and environment step lifecycle. | [Source](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/embodichain/lab/gym/envs/base_env.py#L118-L118) · [BaseEnv documentation](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/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/87d8d9454cd18f19d24434380bcd26bcd0dd376e/embodichain/lab/gym/envs/managers/__init__.py#L17-L20) · [Gym managers and environment lifecycle](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/docs/source/overview/gym/index.rst) | -| Configuration utilities | Defines typed configuration copying, serialization, and required-field validation. | [Source](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/embodichain/utils/configclass.py#L15-L15) · [Configuration utilities documentation](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/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/87d8d9454cd18f19d24434380bcd26bcd0dd376e/embodichain/lab/gym/envs/managers/randomization/__init__.py#L17-L20) · [Domain randomization documentation](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/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/87d8d9454cd18f19d24434380bcd26bcd0dd376e/embodichain/lab/gym/envs/differentiable_env.py#L48-L48) · [DifferentiableEnv documentation](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/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/87d8d9454cd18f19d24434380bcd26bcd0dd376e/embodichain/lab/gym/envs/managers/event_manager.py#L97-L97) · [EventManager documentation](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/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/87d8d9454cd18f19d24434380bcd26bcd0dd376e/embodichain/lab/task_program/compiler/program.py#L1613-L1613) · [Task Program guide](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/docs/source/overview/task_program/index.md) · [TaskProgramCompiler documentation](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/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/87d8d9454cd18f19d24434380bcd26bcd0dd376e/embodichain/lab/task_program/compiler/lowering.py#L1033-L1033) · [Task Program guide](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/docs/source/overview/task_program/index.md) · [SemanticCallCompiler documentation](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/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/87d8d9454cd18f19d24434380bcd26bcd0dd376e/embodichain/lab/task_program/runtime/executor.py#L258-L258) · [Task Program guide](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/docs/source/overview/task_program/index.md) · [SemanticCallExecutor documentation](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/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/87d8d9454cd18f19d24434380bcd26bcd0dd376e/embodichain/lab/task_program/integrations/environment.py#L315-L315) · [Task Program guide](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/docs/source/overview/task_program/index.md) · [TaskProgramEnvironmentAdapter documentation](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/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/87d8d9454cd18f19d24434380bcd26bcd0dd376e/embodichain/lab/gym/envs/task_program/bridge.py#L1149-L1149) · [Task Program guide](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/docs/source/overview/task_program/index.md) · [TaskProgramDemoBridge documentation](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/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/87d8d9454cd18f19d24434380bcd26bcd0dd376e/embodichain/lab/task_program/integrations/scene_expansion.py#L241-L241) · [Task Program scene preparation and measured acceptance](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/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/87d8d9454cd18f19d24434380bcd26bcd0dd376e/embodichain/agents/mllm/task_program.py#L17-L17) · [MLLM frontend documentation](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/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/87d8d9454cd18f19d24434380bcd26bcd0dd376e/embodichain/lab/sim/atomic_skills/engine.py#L85-L85) · [Atomic Skills guide](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/docs/source/overview/sim/atomic_skills/index.md) · [AtomicSkillEngine documentation](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/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/87d8d9454cd18f19d24434380bcd26bcd0dd376e/extensions/embodichain_benchmarks/motion_generation/__init__.py#L17-L21) · [Benchmark API and external adapters](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/docs/source/api_reference/benchmarks.rst) | -| MotionGenerator | Composes planning backends behind a unified robot motion generation interface. | [Source](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/embodichain/lab/sim/motion/motion_generator.py#L310-L310) · [Motion generation guide](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/docs/source/overview/sim/motion/motion_generator.md) · [MotionGenerator documentation](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/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/87d8d9454cd18f19d24434380bcd26bcd0dd376e/embodichain/lab/sim/motion/planners/__init__.py#L17-L22) · [Motion planner guide](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/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/87d8d9454cd18f19d24434380bcd26bcd0dd376e/embodichain/lab/sim/motion/solvers/__init__.py#L17-L20) · [Kinematic solver guide](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/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/87d8d9454cd18f19d24434380bcd26bcd0dd376e/embodichain/lab/sim/motion/workspace/runtime.py#L53-L53) · [Robot workspace documentation](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/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/87d8d9454cd18f19d24434380bcd26bcd0dd376e/embodichain/lab/expansion/session.py#L121-L121) · [Trajectory augmentation documentation](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/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/87d8d9454cd18f19d24434380bcd26bcd0dd376e/embodichain/lab/sim/sim_manager.py#L562-L562) · [SimulationManager documentation](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/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/87d8d9454cd18f19d24434380bcd26bcd0dd376e/embodichain/lab/sim/scene/expansion/__init__.py#L28-L30) · [Scene expansion contracts and fixed candidates](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/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/87d8d9454cd18f19d24434380bcd26bcd0dd376e/embodichain/lab/sim/objects/robot.py#L17-L17) · [Robots and articulations](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/docs/source/overview/sim/sim_articulation.md) | -| Sensors | Provides camera, stereo, and contact observations for the environment. | [Source](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/embodichain/lab/sim/sensors/__init__.py#L17-L20) · [Sensor guide](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/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/87d8d9454cd18f19d24434380bcd26bcd0dd376e/embodichain/lab/sim/physics/__init__.py#L16-L21) · [Physics backend guide](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/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/87d8d9454cd18f19d24434380bcd26bcd0dd376e/embodichain/lab/visualization/runtime.py#L17-L17) · [Browser visualization guide](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/docs/source/overview/sim/visualization/viser.md) | -| Rigid objects | Represents rigid scene objects and their physical state. | [Source](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/embodichain/lab/sim/objects/rigid_object.py#L430-L430) · [Rigid objects documentation](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/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/87d8d9454cd18f19d24434380bcd26bcd0dd376e/embodichain/lab/sim/objects/rigid_object_group.py#L187-L187) · [Rigid object group guide](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/docs/source/overview/sim/sim_rigid_object_group.md) · [Object API reference](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/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/87d8d9454cd18f19d24434380bcd26bcd0dd376e/embodichain/lab/sim/objects/articulation.py#L724-L724) · [Articulations documentation](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/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/87d8d9454cd18f19d24434380bcd26bcd0dd376e/embodichain/lab/sim/objects/deformable/base.py#L49-L49) · [Deformable objects documentation](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/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/87d8d9454cd18f19d24434380bcd26bcd0dd376e/embodichain/lab/sim/objects/light.py#L40-L40) · [Lights documentation](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/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/87d8d9454cd18f19d24434380bcd26bcd0dd376e/embodichain/lab/sim/objects/constraint.py#L36-L36) · [Rigid constraints documentation](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/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/87d8d9454cd18f19d24434380bcd26bcd0dd376e/embodichain/lab/sim/objects/gizmo.py#L458-L458) · [Gizmo documentation](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/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/87d8d9454cd18f19d24434380bcd26bcd0dd376e/embodichain/compute/__init__.py#L17-L21) · [Compute API reference](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/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/87d8d9454cd18f19d24434380bcd26bcd0dd376e/embodichain/compute/trajectory/__init__.py#L17-L20) · [Trajectory computation documentation](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/docs/source/api_reference/embodichain/embodichain.compute.rst) | -| Kinematics computation | Shared numerical kinematics and manipulability algorithms. | [Source](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/embodichain/compute/kinematics/__init__.py#L17-L24) · [Kinematics computation documentation](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/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/87d8d9454cd18f19d24434380bcd26bcd0dd376e/embodichain/lab/gym/envs/managers/dataset_manager.py#L46-L46) · [DatasetManager documentation](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/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/87d8d9454cd18f19d24434380bcd26bcd0dd376e/embodichain/lab/gym/envs/managers/datasets.py#L167-L167) · [LeRobot recording documentation](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/docs/source/overview/gym/dataset_functors.md) | -| Async recording | Moves durable dataset writes to one background worker. | [Source](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/embodichain/lab/gym/envs/managers/async_datasets.py#L58-L58) · [Async recording documentation](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/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/87d8d9454cd18f19d24434380bcd26bcd0dd376e/embodichain/data_pipeline/depth_video/writer.py#L226-L226) · [Depth video documentation](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/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/87d8d9454cd18f19d24434380bcd26bcd0dd376e/embodichain/data_pipeline/depth_video/reader.py#L17-L23) · [Depth sidecar reader documentation](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/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/87d8d9454cd18f19d24434380bcd26bcd0dd376e/embodichain/data_pipeline/depth_video/depth_utils.py#L24-L35) · [Depth quantization documentation](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/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/87d8d9454cd18f19d24434380bcd26bcd0dd376e/embodichain/data_pipeline/engine/data.py#L17-L17) · [Online data streaming guide](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/docs/source/features/online_data.md) | -| OnlineDataset | Exposes live engine samples as an infinite PyTorch IterableDataset. | [Source](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/embodichain/data_pipeline/datasets/online_data.py#L39-L39) · [OnlineDataset documentation](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/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/87d8d9454cd18f19d24434380bcd26bcd0dd376e/embodichain/data_pipeline/datasets/sampler.py#L17-L17) · [Chunk samplers documentation](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/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/87d8d9454cd18f19d24434380bcd26bcd0dd376e/embodichain/data/dataset.py#L17-L17) · [Data assets documentation](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/docs/source/resources/assets.md) · [Asset catalog and task role contracts](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/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/87d8d9454cd18f19d24434380bcd26bcd0dd376e/embodichain/gen_sim/scene_engine/pipeline/generate.py#L17-L17) · [Scene Engine guide](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/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/87d8d9454cd18f19d24434380bcd26bcd0dd376e/embodichain/gen_sim/scene_engine/pipeline/generation/scene_understanding.py#L260-L260) · [Scene Engine guide](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/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/87d8d9454cd18f19d24434380bcd26bcd0dd376e/embodichain/gen_sim/scene_engine/pipeline/generation/scene_generation.py#L96-L96) · [Scene Engine guide](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/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/87d8d9454cd18f19d24434380bcd26bcd0dd376e/embodichain/gen_sim/scene_engine/pipeline/utils/simready_processor.py#L90-L90) · [Scene Engine guide](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/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/87d8d9454cd18f19d24434380bcd26bcd0dd376e/embodichain/gen_sim/scene_engine/pipeline/edit.py#L17-L17) · [Scene editing documentation](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/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/87d8d9454cd18f19d24434380bcd26bcd0dd376e/embodichain/gen_sim/scene_engine/pipeline/editing/scene_edit_layout_generation.py#L32-L32) · [Scene editing documentation](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/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/87d8d9454cd18f19d24434380bcd26bcd0dd376e/embodichain/gen_sim/scene_engine/pipeline/utils/scene_importer.py#L50-L50) · [Scene export documentation](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/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/87d8d9454cd18f19d24434380bcd26bcd0dd376e/embodichain/gen_sim/scene_engine/pipeline/utils/scene_exporter.py#L53-L53) · [Scene export documentation](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/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/87d8d9454cd18f19d24434380bcd26bcd0dd376e/embodichain/gen_sim/asset_engine/pipeline/ingest.py#L172-L178) · [Asset Engine guide](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/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/87d8d9454cd18f19d24434380bcd26bcd0dd376e/embodichain/gen_sim/asset_engine/parser/base.py#L58-L64) · [Asset Engine guide](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/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/87d8d9454cd18f19d24434380bcd26bcd0dd376e/embodichain/gen_sim/gradio_ui/app_processes.py#L109-L109) · [Scene Engine guide](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/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/87d8d9454cd18f19d24434380bcd26bcd0dd376e/embodichain/learning/rl/train.py#L17-L17) · [Training assembly documentation](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/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/87d8d9454cd18f19d24434380bcd26bcd0dd376e/embodichain/learning/rl/algo/__init__.py#L17-L17) · [RL algorithms documentation](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/docs/source/overview/rl/algorithm.md) | -| PPO | Optimizes an actor-critic policy from standard TensorDict rollouts. | [Source](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/embodichain/learning/rl/algo/ppo.py#L52-L52) · [PPO documentation](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/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/87d8d9454cd18f19d24434380bcd26bcd0dd376e/embodichain/learning/rl/algo/grpo.py#L48-L48) · [GRPO documentation](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/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/87d8d9454cd18f19d24434380bcd26bcd0dd376e/embodichain/learning/rl/algo/apg.py#L119-L119) · [APG documentation](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/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/87d8d9454cd18f19d24434380bcd26bcd0dd376e/embodichain/learning/rl/policies/policy.py#L35-L35) · [Policy models documentation](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/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/87d8d9454cd18f19d24434380bcd26bcd0dd376e/embodichain/learning/rl/policies/actor_critic.py#L33-L33) · [ActorCritic documentation](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/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/87d8d9454cd18f19d24434380bcd26bcd0dd376e/embodichain/learning/rl/policies/actor_only.py#L31-L31) · [ActorOnly documentation](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/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/87d8d9454cd18f19d24434380bcd26bcd0dd376e/embodichain/learning/rl/env.py#L17-L17) · [Learning environments documentation](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/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/87d8d9454cd18f19d24434380bcd26bcd0dd376e/embodichain/learning/rl/utils/trainer.py#L17-L17) · [Reinforcement learning overview](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/docs/source/overview/rl/index.rst) | -| SyncCollector | Collects standard vector-environment rollouts into the training buffer. | [Source](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/embodichain/learning/rl/collector/sync_collector.py#L33-L33) · [SyncCollector documentation](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/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/87d8d9454cd18f19d24434380bcd26bcd0dd376e/embodichain/learning/rl/buffer/standard_buffer.py#L27-L27) · [Rollout buffer documentation](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/docs/source/overview/rl/buffer.md) | -| DifferentiableTrainer | Coordinates graph-connected rollouts and analytic policy-gradient updates. | [Source](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/embodichain/learning/rl/differentiable_trainer.py#L77-L77) · [DifferentiableTrainer documentation](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/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/87d8d9454cd18f19d24434380bcd26bcd0dd376e/embodichain/learning/rl/collector/differentiable.py#L145-L145) · [Differentiable collector documentation](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/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/87d8d9454cd18f19d24434380bcd26bcd0dd376e/embodichain/learning/rl/normalization.py#L29-L29) · [Running observation normalization documentation](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/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/87d8d9454cd18f19d24434380bcd26bcd0dd376e/embodichain/learning/rl/policy_evaluation/cli.py#L17-L17) · [Policy evaluation documentation](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/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/87d8d9454cd18f19d24434380bcd26bcd0dd376e/embodichain/lab/devices/device.py#L17-L17) · [Laboratory and device API reference](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/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/87d8d9454cd18f19d24434380bcd26bcd0dd376e/embodichain/lab/devices/device_controller.py#L29-L29) · [Device controller documentation](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/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/87d8d9454cd18f19d24434380bcd26bcd0dd376e/embodichain/toolkits/graspkit/pose_generator.py#L17-L17) · [Grasp generation documentation](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/docs/source/features/toolkits/grasp_generator.rst) | -| URDF assembly | Assembles URDF components, connections, sensors, and mesh resources. | [Source](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/embodichain/toolkits/urdf_assembly/urdf_assembly_manager.py#L81-L81) · [URDF assembly documentation](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/docs/source/features/toolkits/urdf_assembly.md) | -| Convex decomposition | Prepares convex collision meshes for URDF assets. | [Source](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/embodichain/toolkits/acd/cli.py#L17-L17) · [Convex decomposition documentation](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/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/87d8d9454cd18f19d24434380bcd26bcd0dd376e/embodichain/lab/sim/scene/__init__.py#L17-L22) · [Scene services architecture and capabilities](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/docs/source/overview/sim/scene/index.md) · [Scene construction and preparation workflows](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/docs/source/overview/sim/scene/workflows.md) · [Production scene service integrations](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/docs/source/overview/sim/scene/integrations.md) · [Scene composition, layout, state and validation](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/docs/source/api_reference/embodichain/embodichain.lab.sim.scene.rst) · [Shared numerical geometry and layout](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/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/87d8d9454cd18f19d24434380bcd26bcd0dd376e/extensions/embodichain_models/embodichain_models/dynamics/nerd/model.py#L201-L201) · [Official model library](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/docs/source/api_reference/model_catalogs.rst) · [NeRD data and training workflow](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/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/87d8d9454cd18f19d24434380bcd26bcd0dd376e/embodichain/learning/sl/learner.py#L52-L52) · [SL batches, updates and recovery](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/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/87d8d9454cd18f19d24434380bcd26bcd0dd376e/examples/learning/nerd/__main__.py#L81-L81) · [NeRD showcase commands](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/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/87d8d9454cd18f19d24434380bcd26bcd0dd376e/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/87d8d9454cd18f19d24434380bcd26bcd0dd376e/docs/source/api_reference/rwm.rst) · [RWM principles and usage](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/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/87d8d9454cd18f19d24434380bcd26bcd0dd376e/embodichain/gen_sim/task_spec/spec.py#L335-L335) · [TaskSpec protocol API](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/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/87d8d9454cd18f19d24434380bcd26bcd0dd376e/embodichain/gen_sim/task_engine/agent.py#L72-L72) · [Task Engine workflow and artifact guide](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/docs/source/features/generative_sim/task_engine.md) · [Task Engine semantic contracts](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/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/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/embodichain/cli/main.py#L17-L17) · [CLI reference](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/docs/source/guides/cli.md) | +| Environment launcher | Runs task discovery, configuration loading, and Gym environment creation. | [Source](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/embodichain/lab/scripts/run_task.py#L2015-L2015) · [Environment launch guide](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/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/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/embodichain/lab/gym/utils/registration.py#L446-L446) · [Task discovery and supported tasks](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/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/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/extensions/embodichain_tasks/embodichain_tasks/__init__.py#L17-L23) · [Supported tasks](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/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/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/embodichain/lab/gym/utils/gym_utils.py#L1954-L1954) · [Configuration guide](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/docs/source/guides/configuration.md) | +| Component composition | Resolves environment and embodiment components into runnable Gym configurations. | [Source](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/embodichain/lab/gym/utils/_component_composition.py#L17-L17) · [Configuration guide](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/docs/source/guides/configuration.md) | +| EmbodiedEnv | Assembles scenes, managers, and demonstrations with optional Task Program integration. | [Source](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/embodichain/lab/gym/envs/embodied_env.py#L518-L518) · [EmbodiedEnv documentation](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/docs/source/overview/gym/env.md) | +| BaseEnv | Owns the Gym loop, simulation initialisation, and environment step lifecycle. | [Source](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/embodichain/lab/gym/envs/base_env.py#L118-L118) · [BaseEnv documentation](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/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/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/embodichain/lab/gym/envs/managers/__init__.py#L17-L20) · [Gym managers and environment lifecycle](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/docs/source/overview/gym/index.rst) | +| Configuration utilities | Defines typed configuration copying, serialization, and required-field validation. | [Source](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/embodichain/utils/configclass.py#L15-L15) · [Configuration utilities documentation](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/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/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/embodichain/lab/gym/envs/managers/randomization/__init__.py#L17-L20) · [Domain randomization documentation](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/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/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/embodichain/lab/gym/envs/differentiable_env.py#L48-L48) · [DifferentiableEnv documentation](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/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/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/embodichain/lab/gym/envs/managers/event_manager.py#L97-L97) · [EventManager documentation](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/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/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/embodichain/lab/task_program/compiler/program.py#L1613-L1613) · [Task Program guide](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/docs/source/overview/task_program/index.md) · [TaskProgramCompiler documentation](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/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/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/embodichain/lab/task_program/compiler/lowering.py#L1033-L1033) · [Task Program guide](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/docs/source/overview/task_program/index.md) · [SemanticCallCompiler documentation](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/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/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/embodichain/lab/task_program/runtime/executor.py#L258-L258) · [Task Program guide](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/docs/source/overview/task_program/index.md) · [SemanticCallExecutor documentation](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/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/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/embodichain/lab/task_program/integrations/environment.py#L315-L315) · [Task Program guide](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/docs/source/overview/task_program/index.md) · [TaskProgramEnvironmentAdapter documentation](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/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/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/embodichain/lab/gym/envs/task_program/bridge.py#L1177-L1177) · [Task Program guide](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/docs/source/overview/task_program/index.md) · [TaskProgramDemoBridge documentation](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/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/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/embodichain/lab/task_program/integrations/scene_expansion.py#L241-L241) · [Task Program scene preparation and measured acceptance](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/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/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/embodichain/agents/mllm/task_program.py#L17-L17) · [MLLM frontend documentation](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/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/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/embodichain/lab/sim/atomic_skills/engine.py#L85-L85) · [Atomic Skills guide](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/docs/source/overview/sim/atomic_skills/index.md) · [AtomicSkillEngine documentation](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/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/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/extensions/embodichain_benchmarks/motion_generation/__init__.py#L17-L21) · [Benchmark API and external adapters](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/docs/source/api_reference/benchmarks.rst) | +| MotionGenerator | Composes planning backends behind a unified robot motion generation interface. | [Source](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/embodichain/lab/sim/motion/motion_generator.py#L334-L334) · [Motion generation guide](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/docs/source/overview/sim/motion/motion_generator.md) · [MotionGenerator documentation](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/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/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/embodichain/lab/sim/motion/planners/__init__.py#L17-L22) · [Motion planner guide](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/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/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/embodichain/lab/sim/motion/solvers/__init__.py#L17-L20) · [Kinematic solver guide](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/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/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/embodichain/lab/sim/motion/workspace/runtime.py#L53-L53) · [Robot workspace documentation](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/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/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/embodichain/lab/expansion/session.py#L121-L121) · [Trajectory augmentation documentation](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/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/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/embodichain/lab/sim/sim_manager.py#L566-L566) · [SimulationManager documentation](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/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/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/embodichain/lab/sim/scene/expansion/__init__.py#L28-L31) · [Scene expansion contracts and fixed candidates](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/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/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/embodichain/lab/sim/objects/robot.py#L17-L17) · [Robots and articulations](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/docs/source/overview/sim/sim_articulation.md) | +| Sensors | Provides camera, stereo, and contact observations for the environment. | [Source](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/embodichain/lab/sim/sensors/__init__.py#L17-L20) · [Sensor guide](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/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/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/embodichain/lab/sim/physics/__init__.py#L16-L21) · [Physics backend guide](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/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/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/embodichain/lab/visualization/runtime.py#L17-L17) · [Browser visualization guide](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/docs/source/overview/sim/visualization/viser.md) | +| Rigid objects | Represents rigid scene objects and their physical state. | [Source](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/embodichain/lab/sim/objects/rigid_object.py#L434-L434) · [Rigid objects documentation](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/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/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/embodichain/lab/sim/objects/rigid_object_group.py#L187-L187) · [Rigid object group guide](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/docs/source/overview/sim/sim_rigid_object_group.md) · [Object API reference](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/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/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/embodichain/lab/sim/objects/articulation.py#L724-L724) · [Articulations documentation](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/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/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/embodichain/lab/sim/objects/deformable/base.py#L49-L49) · [Deformable objects documentation](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/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/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/embodichain/lab/sim/objects/light.py#L40-L40) · [Lights documentation](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/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/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/embodichain/lab/sim/objects/constraint.py#L36-L36) · [Rigid constraints documentation](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/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/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/embodichain/lab/sim/objects/gizmo.py#L458-L458) · [Gizmo documentation](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/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/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/embodichain/compute/__init__.py#L17-L21) · [Compute API reference](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/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/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/embodichain/compute/trajectory/__init__.py#L17-L20) · [Trajectory computation documentation](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/docs/source/api_reference/embodichain/embodichain.compute.rst) | +| Kinematics computation | Shared numerical kinematics and manipulability algorithms. | [Source](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/embodichain/compute/kinematics/__init__.py#L17-L24) · [Kinematics computation documentation](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/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/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/embodichain/lab/gym/envs/managers/dataset_manager.py#L46-L46) · [DatasetManager documentation](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/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/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/embodichain/lab/gym/envs/managers/datasets.py#L167-L167) · [LeRobot recording documentation](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/docs/source/overview/gym/dataset_functors.md) | +| Async recording | Moves durable dataset writes to one background worker. | [Source](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/embodichain/lab/gym/envs/managers/async_datasets.py#L58-L58) · [Async recording documentation](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/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/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/embodichain/data_pipeline/depth_video/writer.py#L226-L226) · [Depth video documentation](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/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/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/embodichain/data_pipeline/depth_video/reader.py#L17-L23) · [Depth sidecar reader documentation](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/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/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/embodichain/data_pipeline/depth_video/depth_utils.py#L24-L35) · [Depth quantization documentation](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/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/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/embodichain/data_pipeline/engine/data.py#L17-L17) · [Online data streaming guide](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/docs/source/features/online_data.md) | +| OnlineDataset | Exposes live engine samples as an infinite PyTorch IterableDataset. | [Source](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/embodichain/data_pipeline/datasets/online_data.py#L39-L39) · [OnlineDataset documentation](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/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/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/embodichain/data_pipeline/datasets/sampler.py#L17-L17) · [Chunk samplers documentation](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/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/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/embodichain/data/dataset.py#L17-L17) · [Data assets documentation](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/docs/source/resources/assets.md) · [Asset catalog and task role contracts](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/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/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/embodichain/gen_sim/scene_engine/pipeline/generate.py#L17-L17) · [Scene Engine guide](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/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/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/embodichain/gen_sim/scene_engine/pipeline/generation/scene_understanding.py#L260-L260) · [Scene Engine guide](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/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/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/embodichain/gen_sim/scene_engine/pipeline/generation/scene_generation.py#L96-L96) · [Scene Engine guide](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/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/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/embodichain/gen_sim/scene_engine/pipeline/utils/simready_processor.py#L90-L90) · [Scene Engine guide](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/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/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/embodichain/gen_sim/scene_engine/pipeline/edit.py#L17-L17) · [Scene editing documentation](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/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/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/embodichain/gen_sim/scene_engine/pipeline/editing/scene_edit_layout_generation.py#L32-L32) · [Scene editing documentation](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/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/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/embodichain/gen_sim/scene_engine/pipeline/utils/scene_importer.py#L50-L50) · [Scene export documentation](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/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/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/embodichain/gen_sim/scene_engine/pipeline/utils/scene_exporter.py#L53-L53) · [Scene export documentation](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/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/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/embodichain/gen_sim/asset_engine/pipeline/ingest.py#L172-L178) · [Asset Engine guide](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/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/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/embodichain/gen_sim/asset_engine/parser/base.py#L58-L64) · [Asset Engine guide](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/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/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/embodichain/gen_sim/gradio_ui/app_processes.py#L109-L109) · [Scene Engine guide](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/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/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/embodichain/learning/rl/train.py#L17-L17) · [Training assembly documentation](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/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/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/embodichain/learning/rl/algo/__init__.py#L17-L17) · [RL algorithms documentation](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/docs/source/overview/rl/algorithm.md) | +| PPO | Optimizes an actor-critic policy from standard TensorDict rollouts. | [Source](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/embodichain/learning/rl/algo/ppo.py#L52-L52) · [PPO documentation](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/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/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/embodichain/learning/rl/algo/grpo.py#L48-L48) · [GRPO documentation](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/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/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/embodichain/learning/rl/algo/apg.py#L119-L119) · [APG documentation](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/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/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/embodichain/learning/rl/policies/policy.py#L35-L35) · [Policy models documentation](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/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/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/embodichain/learning/rl/policies/actor_critic.py#L33-L33) · [ActorCritic documentation](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/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/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/embodichain/learning/rl/policies/actor_only.py#L31-L31) · [ActorOnly documentation](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/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/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/embodichain/learning/rl/env.py#L17-L17) · [Learning environments documentation](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/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/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/embodichain/learning/rl/utils/trainer.py#L17-L17) · [Reinforcement learning overview](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/docs/source/overview/rl/index.rst) | +| SyncCollector | Collects standard vector-environment rollouts into the training buffer. | [Source](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/embodichain/learning/rl/collector/sync_collector.py#L33-L33) · [SyncCollector documentation](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/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/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/embodichain/learning/rl/buffer/standard_buffer.py#L27-L27) · [Rollout buffer documentation](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/docs/source/overview/rl/buffer.md) | +| DifferentiableTrainer | Coordinates graph-connected rollouts and analytic policy-gradient updates. | [Source](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/embodichain/learning/rl/differentiable_trainer.py#L77-L77) · [DifferentiableTrainer documentation](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/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/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/embodichain/learning/rl/collector/differentiable.py#L145-L145) · [Differentiable collector documentation](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/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/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/embodichain/learning/rl/normalization.py#L29-L29) · [Running observation normalization documentation](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/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/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/embodichain/learning/rl/policy_evaluation/cli.py#L17-L17) · [Policy evaluation documentation](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/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/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/embodichain/lab/devices/device.py#L17-L17) · [Laboratory and device API reference](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/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/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/embodichain/lab/devices/device_controller.py#L29-L29) · [Device controller documentation](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/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/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/embodichain/toolkits/graspkit/pose_generator.py#L17-L17) · [Grasp generation documentation](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/docs/source/features/toolkits/grasp_generator.rst) | +| URDF assembly | Assembles URDF components, connections, sensors, and mesh resources. | [Source](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/embodichain/toolkits/urdf_assembly/urdf_assembly_manager.py#L81-L81) · [URDF assembly documentation](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/docs/source/features/toolkits/urdf_assembly.md) | +| Convex decomposition | Prepares convex collision meshes for URDF assets. | [Source](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/embodichain/toolkits/acd/cli.py#L17-L17) · [Convex decomposition documentation](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/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/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/embodichain/lab/sim/scene/__init__.py#L17-L22) · [Scene services architecture and capabilities](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/docs/source/overview/sim/scene/index.md) · [Scene construction and preparation workflows](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/docs/source/overview/sim/scene/workflows.md) · [Production scene service integrations](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/docs/source/overview/sim/scene/integrations.md) · [Scene composition, layout, state and validation](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/docs/source/api_reference/embodichain/embodichain.lab.sim.scene.rst) · [Shared numerical geometry and layout](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/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/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/extensions/embodichain_models/embodichain_models/dynamics/nerd/model.py#L201-L201) · [Official model library](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/docs/source/api_reference/model_catalogs.rst) · [NeRD data and training workflow](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/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/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/embodichain/learning/sl/learner.py#L52-L52) · [SL batches, updates and recovery](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/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/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/examples/learning/nerd/__main__.py#L81-L81) · [NeRD showcase commands](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/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/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/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/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/docs/source/api_reference/rwm.rst) · [RWM principles and usage](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/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/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/embodichain/gen_sim/task_spec/spec.py#L335-L335) · [TaskSpec protocol API](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/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/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/embodichain/gen_sim/task_engine/agent.py#L72-L72) · [Task Engine workflow and artifact guide](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/docs/source/features/generative_sim/task_engine.md) · [Task Engine semantic contracts](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/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/87d8d9454cd18f19d24434380bcd26bcd0dd376e/embodichain/toolkits/graspkit/pg_grasp/pose_generator.py#L284-L284) · [Grasp generation documentation](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/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/87d8d9454cd18f19d24434380bcd26bcd0dd376e/embodichain/lab/sim/diff/bridge.py#L81-L81) · [Newton autograd bridge documentation](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/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/87d8d9454cd18f19d24434380bcd26bcd0dd376e/embodichain/lab/sim/common.py#L55-L55) · [BatchEntity documentation](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/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/87d8d9454cd18f19d24434380bcd26bcd0dd376e/embodichain/toolkits/acd/urdf_modifider.py#L17-L17) · [Convex decomposition documentation](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/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/87d8d9454cd18f19d24434380bcd26bcd0dd376e/embodichain/lab/gym/envs/task_program/bridge.py#L864-L864) · [Task Program guide](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/docs/source/overview/task_program/index.md) · [BufferedGymCommandSink documentation](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/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/87d8d9454cd18f19d24434380bcd26bcd0dd376e/embodichain/lab/task_program/compiler/program.py#L1343-L1343) · [Task Program guide](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/docs/source/overview/task_program/index.md) · [CompiledTaskProgram documentation](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/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/87d8d9454cd18f19d24434380bcd26bcd0dd376e/embodichain/lab/task_program/semantics/evidence.py#L438-L438) · [Effect evidence documentation](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/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/87d8d9454cd18f19d24434380bcd26bcd0dd376e/embodichain/lab/sim/atomic_skills/runner.py#L367-L367) · [Atomic Skills execution guide](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/docs/source/overview/sim/atomic_skills/index.md) · [ExecutionRunner documentation](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/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/87d8d9454cd18f19d24434380bcd26bcd0dd376e/embodichain/lab/scripts/annotate_grasp.py#L42-L42) · [Grasp generation documentation](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/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/87d8d9454cd18f19d24434380bcd26bcd0dd376e/embodichain/toolkits/graspkit/pg_grasp/_antipodal_backend.py#L71-L71) · [Grasp generation documentation](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/docs/source/features/toolkits/grasp_generator.rst) | -| Gripper collision checks | Checks gripper geometry against the target mesh. | [Source](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/embodichain/toolkits/graspkit/pg_grasp/gripper_collision_checker.py#L39-L39) · [Grasp generation documentation](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/docs/source/features/toolkits/grasp_generator.rst) | -| Antipodal sampling | Samples opposing surface contacts using mesh ray casting. | [Source](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/embodichain/toolkits/graspkit/pg_grasp/antipodal_sampler.py#L62-L62) · [Grasp generation documentation](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/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/87d8d9454cd18f19d24434380bcd26bcd0dd376e/embodichain/lab/sim/cfg/gripper.py#L53-L53) · [Grasp generation documentation](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/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/87d8d9454cd18f19d24434380bcd26bcd0dd376e/embodichain/lab/task_program/integrations/catalog.py#L611-L611) · [Integration catalog documentation](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/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/87d8d9454cd18f19d24434380bcd26bcd0dd376e/embodichain/lab/sim/objects/backends/__init__.py#L19-L19) · [Object backend views documentation](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/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/87d8d9454cd18f19d24434380bcd26bcd0dd376e/embodichain/lab/task_program/runtime/parallel_executor.py#L629-L629) · [Parallel execution documentation](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/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/87d8d9454cd18f19d24434380bcd26bcd0dd376e/embodichain/lab/task_program/language/loader.py#L17-L17) · [Program loading & validation documentation](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/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/87d8d9454cd18f19d24434380bcd26bcd0dd376e/embodichain/lab/task_program/semantics/scene.py#L1113-L1113) · [SceneRegistry documentation](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/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/87d8d9454cd18f19d24434380bcd26bcd0dd376e/embodichain/lab/task_program/integrations/simulation/environment.py#L731-L731) · [Task Program guide](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/docs/source/overview/task_program/index.md) · [Simulation integration API](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/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/87d8d9454cd18f19d24434380bcd26bcd0dd376e/embodichain/lab/task_program/semantics/profiles.py#L1495-L1495) · [RobotSkillProfile documentation](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/docs/source/overview/task_program/robot_profiles.md) | -| EnvironmentStepClock | Maintains execution time on the environment step grid. | [Source](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/embodichain/lab/gym/envs/task_program/bridge.py#L351-L351) · [Task Program guide](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/docs/source/overview/task_program/index.md) · [Gym Task Program API](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/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/87d8d9454cd18f19d24434380bcd26bcd0dd376e/embodichain/lab/sim/objects/deformable/surface.py#L28-L28) · [Surface deformables documentation](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/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/87d8d9454cd18f19d24434380bcd26bcd0dd376e/embodichain/lab/task_program/language/schema.py#L1067-L1067) · [Task Program guide](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/docs/source/overview/task_program/index.md) · [TaskProgramCfg documentation](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/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/87d8d9454cd18f19d24434380bcd26bcd0dd376e/embodichain/toolkits/__init__.py#L17-L21) · [Toolkits documentation](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/docs/source/features/toolkits/index.rst) · [Precision Assembly guide and native demos](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/docs/source/features/toolkits/precision_assembly.md) | -| URDF components | Processes component links, joints, naming, and mesh references. | [Source](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/embodichain/toolkits/urdf_assembly/component.py#L87-L87) · [URDF assembly documentation](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/docs/source/features/toolkits/urdf_assembly.md) | -| URDF connections | Applies component connection rules and sensor attachment joints. | [Source](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/embodichain/toolkits/urdf_assembly/connection.py#L30-L30) · [URDF assembly documentation](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/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/87d8d9454cd18f19d24434380bcd26bcd0dd376e/embodichain/toolkits/urdf_assembly/mesh.py#L33-L33) · [URDF assembly documentation](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/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/87d8d9454cd18f19d24434380bcd26bcd0dd376e/embodichain/toolkits/urdf_assembly/sensor.py#L75-L75) · [URDF assembly documentation](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/docs/source/features/toolkits/urdf_assembly.md) | -| URDF file output | Formats and persists the assembled URDF document. | [Source](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/embodichain/toolkits/urdf_assembly/file_writer.py#L28-L28) · [URDF assembly documentation](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/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/87d8d9454cd18f19d24434380bcd26bcd0dd376e/embodichain/lab/sim/objects/deformable/volume.py#L36-L36) · [Volume deformables documentation](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/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/87d8d9454cd18f19d24434380bcd26bcd0dd376e/embodichain/lab/sim/motion/workspace/analyzer.py#L182-L182) · [Workspace analyzer documentation](https://github.com/DexForce/EmbodiChain/blob/87d8d9454cd18f19d24434380bcd26bcd0dd376e/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/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/embodichain/toolkits/graspkit/pg_grasp/pose_generator.py#L284-L284) · [Grasp generation documentation](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/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/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/embodichain/lab/sim/diff/bridge.py#L81-L81) · [Newton autograd bridge documentation](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/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/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/embodichain/lab/sim/common.py#L55-L55) · [BatchEntity documentation](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/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/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/embodichain/toolkits/acd/urdf_modifider.py#L17-L17) · [Convex decomposition documentation](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/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/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/embodichain/lab/gym/envs/task_program/bridge.py#L892-L892) · [Task Program guide](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/docs/source/overview/task_program/index.md) · [BufferedGymCommandSink documentation](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/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/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/embodichain/lab/task_program/compiler/program.py#L1343-L1343) · [Task Program guide](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/docs/source/overview/task_program/index.md) · [CompiledTaskProgram documentation](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/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/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/embodichain/lab/task_program/semantics/evidence.py#L438-L438) · [Effect evidence documentation](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/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/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/embodichain/lab/sim/atomic_skills/runner.py#L367-L367) · [Atomic Skills execution guide](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/docs/source/overview/sim/atomic_skills/index.md) · [ExecutionRunner documentation](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/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/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/embodichain/lab/scripts/annotate_grasp.py#L42-L42) · [Grasp generation documentation](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/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/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/embodichain/toolkits/graspkit/pg_grasp/_antipodal_backend.py#L71-L71) · [Grasp generation documentation](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/docs/source/features/toolkits/grasp_generator.rst) | +| Gripper collision checks | Checks gripper geometry against the target mesh. | [Source](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/embodichain/toolkits/graspkit/pg_grasp/gripper_collision_checker.py#L96-L96) · [Grasp generation documentation](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/docs/source/features/toolkits/grasp_generator.rst) | +| Antipodal sampling | Samples opposing surface contacts using mesh ray casting. | [Source](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/embodichain/toolkits/graspkit/pg_grasp/antipodal_sampler.py#L62-L62) · [Grasp generation documentation](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/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/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/embodichain/lab/sim/cfg/gripper.py#L53-L53) · [Grasp generation documentation](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/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/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/embodichain/lab/task_program/integrations/catalog.py#L617-L617) · [Integration catalog documentation](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/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/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/embodichain/lab/sim/objects/backends/__init__.py#L19-L19) · [Object backend views documentation](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/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/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/embodichain/lab/task_program/runtime/parallel_executor.py#L629-L629) · [Parallel execution documentation](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/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/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/embodichain/lab/task_program/language/loader.py#L17-L17) · [Program loading & validation documentation](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/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/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/embodichain/lab/task_program/semantics/scene.py#L1113-L1113) · [SceneRegistry documentation](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/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/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/embodichain/lab/task_program/integrations/simulation/environment.py#L731-L731) · [Task Program guide](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/docs/source/overview/task_program/index.md) · [Simulation integration API](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/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/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/embodichain/lab/task_program/semantics/profiles.py#L1495-L1495) · [RobotSkillProfile documentation](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/docs/source/overview/task_program/robot_profiles.md) | +| EnvironmentStepClock | Maintains execution time on the environment step grid. | [Source](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/embodichain/lab/gym/envs/task_program/bridge.py#L351-L351) · [Task Program guide](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/docs/source/overview/task_program/index.md) · [Gym Task Program API](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/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/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/embodichain/lab/sim/objects/deformable/surface.py#L28-L28) · [Surface deformables documentation](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/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/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/embodichain/lab/task_program/language/schema.py#L1067-L1067) · [Task Program guide](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/docs/source/overview/task_program/index.md) · [TaskProgramCfg documentation](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/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/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/embodichain/toolkits/__init__.py#L17-L21) · [Toolkits documentation](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/docs/source/features/toolkits/index.rst) · [Precision Assembly guide and native demos](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/docs/source/features/toolkits/precision_assembly.md) | +| URDF components | Processes component links, joints, naming, and mesh references. | [Source](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/embodichain/toolkits/urdf_assembly/component.py#L87-L87) · [URDF assembly documentation](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/docs/source/features/toolkits/urdf_assembly.md) | +| URDF connections | Applies component connection rules and sensor attachment joints. | [Source](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/embodichain/toolkits/urdf_assembly/connection.py#L30-L30) · [URDF assembly documentation](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/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/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/embodichain/toolkits/urdf_assembly/mesh.py#L33-L33) · [URDF assembly documentation](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/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/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/embodichain/toolkits/urdf_assembly/sensor.py#L75-L75) · [URDF assembly documentation](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/docs/source/features/toolkits/urdf_assembly.md) | +| URDF file output | Formats and persists the assembled URDF document. | [Source](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/embodichain/toolkits/urdf_assembly/file_writer.py#L28-L28) · [URDF assembly documentation](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/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/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/embodichain/lab/sim/objects/deformable/volume.py#L36-L36) · [Volume deformables documentation](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/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/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/embodichain/lab/sim/motion/workspace/analyzer.py#L182-L182) · [Workspace analyzer documentation](https://github.com/DexForce/EmbodiChain/blob/cfdc9f3cf510432ceb8c19af996c746f798a4eb3/docs/source/features/workspace_analyzer/workspace_analyzer.md) | ## Coverage and interpretation diff --git a/docs/architecture/images/trajectory-annotation-fitting.png b/docs/architecture/images/trajectory-annotation-fitting.png new file mode 100644 index 0000000000..03c2919b3a Binary files /dev/null and b/docs/architecture/images/trajectory-annotation-fitting.png differ diff --git a/docs/architecture/images/trajectory-annotation-robots.png b/docs/architecture/images/trajectory-annotation-robots.png new file mode 100644 index 0000000000..fc591e7134 Binary files /dev/null and b/docs/architecture/images/trajectory-annotation-robots.png differ diff --git a/docs/architecture/images/trajectory-annotation.png b/docs/architecture/images/trajectory-annotation.png new file mode 100644 index 0000000000..0653ec9926 Binary files /dev/null and b/docs/architecture/images/trajectory-annotation.png differ diff --git a/docs/architecture/teleop-trajectory-annotation-design.md b/docs/architecture/teleop-trajectory-annotation-design.md new file mode 100644 index 0000000000..938715850e --- /dev/null +++ b/docs/architecture/teleop-trajectory-annotation-design.md @@ -0,0 +1,403 @@ +# 遥操轨迹图像序列原子技能标注工具设计 + +状态:方案已确认,首版已实现。日期:2026-11-09。 + +实现位置:`embodichain/toolkits/trajectory_annotator/`。工具接收一段本地遥操视频、HDF5 相机图像或已有 RGB 图像序列,让用户逐帧查看、标注原子技能区间、检查和删除标注,并保存为可继续编辑的 JSON。 + +本文记录已确认的设计约定;启动方式和快捷键见工具的 [README](../../embodichain/toolkits/trajectory_annotator/README.md)。稳定使用入口包括 CLI、版本化 JSON,以及 `frames.py` 的帧输入协议和 `app.py` 的启动接口。 + +## 1. 目标与首版范围 + +首版完成一个可独立运行的本地工具: + +- 打开一个视频,显示当前画面、frame index、时间和总帧数。 +- 支持播放、暂停、前后一帧、跨多帧、时间轴拖动和输入帧号跳转。 +- 为每段原子技能填写名称,例如“翻转包裹”,并标注起止 frame index。 +- 同时通过时间轴和列表展示全部已完成标注;支持定位、修改、删除、撤销和重做。 +- 自动保存、手动保存,以及重新打开视频后继续标注。 + +初始支持本地 MP4、MOV、MKV、AVI 等容器,实际能否读取取决于所安装 PyAV/FFmpeg 的解码能力;以 MP4/H.264 为主要验收格式。每次运行打开一个视频,默认选第一个视频流,也可显式指定流。支持具有可靠时间戳的恒定帧率和可变帧率视频;首版播放不处理音频。 + +本轮不包含自动识别动作、多相机同步、机器人关节数据切片、多人协作和视频片段导出。这里的“切分”首先形成带原子技能名称的帧区间,后续可以由其他工具据此裁剪视频或切分轨迹数据。 + +## 2. 与现有项目的衔接 + +已核对的现有基础: + +| 当前代码 | 对本设计的影响 | +| --- | --- | +| [`toolkits/__init__.py`](../../embodichain/toolkits/__init__.py) 将 toolkits 定位为独立于仿真循环的工具 | 标注工具归属 toolkits,运行时不创建仿真环境 | +| [`cli/main.py`](../../embodichain/cli/main.py) 通过 `Command` 和延迟导入分发命令,已有 `annotate-grasp` | 增加同类入口 `embodichain annotate-trajectory` | +| [`pyproject.toml`](../../pyproject.toml) 已声明 OpenCV;[`depth_video/reader.py`](../../embodichain/data_pipeline/depth_video/reader.py) 已使用 PyAV | 使用 PyAV 解码,复用 OpenCV 做预览图像编码 | +| [`setup.py`](../../setup.py) 显式列举非 Python 包资源 | 新界面的 HTML、JS、CSS 必须加入 wheel 的 `package_data` | +| [`recording/segments.py`](../../embodichain/data_pipeline/recording/segments.py) 使用半开区间 `start_step/end_step` | 本工具的视频帧区间单独声明语义;将来接入该格式时显式转换端点与时钟 | + +新工具只把原子技能名称当作用户标签,不自动将其绑定到 AtomicActionEngine 或 Task Program 的技能注册项。视频帧也不自动等同于机器人控制 step;两者需要独立的同步关系。 + +## 3. 推荐技术方案 + +采用 **本地 Python 服务 + 浏览器界面**: + +```text +统一 CLI + │ 选择视频/HDF5、指定标注文件 + ▼ +本地 FastAPI / Uvicorn 服务 + ├── FrameSource:统一按索引读取 RGB 图像 + │ ├── PyAV:视频 PTS 索引、按需解码 + │ ├── HDF5:只读单相机 JPEG/PNG 或原始数组 + │ └── ArrayFrameSource:已有内存图像 + ├── OpenCV:缩放并编码预览图像 + ├── 标注校验、JSON 读取与原子保存 + └── 同源静态界面:HTML / CSS / JavaScript + │ + ▼ + 画面、时间轴、标注列表、快捷键 +``` + +前端使用原生 JavaScript 模块,随 Python 包分发,无需 Node 构建流程或 CDN。界面使用中文,原子技能名称支持 Unicode。首版在桌面 Chromium 和 Firefox 上验收。 + +后端只监听 `127.0.0.1`,默认选择空闲端口并打开浏览器;提供 `--no-browser` 供手动访问。视频直接由 Python 进程读取,不通过浏览器上传。服务只操作启动时指定的输入、标注文件和随包静态资源。 + +新增可选依赖组 `trajectory-annotation`,显式声明 `av>=15.1,<18`、`fastapi>=0.115,<1`、`uvicorn>=0.30,<1`,不依赖其他包的传递依赖。OpenCV 和 h5py 沿用现有依赖。已在 conda `py312` 的 Python 3.12 环境验证;Python 3.11 尚未实测。CLI 帮助和普通包导入不加载解码器或启动服务。 + +FastAPI 提供静态资源挂载和自定义响应,足以承载这个单页界面及帧图像接口。参考:[静态文件](https://fastapi.tiangolo.com/tutorial/static-files/)、[自定义响应](https://fastapi.tiangolo.com/advanced/custom-response/)。 + +### 网页界面选型:与 Viser 的取舍 + +当前推荐由 FastAPI 提供服务,自行编写 HTML/CSS/JavaScript 标注界面。用户的使用方式与 Viser 类似:启动一个 Python 命令,在浏览器打开本地地址。 + +Viser 是可行的备选:它提供 `gui.add_image()`、滑块、按钮和文本控件,可以组合出帧预览、帧号选择和区间增删。上游 `main` 文档还提供带 `hotkey` 的 `gui.add_command()`,因此不能笼统地说 Viser 不支持快捷键。参考:[Viser GUI API](https://viser.studio/main/api/core/gui_api/)。 + +需要区分项目实际版本:当前 `pyproject.toml` 固定 `viser==1.0.21`,该标签的 [GUI API 源码](https://github.com/viser-project/viser/blob/v1.0.21/src/viser/_gui_api.py) 尚未包含 `add_command()`。采用上游命令接口需要先评估版本升级及现有仿真可视化的兼容性;保持当前版本则需要额外实现快捷键接入。 + +二维视频、可缩放时间轴和标注列表使用自定义网页,直接管理布局、键盘焦点及“画面显示完成后才能标记”的状态。拟合结果面板已经嵌入 Viser,显示原始外观与蓝色半透明的两套 URDF 机器人,并随视频时间更新全部关节。视频索引、区间模型和 JSON 保存独立于三维查看器;Viser 不承担标注状态或文件保存。 + +### 帧号与实际画面保持一致 + +核心状态为 `displayed_frame_index`,表示**已经完成显示的帧**。用户设置起点、终点时只读取这个值。正在请求的目标帧号与已经显示的帧号分开维护;寻帧期间保留旧画面和旧帧号,显示加载状态,暂时禁用标记操作。 + +浏览器按指定帧号请求预览图像,响应携带实际帧号和时间;PTS 仅对视频存在。图像完成加载后,前端一次性更新画面、帧号、时间和游标。快速拖动、暂停或逐帧操作会使旧请求失效;迟到的响应不能覆盖新的画面。 + +播放时按索引中的相邻帧时间差调度。按 `I` 或 `O` 时立即暂停在当前已显示帧,并使后续播放请求失效。性能不足时允许实际播放速度降低,不积压请求或为了追赶时钟跳过帧;界面显示目标速度和实际预览帧率。 + +## 4. 帧与区间约定 + +| 项目 | 首版约定 | +| --- | --- | +| 帧号基准 | 从 0 开始,合法范围为 `0 ... frame_count - 1` | +| 帧号定义 | 图像序列的索引;视频按完整解码的显示顺序,HDF5 按选定 dataset 的行号,内存图像按数组/列表顺序 | +| 区间 | 闭区间 `[start_frame, end_frame]`,两端均包含 | +| 长度 | `end_frame - start_frame + 1`,允许单帧动作 | +| 排序 | 按起点、终点排序;稳定 ID 不随显示顺序改变 | +| 完整性 | 允许空隙,不要求覆盖整个视频 | +| 重叠 | 单轨道,不允许已完成区间重叠;相邻区间为 `[a, b]` 和 `[b + 1, c]` | +| 原子技能名称 | 必填自由文本,去掉首尾空白;从本项目已有名称提供补全 | +| 时间 | 视频以 PTS 计算相对首帧时间;HDF5/数组按明确 FPS 推算显示时间,不冒充实测时间戳 | + +例如“翻转包裹:120–185”包含 66 帧;Python 切片对应 `frames[120:186]`。与现有 recorder 对接时,只有在已确认一帧对应一个控制 step 的情况下,才能转换为 `start_step=120, end_step=186`。 + +前端提供即时校验,后端在保存前独立校验:端点必须为整数,满足 `0 <= start <= end < frame_count`,名称非空,ID 唯一,区间不重叠。修改一个区间时,重叠检查排除其原版本。错误信息明确指出冲突的标注。 + +## 5. 界面与操作流程 + +```text +标题栏:原子技能区间标注 · ← 前一帧 · → 后一帧 · 空格 播放/暂停 · I/O 起止帧 +┌────────────────────────────────────┬──────────────────────────┐ +│ 视频画面、当前帧、播放控制 │ 已标注原子技能区间 │ +│ │ 名称、起止帧、帧数 │ +│ │ 定位 / 编辑 / 删除 │ +├────────────────────────────────────┤ │ +│ 原子技能时间轴(与视频等宽) ├──────────────────────────┤ +│ 连续拖动 / 缩放 / 按帧号跳转 │ 新建/编辑原子技能区间 │ +│ 区间覆盖各帧单元,游标在帧中心 │ 名称、起止帧、备注 │ +│ │ I/O 标记、提交、取消 │ +└────────────────────────────────────┴──────────────────────────┘ +``` + +- 视频区显示当前帧、总帧数、相对时间、播放状态和寻帧状态,支持输入帧号跳转。 +- 时间轴以帧号为坐标,绘制全部区间、当前帧和未提交草稿。拖动从已标注色块内部开始时也按鼠标位置寻帧,不跳转或吸附到区间起点。支持拖动游标、缩放和横向滚动;精确边界通过逐帧键或整数输入调整。首版不提供拖拽修改区间边界。 +- 列表展示序号、原子技能名称、起止帧和帧数,标注较多时滚动查看;选中行与时间轴高亮联动。起点、终点可分别点击跳转。 +- 原子技能区间编辑区位于右侧已标注列表下方,展示当前草稿和错误原因;新建草稿与已完成标注使用不同视觉样式。 +- 保存状态区分“未保存”“保存中”“已保存”“保存失败”。未提交草稿单独提示,不能因其他标注保存成功而显示为已保存。 + +一次常用流程: + +1. 打开视频,在目标动作开始处暂停,用左右键精调到第一帧。 +2. 按 `I` 设置起点,选择或输入原子技能名称。 +3. 播放或跳转到结束位置,用左右键精调到最后一帧,按 `O`。 +4. 检查名称、起止帧和帧数,按 `Enter` 提交;新标注进入列表和时间轴。 +5. 对下一段重复操作;需要返修时选中标注,进入编辑,重新设置端点后提交。 + +### 快捷键 + +| 快捷键 | 行为 | +| --- | --- | +| `Space` | 播放 / 暂停;到最后一帧后自动暂停,再次播放从第 0 帧开始 | +| `←` / `→` | 暂停并向前 / 向后一帧;在视频边界保持当前位置 | +| `Shift + ←` / `Shift + →` | 暂停并向前 / 向后 10 帧 | +| `Home` / `End` | 暂停并跳到首帧 / 末帧 | +| `G` | 聚焦“跳转到帧”输入框,输入后按 Enter 跳转 | +| `I` | 暂停并把当前显示帧设为草稿起点 | +| `O` | 暂停并把当前显示帧设为草稿终点;没有起点时提示先设置起点 | +| `L` | 聚焦原子技能名称输入框 | +| `Enter` | 提交有效草稿;没有草稿时不操作 | +| `E` | 将选中的已完成标注载入编辑草稿 | +| `Esc` | 取消草稿并退出编辑,不修改原标注 | +| `Delete` / `Backspace` | 删除选中的已完成标注 | +| `Ctrl/Cmd + Z` | 撤销最近一次新增、修改或删除 | +| `Ctrl/Cmd + Shift + Z` | 重做 | +| `Ctrl/Cmd + S` | 立即保存已完成标注,阻止浏览器“保存网页”动作 | +| `?` | 显示 / 隐藏快捷键说明 | + +输入框、下拉框或文本编辑区域获得焦点时,不触发全局播放、标记、删除或撤销快捷键;保留文本编辑的原生行为。此时 `Ctrl/Cmd + S` 仍保存已完成标注。名称输入框中的 Enter 只确认输入并把焦点返回视频区,不直接提交区间。 + +方向键允许长按重复;提交、删除、起止标记等操作忽略按键重复事件。页面失去焦点时暂停播放。 + +### 草稿、编辑与删除 + +同一时刻只有一个草稿,状态为“新建”或“编辑已有 ID”。没有草稿时按 `I` 创建新段,默认沿用上次原子技能名称;仅选中一行不会让 `I` 覆盖该行,必须先按 `E` 或点击“编辑”。草稿中再次按 `I/O` 只更新对应端点。 + +编辑期间,原标注保留到提交成功;取消即可完整恢复。存在草稿时禁止删除已完成标注或切换到另一份编辑草稿,提示先提交或取消;浏览画面和定位其他区间仍可进行。 + +删除同时更新列表和时间轴,并显示“已删除,可撤销”。首版不增加逐条删除确认弹窗;撤销栈记录完整标注数据,支持恢复删除和修改,最多保留本次页面会话的最近 100 次操作。撤销后的标注也参与保存。 + +## 6. 解码、索引与资源预算 + +PyAV 的 `seek()` 以时间戳定位到附近关键帧,之后仍需解码到目标位置,不能把传入参数直接当作 frame index。参考:[PyAV InputContainer.seek](https://pyav.org/docs/stable/api/container.html#av.container.InputContainer.seek)。 + +因此拟采用以下流程: + +1. 启动后在后台检查视频、计算文件 SHA-256,并顺序完整解码选定流,建立 `frame_index → PTS / 关键帧锚点` 索引。总帧数以实际输出为准,并包含解码器 flush 后的尾帧。 +2. 界面在初始化期间显示阶段、已扫描帧数和可取消状态。容器总帧数仅作为可能不准确的进度参考;索引完成前不允许标注。 +3. 连续播放和向后逐帧尽量沿用解码状态。向前逐帧优先读取缓存,缓存未命中则定位到目标之前的关键帧再顺序解码。 +4. 随机跳转后用索引中的 PTS 对齐输出,确认目标帧。无法可靠对齐时退回从头解码计数;仍不一致则报告错误,不返回带有猜测帧号的图像。 +5. 首版要求帧 PTS 存在且严格递增;缺失、重复、非单调时间戳或扫描解码报错时,停止初始化并报告原因,不生成部分可编辑项目。可变帧率本身不构成错误。 + +一个专用工作线程独占解码容器。HTTP 处理、播放和拖动不得同时对同一个容器执行 seek/decode;初始化与交互解码也不并行使用它。前端最多维持一个活动帧请求,合并快速连续的寻帧目标;后端只保留当前任务和最新待处理目标,避免队列随拖动增长。预览解码不能阻塞保存与状态接口。 + +设计工作负载为单用户、单视频,常见输入为 720p/1080p、30 FPS、1–10 分钟;另以 1 小时 1080p 视频检查启动耗时和内存。性能实测记录见第 11 节。 + +资源策略: + +- 索引使用两组 int64 数组,分别保存 PTS 和关键帧锚点,空间复杂度 `O(N)`。每帧有效数据 16 字节,1 小时 30 FPS 的索引约 1.65 MiB;不含数组预留容量、Python、解码器和容器开销。配置有效索引数据的字节预算,增长前检查。 +- 解码像素与预览图像按需生成,不把完整视频预加载为 RGB 数组。预览保持宽高比,默认最长边不超过 1280 像素,缩放不改变帧号。 +- 编码预览使用按字节淘汰的 LRU,默认预算 64 MiB;仅保留少量在用原始帧,解码器内部缓冲单独计入实际内存测量。浏览器释放被替换的图像 URL。 +- 完整扫描的时间成本为 `O(N)`,首次打开长视频可能需要等待;首版不增加持久化索引缓存。一次随机跳转通常解码一个关键帧间隔,最坏回退 `O(N)`。 +- 退出、初始化取消或失败时关闭容器并回收工作线程,不遗留后台解码任务。 + +实现阶段测量初始化耗时、缓存命中/未命中的逐帧延迟、随机跳转 P50/P95、实际播放帧率和峰值 RSS,并记录 CPU、视频编码、分辨率、帧率和关键帧间隔。只有实测支持时才承诺具体延迟或播放速度。 + +### 图像帧输入与 HDF5 适配 + +核心 `FrameSource` 协议定义 `prepare(cancelled, progress) -> FrameSourceInfo`、 +`get_frame(index) -> ImageFrame`、`check_unchanged()` 和 `close()`。 +工厂创建与资源操作都由同一个工作线程拥有;只有基于不可变元数据或 stat 的来源检查可在保存线程执行。 +`ImageFrame` 包含原始尺寸 RGB uint8 图像、实际索引、相对时间、下一帧间隔和可选 PTS;输入缓冲区可借用到下一次读帧,预览结果则拥有独立 JPEG 字节。 +共享核心负责缩放、编码、64 MiB LRU 与请求合并,不感知输入容器。 + +`Hdf5FrameSource` 只读选定 dataset,支持逐帧 JPEG/PNG 字节数组和 N×H×W×3 uint8 原始图像; +原始图像显式指定 RGB/BGR。默认解析复数或单数 observations/observation 下的 images/cam_high;显式错误路径不会自动改写。 +初始化仅解码首帧并分块计算文件摘要,交互按需读一帧,不预载整段或其他相机,也不生成视频。 +FPS 来自参数、图像 dataset/root 的 fps 或 metadata_json.record_fps;缺少时要求提供参数。 +FPS 推算时间在页面和 JSON 中显式标明;HDF5 帧号始终是行索引。 +其准备复杂度为 O(文件字节数) 的摘要扫描,单帧读取与解码只占一帧临时内存;不存在视频的 PTS 索引。 + +`ArrayFrameSource` 接收已有 RGB 数组/列表,按帧哈希而不复制整个批次;调用方在运行期间保持输入不变。 +大型自定义数据通过实现协议按需读取。来源身份必须覆盖图像内容、顺序、相机选择和显示时间定义。 + +## 7. 标注文件与保存恢复 + +默认标注文件与输入同目录:视频为 `episode_0001.segments.json`,HDF5 为 `episode.cam_high.segments.json`;可用参数指定其他位置。文件存在时加载并校验,不存在时创建内存中的空项目,首次保存时才创建 JSON 文件。文件路径相对于标注文件解析,来源身份绑定内容、相机/流、帧率和帧信息;重新打开时校验来源一致。摘要不匹配时拒绝覆盖原标注,要求选择匹配的视频或新的标注文件。 + +以下为结构示意,摘要与身份用零占位,区间不代表真实 demo 的人工标签: + +```json +{ + "schema_version": 2, + "frame_index_base": 0, + "interval": "closed", + "source": { + "kind": "hdf5", + "name": "episode.hdf5 / observations/images/cam_high", + "path": "episode.hdf5", + "identity": "0000000000000000000000000000000000000000000000000000000000000000", + "frame_count": 3644, + "width": 640, + "height": 480, + "fps": 30.0, + "timing": "fixed_fps", + "details": { + "sha256": "0000000000000000000000000000000000000000000000000000000000000000", + "image_key": "observations/images/cam_high", + "encoding": "encoded", + "color_order": "encoded", + "fps": 30.0 + } + }, + "revision": 3, + "segments": [ + { + "id": "flip-001", + "label": "翻转包裹", + "start_frame": 120, + "end_frame": 185, + "note": "包裹翻转完成后结束" + } + ] +} +``` + +`source` 与容器无关;适配器信息放在 `details`。视频详情保留实际 `stream_index`、time_base 和可空的 average_fps; +图像序列使用 `fixed_fps`,不保存伪造 PTS。整张视频时间索引不复制进标注 JSON。 +旧版 v1 的 `source_video` 文档仍可读取、校验;读取不改写文件,下一次成功保存才升级为 v2。 +仅保存已完成标注;草稿和撤销历史保留在页面会话中。 + +新增、修改、删除、撤销和重做后立即标为未保存,1 秒无新修改时自动保存。`Ctrl/Cmd + S` 和“保存”立即触发写入;存在草稿时明确提示“草稿未提交”,保存不会隐式创建标注。 + +保存完整快照时,后端重新校验并在同目录写入临时文件,flush/fsync 后通过 `os.replace` 替换目标文件;只有成功后才返回确认。保存失败时保留原文件和页面修改,显示原因并支持重试。“导出 JSON”下载当前已完成标注的副本,不将本地保存失败显示为成功。 + +写入携带预期修订号,后端拒绝旧页面覆盖新修订。前端串行提交保存请求,只有最新修改已经确认后才显示“已保存”;旧请求的完成不能清除新修改的未保存状态。不同标签页的冲突显示明确错误,不自动合并或覆盖。 + +刷新或关闭页面时,如果存在未保存修改或草稿,触发浏览器离开提示;恢复能力以已经收到保存确认的 JSON 为准。非法 JSON、未知 schema 版本或无效区间需报告具体问题,不用空项目覆盖原文件。 + +## 8. 入口与模块划分 + +使用方式: + +```bash +# 安装项目及本工具的可选依赖。 +python -m pip install -e '.[trajectory-annotation]' + +# 创建标注,或恢复同目录已有标注。 +embodichain annotate-trajectory --video /data/episode_0001.mp4 + +# 指定标注文件;不自动打开浏览器。 +embodichain annotate-trajectory \ + --video /data/episode_0001.mp4 \ + --annotations /data/labels/episode_0001.segments.json \ + --port 8765 \ + --no-browser +``` + +输入参数为 `--video` 或 `--hdf5` 二选一;HDF5 可指定 `--image-key`、`--fps`、`--color-order`。其余可选参数为 `--annotations`、`--video-stream`(仅视频)、`--port` 和 `--no-browser`。`--video-stream` 使用容器实际流索引,`--port` 默认 0。预览和缓存预算集中于工具自己的 `@configclass` 配置,首版不增加通用插件或任务配置系统。 + +```text +embodichain/toolkits/trajectory_annotator/ +├── __init__.py # 包说明;不启动服务或导入仿真 +├── __main__.py # python -m 入口 +├── _cli.py # 参数解析、依赖检查、输入选择 +├── app.py # 公共 FastAPI 工厂与本地启动入口 +├── frames.py # 公共帧协议、结果对象与内存图像适配器 +├── video.py # PyAV 视频输入适配器 +├── hdf5.py # 只读 HDF5 图像输入适配器 +├── _file.py # 分块文件摘要和来源身份 +├── _cfg.py # 内部 @configclass 工具配置 +├── _models.py # 区间校验 +├── _frames.py # 与容器无关的工作线程与预览缓存 +├── _storage.py # 来源校验、版本解析、原子保存 +├── _server.py # HTTP 接口、静态资源与错误映射 +├── web/ +│ ├── index.html +│ ├── app.js # 显示状态、草稿、快捷键、撤销与保存调度 +│ └── styles.css +└── README.md # 安装、启动、快捷键、文件格式 +``` + +最小 HTTP 边界: + +| 接口 | 职责 | +| --- | --- | +| `GET /api/status` | 初始化阶段、进度、就绪或错误信息 | +| `GET /api/project` | 帧源元信息、已保存标注、修订号和界面所需配置 | +| `GET /api/frames/{index}` | 指定帧的 JPEG;响应头带实际帧号、时间、可选 PTS 和请求标识 | +| `PUT /api/annotations` | 接收完整标注快照与预期修订号,校验并落盘 | +| `POST /api/cancel` | 取消尚未完成的初始化 | + +Python 模型和存储独立于 HTTP,可用 CPU 测试。前端管理临时编辑状态,后端负责文件格式与持久化校验。公共帧协议和启动 API 允许从已有图像输入复用整个标注界面,视频与 HDF5 均为协议适配器。可运行示例位于 `examples/toolkits/trajectory_annotation/`。 + +## 9. 验收与实施顺序 + +现有 grasp 标注和 recorder 区间测试无法覆盖新工具的视频寻帧、浏览器快捷键或保存恢复,新增针对这些生产行为的覆盖位于 `tests/toolkits/trajectory_annotator/`;沿用 `tests/test_main.py` 检查统一 CLI 注册。按 `add-test` 的必要性审查合并重复场景。 + +| 验收项 | 验证方式 | +| --- | --- | +| 帧号准确 | 用具有独立可识别帧内容的小视频,将随机跳转和前后逐帧结果与从头顺序解码结果逐帧比较;包含 B 帧、跨关键帧、尾部 flush、非零起始 PTS 和可变帧率 | +| 区间规则 | 验证首末帧、单帧段、相邻段、允许空隙、越界和重叠拒绝;修改失败保留原标注 | +| 核心用户流程 | 浏览器自动化完成定位 → I → 名称 → O → 提交 → 编辑 → 删除 → 撤销 → 保存 → 刷新恢复,并检查列表、时间轴与 JSON 一致 | +| 快捷键与异步显示 | 验证文本焦点、长按、视频边界、播放中标记、快速拖动后迟到响应不会导致画面与标注帧号错位 | +| 保存可靠性 | 验证中文标签往返、摘要不匹配、保存失败保留旧文件、旧修订拒绝和保存期间继续编辑的状态 | +| 独立运行与打包 | CLI 帮助不导入仿真;从构建后的 wheel 读取静态界面并启动本地工具 | +| 资源与体验 | 用短视频和 1 小时视频记录初始化、交互延迟、播放帧率和内存;人工检查界面与键盘可操作性 | + +实施分三步完成同一个首版: + +1. 建立模型、文件格式、视频索引与准确取帧,先验证端点语义和帧号正确性。 +2. 实现浏览器画面、时间轴、列表、草稿、快捷键及编辑删除,完成可操作流程。 +3. 接入自动保存、撤销重做、异常处理和 CLI/wheel 打包,完成针对性验证与使用说明。 + +使用 `pre-commit-check` 选择检查范围;新增公共帧输入和启动 API,并配套接口说明。按项目要求运行上下文影响分析并核对 CLI 相关指导,结果为 **no update**:现有主题关于命令分发、仿真、资产、GenSim 和训练的指导仍然准确;新工具自己的 README 负责其独立使用约定,无需为一次独立工具新增上下文主题。未处理工作区中与本工具无关的已有文件。 + +## 10. 已确认的产品选择 + +首版采用以下约定: + +1. **运行方式**:本地服务加浏览器界面,入口为 `embodichain annotate-trajectory`。 +2. **区间语义**:0 基帧号,起止帧均包含;单轨道、允许空隙、不允许重叠。 +3. **标注内容**:自由输入原子技能名称,可选备注;支持修改、删除、撤销和重做。 +4. **输出范围**:首版保存/恢复 JSON 区间标注;实际裁剪视频或机器人轨迹作为后续扩展。 + +用户已确认技术方案后开始实现;当前版本按以上约定提供工具。 + +## 11. 首版验证记录 + +验证环境为 conda `py312`:Python 3.12.15、PyAV 15.1.0、OpenCV 4.11.0.86、FastAPI 0.162.2、Uvicorn 0.54.0。视频首版通过 16 项后端/解码测试、8 项 Chromium/Firefox 浏览器测试,以及 3 项已有 CLI 注册与分发检查。HDF5 扩展复用这些测试并补充图像输入覆盖,最终结果见下文。实际构建 wheel 后,在源码目录外使用其中的静态资源和解码接口完成启动检查。帧协议和文件适配器扩展后新增公共 Python API,配套 API 文档覆盖检查。 + +性能检查使用合成的静态图形 H.264 视频(1920×1080、30 FPS、关键帧间隔 30、含 B 帧),硬件为 AMD Ryzen 9 9950X,解码线程数 2,预览最长边 1280、JPEG 质量 85。它用于验证长视频资源行为,不代表真实遥操视频的编码复杂度。下面保留视频首版、抽象帧输入前的基线记录,为冷启动完整扫描与每种路径 50 次取帧的结果;取帧计时不含 HTTP 和浏览器渲染: + +| 视频 | 完整索引时间 | 随机取帧中位数 / P95 | 顺序取帧中位数 | 有效索引数据 | 采样进程峰值 RSS | +| --- | --- | --- | --- | --- | --- | +| 1 分钟,1800 帧 | 0.91 秒 | 11.83 / 18.02 毫秒 | 2.79 毫秒 | 0.027 MiB | 535.20 MiB | +| 1 小时,108000 帧 | 51.79 秒 | 11.58 / 17.07 毫秒 | 2.80 毫秒 | 1.648 MiB | 540.74 MiB | + +RSS 包含 Python、项目配置依赖和原生解码库;10 毫秒采样可能漏过瞬时峰值。两组独立进程相对索引前运行时的峰值增量分别为 38.78 和 44.18 MiB。合成素材上的 150 次取帧均通过索引/PTS 一致性检查;独立像素一致性由小型真实编码夹具的顺序解码对照测试验证。真实用户视频、Python 3.11 及浏览器端持续播放吞吐尚未单独测量,不据此承诺固定帧率。 + +### 图像帧接口扩展验证 + +真实 HDF5 demo 为 `/home/oem/piper-sim-data/label_down/episode_20261022T073344_323022Z/episode.hdf5`, +实际 dataset 为 `observations/images/cam_high`(复数),3644 个变长 uint8 JPEG、640×480、metadata_json.record_fps=30。 +自动化覆盖压缩/原始图像、RGB/BGR、单复数路径、按需读取、帧时间、相机/帧率/内容身份、保存恢复和线程所有权; +已有视频 PTS/像素对照、浏览器异步定位与保存失败测试继续复用。 + +相同硬件/参数、每种取帧路径 50 次的实测(不含 HTTP/浏览器): + +| 输入 | 准备耗时 | 随机预览中位数 / P95 | 顺序预览中位数 | 采样进程峰值 RSS | +| --- | --- | --- | --- | --- | +| 真实 HDF5,3644 帧,640×480 | 0.215 秒 | 0.993 / 1.101 毫秒 | 0.918 毫秒 | 525.70 MiB | +| 1 分钟 1080p 视频,1800 帧,帧接口抽象后 | 0.891 秒 | 12.148 / 18.500 毫秒 | 2.783 毫秒 | 542.76 MiB | + +HDF5 准备耗时包含全文件 SHA-256,操作系统文件缓存未主动清除;不是磁盘冷缓存性能承诺。 +HDF5 不分配 PTS 表,采样峰值相对准备前运行时增加 29.07 MiB,未保留完整 RGB 序列。 +视频随机预览中位数相较基线约增加 0.32 毫秒(约 3%),顺序读取接近;统一 RGB 输入多一次颜色/缩放处理,保留该取舍以简化协议。 +每组 150 次请求均通过索引/时间一致性检查;独立像素正确性依赖生产测试的对照验证,不将 benchmark 自身的索引检查当作像素验证。 +运行示例和正式 CLI 见工具 README;示例默认将标注写入 outputs/trajectory_annotation,源文件只读。 + +HDF5 接入阶段在 py312 中完成:31 项工具测试(含 Chromium/Firefox)、43 项统一 CLI 测试和 11 项 API 文档检查测试,共 85 项通过。 +真实 demo 另经两种浏览器验证首尾跳转、逐帧、区间创建、保存、刷新恢复和删除;对第 0、1、1822、3643 帧做独立 JPEG 像素对照,预览与原帧平均绝对误差为 0.794–1.013/255。 +构建最终 wheel 后,在源码目录外验证帧适配器、网页资源和两个 example 均已打包,并用真实 HDF5 完成 ASGI 启动/末帧请求检查。正式 --hdf5 CLI 与 Ctrl+C 正常退出也通过。 +Black 26.10.1 全仓检查和 4442/4442 公共 API 文档覆盖检查通过。 + +按用户要求在 py312 安装 docs/requirements.txt 的文档依赖(含 Sphinx 7.4.7)。新增 API 页在包含 autodocsumm 的独立 Sphinx 严格构建中无警告通过。 +仓库的 architecture_sphinx 扩展要求相关源码已提交,因此在只包含本次改动的临时独立工作副本中运行全量 dummy 构建;构建成功,日志报告其他文档的 1194 条警告,新增帧标注 API 无诊断。临时工作副本已清理,原工作区保留未提交改动。 + +## 12. 原子技能时间轴交互修正 + +按原子技能区间统一界面、前端状态/函数/DOM 和后端校验命名;JSON/API 的 segments 字段保留现有兼容格式。 +时间轴从按钮色块改为可选中的可视区域,所有指针事件由时间轴统一捕获,不在点击或拖动后跳到技能起点。 +删除色块的最小宽度、边框占位与外层内边距,标签在独立子元素中裁切;区间几何按 [start/N, (end+1)/N] 绘制,游标和刻度按 (index+0.5)/N 对齐。 +新增浏览器回归检查覆盖从区间内部拖动、穿过其他区间与空隙、单帧/首尾帧色块、缩放及横向滚动;已有键盘编辑测试同时检查新布局与标题栏提示。 +上下文审查为 no update:改动位于独立标注工具内,命令分发和其他已路由子系统的所有权与行为约定保持准确,使用约定由本工具 README 维护。 + +本轮验证:33 项工具测试(含 10 项 Chromium/Firefox 浏览器用例)和 43 项 CLI 测试共 76 项通过;Black 26.10.1 与公共 API 覆盖检查通过。 +真实 HDF5 demo 在 Chromium/Firefox 中验证了自由拖动及小于 1 像素的单帧色块几何,并检查 1440×1100、1024×900、390×844 布局无水平溢出。 +长序列的指针定位测试放大至每帧覆盖多个像素,避免浏览器鼠标坐标像素取整影响断言;颜色区间几何在原始缩略比例下独立核对。 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/index.rst b/docs/source/api_reference/index.rst index 192155d230..99b88d87e9 100644 --- a/docs/source/api_reference/index.rst +++ b/docs/source/api_reference/index.rst @@ -85,6 +85,7 @@ documentation. CI runs this same checker after style checks and before tests. :maxdepth: 1 public_api + skill_fitting embodichain/embodichain.gen_sim.task_engine benchmarks locomotion_tasks diff --git a/docs/source/api_reference/public_api.rst b/docs/source/api_reference/public_api.rst index 4fc4219df0..39bea20b8c 100644 --- a/docs/source/api_reference/public_api.rst +++ b/docs/source/api_reference/public_api.rst @@ -1716,6 +1716,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 ----------------------------------- @@ -1729,6 +1787,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 ------------------------------------ @@ -1820,6 +1901,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 -------------------------------- @@ -1831,6 +1927,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 ------------------------------ @@ -1842,6 +1958,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 --------------------------------- @@ -1870,6 +2010,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 --------------------------------- @@ -1961,6 +2123,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 -------------------------------- @@ -1974,6 +2162,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 ---------------------------------- @@ -3724,6 +3938,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 ------------------------------------------ @@ -4363,6 +4603,8 @@ native simulation windows and browser visualization. MarkerPrototypeCfg MarkerGroup + MarkerFramePublisher + embodichain.lab.visualization.markers.cfg ---------------------------------------- @@ -4373,6 +4615,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 ------------------------------------------ @@ -6209,3 +6467,18 @@ contracts. The package-level imports above are the convenient extension API. discover_sensor_plugins 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: diff --git a/docs/source/api_reference/skill_fitting.rst b/docs/source/api_reference/skill_fitting.rst new file mode 100644 index 0000000000..38424c41e0 --- /dev/null +++ b/docs/source/api_reference/skill_fitting.rst @@ -0,0 +1,62 @@ +Video-selected Atomic Motion Reconstruction +=========================================== + +The fit-trajectory command maps an annotator's inclusive video interval to +explicit joint timestamps, estimates a motion goal and invokes the existing +Atomic Skill engine. PickUpMotionGoal, SlideMotionGoal, TwistMotionGoal and +HandOverMotionGoal accept explicit world TCP anchors without an object identity +or mesh. These goals declare no held-object effect. Collision evidence and +physical success remain unverified. + +The fit preserves the original joint clock. A bounded monotone projection fits +the phase clock to the independently generated native path. Phase endpoints and +Pickup's internal pre-grasp anchor remain mandatory. Native reconstruction +errors are checked separately from the final boundary correction. Three +original samples at each outer boundary retain the original discrete +position/velocity/acceleration stencils. This does not establish continuous-time +smoothness of noisy measurements or physical task success. + +The source joint order, calibration, open/grasp commands, derivative limits and +video/joint time offset must be explicit. HDF5 image timing derived from FPS is +identified as derived, not measured. Missing hand events can be supplied as a +complete segment-relative phase partition; video pixels are not automatically +interpreted as contact evidence. + +Recording and persistence +------------------------- + +.. automodule:: embodichain.data_pipeline.skill_fitting + :members: + +Native skill fitting +-------------------- + +.. automodule:: embodichain.lab.sim.atomic_skills.fitting + :members: + +Task Program execution bindings +------------------------------- + +Use ``export_task_program`` to bind an accepted Pickup fit to a supplied +execution context and its explicitly assigned demonstration spans. The export +preserves the execution cadence and records how timed source events map to +compiled invocations. Use ``load_invocation_bindings`` to verify the sealed files +and validate the invocation policies against a fresh compile before execution. + +.. automodule:: embodichain.toolkits.skill_fitting.task_program + :members: + +Numerical primitives +-------------------- + +.. automodule:: embodichain.compute.trajectory.phase_alignment + :members: + +.. automodule:: embodichain.compute.trajectory.splicing + :members: + +Command line +------------ + +.. automodule:: embodichain.toolkits.skill_fitting.cli + :members: diff --git a/docs/source/api_reference/trajectory_annotator.rst b/docs/source/api_reference/trajectory_annotator.rst new file mode 100644 index 0000000000..f88ec956b7 --- /dev/null +++ b/docs/source/api_reference/trajectory_annotator.rst @@ -0,0 +1,86 @@ +:orphan: + +Trajectory image annotation +=========================== + +The annotation application consumes indexed RGB images through a common frame +source. Video and HDF5 adapters own their input resources; the application owns +one worker thread, a bounded JPEG preview cache and revision-checked JSON output. +Frame indices are zero-based and action intervals include both endpoints. + +Frame input contract +-------------------- + +.. currentmodule:: embodichain.toolkits.trajectory_annotator.frames + +.. autosummary:: + + ImageFrame + FrameSourceInfo + FrameSource + ArrayFrameSource + +.. autoclass:: ImageFrame + :members: + +.. autoclass:: FrameSourceInfo + :members: + +.. autoclass:: FrameSource + :members: + +.. autoclass:: ArrayFrameSource + :members: + +File adapters +------------- + +Video uses presentation timestamps and a full presentation-order index. HDF5 +reads a selected camera dataset on demand; its display time is derived from an +explicit or recorded FPS. Neither adapter preloads the full RGB sequence. + +.. currentmodule:: embodichain.toolkits.trajectory_annotator.hdf5 + +.. autosummary:: + + Hdf5FrameSource + +.. autoclass:: Hdf5FrameSource + :members: + +.. currentmodule:: embodichain.toolkits.trajectory_annotator.video + +.. autosummary:: + + VideoFrameSource + +.. autoclass:: VideoFrameSource + :members: + +Application entry points +------------------------ + +Pass a factory returning a fresh source. Construction, preparation, reads and +cleanup run on the same worker thread. Caller-owned image arrays must remain +unchanged for the application's lifetime. + +.. currentmodule:: embodichain.toolkits.trajectory_annotator.app + +.. autosummary:: + + create_annotation_app + launch_annotation + +.. autofunction:: create_annotation_app + +.. autofunction:: launch_annotation + +For example:: + + from embodichain.toolkits.trajectory_annotator.app import launch_annotation + from embodichain.toolkits.trajectory_annotator.hdf5 import Hdf5FrameSource + + launch_annotation( + lambda: Hdf5FrameSource("episode.hdf5"), + "episode.cam_high.segments.json", + ) 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/cli/main.py b/embodichain/cli/main.py index 28fea09094..4a21d07140 100644 --- a/embodichain/cli/main.py +++ b/embodichain/cli/main.py @@ -37,6 +37,11 @@ class Command: COMMANDS = ( + Command( + name="fit-trajectory", + target="embodichain.toolkits.skill_fitting.cli:main", + help="Fit a video-selected joint segment through native atomic skills.", + ), Command( name="gensim", target="embodichain.cli.gensim:main", @@ -167,6 +172,11 @@ class Command: target="embodichain.lab.scripts.annotate_grasp:cli", help="Interactively annotate a grasp region on a mesh.", ), + Command( + name="annotate-trajectory", + target="embodichain.toolkits.trajectory_annotator._cli:cli", + help="Annotate atomic skill frame intervals in an image sequence.", + ), Command( name="decompose-urdf", target="embodichain.toolkits.acd.cli:main", 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/compute/trajectory/phase_alignment.py b/embodichain/compute/trajectory/phase_alignment.py new file mode 100644 index 0000000000..ad2cc9b7d7 --- /dev/null +++ b/embodichain/compute/trajectory/phase_alignment.py @@ -0,0 +1,92 @@ +# ---------------------------------------------------------------------------- +# 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 monotone clock fitting to an independently generated joint path.""" + +from __future__ import annotations + +import numpy as np + +__all__ = ["align_joint_phase"] + + +def align_joint_phase( + native: np.ndarray, observations: np.ndarray +) -> tuple[np.ndarray, np.ndarray]: + """Project observations onto native edges with monotone progress. + + The output always lies on the native joint polyline. The observations + determine only the phase clock. Coarse-to-fine searches operate in bounded + chunks instead of allocating a full source-by-native distance matrix. + Call separately between every required skill anchor. + + Args: + native: Independently generated native path, shaped (N,D). + observations: Source phase on its unchanged clock, shaped (T,D). + + Returns: + Native-path samples on the observed clock and normalized native index + progress. Endpoint and direction mismatches remain visible as errors. + """ + path, source = (np.asarray(v, dtype=np.float64) for v in (native, observations)) + if ( + path.ndim != 2 + or source.ndim != 2 + or not len(path) + or not len(source) + or path.shape[1] != source.shape[1] + or not np.isfinite(path).all() + or not np.isfinite(source).all() + or max(len(path), len(source)) > 200_000 + ): + raise ValueError("Phase alignment requires bounded finite joint paths.") + if len(path) == 1 or np.max(np.abs(path - path[0])) < 1e-12: + return np.repeat(path[:1], len(source), axis=0), np.linspace(0, 1, len(source)) + coarse = np.unique(np.linspace(0, len(path) - 1, min(128, len(path))).astype(int)) + radius = int(np.ceil((len(path) - 1) / max(1, len(coarse) - 1))) + 2 + offsets = np.arange(-radius, radius + 1) + indices = np.empty(len(source)) + for start in range(0, len(source), 64): + values = source[start : start + 64] + distances = np.sum((values[:, None] - path[coarse][None]) ** 2, axis=-1) + centers = coarse[np.argmin(distances, axis=1)] + edges = np.clip(centers[:, None] + offsets, 0, len(path) - 2) + left, delta = path[edges], path[edges + 1] - path[edges] + squared = np.sum(delta**2, axis=-1) + fraction = np.clip( + np.sum((values[:, None] - left) * delta, axis=-1) + / np.maximum(squared, 1e-30), + 0, + 1, + ) + error = np.sum( + (values[:, None] - left - fraction[..., None] * delta) ** 2, axis=-1 + ) + best = np.argmin(error, axis=1) + indices[start : start + len(values)] = ( + edges[np.arange(len(values)), best] + fraction[np.arange(len(values)), best] + ) + indices = np.maximum.accumulate(indices) + indices[0], indices[-1] = 0.0, len(path) - 1.0 + if len(source) == 1: + indices[0] = len(path) - 1.0 + result = np.column_stack( + [ + np.interp(indices, np.arange(len(path)), path[:, joint]) + for joint in range(path.shape[1]) + ] + ) + return result, indices / (len(path) - 1) diff --git a/embodichain/compute/trajectory/splicing.py b/embodichain/compute/trajectory/splicing.py new file mode 100644 index 0000000000..b0ab7bdd78 --- /dev/null +++ b/embodichain/compute/trajectory/splicing.py @@ -0,0 +1,336 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""Fixed-clock joint replacement with unchanged boundary derivative stencils.""" + +from __future__ import annotations + +import numpy as np +from numpy.typing import NDArray + +__all__ = ["splice_joint_segment", "joint_path_diagnostics"] + + +def _path(qpos: np.ndarray, timestamps: np.ndarray) -> tuple[np.ndarray, np.ndarray]: + q, t = np.asarray(qpos), np.asarray(timestamps, dtype=np.float64) + if ( + q.ndim != 2 + or q.dtype.kind != "f" + or len(q) < 3 + or t.shape != (len(q),) + or not np.isfinite(q).all() + or not np.isfinite(t).all() + or np.any(np.diff(t) <= 0) + ): + raise ValueError( + "Expected finite floating (T,D) joints and increasing (T,) times." + ) + return q, t + + +def splice_joint_segment( + original: np.ndarray, + replacement: np.ndarray, + timestamps: np.ndarray, + *, + start: int, + stop: int, + joint_indices: tuple[int, ...], + blend_samples: int = 12, + anchors: tuple[int, ...] = (), +) -> NDArray[np.floating]: + """Replace a half-open segment on its original clock. + + A quintic correction envelope preserves three original samples at both + ends. Thus the three-point position/velocity/acceleration stencils crossing + the outer joins are identical to the original. This is a discrete C2 + boundary contract, not a claim that noisy observations are a smooth curve. + Anchor rows use the native replacement exactly. Callers must independently + validate TCP/contact constraints and limits after blending. + + Args: + original: Full source joints, shaped (T,D). + replacement: Native reconstructed segment, shaped (stop-start,D). + timestamps: Source sample times; never retimed. + start: First included source sample. + stop: First excluded source sample. + joint_indices: Explicit editable columns. + blend_samples: Width of each quintic boundary connection. + anchors: Absolute source indices that must retain the native solution. + + Returns: + An owned array with source dtype and unchanged values outside the mask. + """ + q, _ = _path(original, timestamps) + count = stop - start + candidate = np.asarray(replacement) + if ( + type(start) is not int + or type(stop) is not int + or not 0 <= start < stop <= len(q) + or count < 12 + or candidate.shape != (count, q.shape[1]) + or not np.isfinite(candidate).all() + or type(blend_samples) is not int + or blend_samples < 4 + or not joint_indices + or len(set(joint_indices)) != len(joint_indices) + or any(type(i) is not int or not 0 <= i < q.shape[1] for i in joint_indices) + ): + raise ValueError("Invalid replacement, editable columns, or connection width.") + width = min(blend_samples, (count - 6) // 2) + interior_anchors = [a - start for a in anchors if start + 3 <= a < stop - 3] + if interior_anchors: + width = min(width, min(interior_anchors) - 3, count - 4 - max(interior_anchors)) + if width < 3: + raise ValueError("Boundary connection overlaps a protected skill anchor.") + u = np.linspace(0.0, 1.0, width + 1) + ramp = u**3 * (10.0 + u * (-15.0 + 6.0 * u)) + weight = np.ones(count) + weight[:3] = weight[-3:] = 0.0 + weight[3 : 3 + width + 1] = ramp + weight[-3 - width - 1 : -3] = ramp[::-1] + ids = list(joint_indices) + for anchor in anchors: + if type(anchor) is not int or not start <= anchor < stop: + raise ValueError("Anchor is outside the selected segment.") + index = anchor - start + if index < 3 or index >= count - 3: + if not np.allclose( + candidate[index, ids], q[anchor, ids], atol=1e-6, rtol=0 + ): + raise ValueError( + "Native anchor conflicts with the immutable boundary stencil." + ) + else: + # A hard anchor cannot introduce an isolated blend-weight jump. + if weight[index] < 1.0 - 1e-12: + raise ValueError( + "Boundary connection overlaps a protected skill anchor." + ) + result = q.copy() + source = q[start:stop, ids] + result[start:stop, ids] = source + weight[:, None] * ( + candidate[:, ids].astype(np.float64) - source + ) + # Preserve these bytes, including floating signed zero. + result[start : start + 3, ids] = q[start : start + 3, ids] + result[stop - 3 : stop, ids] = q[stop - 3 : stop, ids] + return result + + +def _connect_joint_segment( + original: np.ndarray, + replacement: np.ndarray, + timestamps: np.ndarray, + *, + start: int, + stop: int, + joint_indices: tuple[int, ...], + protected: tuple[int, int], + blend_samples: int, + velocity_limits: np.ndarray, + acceleration_limits: np.ndarray, +) -> NDArray[np.floating]: + """Join a native path with quintic bridges, leaving its contact core exact. + + Three original rows at each join retain the original discrete derivative + stencils. Unlike a pointwise blend, the bridges do not inherit interior + measurement noise. ``protected`` uses segment-relative half-open bounds. + """ + q, t = _path(original, timestamps) + candidate = np.asarray(replacement) + count = stop - start + first, last = protected + if ( + not 0 <= start < stop <= len(q) + or candidate.shape != (count, q.shape[1]) + or not np.isfinite(candidate).all() + or not 6 <= first < last <= count - 6 + or blend_samples < 4 + or not joint_indices + or len(set(joint_indices)) != len(joint_indices) + or any(not 0 <= j < q.shape[1] for j in joint_indices) + ): + raise ValueError( + "Native contact core needs room for both boundary connections." + ) + ids = list(joint_indices) + result = q.copy() + result[start:stop, ids] = candidate[:, ids] + segment = result[start:stop, ids].astype(np.float64) + source = q[start:stop, ids].astype(np.float64) + clock = t[start:stop] + + def derivatives( + values: np.ndarray, indices: np.ndarray, at: int + ) -> tuple[np.ndarray, np.ndarray]: + offset = clock[indices] - clock[at] + polynomial = np.linalg.solve( + np.column_stack((np.ones(3), offset, offset**2)), values[indices] + ) + return polynomial[1], 2 * polynomial[2] + + def bridge(a: int, b: int, left_source: bool) -> None: + duration = clock[b] - clock[a] + if left_source: + p0, p1 = source[a], segment[b] + v0, a0 = derivatives(source, np.arange(a - 2, a + 1), a) + v1, a1 = derivatives(segment, np.arange(b - 1, b + 2), b) + else: + p0, p1 = segment[a], source[b] + v0, a0 = derivatives(segment, np.arange(a - 1, a + 2), a) + v1, a1 = derivatives(source, np.arange(b, b + 3), b) + c0, c1, c2 = p0, v0 * duration, a0 * duration**2 / 2 + rhs = np.stack( + (p1 - c0 - c1 - c2, v1 * duration - c1 - 2 * c2, a1 * duration**2 - 2 * c2) + ) + c3, c4, c5 = np.linalg.solve( + np.array([[1, 1, 1], [3, 4, 5], [6, 12, 20]], dtype=float), rhs + ) + u = ((clock[a : b + 1] - clock[a]) / duration)[:, None] + segment[a : b + 1] = c0 + u * (c1 + u * (c2 + u * (c3 + u * (c4 + u * c5)))) + + def boundary_index(indices: range, left: bool) -> int: + # Preserve noisy measured boundary samples and start the connection at + # a quieter stencil inside the free phase. Do not propagate boundary + # noise into a long quintic polynomial or alter an unrelated interval. + best, best_score = indices[0], float("inf") + for index in indices: + stencil = ( + np.arange(index - 2, index + 1) if left else np.arange(index, index + 3) + ) + velocity, acceleration = derivatives(source, stencil, index) + score = max( + float(np.max(np.abs(velocity) / velocity_limits[ids])), + float(np.max(np.abs(acceleration) / acceleration_limits[ids])), + ) + if score < 0.25: + return index + if score < best_score: + best, best_score = index, score + return best + + a = boundary_index(range(2, max(3, first // 3)), True) + b = boundary_index( + range(count - 3, min(count - 4, last + 2 * (count - last) // 3), -1), False + ) + left_end = min(first - 1, max(a + blend_samples, (a + first) // 2)) + right_start = max(last, min(b - blend_samples, last + (b - last) // 4)) + bridge(a, left_end, True) + bridge(right_start, b, False) + segment[: a + 1] = source[: a + 1] + segment[b:] = source[b:] + result[start:stop, ids] = segment.astype(q.dtype) + result[start : start + 3, ids] = q[start : start + 3, ids] + result[stop - 3 : stop, ids] = q[stop - 3 : stop, ids] + return result + + +def _replacement_diagnostics( + original: np.ndarray, + proposed: np.ndarray, + timestamps: np.ndarray, + *, + lower: np.ndarray, + upper: np.ndarray, + velocity_limits: np.ndarray, + acceleration_limits: np.ndarray, +) -> dict[str, object]: + """Qualify every changed position/edge/acceleration stencil, including joins. + + Unchanged source data may already violate limits. It remains reported by + ``joint_path_diagnostics`` and cannot be repaired by editing another arm or + another interval. This check never excuses a newly generated violation. + """ + q, t = _path(proposed, timestamps) + changed = q != original + velocity_mask = changed[1:] | changed[:-1] + acceleration_mask = velocity_mask[1:] | velocity_mask[:-1] + dt = np.diff(t) + velocity = np.diff(q.astype(float), axis=0) / dt[:, None] + acceleration = np.diff(velocity, axis=0) / ((dt[1:] + dt[:-1]) * 0.5)[:, None] + peak_v = np.max(np.where(velocity_mask, np.abs(velocity), 0), axis=0) + peak_a = np.max(np.where(acceleration_mask, np.abs(acceleration), 0), axis=0) + positions = changed & ((q < lower - 1e-6) | (q > upper + 1e-6)) + return { + "accepted": bool( + not positions.any() + and np.all(peak_v <= velocity_limits * (1 + 1e-5)) + and np.all(peak_a <= acceleration_limits * (1 + 1e-5)) + ), + "position_violations": int(positions.sum()), + "peak_velocity": peak_v.tolist(), + "peak_acceleration": peak_a.tolist(), + "velocity_ratio": float(np.max(peak_v / velocity_limits)), + "acceleration_ratio": float(np.max(peak_a / acceleration_limits)), + "scope": "modified positions and their velocity/acceleration stencils", + } + + +def joint_path_diagnostics( + qpos: np.ndarray, + timestamps: np.ndarray, + *, + lower: np.ndarray, + upper: np.ndarray, + velocity_limits: np.ndarray, + acceleration_limits: np.ndarray, +) -> dict[str, object]: + """Measure finite-grid limits using physical timestamps. + + Args: + qpos: Full joint reference, shaped (T,D). + timestamps: Strictly increasing physical sample times. + lower: Per-joint position lower bounds. + upper: Per-joint position upper bounds. + velocity_limits: Positive per-joint speed caps. + acceleration_limits: Positive per-joint acceleration caps. + + Returns: + Limit acceptance and worst per-joint derivative magnitudes. Collision + and physical effect verification are intentionally absent. + """ + q, t = _path(qpos, timestamps) + arrays = [ + np.asarray(v, dtype=np.float64) + for v in (lower, upper, velocity_limits, acceleration_limits) + ] + if any(v.shape != (q.shape[1],) or not np.isfinite(v).all() for v in arrays): + raise ValueError("Limits must be finite per-joint vectors.") + lo, hi, speed, accel = arrays + if np.any(lo >= hi) or np.any(speed <= 0) or np.any(accel <= 0): + raise ValueError("Position intervals and derivative caps must be positive.") + # Edge velocities cannot hide a single-step jump through central averaging. + edge_v = np.diff(q.astype(np.float64), axis=0) / np.diff(t)[:, None] + edge_a = np.diff(edge_v, axis=0) / ( + (np.diff(t)[1:] + np.diff(t)[:-1])[:, None] * 0.5 + ) + peak_v = np.max(np.abs(edge_v), axis=0) + peak_a = np.max(np.abs(edge_a), axis=0) + return { + "accepted": bool( + np.all(q >= lo - 1e-6) + and np.all(q <= hi + 1e-6) + and np.all(peak_v <= speed * (1 + 1e-5)) + and np.all(peak_a <= accel * (1 + 1e-5)) + ), + "position_violations": int(np.count_nonzero((q < lo - 1e-6) | (q > hi + 1e-6))), + "peak_velocity": peak_v.tolist(), + "peak_acceleration": peak_a.tolist(), + "velocity_ratio": float(np.max(peak_v / speed)), + "acceleration_ratio": float(np.max(peak_a / accel)), + } 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/data_pipeline/skill_fitting.py b/embodichain/data_pipeline/skill_fitting.py new file mode 100644 index 0000000000..949e1c6db4 --- /dev/null +++ b/embodichain/data_pipeline/skill_fitting.py @@ -0,0 +1,340 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""Joint recordings, explicit video alignment, and self-contained fit bundles.""" + +from __future__ import annotations + +from dataclasses import dataclass +import hashlib +import json +import math +import os +from pathlib import Path +import tempfile +from typing import Any +import zipfile + +import numpy as np + +__all__ = [ + "JointRecording", + "load_joint_recording", + "map_video_interval", + "save_skill_fit", + "load_skill_fit", + "file_digest", +] + +_MAX_BYTES = 256 * 1024 * 1024 + + +def file_digest(path: str | Path) -> str: + """Hash a source without retaining its contents. + + Args: + path: Existing source file. + + Returns: + Lowercase SHA-256 digest. + """ + digest = hashlib.sha256() + with Path(path).open("rb") as stream: + for block in iter(lambda: stream.read(1024 * 1024), b""): + digest.update(block) + return digest.hexdigest() + + +@dataclass(frozen=True) +class JointRecording: + """Owned joints with explicit order, clock and observation semantics. + + Args: + timestamps: Strictly increasing physical times in seconds. + qpos: Numeric (T,D) samples in robot joint units. + joint_names: Ordered, unique joint identifiers. + semantics: measured_state, actual_submission or design_reference. + """ + + timestamps: np.ndarray + qpos: np.ndarray + joint_names: tuple[str, ...] + semantics: str + + def __post_init__(self) -> None: + q = np.asarray(self.qpos) + t = np.asarray(self.timestamps, dtype=np.float64) + names = tuple(self.joint_names) + if ( + q.ndim != 2 + or q.shape[1] == 0 + or q.dtype.kind not in "fi" + or len(q) < 12 + or q.nbytes + t.nbytes > _MAX_BYTES + or t.shape != (len(q),) + or not np.isfinite(q).all() + or not np.isfinite(t).all() + or np.any(np.diff(t) <= 0) + or len(names) != q.shape[1] + or len(set(names)) != len(names) + or any(not isinstance(n, str) or not n or len(n) > 256 for n in names) + ): + raise ValueError("Invalid joint recording, joint order, or physical clock.") + if self.semantics not in { + "measured_state", + "actual_submission", + "design_reference", + }: + raise ValueError( + "Declare measured_state, actual_submission or design_reference." + ) + q, t = ( + np.array( + q, dtype=q.dtype if q.dtype.kind == "f" else np.float64, copy=True + ), + t.copy(), + ) + q.setflags(write=False) + t.setflags(write=False) + object.__setattr__(self, "qpos", q) + object.__setattr__(self, "timestamps", t) + object.__setattr__(self, "joint_names", names) + + +def _read_npz(path: Path) -> dict[str, np.ndarray]: + """Preflight decoded NPY allocation sizes before materializing arrays.""" + total = 0 + with zipfile.ZipFile(path) as archive: + names = archive.namelist() + if len(names) > 64 or len(set(names)) != len(names): + raise ValueError("Too many or duplicate NPZ members.") + for member in archive.infolist(): + if not member.filename.endswith(".npy") or member.file_size > _MAX_BYTES: + raise ValueError("Invalid or oversized NPZ member.") + with archive.open(member) as stream: + version = np.lib.format.read_magic(stream) + if version == (1, 0): + shape, _, dtype = np.lib.format.read_array_header_1_0(stream) + elif version == (2, 0): + shape, _, dtype = np.lib.format.read_array_header_2_0(stream) + else: + raise ValueError("Unsupported NPY header version.") + size = math.prod(shape) * dtype.itemsize + total += size + if dtype.hasobject or total > _MAX_BYTES or size > member.file_size: + raise ValueError("NPZ decoded arrays exceed the allocation budget.") + with np.load(path, allow_pickle=False) as data: + return {name: data[name] for name in data.files} + + +def load_joint_recording( + path: str | Path, + *, + joint_names: tuple[str, ...], + semantics: str, + qpos_key: str = "qpos", + timestamps_key: str = "timestamps", + sample_dt: float | None = None, + env_index: int = 0, +) -> JointRecording: + """Load NPZ, HDF5, or a trusted local Atomic replay PT file. + + PT loading uses PyTorch's weights-only reader. NPZ is the portable input. + Missing clocks require an explicit sample_dt; recording FPS is never guessed. + + Args: + path: Source recording. + joint_names: Expected robot order, also used when the file has no names. + semantics: Explicit meaning of qpos. + qpos_key: NPZ/HDF5 key, or PT positions key. + timestamps_key: Physical time key. + sample_dt: Explicit period for recordings without timestamps or dt. + env_index: Selected row of a batched PT replay. + + Returns: + A validated recording reordered by explicit joint names. + """ + source = Path(path) + if type(env_index) is not int or env_index < 0: + raise ValueError("env_index must be a nonnegative integer.") + if source.suffix == ".npz": + data = _read_npz(source) + elif source.suffix in {".h5", ".hdf5"}: + import h5py + + with h5py.File(source, "r") as handle: + keys = [qpos_key] + ([timestamps_key] if timestamps_key in handle else []) + if any(handle[k].dtype.hasobject for k in keys): + raise ValueError( + "Joint and time datasets must contain fixed-size numeric values." + ) + if ( + sum(handle[k].size * handle[k].dtype.itemsize for k in keys) + > _MAX_BYTES + ): + raise ValueError("HDF5 numeric input exceeds the allocation budget.") + data = {key: handle[key][...] for key in keys} + elif source.suffix == ".pt": + import torch + + if source.stat().st_size > _MAX_BYTES: + raise ValueError("PT replay exceeds the input budget.") + raw = torch.load(source, weights_only=True, map_location="cpu") + if qpos_key == "qpos": + qpos_key = "positions" + data = {} + for name in (qpos_key, timestamps_key, "dt", "planned_dt"): + if name in raw: + value = raw[name] + if not isinstance(value, torch.Tensor): + raise ValueError("PT numeric fields must be tensors.") + value = value.numpy() + if value.ndim == (3 if name == qpos_key else 2): + value = value[env_index] + data[name] = value + if timestamps_key not in data: + interval_key = "planned_dt" if qpos_key == "planned_positions" else "dt" + if interval_key in data: + data[timestamps_key] = np.cumsum(data[interval_key], dtype=np.float64) + else: + raise ValueError("Joint recordings support .npz, .h5/.hdf5 and local .pt.") + q = np.asarray(data[qpos_key]) + if timestamps_key in data: + times = np.asarray(data[timestamps_key], dtype=np.float64) + elif sample_dt is not None and math.isfinite(sample_dt) and sample_dt > 0: + times = np.arange(len(q), dtype=np.float64) * sample_dt + else: + raise ValueError("Missing joint timestamps; provide an explicit sample_dt.") + if "joint_names" in data: + source_names = tuple(str(v) for v in data["joint_names"]) + if len(set(source_names)) != len(source_names) or set(source_names) != set( + joint_names + ): + raise ValueError("Source and robot joint identifiers differ.") + q = q[:, [source_names.index(name) for name in joint_names]] + return JointRecording(times, q, joint_names, semantics) + + +def map_video_interval( + frame_times: np.ndarray, + frame_durations: np.ndarray, + joint_times: np.ndarray, + *, + start_frame: int, + end_frame: int, + time_offset: float, +) -> tuple[int, int]: + """Map an inclusive video interval to a half-open joint interval. + + Args: + frame_times: Video presentation times relative to its first frame. + frame_durations: Positive display duration of every indexed frame. + joint_times: Strictly increasing physical joint times. + start_frame: Included annotation start. + end_frame: Included annotation end. + time_offset: Joint time corresponding to video time zero. + + Returns: + First included and first excluded joint indices. + """ + ft, fd, jt = ( + np.asarray(v, dtype=np.float64) + for v in (frame_times, frame_durations, joint_times) + ) + if ( + ft.ndim != 1 + or fd.shape != ft.shape + or jt.ndim != 1 + or not len(jt) + or not np.isfinite(ft).all() + or not np.isfinite(fd).all() + or not np.isfinite(jt).all() + or not math.isfinite(time_offset) + or np.any(np.diff(ft) <= 0) + or np.any(fd <= 0) + or np.any(np.diff(jt) <= 0) + or type(start_frame) is not int + or type(end_frame) is not int + or not 0 <= start_frame <= end_frame < len(ft) + ): + raise ValueError("Invalid indexed video clock or inclusive frame interval.") + left = ft[start_frame] + time_offset + right = ft[end_frame] + fd[end_frame] + time_offset + tolerance = 1e-7 + if left < jt[0] - tolerance or left > jt[-1] + tolerance or right <= jt[0]: + raise ValueError("Video interval is outside the joint recording clock.") + if ( + right + > jt[-1] + max(float(fd[end_frame]), float(np.median(np.diff(jt)))) + tolerance + ): + raise ValueError("Video interval exceeds the joint recording.") + return ( + int(np.searchsorted(jt, left - tolerance, side="left")), + int(np.searchsorted(jt, right - tolerance, side="left")), + ) + + +def save_skill_fit( + path: str | Path, arrays: dict[str, np.ndarray], metadata: dict[str, Any] +) -> None: + """Atomically write a standalone numeric result, without pickle. + + Args: + path: Destination NPZ. + arrays: Original, native and patched samples plus visualization geometry. + metadata: Versioned fit parameters, profile, clocks and diagnostics. + """ + destination = Path(path) + if metadata.get("schema_version") != 1: + raise ValueError("Fit metadata must declare schema_version=1.") + values = {k: np.asarray(v) for k, v in arrays.items()} + if "metadata_json" in values or any(v.dtype.hasobject for v in values.values()): + raise ValueError("Result arrays cannot use objects or reserved names.") + if sum(v.nbytes for v in values.values()) > _MAX_BYTES: + raise ValueError("Result exceeds the numeric budget.") + values["metadata_json"] = np.array( + json.dumps(metadata, allow_nan=False, sort_keys=True) + ) + destination.parent.mkdir(parents=True, exist_ok=True) + fd, temporary = tempfile.mkstemp(dir=destination.parent, suffix=".npz") + try: + with os.fdopen(fd, "wb") as stream: + np.savez_compressed(stream, **values) + stream.flush() + os.fsync(stream.fileno()) + os.replace(temporary, destination) + finally: + if os.path.exists(temporary): + os.unlink(temporary) + + +def load_skill_fit(path: str | Path) -> tuple[dict[str, np.ndarray], dict[str, Any]]: + """Read a self-contained result without the source recording or simulator. + + Args: + path: NPZ written by save_skill_fit. + + Returns: + Owned numeric arrays and metadata. + """ + data = _read_npz(Path(path)) + metadata = json.loads(str(data.pop("metadata_json"))) + if metadata.get("schema_version") != 1: + raise ValueError("Unsupported skill-fit schema.") + required = {"timestamps", "original_qpos", "native_qpos", "patched_qpos"} + if not required <= data.keys(): + raise ValueError("Fit bundle is missing trajectory arrays.") + return data, metadata diff --git a/embodichain/gen_sim/README.md b/embodichain/gen_sim/README.md index c4a5b2912d..e6efebb94a 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 1fb65fae37..67b697fb07 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 f37af1d132..7398cf4853 100644 --- a/embodichain/gen_sim/_run_config.py +++ b/embodichain/gen_sim/_run_config.py @@ -82,13 +82,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..b5afd08f5b --- /dev/null +++ b/embodichain/lab/expansion/gym_workloads.py @@ -0,0 +1,1111 @@ +# ---------------------------------------------------------------------------- +# 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 + row_context = ExpansionRowContext( + row, + identity, + assignment.ordinal, + request.attempt_id, + request.directory, + case, + request.recipe, + ) + factory = None + if ctx.extensions.plan_transform_factory is not None: + factory = ctx.extensions.plan_transform_factory(self.env, row_context) + if factory is None: + raise ValueError("plan_transform_factory returned no factory") + if group["adapter"].configured_nominal_sources and not getattr( + factory, "reject_nominal_source_overlap", False + ): + raise ValueError( + "A workload transform must declare call-local nominal-source exclusion." + ) + nominal = ( + None + if ctx.extensions.nominal_source_factory is None + else ctx.extensions.nominal_source_factory(self.env, row_context) + ) + adapter = group["adapter"].for_env_ids((row,)) + bridge_options = { + "plan_transform_factory": factory, + "isolate_policy_sessions": True, + } + if nominal is not None: + bridge_options["nominal_source_factory"] = nominal + if ctx.extensions.invocation_policy_bindings: + bridge_options["invocation_policy_bindings"] = ( + ctx.extensions.invocation_policy_bindings + ) + bridge = adapter.create_bridge(ctx.program, **bridge_options) + # 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 + or common.nominal_source_factory + or common.invocation_policy_bindings + ): + 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 workload.extensions.invocation_policy_bindings: + from embodichain.lab.task_program.integrations.invocation_policy import ( + BoundInvocationPolicies, + ) + + BoundInvocationPolicies( + ctx.program, workload.extensions.invocation_policy_bindings + ) + if ctx.feedback is not None: + bind = getattr(ctx.feedback, "bind_program", None) + if bind is not None: + bind(adapter, ctx.program) + 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..b326efe219 --- /dev/null +++ b/embodichain/lab/expansion/host_extensions.py @@ -0,0 +1,138 @@ +# ---------------------------------------------------------------------------- +# 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, TYPE_CHECKING +from .contracts import CandidateIdentity, SceneCase, ValidationResult +from .recipes import ExpansionRecipe + +if TYPE_CHECKING: + from embodichain.lab.task_program.integrations.invocation_policy import ( + InvocationPolicyBinding, + ) + +__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. + nominal_source_factory: Receives environment and immutable row identity; + returns an attempt-local Task Program nominal source factory. + invocation_policy_bindings: Presets pinned to exact compiled invocations. + 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 + nominal_source_factory: Callable[[Any, ExpansionRowContext], Any] | None = field( + default=None, kw_only=True + ) + invocation_policy_bindings: tuple[InvocationPolicyBinding, ...] = field( + default=(), kw_only=True + ) + 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 type(self.invocation_policy_bindings) is not tuple: + raise TypeError("invocation_policy_bindings must be a tuple") + 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", + "nominal_source_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..64b9be2fcb --- /dev/null +++ b/embodichain/lab/expansion/task_program_jobs.py @@ -0,0 +1,556 @@ +# ---------------------------------------------------------------------------- +# 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 Callable, 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 + from embodichain.lab.task_program.runtime.expansion import ( + TaskProgramNominalSourceFactory, + ) + from embodichain.lab.task_program.integrations.invocation_policy import ( + InvocationPolicyBinding, + ) + from .host_extensions import ExpansionRowContext, ExpansionValidator + +__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, + nominal_source_factory=None, + invocation_policy_bindings=(), +): + """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 if operator != "object_pose" else None, + nominal_source_factory=nominal_source_factory, + invocation_policy_bindings=invocation_policy_bindings, + 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, + validator_factory=None, + nominal_source_factory=None, + invocation_policy_bindings=(), +): + """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, + nominal_source_factory=nominal_source_factory, + invocation_policy_bindings=invocation_policy_bindings, + ), + ) + ) + renderer = settings["collection_renderer"] + if renderer == "auto": + renderer = viewer["renderer"] if settings["capture_media"] else None + common = GymExpansionHostExtensions( + collection_renderer=renderer, + validator_factory=validator_factory + or ( + 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, + validator_factory: Callable[[Any, Path], ExpansionValidator] | None = None, + nominal_source_factory: ( + Callable[[Any, ExpansionRowContext], TaskProgramNominalSourceFactory] | None + ) = None, + invocation_policy_bindings: tuple[InvocationPolicyBinding, ...] = (), +) -> 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. + validator_factory: Optional task-owned measured validation port. + nominal_source_factory: Optional independent factory for each attempted row. + invocation_policy_bindings: Named presets bound to the exact program calls. + + 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 {}) + if (deployment.parent / "invocation_bindings.json").is_file(): + from embodichain.toolkits.skill_fitting.task_program import ( + load_invocation_bindings, + ) + + sealed = load_invocation_bindings(deployment) + if invocation_policy_bindings and invocation_policy_bindings != sealed: + raise ValueError( + "Explicit invocation policies differ from the sealed deployment." + ) + invocation_policy_bindings = sealed + for name in ( + "manifest.json", + "compile_report.json", + "source_map.json", + "invocation_bindings.json", + ): + path = deployment.parent / name + sources[f"fitted_{name}"] = {"path": str(path), "sha256": file_digest(path)} + 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, + validator_factory=validator_factory, + nominal_source_factory=nominal_source_factory, + invocation_policy_bindings=invocation_policy_bindings, + ) + 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/gym/envs/wrapper/replay.py b/embodichain/lab/gym/envs/wrapper/replay.py index 0371137895..144feee414 100644 --- a/embodichain/lab/gym/envs/wrapper/replay.py +++ b/embodichain/lab/gym/envs/wrapper/replay.py @@ -66,6 +66,9 @@ class ReplayWrapper(gym.Wrapper): Expert recordings carry the canonical schema so qpos/qvel targets can be reconstructed before stepping. Schema metadata alone cannot identify controller actions: older position-only raw policy recordings carry it too. + External recordings may declare ``meta.state_joint_names`` to align their + complete state and explicit expert commands with the native robot's order. + Joint sets must match exactly; raw policy commands are not reinterpreted. Physical-objective dynamic replay currently supports one environment and full resets only. Stepping past exhaustion raises instead of extending @@ -104,6 +107,7 @@ def __init__( ) self._mode = mode self._trajectory = load_trajectory(trajectory) + self._align_named_joint_layout() meta = self._trajectory["meta"] self._cable_replay = "cables" in self._trajectory["states"].keys() self._cable_source_origins: torch.Tensor | None = None @@ -202,6 +206,97 @@ def __init__( self._replay_steps = self._replay_cursor.steps self._commands = _ReplayCommands.from_recording(self._trajectory) + def _align_named_joint_layout(self) -> None: + """Align explicitly named external recordings to native state/action order. + + Legacy recordings without ``state_joint_names`` retain their strict + numeric layout. Named recordings must describe the exact robot and + active joint sets; names cannot authorize a missing or extra actuator. + """ + data = self._trajectory + meta = data["meta"] + if "state_joint_names" not in meta: + return + names = meta["state_joint_names"] + native = list(self.env.robot.joint_names) + if ( + not isinstance(names, (list, tuple)) + or any(not isinstance(n, str) for n in names) + or len(set(names)) != len(names) + or set(names) != set(native) + or len(names) != self.env.robot.dof + or meta.get("robot_dof") != len(names) + ): + raise ValueError( + "Trajectory state_joint_names must match the complete robot joint set." + ) + ids = meta.get("active_joint_ids", []) + if ( + not isinstance(ids, (list, tuple)) + or any(type(i) is not int or not 0 <= i < len(names) for i in ids) + or len(set(ids)) != len(ids) + ): + raise ValueError( + "Trajectory active_joint_ids must be unique state-column indices." + ) + source_active = [names[i] for i in ids] + target_active = [native[i] for i in self.env.active_joint_ids] + if set(source_active) != set(target_active) or len(source_active) != len( + target_active + ): + raise ValueError( + "Trajectory active joint names differ from the replay environment." + ) + state_order = [names.index(n) for n in native] + states = data["states"].clone() + if "robot" not in states.keys() or "qpos" not in states["robot"].keys(): + raise ValueError("Named trajectory requires complete robot qpos states.") + for key in ("qpos", "qvel", "qf"): + if key in states["robot"].keys(): + value = states["robot"][key] + if value.shape != (*states.batch_size, len(names)): + raise ValueError( + f"Named trajectory robot {key} does not match its state layout." + ) + states["robot"][key] = value[..., state_order] + actions = data["actions"] + metadata = dict(meta) + if meta.get("action_kind") in {"expert", "expert_controller"}: + spec = build_expert_action_spec( + joint_names=source_active, + joint_command_mode=meta.get("joint_command_mode"), + ) + expected = spec.metadata(step_dt=meta.get("step_dt", 0)) + if ( + any( + meta.get(k) != expected[k] + for k in ("joint_names", "qpos_slice", "qvel_slice") + ) + or actions.ndim != 3 + or actions.shape[-1] != spec.width + ): + raise ValueError( + "Named expert trajectory action schema does not match its source joint order." + ) + order = [source_active.index(n) for n in target_active] + if spec.qvel_slice is not None: + order += [i + len(source_active) for i in order] + actions = actions[..., order] + metadata["joint_names"] = target_active + elif self._mode == "dynamic" and source_active != target_active: + raise ValueError( + "Raw policy actions cannot be reordered by joint names; use explicit expert targets." + ) + metadata.update( + state_joint_names=native, active_joint_ids=list(self.env.active_joint_ids) + ) + self._trajectory = { + **data, + "states": states, + "actions": actions, + "meta": metadata, + } + def _validate_action_schema(self, meta: dict) -> None: """Validate explicit controller recordings before any simulation step.""" kind = meta.get("action_kind", "policy") diff --git a/embodichain/lab/gym/utils/_component_composition.py b/embodichain/lab/gym/utils/_component_composition.py index 4ea34fdc45..6e64053d1b 100644 --- a/embodichain/lab/gym/utils/_component_composition.py +++ b/embodichain/lab/gym/utils/_component_composition.py @@ -180,7 +180,14 @@ def _resolve_local_resource_paths(value: object, *, base_dir: Path) -> None: if isinstance(path, str) and not Path(path).is_absolute(): spawn["file_path"] = str((base_dir / path).resolve()) if ( - key in {"fpath", "urdf_path", "xml_path", "trajectory"} + key + in { + "fpath", + "urdf_path", + "xml_path", + "trajectory", + "decomposition_fpath", + } and isinstance(child, str) and child.startswith(("./", "../")) ): diff --git a/embodichain/lab/gym/utils/gym_utils.py b/embodichain/lab/gym/utils/gym_utils.py index ea24b06d03..de114a43ca 100644 --- a/embodichain/lab/gym/utils/gym_utils.py +++ b/embodichain/lab/gym/utils/gym_utils.py @@ -734,12 +734,14 @@ def config_to_cfg( from embodichain.lab.sim.shapes import CompoundShapeCfg def resolve_shape_paths(shape: object) -> None: - if ( - isinstance(shape, MeshCfg) - and isinstance(shape.fpath, (str, Path)) - and shape.fpath - ): - shape.fpath = get_data_path(shape.fpath) + if isinstance(shape, MeshCfg): + if isinstance(shape.fpath, (str, Path)) and shape.fpath: + shape.fpath = get_data_path(shape.fpath) + collision = shape.collision + if collision is not None and collision.decomposition_fpath is not None: + collision.decomposition_fpath = get_data_path( + collision.decomposition_fpath + ) elif isinstance(shape, CompoundShapeCfg): for part in shape.parts: resolve_shape_paths(part.shape) @@ -754,6 +756,8 @@ def resolve_asset_paths( asset.shape.fpath = get_data_path(path) elif isinstance(asset, ArticulationCfg) and path is not None: asset.fpath = get_data_path(path) + if isinstance(getattr(asset, "shape", None), MeshCfg): + resolve_shape_paths(asset.shape) if isinstance(asset, RigidObjectCfg): if isinstance(asset.shape, CompoundShapeCfg): resolve_shape_paths(asset.shape) diff --git a/embodichain/lab/scripts/run_task.py b/embodichain/lab/scripts/run_task.py index c3eec2f179..960f53cdec 100644 --- a/embodichain/lab/scripts/run_task.py +++ b/embodichain/lab/scripts/run_task.py @@ -1622,6 +1622,21 @@ def main( fluid_reader: FluidTrajectoryReader | None = None, ) -> None: """Run the selected workflow without taking ownership of ``env``.""" + if getattr(args, "skill_sequence", None): + from embodichain.toolkits.skill_fitting._execution import execute_sequence + + if ( + getattr(args, "expansion_profile", None) is not None + or "expansion" in gym_config + ): + from embodichain.toolkits.skill_fitting._sequence_expansion import ( + run_sequence_expansion, + ) + + run_sequence_expansion(env, args, gym_config) + else: + execute_sequence(env, Path(args.skill_sequence)) + return if getattr(args, "replay", False): log_info("Replay mode.", color="green") replay( @@ -1869,6 +1884,11 @@ def _create_parser() -> argparse.ArgumentParser: add_env_launcher_args_to_parser(parser, require_gym_config=True) parser.set_defaults(viser_image_fps=None) + parser.add_argument( + "--skill-sequence", + type=str, + help="Execute a fitted native-skill sequence with recorded qpos gaps (no replay trajectory).", + ) parser.add_argument( "--task-program", @@ -2027,6 +2047,12 @@ def cli(argv: Sequence[str] | None = None) -> None: parser = _create_parser() args = parser.parse_args(argv) + if getattr(args, "skill_sequence", None) and ( + args.replay or args.preview or args.task_program or args.real2sim_profile + ): + raise ValueError( + "--skill-sequence cannot be combined with replay, preview or Task Program." + ) if getattr(args, "record_fluid_state", False) and not getattr( args, "record_trajectory", False ): @@ -2051,6 +2077,12 @@ def cli(argv: Sequence[str] | None = None) -> None: execute_init_hooks() env_cfg, gym_config, action_config = build_env_cfg_from_args(args) + if getattr(args, "skill_sequence", None) and any( + k in gym_config for k in ("task_program", "real2sim") + ): + raise ValueError( + "Native fitted sequences require a plain physical Gym configuration." + ) if getattr(args, "particle_spacing", None) is not None: configure_fluid_spacing(env_cfg, args.particle_spacing) collection = _mapping_or_empty(gym_config.get("collection"), "collection") @@ -2130,7 +2162,25 @@ def cli(argv: Sequence[str] | None = None) -> None: or "expansion" in gym_config ): expansion_request = _resolve_expansion_request(args, gym_config) - if expansion_request is not None: + if expansion_request is not None and getattr(args, "skill_sequence", None): + from embodichain.toolkits.skill_fitting._sequence_expansion import ( + validate_sequence_expansion, + validate_sequence_collection, + ) + + validate_sequence_expansion( + expansion_request[0], + Path(args.skill_sequence).resolve(strict=True), + env_cfg.num_envs, + ) + validate_sequence_collection( + args, + gym_config, + expansion_request[0], + expansion_request[1], + expansion_request[3], + ) + elif expansion_request is not None: from embodichain.lab.sim.motion.expansion import CombinedExpansionProfile profile = expansion_request[0] diff --git a/embodichain/lab/sim/atomic_skills/__init__.py b/embodichain/lab/sim/atomic_skills/__init__.py index 6c7a7dbd99..e798ecc9e0 100644 --- a/embodichain/lab/sim/atomic_skills/__init__.py +++ b/embodichain/lab/sim/atomic_skills/__init__.py @@ -201,8 +201,10 @@ CoordinatedPlacementOptions, EndEffectorPoseGoal, GraspGoal, + PickUpMotionGoal, HandOver, HandOverGoal, + HandOverMotionGoal, HandOverOptions, HeldObjectPoseGoal, JointPositionGoal, @@ -235,10 +237,12 @@ PushObjectToolCalibration, Slide, SlideGoal, + SlideMotionGoal, SlideJointTarget, SlideOptions, Twist, TwistGoal, + TwistMotionGoal, TwistOptions, ) from .runner import ( @@ -388,8 +392,10 @@ "GRASP_COMMAND", "GRASP_CAPABILITY", "GraspGoal", + "PickUpMotionGoal", "HandOver", "HandOverGoal", + "HandOverMotionGoal", "HandOverOptions", "HeldObjectPoseGoal", "HeldObjectState", @@ -454,10 +460,12 @@ "SlideAffordance", "Slide", "SlideGoal", + "SlideMotionGoal", "SlideJointTarget", "SlideOptions", "Twist", "TwistGoal", + "TwistMotionGoal", "TwistOptions", "RecoveryPolicy", "PlanningRecovery", 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/demo_skill.py b/embodichain/lab/sim/atomic_skills/demo_skill.py index fcca25fd87..c66ac54029 100644 --- a/embodichain/lab/sim/atomic_skills/demo_skill.py +++ b/embodichain/lab/sim/atomic_skills/demo_skill.py @@ -322,8 +322,19 @@ def instantiate( device=context.robot.qpos.device, dtype=context.robot.qpos.dtype, ).unsqueeze(0) + entry_observed = context.robot.qpos + if spec.initial_held_role is not None: + # A verified squeeze can deflect passive follower joints in contact. + # Those joints obey mimic relationships on commanded trajectory samples. + robot = self.motion_generator.robot + passive = set(getattr(robot, "mimic_ids", ())) + independent = [ + index for index in range(initial.shape[-1]) if index not in passive + ] + initial = initial[:, independent] + entry_observed = entry_observed[:, independent] entry_matches = torch.allclose( - initial, context.robot.qpos, atol=spec.entry_tolerance, rtol=0 + initial, entry_observed, atol=spec.entry_tolerance, rtol=0 ) if not entry_matches and not spec.allow_entry_connection: raise ValueError("DemoSkill entry posture differs from the recorded state") @@ -466,6 +477,30 @@ def instantiate( ) self.monitor.spec = spec self.monitor.sample_count = path.shape[1] + if entry_matches and spec.initial_held_role is not None: + hand = request.binding.endpoint("primary", "grasp").require_target( + JointPositionTarget + ) + controlled = set(arm.joint_ids) | set(hand.joint_ids) + for child, parent in zip( + getattr(generator.robot, "mimic_ids", ()), + getattr(generator.robot, "mimic_parents", ()), + strict=True, + ): + if parent in controlled: + controlled.add(child) + unowned = sorted(set(range(path.shape[-1])) - controlled) + if unowned and torch.allclose( + path[:, :, unowned], + path[:, :1, unowned].expand(-1, path.shape[1], -1), + atol=1e-6, + rtol=0, + ): + path[:, :, unowned] = context.robot.qpos[:, None, unowned] + # The held-entry guard first qualified the measured posture. Use that + # exact posture for the zero-time arm sample while keeping gripper load. + ids = list(arm.joint_ids) + path[:, 0, ids] = context.robot.qpos[:, ids] self._nominal_spec = spec timed = TimedTrajectory.from_uniform_step( path, env_ids=context.env_ids, step_dt=context.require_control_dt() diff --git a/embodichain/lab/sim/atomic_skills/fitting.py b/embodichain/lab/sim/atomic_skills/fitting.py new file mode 100644 index 0000000000..2a2f6016e6 --- /dev/null +++ b/embodichain/lab/sim/atomic_skills/fitting.py @@ -0,0 +1,1119 @@ +# ---------------------------------------------------------------------------- +# 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. +# ---------------------------------------------------------------------------- + +"""Identify explicit motion goals and reconstruct them through native skills.""" + +from __future__ import annotations + +from dataclasses import asdict, replace +import time +from typing import Any, TYPE_CHECKING + +import numpy as np +from scipy.spatial.transform import Rotation +import torch + +from embodichain.compute.trajectory.phase_alignment import align_joint_phase +from embodichain.compute.trajectory.splicing import ( + _connect_joint_segment, + _replacement_diagnostics, + joint_path_diagnostics, + splice_joint_segment, +) +from embodichain.data_pipeline.skill_fitting import JointRecording +from embodichain.utils import configclass +from .control import OPEN_COMMAND, GRASP_COMMAND +from .policies import MotionPolicy, RecoveryPolicy +from .primitives import ( + GraspGoal, + PickUpMotionGoal, + PickUpOptions, + SlideMotionGoal, + SlideOptions, + TwistMotionGoal, + TwistOptions, + HandOverMotionGoal, + HandOverOptions, +) +from .state import RobotObservation + +if TYPE_CHECKING: + from .engine import AtomicSkillEngine + +__all__ = ["SkillFitCfg", "fit_skill_segment"] + + +@configclass +class SkillFitCfg: + """Finite fitting budgets and acceptance tolerances in physical units.""" + + control_dt: float = 0.01 + blend_samples: int = 12 + max_samples: int = 20_000 + max_candidates: int = 3 + max_seconds: float = 120.0 + max_joint_rmse: float = 0.15 + max_tcp_rmse_m: float = 0.015 + max_rotation_rmse_rad: float = 0.08 + max_contact_error_m: float = 0.005 + max_contact_rotation_rad: float = 0.03 + min_twist_angle_rad: float = 0.05 + grasp_sample_count: int = 4000 + grasp_max_candidates: int = 64 + grasp_rotation_weight_m: float = 0.05 + grasp_approach_deviation_rad: float = np.pi / 3 + grasp_filter_ground_collision: bool = False + max_grasp_distance_m: float = 0.2 + grasp_contact_inset_m: float = 0.004 + + def __post_init__(self) -> None: + for key, value in asdict(self).items(): + if key == "grasp_filter_ground_collision": + if type(value) is not bool: + raise ValueError("grasp_filter_ground_collision must be a boolean.") + continue + if isinstance(value, bool) or not np.isfinite(value) or value <= 0: + raise ValueError(f"SkillFitCfg.{key} must be finite and positive.") + for key in ( + "blend_samples", + "max_samples", + "max_candidates", + "grasp_sample_count", + "grasp_max_candidates", + ): + if type(getattr(self, key)) is not int: + raise ValueError(f"{key} must be an integer.") + if ( + self.max_candidates > 9 + or self.max_samples > 200_000 + or self.blend_samples < 4 + or self.grasp_sample_count > 100_000 + or self.grasp_max_candidates > 256 + ): + raise ValueError("Fitting budgets are outside supported bounds.") + if self.grasp_approach_deviation_rad > np.pi / 2: + raise ValueError("grasp_approach_deviation_rad must not exceed pi/2.") + + +def _transitions( + q: np.ndarray, opened: np.ndarray, closed: np.ndarray +) -> tuple[list, list]: + delta = closed - opened + progress = ((q - opened) @ delta) / (delta @ delta) + changes = np.diff(progress) + result = [] + for sign in (1, -1): + ids = np.flatnonzero(sign * changes > max(1e-5, np.ptp(progress) * 1e-3)) + 1 + groups = np.split(ids, np.flatnonzero(np.diff(ids) > 2) + 1) if len(ids) else [] + # A meaningful open/close event covers at least 10% of the profile travel. + result.append( + [ + (int(g[0] - 1), int(g[-1] + 1)) + for g in groups + if len(g) and abs(progress[g[-1]] - progress[g[0] - 1]) >= 0.1 + ] + ) + return result[0], result[1] + + +def _rot_error(left: np.ndarray, right: np.ndarray) -> np.ndarray: + return Rotation.from_matrix( + left[..., :3, :3] @ right[..., :3, :3].swapaxes(-1, -2) + ).magnitude() + + +def _approach( + poses: np.ndarray, close_start: int, *, relaxed: bool = False +) -> tuple[np.ndarray, float, int]: + grasp = poses[close_start] + if relaxed: + # Real approach motion may rotate and contain measurement noise. Estimate + # its final direction over a finite window, rather than demanding that + # multiple observed rotations coincide to 3e-5 radians. + if close_start < 6: + raise ValueError("Select an approach before the closing event.") + index = max(1, close_start // 2) + offset = grasp[:3, 3] - np.median( + poses[max(0, index - 2) : index + 3, :3, 3], axis=0 + ) + distance = float(np.linalg.norm(offset)) + if distance < 1e-4: + raise ValueError("Approach displacement is not observable.") + direction = offset / distance + # A curved reach is not its final contact approach. Prefer the robust + # calibrated TCP +Z near closure when it agrees with the observed + # displacement within 45 degrees; retain displacement for other TCP + # conventions or a genuinely side-on reach. + axis = np.median( + poses[max(0, close_start - 4) : close_start + 1, :3, 2], axis=0 + ) + axis /= np.linalg.norm(axis) + if np.dot(axis, direction) >= np.sqrt(0.5): + direction = axis + distance = float(np.dot(offset, direction)) + return direction, distance, index + candidates = np.flatnonzero(_rot_error(poses[:close_start], grasp) < 0.00003) + if len(candidates) < 2: + raise ValueError("No stable TCP orientation before grasp; provide phase hints.") + # The farthest point after attaining the grasp orientation is the pre-grasp. + offsets = grasp[:3, 3] - poses[candidates, :3, 3] + norms = np.linalg.norm(offsets, axis=1) + index = int(candidates[np.flatnonzero(norms >= np.max(norms) - 1e-8)[-1]]) + distance = float(np.max(norms)) + if distance < 1e-4: + raise ValueError("Approach displacement is not observable.") + return (grasp[:3, 3] - poses[index, :3, 3]) / distance, distance, index + + +def _twist_line( + poses: np.ndarray, minimum_angle: float +) -> tuple[np.ndarray, np.ndarray, float, float]: + rotation = Rotation.from_matrix(poses[:, :3, :3] @ poses[0, :3, :3].T) + vectors = rotation.as_rotvec() + _, singular, vt = np.linalg.svd(vectors, full_matrices=False) + axis = vt[0] + angles = np.unwrap(vectors @ axis) + angle = float(angles[-1]) + if abs(angle) < minimum_angle: + raise ValueError("Twist angle is too small to identify a rotation line.") + if angle < 0: + axis, angles, angle = -axis, -angles, -angle + rotations = Rotation.from_rotvec(angles[:, None] * axis).as_matrix() + matrix = np.eye(3)[None] - rotations + right = poses[:, :3, 3] - np.einsum("nij,j->ni", rotations, poses[0, :3, 3]) + # Resolve the axis-point gauge by choosing its closest point to the initial TCP. + origin = np.linalg.lstsq( + np.concatenate((matrix.reshape(-1, 3), axis[None]), axis=0), + np.concatenate((right.reshape(-1), [axis @ poses[0, :3, 3]])), + rcond=None, + )[0] + predicted = origin + np.einsum("nij,j->ni", rotations, poses[0, :3, 3] - origin) + error = float(np.sqrt(np.mean(np.sum((predicted - poses[:, :3, 3]) ** 2, axis=1)))) + return axis, origin, angle, error + + +def _phase_map( + q_native: np.ndarray, + native_lengths: dict[str, int], + phases: list[dict[str, Any]], + observations: np.ndarray, + approach_anchor: tuple[int, int] | None = None, + source_times: np.ndarray | None = None, +) -> tuple[np.ndarray, np.ndarray]: + if list(native_lengths) != [p["name"] for p in phases]: + raise ValueError( + f"Source phases must match native phases {list(native_lengths)}." + ) + output = np.empty_like(observations, dtype=np.float64) + timing = np.empty(len(observations)) + cursor = 0 + for phase in phases: + count = native_lengths[phase["name"]] + part = q_native[cursor : cursor + count] + cursor += count + start, stop = phase["start"], phase["stop"] + if stop <= start: + raise ValueError("Every source phase must contain a sample.") + if source_times is not None: + clock = source_times[start:stop] + progress = ( + (clock - clock[0]) / (clock[-1] - clock[0]) + if len(clock) > 1 + else np.zeros(1) + ) + native_clock = np.linspace(0, 1, len(part)) + output[start:stop] = np.column_stack( + [np.interp(progress, native_clock, column) for column in part.T] + ) + timing[start:stop] = progress + elif phase["name"] == "approach" and approach_anchor is not None: + source_anchor, native_anchor = approach_anchor + if not start < source_anchor < stop or not 0 < native_anchor < count - 1: + raise ValueError("Pre-grasp anchor must be inside its approach phase.") + first, clock_a = align_joint_phase( + part[: native_anchor + 1], observations[start : source_anchor + 1] + ) + second, clock_b = align_joint_phase( + part[native_anchor:], observations[source_anchor:stop] + ) + output[start : source_anchor + 1], output[source_anchor:stop] = ( + first, + second, + ) + timing[start : source_anchor + 1] = ( + clock_a * native_anchor / max(1, count - 1) + ) + timing[source_anchor:stop] = ( + native_anchor + clock_b * (count - 1 - native_anchor) + ) / max(1, count - 1) + else: + output[start:stop], timing[start:stop] = align_joint_phase( + part, observations[start:stop] + ) + if cursor != len(q_native): + raise ValueError("Native phase lengths do not cover the generated plan.") + return output, timing + + +def fit_skill_segment( + recording: JointRecording, + engine: AtomicSkillEngine, + *, + skill_id: str, + endpoints: dict[str, dict[str, str]], + start: int, + stop: int, + acceleration_limits: np.ndarray, + cfg: SkillFitCfg | None = None, + phase_hints: list[dict[str, Any]] | None = None, + scene_grasps: tuple[GraspGoal, ...] | None = None, +) -> tuple[dict[str, np.ndarray], dict[str, Any]]: + """Regenerate one segment using observed motion or supplied scene grasps. + + Only the native AtomicSkillEngine creates candidate joint motion. Source + joints supply observations, initial seeds, phase clocks and outer boundary + stencils. The returned reference is not a measured-state prediction. + + Args: + recording: Explicitly timed, named source joints. + engine: Engine bound to a one-row robot kinematics port. + skill_id: pick_up, slide, twist or hand_over. + endpoints: Existing explicit participant-to-control-part bindings. + start: First included recording sample. + stop: First excluded recording sample. + acceleration_limits: Explicit per-joint acceleration caps. + cfg: Numerical budgets and independent native/patched acceptance gates. + phase_hints: Optional complete contiguous phase partition. Indices are + relative to this segment, with half-open bounds. + scene_grasps: Ranked geometry-qualified object grasps for a single-arm + skill. Scene fitting regenerates the skill from these anchors and + qualifies the modified trajectory, without requiring pointwise + agreement with the noisy source. Omission retains motion-only fitting. + + Returns: + Full-length original/native/patched arrays and fit metadata. Rejected + fits retain their native candidate and leave patched_qpos unchanged. + """ + cfg = SkillFitCfg() if cfg is None else cfg + cfg.validate() + cfg.__post_init__() + if scene_grasps is not None and (not scene_grasps or skill_id == "hand_over"): + raise ValueError( + "Scene grasp fitting requires a single-arm skill and at least one valid object grasp." + ) + if scene_grasps is not None: + if len(scene_grasps) > cfg.max_candidates or any( + not isinstance(g, GraspGoal) + or not isinstance(g.grasp_xpos, torch.Tensor) + or g.grasp_xpos.shape != (4, 4) + or not isinstance(g.object_pose, torch.Tensor) + or g.object_pose.shape != (4, 4) + for g in scene_grasps + ): + raise ValueError( + "Scene candidates must be explicit single-row object/TCP poses within max_candidates." + ) + began = time.monotonic() + if skill_id not in {"pick_up", "slide", "twist", "hand_over"}: + raise ValueError("Unsupported reconstruction skill.") + if tuple(engine.robot.joint_names) != recording.joint_names: + raise ValueError("Recording must use the engine's exact named joint order.") + if ( + not 0 <= start < stop <= len(recording.qpos) + or not 12 <= stop - start <= cfg.max_samples + ): + raise ValueError("Invalid or oversized selected segment.") + q = recording.qpos[start:stop] + times = recording.timestamps[start:stop] + robot = engine.robot + device = engine.device + hints = {} if phase_hints is None else {p["name"]: p for p in phase_hints} + if phase_hints is not None and ( + not phase_hints + or len(hints) != len(phase_hints) + or phase_hints[0]["start"] != 0 + or phase_hints[-1]["stop"] != len(q) + or any( + type(p["start"]) is not int + or type(p["stop"]) is not int + or not 0 <= p["start"] < p["stop"] <= len(q) + for p in phase_hints + ) + or any(a["stop"] != b["start"] for a, b in zip(phase_hints, phase_hints[1:])) + ): + raise ValueError("Phase hints must be a complete contiguous segment partition.") + participant_data = {} + for role, ports in endpoints.items(): + arm, hand = ports["motion"], ports["grasp"] + arm_ids, hand_ids = robot.get_joint_ids(arm), robot.get_joint_ids(hand) + profile = engine.control_profiles[hand] + opened = profile.commands[OPEN_COMMAND].positions + closed = profile.commands[GRASP_COMMAND].positions + opened = torch.as_tensor(opened).cpu().numpy().reshape(-1) + closed = torch.as_tensor(closed).cpu().numpy().reshape(-1) + with torch.no_grad(): + poses = ( + robot.compute_fk( + torch.as_tensor(q[:, arm_ids], device=device, dtype=torch.float32), + name=arm, + to_matrix=True, + ) + .cpu() + .numpy() + .astype(np.float64) + ) + closes, opens = _transitions(q[:, hand_ids], opened, closed) + close_name = "pickup_close" if role == "source" else "close" + open_name = ( + "release" + if role == "source" + else ("receive_release" if role == "destination" else "open") + ) + if close_name in hints: + closes = [(hints[close_name]["start"], hints[close_name]["stop"])] + if open_name in hints: + opens = [(hints[open_name]["start"], hints[open_name]["stop"])] + participant_data[role] = (poses, closes, opens, arm_ids, hand_ids) + + params: dict[str, Any] = { + "pose_origin": "scene_affordance" if scene_grasps is not None else "fk_derived" + } + contact_ranges: list[tuple[str, int, int]] = [] + + def tensor(value: np.ndarray) -> torch.Tensor: + return torch.tensor(value, device=device, dtype=torch.float32) + + def partition(boundaries: list[tuple[str, int]]) -> list[dict[str, Any]]: + return [ + {"name": name, "start": left, "stop": right} + for (name, left), (_, right) in zip(boundaries, boundaries[1:]) + ] + + if skill_id != "hand_over": + if set(endpoints) != {"primary"}: + raise ValueError("Single-arm skills require the primary participant.") + poses, closes, opens, _, _ = participant_data["primary"] + if len(closes) != 1: + raise ValueError( + "Expected one observable closure; select a complete skill or provide matching command data." + ) + close_start, lift_start = closes[0] + if scene_grasps is None and ( + np.max( + np.linalg.norm( + poses[close_start:lift_start, :3, 3] - poses[close_start, :3, 3], + axis=1, + ) + ) + > cfg.max_contact_error_m + or np.max(_rot_error(poses[close_start:lift_start], poses[close_start])) + > cfg.max_contact_rotation_rad + ): + raise ValueError( + "The native skill requires a stationary arm during closure." + ) + direction, pre_distance, pre_index = _approach( + poses, close_start, relaxed=scene_grasps is not None + ) + if "reach" in hints: + pre_index = hints["reach"]["start"] + offset = poses[close_start, :3, 3] - poses[pre_index, :3, 3] + pre_distance = float(np.linalg.norm(offset)) + if pre_distance <= 1e-4: + raise ValueError("Hinted pre-grasp displacement is not observable.") + direction = offset / pre_distance + observed_grasp = poses[close_start] + grasp = ( + observed_grasp + if scene_grasps is None + else scene_grasps[0].grasp_xpos.detach().cpu().numpy() + ) + close_count = lift_start - close_start + if skill_id == "pick_up": + lift = poses[-1, :3, 3] - observed_grasp[:3, 3] + if lift[2] <= 0 or ( + scene_grasps is None + and np.linalg.norm(lift[:2]) > cfg.max_contact_error_m + ): + raise ValueError("Pickup requires an observable world-Z lift.") + if ( + scene_grasps is None + and float(_rot_error(poses[-1], grasp)) > cfg.max_contact_rotation_rad + ): + raise ValueError("Pickup lift changes TCP orientation.") + goal = ( + PickUpMotionGoal(tensor(grasp)) + if scene_grasps is None + else scene_grasps[0] + ) + options = PickUpOptions( + lift_height=float(lift[2]), + pre_grasp_distance=pre_distance, + approach_direction=tensor(direction), + hand_interp_steps=close_count, + ) + phases = partition( + [ + ("approach", 0), + ("close", close_start), + ("lift", lift_start), + ("end", len(q)), + ] + ) + params.update( + lift_height=float(lift[2]), + pre_grasp_distance=pre_distance, + approach_direction=direction.tolist(), + grasp_xpos=grasp.tolist(), + ) + contact_ranges = [("primary", close_start, len(q))] + else: + releases = [event for event in opens if event[0] >= lift_start] + if len(releases) != 1: + raise ValueError("Slide/Twist requires one observable release.") + open_start, retract_start = releases[0] + if skill_id == "slide": + move = poses[open_start, :3, 3] - observed_grasp[:3, 3] + distance = float(np.linalg.norm(move)) + if distance < 1e-4: + raise ValueError("Slide displacement is not observable.") + rail = move / distance + retract_distance = np.linalg.norm( + poses[-1, :3, 3] - poses[open_start, :3, 3] + ) + mode = "push" if retract_distance > cfg.max_contact_error_m else "pull" + axis = rail if mode == "push" else -rail + projected = ( + poses[lift_start:open_start, :3, 3] - observed_grasp[:3, 3] + ) @ rail + residual = ( + poses[lift_start:open_start, :3, 3] + - observed_grasp[:3, 3] + - projected[:, None] * rail + ) + if np.max(np.linalg.norm(residual, axis=1)) > cfg.max_contact_error_m: + raise ValueError("Slide contact motion is not a straight rail.") + goal = SlideMotionGoal(tensor(grasp), tensor(axis)) + options = SlideOptions( + direction=mode, + translation_distance=distance, + approach_distance=pre_distance, + hand_interp_steps=close_count, + ) + boundaries = [ + ("approach", 0), + ("reach", pre_index), + ("close", close_start), + (mode, lift_start), + ("open", open_start), + ] + if mode == "push": + boundaries.append(("return", retract_start)) + phases = partition(boundaries + [("end", len(q))]) + params.update( + translation_axis=axis.tolist(), + translation_distance=distance, + direction=mode, + approach_distance=pre_distance, + grasp_xpos=grasp.tolist(), + ) + else: + axis, origin, angle, error = _twist_line( + poses[lift_start : open_start + 1], cfg.min_twist_angle_rad + ) + if error > cfg.max_contact_error_m: + raise ValueError("Twist does not fit a rigid rotation line.") + goal = TwistMotionGoal(tensor(grasp), tensor(axis), tuple(origin)) + options = TwistOptions( + twist_angle=angle, + pre_grasp_distance=pre_distance, + hand_interp_steps=close_count, + twist_waypoint_count=2, + ) + phases = partition( + [ + ("approach", 0), + ("reach", pre_index), + ("close", close_start), + ("twist", lift_start), + ("open", open_start), + ("retract", retract_start), + ("end", len(q)), + ] + ) + params.update( + twist_axis=axis.tolist(), + axis_origin=origin.tolist(), + twist_angle=angle, + pre_grasp_distance=pre_distance, + grasp_xpos=grasp.tolist(), + axis_fit_rmse_m=error, + axis_point_convention="closest_to_initial_tcp", + ) + contact_ranges = [("primary", close_start, open_start)] + else: + if set(endpoints) != {"source", "destination"}: + raise ValueError( + "Handover requires explicit source/destination participants." + ) + source, source_closes, source_opens, source_ids, _ = participant_data["source"] + receive, receive_closes, receive_opens, _, _ = participant_data["destination"] + if len(receive_closes) != 1 or len(source_opens) != 1 or len(source_closes) > 1: + raise ValueError( + "Select one complete handover with observable hand events." + ) + rc0, rc1 = receive_closes[0] + so0, so1 = source_opens[0] + if rc1 >= so0: + raise ValueError("Handover needs a receiving hold before source release.") + for observed in (source, receive): + hold = observed[rc1 - 1 : so0 + 1] + if ( + np.max(np.linalg.norm(hold[:, :3, 3] - hold[0, :3, 3], axis=1)) + > cfg.max_contact_error_m + or np.max(_rot_error(hold, hold[0])) > cfg.max_contact_rotation_rad + ): + raise ValueError( + "Handover exchange requires stationary overlap anchors." + ) + moving = ( + np.flatnonzero( + np.max(np.abs(np.diff(q[:, source_ids], axis=0)), axis=1) > 1e-5 + ) + + 1 + ) + transfer_end = ( + hints["approach"]["start"] + if "approach" in hints + else int(max((i for i in moving if i < rc0), default=0)) + 1 + ) + retreat_end = ( + hints["place"]["start"] + if "place" in hints + else int(max((i for i in moving if i >= so1), default=so1)) + 1 + ) + if not transfer_end < rc0 or retreat_end >= len(q): + raise ValueError( + "Handover phase split needs explicit phase hints or stationary boundaries." + ) + pickup = source[source_closes[0][0]] if source_closes else None + placement = receive_opens[0] if len(receive_opens) == 1 else None + goal = HandOverMotionGoal( + tensor(source[rc1 - 1]), + tensor(receive[rc1 - 1]), + tensor(source[retreat_end - 1]), + None if pickup is None else tensor(pickup), + None if placement is None else tensor(receive[placement[0]]), + None if placement is None else tensor(receive[-1]), + ) + options = HandOverOptions( + arm_selection="bound", + release_at_target=placement is not None, + hand_interp_steps=rc1 - rc0, + hold_steps=so0 - rc1, + retreat_steps=max(3, retreat_end - so1), + ) + boundaries = [] + if source_closes: + sc0, sc1 = source_closes[0] + boundaries.extend([("pickup_approach", 0), ("pickup_close", sc0)]) + transfer_start = sc1 + else: + transfer_start = 0 + boundaries.extend( + [ + ("transfer", transfer_start), + ("approach", transfer_end), + ("close", rc0), + ("hold", rc1), + ("release", so0), + ("retreat", so1), + ] + ) + if placement: + boundaries.extend( + [ + ("place", retreat_end), + ("receive_release", placement[0]), + ("receive_retreat", placement[1]), + ] + ) + phases = partition(boundaries + [("end", len(q))]) + contact_ranges = [ + ("source", source_closes[0][0] if source_closes else 0, so0), + ("destination", rc0, placement[0] if placement else len(q)), + ] + params.update( + source_exchange=source[rc1 - 1].tolist(), + destination_exchange=receive[rc1 - 1].tolist(), + relative_exchange=( + np.linalg.inv(source[rc1 - 1]) @ receive[rc1 - 1] + ).tolist(), + source_retreat=source[retreat_end - 1].tolist(), + ) + + if phase_hints is not None: + if [p["name"] for p in phase_hints] != [p["name"] for p in phases]: + raise ValueError( + "Phase hints must name the complete native phase sequence." + ) + phases = [dict(p) for p in phase_hints] + if ( + phases[0]["start"] != 0 + or phases[-1]["stop"] != len(q) + or any( + type(p["start"]) is not int + or type(p["stop"]) is not int + or p["stop"] <= p["start"] + for p in phases + ) + or any(a["stop"] != b["start"] for a, b in zip(phases, phases[1:])) + ): + raise ValueError("Phases must be a complete nonempty contiguous partition.") + context = engine.initial_context(control_dt=cfg.control_dt) + context = replace( + context, + robot=RobotObservation( + timestamp=0, + qpos=tensor(q[0])[None], + qvel=torch.zeros_like(tensor(q[0])[None]), + ), + ) + if context.batch_size != 1: + raise ValueError("Skill reconstruction currently requires B=1.") + # Native timing remains inspectable; phasewise mapping has its own fixed + # source clock and all final derivative checks occur after that mapping. + policy = MotionPolicy( + strategy="motion_gen" if skill_id == "pick_up" else "ik_interp", + sample_count=max(len(q), 80), + acceleration_limit=0.5, + ) + owned = sorted( + {j for data in participant_data.values() for ids in data[3:] for j in ids} + ) + editable = tuple(owned) + untouched = [i for i in range(q.shape[1]) if i not in editable] + limits = robot.get_qpos_limits()[0].cpu().numpy() + velocity = robot.get_qvel_limits()[0].cpu().numpy() + acceleration = np.asarray(acceleration_limits, dtype=np.float64) + + source_phases = [dict(p) for p in phases] + phase_options = [phases] + if scene_grasps is not None and skill_id == "pick_up" and phase_hints is None: + # Only the outer clock/boundaries are fixed for scene reconstruction. + # A changed grasp may need more time to connect to the recorded end. + # The fitted command may open wider than the observed grasp. Allocate + # enough ticks to close at the same velocity and acceleration limits. + # Explicit phase hints remain authoritative. + closing = lift_start - close_start + hand = endpoints["primary"]["grasp"] + hand_ids = participant_data["primary"][4] + hand_profile = engine.control_profiles[hand] + travel = ( + torch.as_tensor(hand_profile.commands[OPEN_COMMAND].positions).cpu().numpy() + - torch.as_tensor(hand_profile.commands[GRASP_COMMAND].positions) + .cpu() + .numpy() + ) + source_dt = float(np.min(np.diff(times))) + max_step = np.minimum( + velocity[hand_ids] * source_dt, + acceleration[hand_ids] * source_dt**2, + ) + closing = max(closing, int(np.ceil(np.max(np.abs(travel) / max_step))) + 1) + free = len(q) - closing + if free >= 4: + native_max_step = np.minimum( + velocity[hand_ids] * cfg.control_dt, + acceleration[hand_ids] * cfg.control_dt**2, + ) + native_closing = int(np.ceil(np.max(np.abs(travel) / native_max_step))) + 1 + options = replace(options, hand_interp_steps=max(closing, native_closing)) + for fraction in (0.5, 0.45, 0.4, 0.55, 0.6): + before = int(round(free * fraction)) + alternative = [ + dict(name="approach", start=0, stop=before), + dict(name="close", start=before, stop=before + closing), + dict(name="lift", start=before + closing, stop=len(q)), + ] + if alternative not in phase_options: + phase_options.append(alternative) + + def connect_scene( + values: np.ndarray, fit_phases: list[dict[str, Any]] + ) -> np.ndarray: + first_contact = next(p["start"] for p in fit_phases if p["name"] == "close") + last_contact = ( + next(p["stop"] for p in fit_phases if p["name"] == "close") + if skill_id == "pick_up" + else next(p["start"] for p in fit_phases if p["name"] == "open") + ) + return _connect_joint_segment( + recording.qpos, + values, + recording.timestamps, + start=start, + stop=stop, + joint_indices=editable, + blend_samples=cfg.blend_samples, + protected=(first_contact, last_contact), + velocity_limits=velocity, + acceleration_limits=acceleration, + ) + + def qualify_scene(values: np.ndarray) -> dict[str, object]: + return _replacement_diagnostics( + recording.qpos, + values, + recording.timestamps, + lower=limits[:, 0], + upper=limits[:, 1], + velocity_limits=velocity, + acceleration_limits=acceleration, + ) + + candidates = [] + candidate_checks = [] + attempts = ( + len(scene_grasps) + if scene_grasps is not None + else (cfg.max_candidates if skill_id == "pick_up" else 1) + ) + for candidate in range(attempts): + if time.monotonic() - began > cfg.max_seconds: + break + distance = ( + pre_distance + + (0.0, -0.005, 0.005, -0.01, 0.01, -0.02, 0.02, -0.03, 0.03)[candidate] + if skill_id == "pick_up" and scene_grasps is None + else None + ) + trial = ( + replace(options, pre_grasp_distance=max(0.0001, distance)) + if distance is not None + else options + ) + trial_goal = goal + if scene_grasps is not None: + selected = scene_grasps[candidate] + if skill_id == "pick_up": + trial_goal = selected + trial = replace(trial, approach_direction=selected.grasp_xpos[:3, 2]) + else: + trial_goal = replace(goal, grasp_xpos=selected.grasp_xpos) + invocation = engine.make_invocation( + skill_id, + trial_goal, + control_parts=endpoints, + motion_policy=policy, + skill_options=trial, + recovery_policy=RecoveryPolicy(max_replans=0, max_skill_retries=0), + ) + plan = engine.plan(invocation, context) + if not bool(plan.plan_success.all()): + candidate_checks.append({"rank": candidate, "plan_success": False}) + continue + native = plan.joint_trajectory.positions[0].detach().cpu().numpy() + lengths = {p.name: p.stop - p.start for p in plan.segments} + anchor = None + if skill_id == "pick_up" and scene_grasps is None: + with torch.no_grad(): + approach_fk = ( + robot.compute_fk( + tensor( + native[ + : lengths["approach"], participant_data["primary"][3] + ] + ), + name=endpoints["primary"]["motion"], + to_matrix=True, + ) + .cpu() + .numpy() + ) + target_pre = grasp[:3, 3] - direction * trial.pre_grasp_distance + native_pre = int( + np.argmin(np.linalg.norm(approach_fk[:, :3, 3] - target_pre, axis=1)) + ) + anchor = ( + (pre_index, native_pre) if pre_index > 0 and native_pre > 0 else None + ) + selected_trial = None + best_penalty = float("inf") + for fit_phases in phase_options: + mapped, timing = _phase_map( + native, + lengths, + fit_phases, + q, + anchor, + source_times=times if scene_grasps is not None else None, + ) + mapped[:, untouched] = q[:, untouched] + score = float(np.sqrt(np.mean((mapped[:, owned] - q[:, owned]) ** 2))) + check = ( + None + if scene_grasps is None + else qualify_scene(connect_scene(mapped, fit_phases)) + ) + qualifies = check is None or bool(check["accepted"]) + penalty = ( + 0.0 + if check is None + else max( + check["velocity_ratio"], + check["acceleration_ratio"], + 2.0 if check["position_violations"] else 0.0, + ) + ) + if qualifies or selected_trial is None or penalty < best_penalty: + # Keep one schedule's arrays rather than a path per trial. + selected_trial = ( + score, + mapped, + native, + lengths, + trial, + timing, + trial_goal, + candidate, + qualifies, + fit_phases, + ) + best_penalty = penalty + if check is not None: + candidate_checks.append( + { + "rank": candidate, + "plan_success": True, + "phases": fit_phases, + "limits": check, + } + ) + if qualifies or time.monotonic() - began > cfg.max_seconds: + break + assert selected_trial is not None + qualifies = selected_trial[8] + candidates.append(selected_trial) + if scene_grasps is not None and qualifies: + # Candidate order is proximity to the real grasp, independent of + # source joint RMSE. Keep the closest fully qualified replacement. + break + if not candidates: + raise ValueError("Native skill generation failed within the fitting budget.") + ( + score, + mapped, + raw_native, + lengths, + selected_options, + timing, + goal, + selected_index, + _, + phases, + ) = min( + candidates, + key=( + (lambda item: item[0]) + if scene_grasps is None + else (lambda item: (not item[8], item[7])) + ), + ) + if skill_id == "pick_up": + params["pre_grasp_distance"] = selected_options.pre_grasp_distance + # Every non-participant column stays observed, including passive/mimic state. + candidate_full = recording.qpos.copy() + candidate_full[start:stop] = mapped + if scene_grasps is None: + patched = splice_joint_segment( + recording.qpos, + mapped, + recording.timestamps, + start=start, + stop=stop, + joint_indices=editable, + blend_samples=cfg.blend_samples, + anchors=tuple(start + p["start"] for p in phases[1:]), + ) + else: + patched = connect_scene(mapped, phases) + params.update( + grasp_xpos=scene_grasps[selected_index].grasp_xpos.detach().cpu().tolist(), + object_pose=scene_grasps[selected_index] + .object_pose.detach() + .cpu() + .tolist(), + object_id=scene_grasps[selected_index].semantics.entity_id, + grasp_rotation_error_rad=float( + _rot_error( + scene_grasps[selected_index].grasp_xpos.detach().cpu().numpy(), + observed_grasp, + ) + ), + ) + errors = {} + failures = [] + for role, (observed, _, _, arm_ids, _) in participant_data.items(): + + def fk(values: np.ndarray) -> np.ndarray: + with torch.no_grad(): + return ( + robot.compute_fk( + tensor(values[:, arm_ids]), + name=endpoints[role]["motion"], + to_matrix=True, + ) + .cpu() + .numpy() + ) + + native_tcp, patched_tcp = fk(mapped), fk(patched[start:stop]) + position = np.linalg.norm(native_tcp[:, :3, 3] - observed[:, :3, 3], axis=1) + rotation = _rot_error(native_tcp, observed) + errors[role] = { + "native_tcp_rmse_m": float(np.sqrt(np.mean(position**2))), + "native_rotation_rmse_rad": float(np.sqrt(np.mean(rotation**2))), + } + if scene_grasps is None and ( + errors[role]["native_tcp_rmse_m"] > cfg.max_tcp_rmse_m + or errors[role]["native_rotation_rmse_rad"] > cfg.max_rotation_rmse_rad + ): + failures.append(f"{role}: native TCP reconstruction exceeds tolerance") + for contact_role, a, b in contact_ranges: + if scene_grasps is not None: + continue + if contact_role == role: + if ( + position[a:b].max() > cfg.max_contact_error_m + or rotation[a:b].max() > cfg.max_contact_rotation_rad + ): + failures.append( + f"{role}: native contact path exceeds observation tolerance" + ) + correction = np.linalg.norm( + patched_tcp[a:b, :3, 3] - native_tcp[a:b, :3, 3], axis=1 + ) + correction_rotation = _rot_error(patched_tcp[a:b], native_tcp[a:b]) + if ( + correction.max() > cfg.max_contact_error_m + or correction_rotation.max() > cfg.max_contact_rotation_rad + ): + failures.append( + f"{role}: boundary correction changes protected contact motion" + ) + if scene_grasps is None and score > cfg.max_joint_rmse: + failures.append("native joint RMSE exceeds tolerance") + diagnostics = joint_path_diagnostics( + patched, + recording.timestamps, + lower=limits[:, 0], + upper=limits[:, 1], + velocity_limits=velocity, + acceleration_limits=acceleration, + ) + source_diagnostics = None + if scene_grasps is not None: + source_diagnostics = joint_path_diagnostics( + recording.qpos, + recording.timestamps, + lower=limits[:, 0], + upper=limits[:, 1], + velocity_limits=velocity, + acceleration_limits=acceleration, + ) + diagnostics = qualify_scene(patched) + if not diagnostics["accepted"]: + failures.append("patched path exceeds joint limits") + accepted = not failures + + def serializable(value: Any) -> Any: + if isinstance(value, torch.Tensor): + return value.detach().cpu().tolist() + if isinstance(value, dict): + return {k: serializable(v) for k, v in value.items()} + if isinstance(value, (tuple, list)): + return [serializable(v) for v in value] + return value + + report = { + "schema_version": 1, + "skill_id": skill_id, + "accepted": accepted, + "failures": failures, + "parameters": params, + "phases": phases, + "source_phases": source_phases, + "native_phase_lengths": lengths, + "native_joint_rmse": score, + "native_options": serializable(asdict(selected_options)), + "native_goal": ( + serializable(asdict(goal)) + if scene_grasps is None + else { + "object_id": params["object_id"], + "object_pose": params["object_pose"], + "grasp_xpos": params["grasp_xpos"], + } + ), + "initial_qpos": q[0].tolist(), + "algorithm_revision": 1 if scene_grasps is None else 3, + "per_joint_native_rmse": np.sqrt(np.mean((mapped - q) ** 2, axis=0)).tolist(), + "time_mapping": ( + "phasewise_monotone_native_path_projection" + if scene_grasps is None + else "phasewise_physical_clock" + ), + "native_motion_policy": { + "strategy": policy.strategy, + "sample_count": policy.sample_count, + "acceleration_limit": policy.acceleration_limit, + "control_dt": cfg.control_dt, + }, + "patched_joint_rmse": float( + np.sqrt(np.mean((patched[start:stop, owned] - q[:, owned]) ** 2)) + ), + "tcp_errors": errors, + "limits": diagnostics, + "source_limits": source_diagnostics, + "selected_grasp_rank": selected_index if scene_grasps is not None else None, + "start": start, + "stop": stop, + "editable_joints": owned, + "qpos_semantics": recording.semantics, + "output_semantics": "design_reference", + "physical_success": "unknown", + "collision_evidence": "unverified", + "fit_cfg": asdict(cfg), + "candidate_count": len(candidates), + "candidate_checks": candidate_checks, + "elapsed_seconds": time.monotonic() - began, + "joint_names": list(recording.joint_names), + } + return { + "timestamps": recording.timestamps.copy(), + "original_qpos": recording.qpos.copy(), + "native_qpos": candidate_full, + "patched_qpos": patched if accepted else recording.qpos.copy(), + "proposed_qpos": patched, + "native_phase_qpos": raw_native, + "phase_progress": timing, + "modified_mask": ( + (patched != recording.qpos) + if accepted + else np.zeros_like(patched, dtype=bool) + ), + }, report 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/__init__.py b/embodichain/lab/sim/atomic_skills/primitives/__init__.py index fef056f58a..4440dea4eb 100644 --- a/embodichain/lab/sim/atomic_skills/primitives/__init__.py +++ b/embodichain/lab/sim/atomic_skills/primitives/__init__.py @@ -31,7 +31,7 @@ CoordinatedPlacementGoal, CoordinatedPlacementOptions, ) -from .hand_over import HandOver, HandOverGoal, HandOverOptions +from .hand_over import HandOver, HandOverGoal, HandOverMotionGoal, HandOverOptions from .move_end_effector import ( EndEffectorPoseGoal, MoveEndEffector, @@ -44,7 +44,7 @@ ) from .move_joints import JointPositionGoal, MoveJoints, MoveJointsOptions from .open_door import OpenDoor, OpenDoorGoal, OpenDoorOptions -from .pick_up import GraspGoal, PickUp, PickUpOptions +from .pick_up import GraspGoal, PickUpMotionGoal, PickUp, PickUpOptions from .place import AssembleGoal, Place, PlaceGoal, PlaceOptions from .press import ( Press, @@ -63,10 +63,11 @@ from .slide import ( Slide, SlideGoal, + SlideMotionGoal, SlideJointTarget, SlideOptions, ) -from .twist import Twist, TwistGoal, TwistOptions +from .twist import Twist, TwistGoal, TwistMotionGoal, TwistOptions BUILTIN_SKILL_TYPES: tuple[type[AtomicSkill], ...] = ( ExecuteDemoSkill, @@ -104,8 +105,10 @@ "CoordinatedPlacementOptions", "EndEffectorPoseGoal", "GraspGoal", + "PickUpMotionGoal", "HandOver", "HandOverGoal", + "HandOverMotionGoal", "HandOverOptions", "HeldObjectPoseGoal", "JointPositionGoal", @@ -138,9 +141,11 @@ "PushObjectToolCalibration", "Slide", "SlideGoal", + "SlideMotionGoal", "SlideJointTarget", "SlideOptions", "Twist", "TwistGoal", + "TwistMotionGoal", "TwistOptions", ] 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..8684fefb03 100644 --- a/embodichain/lab/sim/atomic_skills/primitives/hand_over.py +++ b/embodichain/lab/sim/atomic_skills/primitives/hand_over.py @@ -87,6 +87,43 @@ ) +@dataclass(frozen=True, slots=True, eq=False) +class HandOverMotionGoal: + """Explicit dual-TCP anchors for effect-free handover reconstruction. + + Args: + source_exchange: Source TCP at the exchange. + destination_exchange: Receiving TCP at the exchange. + source_retreat: Released source TCP retreat target. + source_grasp: Optional initial pickup TCP; omission starts closed. + destination_place: Optional receiving TCP placement target. + destination_retreat: Receiving TCP after release; required with place. + """ + + goal_kind: ClassVar[str] = "handover_motion" + source_exchange: PoseGoalValue + destination_exchange: PoseGoalValue + source_retreat: PoseGoalValue + source_grasp: PoseGoalValue | None = None + destination_place: PoseGoalValue | None = None + destination_retreat: PoseGoalValue | None = None + + def __post_init__(self) -> None: + for name in ( + "source_exchange", + "destination_exchange", + "source_retreat", + "source_grasp", + "destination_place", + "destination_retreat", + ): + value = getattr(self, name) + if value is not None: + validate_pose_goal(value, name, allow_waypoints=False) + if (self.destination_place is None) != (self.destination_retreat is None): + raise ValueError("Placement requires both receiving TCP anchors.") + + @dataclass(frozen=True, slots=True, eq=False) class HandOverGoal(ObjectSkillGoal): """Object to pick and hand over, plus its final object pose.""" @@ -206,7 +243,7 @@ class _DirectionalPlan: policy_batching: dict[str, object] | None = None -class HandOver(AtomicSkill[HandOverGoal, HandOverOptions]): +class HandOver(AtomicSkill[HandOverGoal | HandOverMotionGoal, HandOverOptions]): """Pick an object with the nearer arm and transfer it to the other arm. For each environment, the action chooses the arm whose root link is closer @@ -238,7 +275,7 @@ class HandOver(AtomicSkill[HandOverGoal, HandOverOptions]): """ skill_id: ClassVar[str] = "hand_over" - GoalType: ClassVar[type] = HandOverGoal + GoalType: ClassVar[tuple[type, ...]] = (HandOverGoal, HandOverMotionGoal) OptionsType: ClassVar[type] = HandOverOptions open_loop: ClassVar[bool] = True _SURFACE_POINT_COUNT: ClassVar[int] = 1000 @@ -281,6 +318,8 @@ def _scene_dependencies( ) -> tuple[str, ...]: """Track both the initial object and any late-bound final target.""" dependencies = set(super()._scene_dependencies(request)) + if isinstance(request.goal, HandOverMotionGoal): + return tuple(sorted(dependencies)) entity_id = request.goal.semantics.entity_id if entity_id is not None: dependencies.add(entity_id) @@ -358,6 +397,8 @@ def _plan( ) -> SkillPlan: """Plan pickup and transfer, with optional placement and release.""" goal = self.require_goal(request) + if isinstance(goal, HandOverMotionGoal): + return self._plan_motion(request, context) options = request.skill_options resources = self._resolve_resources(request) # Each segment plans one explicitly named arm. The task layer merges @@ -681,6 +722,145 @@ def _plan( ), ) + def _plan_motion( + self, + request: ResolvedSkillRequest[HandOverMotionGoal, HandOverOptions], + context: PlanningContext, + ) -> SkillPlan: + """Compose native arm/hand phases without an object attachment claim.""" + goal, options = request.goal, request.skill_options + if options.arm_selection != "bound": + raise ValueError("Handover motion goals require bound participant slots.") + if options.release_at_target != (goal.destination_place is not None): + raise ValueError( + "release_at_target must match the explicit placement anchors." + ) + resources = self._resolve_resources(request) + source, destination = resources.first, resources.second + count = max(3, request.motion_policy.sample_count // 8) + full = context.last_qpos.clone() + segments: list[torch.Tensor] = [] + lengths: dict[str, int] = {} + success = torch.ones(context.batch_size, dtype=torch.bool, device=self.device) + + def pose(value: PoseGoalValue) -> torch.Tensor: + return resolve_batched_pose( + resolve_pose_goal(value, context, name="motion_anchor"), + num_envs=context.batch_size, + device=self.device, + name="motion_anchor", + ) + + def append( + name: str, + participant: _Participant | None = None, + target: torch.Tensor | None = None, + command: torch.Tensor | None = None, + n: int = count, + ) -> None: + nonlocal full, success + if participant is not None and target is not None: + valid, path = plan_named_arm_trajectory( + self.motion_generator, + participant.arm.control_part, + full[:, list(participant.arm.joint_ids)], + target, + n, + request.motion_policy, + context.require_control_dt(), + ) + success &= valid + result = full[:, None].expand(-1, path.shape[1], -1).clone() + result[:, :, list(participant.arm.joint_ids)] = path + else: + result = full[:, None].expand(-1, n, -1).clone() + if participant is not None and command is not None: + ids = list(participant.hand.joint_ids) + result[:, :, ids] = interpolate_hand_qpos( + full[:, ids], command, n_waypoints=result.shape[1] + ) + full = result[:, -1].clone() + lengths[name] = result.shape[1] + segments.append(result) + + if goal.source_grasp is not None: + grasp = pose(goal.source_grasp) + pre = translate_pose_world( + grasp, -grasp[:, :3, 2] * options.pre_grasp_distance + ) + append( + "pickup_approach", + source, + torch.stack((pre, grasp), dim=1), + source.hand_open_qpos, + ) + append( + "pickup_close", + source, + command=source.hand_grasp_qpos, + n=options.hand_interp_steps, + ) + else: + # Retain the profile's effort-producing command on a closed source. + full[:, list(source.hand.joint_ids)] = source.hand_grasp_qpos + append("transfer", source, pose(goal.source_exchange)[:, None]) + receive = pose(goal.destination_exchange) + pre = translate_pose_world( + receive, -receive[:, :3, 2] * options.pre_grasp_distance + ) + append( + "approach", + destination, + torch.stack((pre, receive), dim=1), + destination.hand_open_qpos, + ) + append( + "close", + destination, + command=destination.hand_grasp_qpos, + n=options.hand_interp_steps, + ) + append("hold", n=max(1, options.hold_steps)) + append( + "release", + source, + command=source.hand_open_qpos, + n=options.hand_interp_steps, + ) + append( + "retreat", + source, + pose(goal.source_retreat)[:, None], + n=options.retreat_steps, + ) + if goal.destination_place is not None: + append("place", destination, pose(goal.destination_place)[:, None]) + append( + "receive_release", + destination, + command=destination.hand_open_qpos, + n=options.hand_interp_steps, + ) + append( + "receive_retreat", destination, pose(goal.destination_retreat)[:, None] + ) + return self.build_plan( + request, + context, + success=success, + trajectory=TimedTrajectory.from_uniform_step( + torch.cat(segments, dim=1), + env_ids=context.env_ids, + step_dt=context.require_control_dt(), + ), + segment_lengths=lengths, + expected_effects=StateDelta(), + diagnostics=PlannerDiagnostics( + backend=self.planning_services.planner_name, + metadata={"grounding": "explicit_tcp", "physical_success": "unknown"}, + ), + ) + def _plan_existing_hold( self, request: ResolvedSkillRequest[HandOverGoal, HandOverOptions], @@ -740,6 +920,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 +962,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 +1450,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 +2151,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, @@ -2304,4 +2489,4 @@ def _assemble_segment( ) -__all__ = ["HandOver", "HandOverGoal", "HandOverOptions"] +__all__ = ["HandOver", "HandOverGoal", "HandOverMotionGoal", "HandOverOptions"] diff --git a/embodichain/lab/sim/atomic_skills/primitives/move_held_object.py b/embodichain/lab/sim/atomic_skills/primitives/move_held_object.py index c567926f82..08772c5086 100644 --- a/embodichain/lab/sim/atomic_skills/primitives/move_held_object.py +++ b/embodichain/lab/sim/atomic_skills/primitives/move_held_object.py @@ -18,7 +18,7 @@ from __future__ import annotations -from dataclasses import dataclass, field +from dataclasses import dataclass, field, replace import math from typing import ClassVar @@ -110,6 +110,19 @@ def __post_init__(self) -> None: class MoveHeldObjectOptions(SkillOptions): """Per-invocation held-object transport behavior.""" + verified_terminal_object_to_eef: torch.Tensor | None = None + """Optional measured terminal relation for a demonstrated in-hand pivot.""" + + def __post_init__(self) -> None: + pose = self.verified_terminal_object_to_eef + if pose is not None: + validate_pose_goal( + pose, "verified_terminal_object_to_eef", allow_waypoints=False + ) + if not isinstance(pose, torch.Tensor) or pose.shape != (4, 4): + raise ValueError("verified terminal relation must be one SE(3) tensor") + object.__setattr__(self, "verified_terminal_object_to_eef", pose.clone()) + class MoveHeldObject(AtomicSkill[HeldObjectPoseGoal, MoveHeldObjectOptions]): """Move the held object through exact target poses with a closed hand. @@ -335,13 +348,23 @@ def plan_target(poses: torch.Tensor) -> PlanResult: control_dt=context.require_control_dt(), ) + terminal_held = held_object + terminal_relation = options.verified_terminal_object_to_eef + if terminal_relation is not None: + terminal_held = replace( + held_object, + object_to_eef=terminal_relation.to(start_arm_qpos) + .unsqueeze(0) + .expand(context.batch_size, -1, -1) + .clone(), + ) return self.build_plan( request, context, success=success, trajectory=timed, expected_effects=( - StateDelta(held_object_updates={task_state_key: held_object}) + StateDelta(held_object_updates={task_state_key: terminal_held}) if target.object_id is not None else None ), diff --git a/embodichain/lab/sim/atomic_skills/primitives/pick_up.py b/embodichain/lab/sim/atomic_skills/primitives/pick_up.py index 3213f9eb22..aa4692b551 100644 --- a/embodichain/lab/sim/atomic_skills/primitives/pick_up.py +++ b/embodichain/lab/sim/atomic_skills/primitives/pick_up.py @@ -91,6 +91,24 @@ ) +@dataclass(frozen=True, slots=True, eq=False) +class PickUpMotionGoal: + """Explicit world TCP anchor for effect-free pickup motion reconstruction. + + This goal shares PickUp's native phase generator. It carries no object + identity and creates no held-object state or physical-success evidence. + + Args: + grasp_xpos: Observed or FK-derived world grasp TCP transform. + """ + + goal_kind: ClassVar[str] = "pickup_motion" + grasp_xpos: PoseGoalValue + + def __post_init__(self) -> None: + validate_pose_goal(self.grasp_xpos, "grasp_xpos", allow_waypoints=False) + + @dataclass(frozen=True, slots=True, eq=False) class GraspGoal(ObjectSkillGoal): """Pickup target with an affordance-selected or supplied grasp pose.""" @@ -222,8 +240,17 @@ class PickUpOptions(SkillOptions): observed rotation instead of a shared world vector. """ + verified_object_to_eef: torch.Tensor | None = None + """Expected measured attachment relation when the command is servo-compensated.""" + + grasp_relative_approach: tuple[torch.Tensor, ...] = () + """Optional TCP waypoints relative to the resolved grasp, ordered before close.""" + 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.""" @@ -282,6 +309,20 @@ def __post_init__(self) -> None: or not torch.isfinite(self.obj_upright_direction).all() ): raise ValueError("obj_upright_direction must be a finite (3,) tensor.") + for index, pose in enumerate(self.grasp_relative_approach): + _validate_single_se3(pose, f"grasp_relative_approach[{index}]") + object.__setattr__( + self, + "grasp_relative_approach", + tuple(pose.clone() for pose in self.grasp_relative_approach), + ) + if self.verified_object_to_eef is not None: + _validate_single_se3(self.verified_object_to_eef, "verified_object_to_eef") + object.__setattr__( + self, + "verified_object_to_eef", + self.verified_object_to_eef.clone(), + ) _validate_single_se3(self.grasp_frame_to_eef, "grasp_frame_to_eef") if type(self.cartesian_lift) is not bool: raise TypeError("cartesian_lift must be boolean.") @@ -324,11 +365,11 @@ def __post_init__(self) -> None: ) -class PickUp(AtomicSkill[GraspGoal, PickUpOptions]): +class PickUp(AtomicSkill[GraspGoal | PickUpMotionGoal, PickUpOptions]): """Approach a grasp pose, close the gripper, lift.""" skill_id: ClassVar[str] = "pick_up" - GoalType: ClassVar[type] = GraspGoal + GoalType: ClassVar[tuple[type, ...]] = (GraspGoal, PickUpMotionGoal) OptionsType: ClassVar[type] = PickUpOptions binding_contract: ClassVar[SkillBindingContract] = SkillBindingContract( slots=( @@ -355,6 +396,8 @@ def _scene_dependencies( ) -> tuple[str, ...]: """Monitor the acquired object and late-bound downstream targets.""" dependencies = set(super()._scene_dependencies(request)) + if isinstance(request.goal, PickUpMotionGoal): + return tuple(sorted(dependencies)) entity_id = request.goal.semantics.entity_id # An explicit object pose is a scene-independent planning input. Do # not retain the semantic entity as a dynamic dependency in that @@ -382,6 +425,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 +456,13 @@ 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.grasp_relative_approach: + waypoints[:1] = [ + grasp_xpos @ waypoint.to(grasp_xpos) + for waypoint in options.grasp_relative_approach + ] + phase_names[:1] = ["pre_grasp"] * len(options.grasp_relative_approach) if options.initial_lift_height > 0.0: start_xpos = self.robot.compute_fk( qpos=start_arm_qpos, @@ -427,6 +479,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": @@ -547,6 +605,8 @@ def _plan( ) -> SkillPlan: """Plan approach, close, and lift segments without committing attachment.""" target = self.require_goal(request) + if isinstance(target, PickUpMotionGoal): + return self._plan_motion(request, context) options = replace( request.skill_options, downstream_object_target_poses=tuple( @@ -712,6 +772,73 @@ def _plan( provenance=None, ) + def _plan_motion( + self, + request: ResolvedSkillRequest[PickUpMotionGoal, PickUpOptions], + context: PlanningContext, + ) -> SkillPlan: + """Use the native pickup phases with explicit TCP grounding only.""" + options = request.skill_options + if ( + options.fixed_object_to_eef is not None + or options.rotate_upright is not None + or options.downstream_object_target_poses + or options.approach_along_grasp_axis + ): + raise ValueError("Pickup motion goals cannot use object-relative options.") + arm = request.binding.endpoint("primary", "motion").require_target( + JointPositionTarget + ) + hand = request.binding.endpoint("primary", "grasp") + hand_target = hand.require_target(JointPositionTarget) + pose = resolve_pose_target( + resolve_pose_goal(request.goal.grasp_xpos, context, name="grasp_xpos"), + num_envs=context.batch_size, + device=self.device, + ) + direction = options.approach_direction.to(pose) + if direction.ndim == 2 and direction.shape[0] != context.batch_size: + raise ValueError("approach_direction must match the planning batch.") + direction = direction / torch.linalg.vector_norm( + direction, dim=-1, keepdim=True + ) + commands = { + command: hand.joint_positions( + command, + num_envs=context.batch_size, + device=self.device, + dtype=context.robot.qpos.dtype, + ) + for command in (OPEN_COMMAND, GRASP_COMMAND) + } + success, full, lengths = self._get_full_pickup_trajectory( + pose, + context.robot.qpos[:, list(arm.joint_ids)], + context.last_qpos, + request.motion_policy, + options, + direction, + arm, + hand_target, + commands[OPEN_COMMAND], + commands[GRASP_COMMAND], + context.require_control_dt(), + ) + return self.build_plan( + request, + context, + success=success, + trajectory=TimedTrajectory.from_uniform_step( + full, env_ids=context.env_ids, step_dt=context.require_control_dt() + ), + expected_effects=StateDelta(), + segment_lengths=lengths, + diagnostics=PlannerDiagnostics( + backend=self.planning_services.planner_name, + metadata={"grounding": "explicit_tcp", "physical_success": "unknown"}, + ), + ) + def _plan_from_nominal( self, request: ResolvedSkillRequest[GraspGoal, PickUpOptions], @@ -720,6 +847,8 @@ def _plan_from_nominal( ) -> SkillPlan: """Assemble a Pick from an explicitly supplied external motion.""" target = self.require_goal(request) + if isinstance(target, PickUpMotionGoal): + raise ValueError("Pickup motion goals require native generation.") motion = self._validate_nominal_motion( request, context, nominal_source, segments=("approach", "close", "lift") ) @@ -848,6 +977,9 @@ def _assemble_pick_plan( sem = target.semantics state = context object_to_eef = torch.bmm(pose_inv(object_pose), grasp_xpos) + if options.verified_object_to_eef is not None: + object_to_eef = options.verified_object_to_eef.to(object_pose) + object_to_eef = object_to_eef.unsqueeze(0).expand_as(object_pose).clone() held = HeldObjectState( semantics=sem, object_to_eef=object_to_eef, grasp_xpos=grasp_xpos ) @@ -1191,4 +1323,4 @@ def _upright_adjusted_grasp_poses( return adjusted_grasp_xpos -__all__ = ["GraspGoal", "PickUp", "PickUpOptions"] +__all__ = ["PickUpMotionGoal", "GraspGoal", "PickUp", "PickUpOptions"] 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/primitives/slide.py b/embodichain/lab/sim/atomic_skills/primitives/slide.py index f041d7f053..07e113f000 100644 --- a/embodichain/lab/sim/atomic_skills/primitives/slide.py +++ b/embodichain/lab/sim/atomic_skills/primitives/slide.py @@ -113,6 +113,27 @@ def __post_init__(self) -> None: ) +@dataclass(frozen=True, slots=True, eq=False) +class SlideMotionGoal: + """World TCP and rail direction for effect-free native Slide motion. + + Args: + grasp_xpos: Explicit world grasp TCP transform. + translation_axis: World rail direction; Options owns push/pull sign. + """ + + goal_kind: ClassVar[str] = "slide_motion" + grasp_xpos: PoseGoalValue + translation_axis: torch.Tensor + + def __post_init__(self) -> None: + validate_pose_goal(self.grasp_xpos, "grasp_xpos", allow_waypoints=False) + axis = self.translation_axis + if axis.shape != (3,) or not torch.isfinite(axis).all() or axis.norm() < 1e-8: + raise ValueError("translation_axis must be a finite nonzero world vector.") + object.__setattr__(self, "translation_axis", axis.clone()) + + @dataclass(frozen=True, slots=True, eq=False) class SlideGoal(ObjectSkillGoal): """Translating articulation link described by a slide affordance.""" @@ -206,11 +227,11 @@ def __post_init__(self) -> None: raise ValueError("Release retreat requires hand_interp_steps >= 2.") -class Slide(AtomicSkill[SlideGoal, SlideOptions]): +class Slide(AtomicSkill[SlideGoal | SlideMotionGoal, SlideOptions]): """Open-loop approach, grasp, and axis-constrained sliding motion.""" skill_id: ClassVar[str] = "slide" - GoalType: ClassVar[type] = SlideGoal + GoalType: ClassVar[tuple[type, ...]] = (SlideGoal, SlideMotionGoal) OptionsType: ClassVar[type] = SlideOptions open_loop: ClassVar[bool] = True binding_contract: ClassVar[SkillBindingContract] = SkillBindingContract( @@ -246,16 +267,21 @@ def _plan( "must preserve every Cartesian sample." ) target = self.require_goal(request) - affordance = self._require_slide_affordance(target.semantics) + affordance = ( + None + if isinstance(target, SlideMotionGoal) + else self._require_slide_affordance(target.semantics) + ) + joint_target = target.joint_target if isinstance(target, SlideGoal) else None options = request.skill_options interpolation_dt = context.require_control_dt() # Resolve late-bound scene targets before any already-satisfied fast # path so invalid or low-confidence references fail at the goal # boundary instead of being reported as a successful hold. - resolved_link_pose = resolve_pose_goal( - target.target_pose, - context, - name="target_pose", + resolved_link_pose = ( + resolve_pose_goal(target.target_pose, context, name="target_pose") + if isinstance(target, SlideGoal) + else None ) direction = options.direction displacement = None @@ -264,18 +290,18 @@ def _plan( ) reached = torch.zeros_like(joint_valid) diagnostics: PlannerDiagnostics | None = None - if target.joint_target is not None: + if joint_target is not None: displacement, joint_valid, reached = self._joint_displacement( - target.joint_target, affordance, context + joint_target, affordance, context ) active = joint_valid & ~reached diagnostics = PlannerDiagnostics( backend=self.planning_services.planner_name, metadata={ "joint_target": { - "articulation_id": target.joint_target.articulation_id, - "joint_name": target.joint_target.joint_name, - "position": target.joint_target.position, + "articulation_id": joint_target.articulation_id, + "joint_name": joint_target.joint_name, + "position": joint_target.position, "already_satisfied": reached.detach().cpu().tolist(), } }, @@ -322,33 +348,54 @@ def _plan( dtype=context.robot.qpos.dtype, ) - link_pose = resolve_pose_target( - resolved_link_pose, - num_envs=self.num_envs, - device=self.device, - ) - translation_axis = affordance.translation_axis.to( - device=self.device, dtype=torch.float32 - ) - translation_axis = translation_axis / torch.linalg.vector_norm(translation_axis) - translation_axis_world = torch.matmul(link_pose[:, :3, :3], translation_axis) - grasp_generator = self.planning_services.grasp_pose_generator( - grasp_target.target_id - ) - candidates = affordance.get_grasp_candidates( - grasp_generator, - link_pose, - translation_axis_world, - ) - grasp_sample = affordance.sample_candidates( - candidates, - sampling=context.affordance_sampling, - env_ids=context.env_ids, - key=(request.invocation_id or self.skill_id) + ":grasp", - reference_poses=link_pose, - ) - grasp_success = grasp_sample.success - grasp_xpos = grasp_sample.poses.to(device=self.device, dtype=torch.float32) + if isinstance(target, SlideMotionGoal): + grasp_xpos = resolve_pose_target( + resolve_pose_goal(target.grasp_xpos, context, name="grasp_xpos"), + num_envs=self.num_envs, + device=self.device, + ) + axis = target.translation_axis.to(grasp_xpos) + translation_axis_world = (axis / axis.norm()).expand(self.num_envs, 3) + grasp_success = torch.ones( + self.num_envs, dtype=torch.bool, device=self.device + ) + grasp_metadata = { + "grounding": "explicit_tcp", + "physical_success": "unknown", + } + else: + link_pose = resolve_pose_target( + resolved_link_pose, + num_envs=self.num_envs, + device=self.device, + ) + translation_axis = affordance.translation_axis.to( + device=self.device, dtype=torch.float32 + ) + translation_axis = translation_axis / torch.linalg.vector_norm( + translation_axis + ) + translation_axis_world = torch.matmul( + link_pose[:, :3, :3], translation_axis + ) + grasp_generator = self.planning_services.grasp_pose_generator( + grasp_target.target_id + ) + candidates = affordance.get_grasp_candidates( + grasp_generator, + link_pose, + translation_axis_world, + ) + grasp_sample = affordance.sample_candidates( + candidates, + sampling=context.affordance_sampling, + env_ids=context.env_ids, + key=(request.invocation_id or self.skill_id) + ":grasp", + reference_poses=link_pose, + ) + grasp_success = grasp_sample.success + grasp_xpos = grasp_sample.poses.to(device=self.device, dtype=torch.float32) + grasp_metadata = grasp_sample.metadata grasp_success = normalize_success_mask( grasp_success, num_envs=self.num_envs, @@ -356,7 +403,7 @@ def _plan( name="Slide grasp-pose success", ) if not grasp_success.any(): - if target.joint_target is not None and reached.any(): + if joint_target is not None and reached.any(): return self.build_plan( request, context, @@ -570,7 +617,7 @@ def _plan( full[:, offset:, arm_joint_ids] = return_arm full[:, offset:, hand_joint_ids] = hand_open_qpos.unsqueeze(1) - if target.joint_target is not None: + if joint_target is not None: full[reached] = context.robot.qpos[reached, None] success = (success & joint_valid) | reached @@ -588,7 +635,8 @@ def _plan( backend=self.planning_services.planner_name, metadata={ **(diagnostics.metadata if diagnostics is not None else {}), - "affordance_sample": {"grasp": grasp_sample.metadata}, + "affordance_sample": {"grasp": grasp_metadata}, + **(grasp_metadata if isinstance(target, SlideMotionGoal) else {}), }, ), segment_lengths=segment_lengths, @@ -704,6 +752,7 @@ def _plan_pose_segment( __all__ = [ + "SlideMotionGoal", "Slide", "SlideGoal", "SlideJointTarget", diff --git a/embodichain/lab/sim/atomic_skills/primitives/twist.py b/embodichain/lab/sim/atomic_skills/primitives/twist.py index 9e48768b31..8c3c1f783d 100644 --- a/embodichain/lab/sim/atomic_skills/primitives/twist.py +++ b/embodichain/lab/sim/atomic_skills/primitives/twist.py @@ -74,6 +74,33 @@ ) +@dataclass(frozen=True, slots=True, eq=False) +class TwistMotionGoal: + """World TCP and rotation line for effect-free native Twist motion. + + Args: + grasp_xpos: Explicit world grasp TCP transform. + twist_axis: World rotation direction. + axis_origin: One world point on the rotation line. + """ + + goal_kind: ClassVar[str] = "twist_motion" + grasp_xpos: PoseGoalValue + twist_axis: torch.Tensor + axis_origin: tuple[float, float, float] + + def __post_init__(self) -> None: + validate_pose_goal(self.grasp_xpos, "grasp_xpos", allow_waypoints=False) + axis = self.twist_axis + if axis.shape != (3,) or not torch.isfinite(axis).all() or axis.norm() < 1e-8: + raise ValueError("twist_axis must be a finite nonzero world vector.") + if len(self.axis_origin) != 3 or not all( + math.isfinite(v) for v in self.axis_origin + ): + raise ValueError("axis_origin must be one finite world point.") + object.__setattr__(self, "twist_axis", axis.clone()) + + @dataclass(frozen=True, slots=True, eq=False) class TwistGoal(ObjectSkillGoal): """Target object described by a twist affordance.""" @@ -115,11 +142,11 @@ def __post_init__(self) -> None: raise ValueError("twist_angle must be finite.") -class Twist(AtomicSkill[TwistGoal, TwistOptions]): +class Twist(AtomicSkill[TwistGoal | TwistMotionGoal, TwistOptions]): """Open-loop approach, grasp, twist, release, and retract motion.""" skill_id: ClassVar[str] = "twist" - GoalType: ClassVar[type] = TwistGoal + GoalType: ClassVar[tuple[type, ...]] = (TwistGoal, TwistMotionGoal) OptionsType: ClassVar[type] = TwistOptions open_loop: ClassVar[bool] = True binding_contract: ClassVar[SkillBindingContract] = SkillBindingContract( @@ -157,7 +184,11 @@ def _plan( "twist path must preserve every Cartesian sample." ) target = self.require_goal(request) - affordance = self._require_twist_affordance(target.semantics) + affordance = ( + None + if isinstance(target, TwistMotionGoal) + else self._require_twist_affordance(target.semantics) + ) options = request.skill_options interpolation_dt = context.require_control_dt() binding = request.binding @@ -183,24 +214,46 @@ def _plan( dtype=context.robot.qpos.dtype, ) - link_pose = resolve_pose_target( - resolve_pose_goal(target.target_pose, context, name="target_pose"), - num_envs=self.num_envs, - device=self.device, - ) - grasp_sample = affordance.sample_grasp_pose( - link_pose, - sampling=context.affordance_sampling, - env_ids=context.env_ids, - key=(request.invocation_id or self.skill_id) + ":grasp", - ) - grasp_xpos = grasp_sample.poses.to(device=self.device, dtype=torch.float32) - grasp_xpos = find_symmetric_nearest_xpos( - grasp_xpos, - reference_xpos=self.robot.compute_fk( - qpos=start_arm_qpos, name=control_part, to_matrix=True - ), - ) + if isinstance(target, TwistMotionGoal): + grasp_xpos = resolve_pose_target( + resolve_pose_goal(target.grasp_xpos, context, name="grasp_xpos"), + num_envs=self.num_envs, + device=self.device, + ) + # Identity is the world coordinate frame, not an invented object pose. + link_pose = torch.eye(4, device=self.device).expand(self.num_envs, 4, 4) + twist_axis = target.twist_axis + axis_origin = target.axis_origin + grasp_success = torch.ones( + self.num_envs, dtype=torch.bool, device=self.device + ) + grasp_metadata = { + "grounding": "explicit_tcp", + "physical_success": "unknown", + } + else: + link_pose = resolve_pose_target( + resolve_pose_goal(target.target_pose, context, name="target_pose"), + num_envs=self.num_envs, + device=self.device, + ) + grasp_sample = affordance.sample_grasp_pose( + link_pose, + sampling=context.affordance_sampling, + env_ids=context.env_ids, + key=(request.invocation_id or self.skill_id) + ":grasp", + ) + grasp_xpos = grasp_sample.poses.to(device=self.device, dtype=torch.float32) + grasp_xpos = find_symmetric_nearest_xpos( + grasp_xpos, + reference_xpos=self.robot.compute_fk( + qpos=start_arm_qpos, name=control_part, to_matrix=True + ), + ) + twist_axis = affordance.twist_axis + axis_origin = affordance.require_axis_origin() + grasp_success = grasp_sample.success + grasp_metadata = grasp_sample.metadata pre_grasp_xpos = translate_pose_world( grasp_xpos, -grasp_xpos[:, :3, 2] * options.pre_grasp_distance, @@ -213,8 +266,8 @@ def _plan( twist_xpos = self._twisted_grasp_poses( link_pose, grasp_xpos, - affordance.twist_axis, - affordance.require_axis_origin(), + twist_axis, + axis_origin, options.twist_angle, n_twist - 1, ) @@ -233,7 +286,7 @@ def _plan( grasp_xpos[:, :3, 2], n_waypoints=n_reach - 1, ) - reach_active = grasp_sample.success & approach_success + reach_active = grasp_success & approach_success reach_hold_pose = self.robot.compute_fk( qpos=approach_arm[:, -1], name=control_part, @@ -269,8 +322,8 @@ def sample_twist_path(progress: torch.Tensor) -> torch.Tensor: sampled = self._twisted_grasp_poses_at_fractions( link_pose, grasp_xpos, - affordance.twist_axis, - affordance.require_axis_origin(), + twist_axis, + axis_origin, options.twist_angle, progress, ) @@ -299,7 +352,7 @@ def sample_twist_path(progress: torch.Tensor) -> torch.Tensor: interpolation_dt=interpolation_dt, ) success = ( - grasp_sample.success + grasp_success & approach_success & reach_success & twist_success @@ -379,7 +432,10 @@ def sample_twist_path(progress: torch.Tensor) -> torch.Tensor: expected_effects=StateDelta(), diagnostics=PlannerDiagnostics( backend=self.planning_services.planner_name, - metadata={"affordance_sample": grasp_sample.metadata}, + metadata={ + "affordance_sample": grasp_metadata, + **(grasp_metadata if isinstance(target, TwistMotionGoal) else {}), + }, ), segment_lengths={ "approach": lengths[0], @@ -492,4 +548,4 @@ def _twisted_grasp_poses_at_fractions( return eef_poses -__all__ = ["Twist", "TwistGoal", "TwistOptions"] +__all__ = ["TwistMotionGoal", "Twist", "TwistGoal", "TwistOptions"] 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..7cc6bce59f 100644 --- a/embodichain/lab/sim/motion/expansion/combined.py +++ b/embodichain/lab/sim/motion/expansion/combined.py @@ -138,8 +138,10 @@ class _CombinedSourceCfg: trajectory_call_indices: tuple[int, ...] = (1, 3, 5) def __post_init__(self) -> None: - if self.kind != "task_program": - raise ValueError("combined source.kind must be task_program") + if self.kind not in {"task_program", "skill_sequence"}: + raise ValueError( + "combined source.kind must be task_program or skill_sequence" + ) for name in ("source_id", "source_revision", "template_id"): _text(getattr(self, name), f"source.{name}") if self.unit_scope != "episode": @@ -236,8 +238,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 +530,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..c9b29aba81 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 @@ -325,11 +349,29 @@ class MotionGenerator: } def __init__( - self, cfg: MotionGenCfg, *, env_ids: tuple[int, ...] | None = None + self, + cfg: MotionGenCfg, + *, + env_ids: tuple[int, ...] | None = None, + planner: BasePlanner | None = None, ) -> None: + """Create a generator with an optional already-bound planner. + Args: + cfg: Backend configuration used when no planner is supplied. + env_ids: Optional physical row selection. + planner: Borrowed planner, including a planner bound to an offline + kinematics port. It must use the configured backend type. + """ # Create planner based on planner_type - self.planner: BasePlanner = self._create_planner(cfg.planner_cfg) + if ( + planner is not None + and planner.cfg.planner_type != cfg.planner_cfg.planner_type + ): + raise ValueError("Explicit planner must use the configured backend type.") + self.planner: BasePlanner = ( + self._create_planner(cfg.planner_cfg) if planner is None else planner + ) self.robot = self.planner.robot self.device = self.robot.device @@ -761,11 +803,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 +1288,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..a0c684b1e1 100644 --- a/embodichain/lab/sim/motion/planners/base_planner.py +++ b/embodichain/lab/sim/motion/planners/base_planner.py @@ -22,7 +22,7 @@ from abc import ABC, abstractmethod from collections.abc import Mapping from dataclasses import MISSING, dataclass -from typing import Literal +from typing import Literal, TYPE_CHECKING from embodichain.utils import logger from embodichain.utils import configclass @@ -30,6 +30,9 @@ from .utils import MoveType, PlanState, PlanResult from ._joint_limits import _resolve_joint_constraints +if TYPE_CHECKING: + from embodichain.lab.sim.objects import Robot + __all__ = [ "BasePlannerCfg", "CollisionWorldInfo", @@ -184,8 +187,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) @@ -208,14 +213,25 @@ class BasePlanner(ABC): cfg: Configuration object for the planner. """ - def __init__(self, cfg: BasePlannerCfg): + def __init__(self, cfg: BasePlannerCfg, *, robot: Robot | None = None): + """Bind a simulator robot or an explicit offline kinematics adapter. + + Args: + cfg: Planner configuration. + robot: Borrowed robot-compatible kinematics port. When supplied, + construction does not look up a SimulationManager. + """ self.cfg: BasePlannerCfg = cfg if cfg.robot_uid is MISSING: logger.log_error("robot_uid is required in planner config", ValueError) - self.robot = SimulationManager.get_instance(cfg.sim_instance_id).get_robot( - cfg.robot_uid + self.robot = ( + robot + if robot is not None + else SimulationManager.get_instance(cfg.sim_instance_id).get_robot( + cfg.robot_uid + ) ) if self.robot is None: logger.log_error(f"Robot {cfg.robot_uid} not found", ValueError) 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 1d58820f6f..31a95a782e 100644 --- a/embodichain/lab/sim/objects/articulation.py +++ b/embodichain/lab/sim/objects/articulation.py @@ -2039,8 +2039,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 1a7967773b..3ea1daf78c 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 86b2f1805e..40a27435ae 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 4f0967292a..48739b8201 100644 --- a/embodichain/lab/sim/sensors/__init__.py +++ b/embodichain/lab/sim/sensors/__init__.py @@ -29,6 +29,7 @@ ContactSensor, ContactSensorCfg, ArticulationContactFilterCfg, + ContactPermissionChecker, ) from .registry import ( SensorRegistration, 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/sim/shapes.py b/embodichain/lab/sim/shapes.py index 4b62c0bdd6..daf0b37743 100755 --- a/embodichain/lab/sim/shapes.py +++ b/embodichain/lab/sim/shapes.py @@ -75,6 +75,13 @@ class MeshCollisionCfg: VisACD requires a DexSim build with CUDA/OptiX support. """ + decomposition_fpath: str | None = None + """Precomputed convex parts in an OBJ, one object/group per convex piece. + + Valid only for ``convex_decomposition``. Parts use the source mesh's local + frame and units, before body scaling. Spawn loads each part as a separate + convex collider without running decomposition; the visual mesh is retained. + """ force_sdf: bool | None = None """Optional Newton SDF strategy override; False uses an eagerly baked SDF.""" @@ -127,10 +134,22 @@ def __post_init__(self) -> None: ) if self.acd_method not in (None, "visacd", "coacd", "vhacd"): raise ValueError("acd_method must be 'visacd', 'coacd', or 'vhacd'.") + if self.decomposition_fpath is not None and ( + not isinstance(self.decomposition_fpath, str) + or not self.decomposition_fpath.strip() + ): + raise ValueError("decomposition_fpath must be a non-empty path.") elif self.max_hulls is not None or self.acd_method is not None: raise ValueError( "max_hulls and acd_method are valid only for convex_decomposition." ) + if ( + self.decomposition_fpath is not None + and self.approximation != "convex_decomposition" + ): + raise ValueError( + "decomposition_fpath is valid only for convex_decomposition." + ) sdf_values = { "force_sdf": self.force_sdf, @@ -263,6 +282,7 @@ def _mesh_collision_cfg_init_with_legacy_max_hulls( sdf_padding: float | None = None, *, max_convex_hull_num: int | None = None, + decomposition_fpath: str | None = None, ) -> None: """Initialize with the deprecated hull-count spelling at the API boundary.""" if max_convex_hull_num is not None: @@ -297,6 +317,7 @@ def _mesh_collision_cfg_init_with_legacy_max_hulls( approximation="convex_hull" if approximation is None else approximation, max_hulls=max_hulls, acd_method=acd_method, + decomposition_fpath=decomposition_fpath, force_sdf=force_sdf, sdf_resolution=sdf_resolution, is_hydroelastic=is_hydroelastic, diff --git a/embodichain/lab/sim/spawn/_convex_parts.py b/embodichain/lab/sim/spawn/_convex_parts.py new file mode 100644 index 0000000000..2e7e6a0701 --- /dev/null +++ b/embodichain/lab/sim/spawn/_convex_parts.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. +# ---------------------------------------------------------------------------- + +"""Read precomputed object-local convex pieces without decomposition.""" + +from __future__ import annotations + +from pathlib import Path + +import numpy as np + +__all__ = [] + + +def load_convex_parts(path: str | Path) -> list[tuple[np.ndarray, np.ndarray]]: + """Require non-empty, finite, three-dimensional indexed convex-piece inputs.""" + from dexsim.kit.meshproc.convex_cache import load_obj_as_convex_parts + + source = Path(path).expanduser().resolve(strict=True) + parts = load_obj_as_convex_parts(str(source)) + if not parts: + raise ValueError( + f"Precomputed decomposition contains no convex parts: {source}" + ) + for vertices, triangles in parts: + if ( + vertices.ndim != 2 + or vertices.shape[1:] != (3,) + or len(vertices) < 4 + or not np.isfinite(vertices).all() + or triangles.ndim != 2 + or triangles.shape[1:] != (3,) + or len(triangles) < 4 + or triangles.min() < 0 + or triangles.max() >= len(vertices) + or np.linalg.matrix_rank(vertices.astype(np.float64) - vertices[0]) != 3 + ): + raise ValueError(f"Invalid precomputed convex part in {source}") + return parts diff --git a/embodichain/lab/sim/spawn/descriptors.py b/embodichain/lab/sim/spawn/descriptors.py index cd397b8e86..ab8dc9ab8c 100644 --- a/embodichain/lab/sim/spawn/descriptors.py +++ b/embodichain/lab/sim/spawn/descriptors.py @@ -427,7 +427,31 @@ def rigid_desc_from_cfg( collision.render_source_index = ( None if cfg.visuals is not None or not renders else index ) - collisions.append(collision) + mesh_collision = ( + part_shape.collision if isinstance(part_shape, MeshCfg) else None + ) + if ( + mesh_collision is not None + and mesh_collision.decomposition_fpath is not None + ): + from ._convex_parts import load_convex_parts + + convex_parts = load_convex_parts(mesh_collision.decomposition_fpath) + if len(convex_parts) > max_hulls: + raise ValueError("Precomputed decomposition exceeds max_hulls.") + collisions.extend( + replace( + collision, + file_path=None, + vertices=vertices, + triangles=triangles, + approximation=CollisionApproximation.CONVEX_HULL, + render_source_index=None, + ) + for vertices, triangles in convex_parts + ) + else: + collisions.append(collision) if cfg.particle_collision_shape is not None: boundary_shape = cfg.particle_collision_shape 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_composition.py b/embodichain/lab/task_program/integrations/_configured_composition.py index 55647ff708..df96780357 100644 --- a/embodichain/lab/task_program/integrations/_configured_composition.py +++ b/embodichain/lab/task_program/integrations/_configured_composition.py @@ -713,7 +713,9 @@ def _compose_integration_payload( _normalize_skill_options_key(task["profile"], path="task integration.profile"), path="task integration.profile", required=frozenset({"defaults", "skill_options", "effect_monitors"}), - optional=frozenset({"skill_presets", "grounding_providers"}), + optional=frozenset( + {"skill_presets", "grounding_providers", "invocation_presets"} + ), ) preset = { "preset_id": deepcopy(policy["preset_id"]), @@ -730,6 +732,30 @@ def _compose_integration_payload( preset["required_planner"] = deepcopy(policy["planner"]["type"]) elif "required_planner" in policy: preset["required_planner"] = deepcopy(policy["required_planner"]) + named = _owned_mapping( + profile.get("invocation_presets", {}), path="profile.invocation_presets" + ) + extra_presets = [] + for key, item in named.items(): + path = f"profile.invocation_presets.{key}" + name = _identifier(key, path=path) + if name == preset["preset_id"]: + raise ValueError("An invocation preset cannot replace the default preset.") + override = _mapping( + item, path=path, required=frozenset({"skill_options", "motion"}) + ) + options = _owned_mapping( + override["skill_options"], path=f"{path}.skill_options" + ) + motion = _owned_mapping(override["motion"], path=f"{path}.motion") + extra_presets.append( + { + **deepcopy(preset), + "preset_id": name, + "skill_options": {**preset["skill_options"], **options}, + "motion": {**preset["motion"], **motion}, + } + ) scene_payload = { key: deepcopy(value) for key, value in scene.items() if key != "contract_id" @@ -739,7 +765,7 @@ def _compose_integration_payload( "resources": deepcopy(skill_profile["resources"]), "command_presets": deepcopy(skill_profile["command_presets"]), "defaults": deepcopy(profile["defaults"]), - "presets": [preset], + "presets": [preset, *extra_presets], "default_preset": None, "skill_presets": deepcopy(profile.get("skill_presets", {})), "grounding_providers": deepcopy(profile.get("grounding_providers", {})), 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..ccf6074412 --- /dev/null +++ b/embodichain/lab/task_program/integrations/_native_plan_expansion.py @@ -0,0 +1,457 @@ +# ---------------------------------------------------------------------------- +# 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 + if not self.options["free_phases"].get(request.invocation.skill_id): + 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..8eef2dec5e 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", @@ -1114,6 +1115,9 @@ def _decode_skill_options(value: object, *, path: str) -> SkillOptions: "translation_distance", "preshape_fraction", "approach_along_grasp_axis", + "grasp_relative_approach", + "verified_object_to_eef", + "verified_terminal_object_to_eef", "cartesian_lift", "release_retreat_distance", "push_approach_offset", @@ -1160,6 +1164,8 @@ def _decode_skill_options(value: object, *, path: str) -> SkillOptions: *( ( "approach_along_grasp_axis", + "grasp_relative_approach", + "verified_object_to_eef", "initial_lift_height", "cartesian_lift", ) @@ -1171,6 +1177,28 @@ def _decode_skill_options(value: object, *, path: str) -> SkillOptions: ), ) kwargs: dict[str, object] = {} + if "verified_object_to_eef" in config: + import torch + + kwargs["verified_object_to_eef"] = torch.tensor( + _pose_matrix( + config["verified_object_to_eef"], + path=f"{path}.verified_object_to_eef", + ), + dtype=torch.float32, + ).reshape(4, 4) + if "grasp_relative_approach" in config: + import torch + + key = f"{path}.grasp_relative_approach" + kwargs["grasp_relative_approach"] = tuple( + torch.tensor( + _pose_matrix(item, path=f"{key}[{index}]"), dtype=torch.float32 + ).reshape(4, 4) + for index, item in enumerate( + _sequence(config["grasp_relative_approach"], path=key) + ) + ) if "cartesian_lift" in config: kwargs["cartesian_lift"] = _boolean( config["cartesian_lift"], path=f"{path}.cartesian_lift" @@ -1288,6 +1316,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 +1342,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"], @@ -1337,8 +1372,23 @@ def _decode_skill_options(value: object, *, path: str) -> SkillOptions: ) return PlaceOptions(**kwargs) if kind == "move_held_object": - _mapping(value, path=path, required=frozenset({"kind"})) - return MoveHeldObjectOptions() + config = _mapping( + value, + path=path, + required=frozenset({"kind"}), + optional=frozenset({"verified_terminal_object_to_eef"}), + ) + key = "verified_terminal_object_to_eef" + if key not in config: + return MoveHeldObjectOptions() + import torch + + return MoveHeldObjectOptions( + verified_terminal_object_to_eef=torch.tensor( + _pose_matrix(config[key], path=f"{path}.{key}"), + dtype=torch.float32, + ).reshape(4, 4) + ) if kind == "move_joints": _mapping( value, @@ -1792,6 +1842,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 +1855,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 +3555,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 +3567,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/demo_skill.py b/embodichain/lab/task_program/integrations/demo_skill.py index ea3598cbd7..e4111ecb92 100644 --- a/embodichain/lab/task_program/integrations/demo_skill.py +++ b/embodichain/lab/task_program/integrations/demo_skill.py @@ -153,6 +153,31 @@ def lower(self, call, context, *, bound, option_template): "DemoSkill roles must belong to the registered simulation" ) roles[name], uids[name] = ref.entity_id, entity.uid + initial_role = getattr(owner, "initial_held_role", None) + if initial_role is not None: + primary = bound.binding.resources.get("primary") + motion = ( + None if primary is None else primary.endpoints.get("motion") + ) + grasp = None if primary is None else primary.endpoints.get("grasp") + if ( + motion is None + or grasp is None + or motion.task_state_key != grasp.task_state_key + ): + raise ValueError( + "DemoSkill entry requires a shared motion and grasp state key" + ) + held = context.task.get_held_object(motion.task_state_key) + if ( + held is None + or held.semantics.entity_id != roles[initial_role] + or held.env_mask is None + or not bool(held.env_mask.all()) + ): + raise ValueError( + "DemoSkill entry requires verified held-object evidence" + ) semantics = None registered_effect = None if owner.terminal_held_role is not None or owner.release_on_exit: @@ -167,7 +192,7 @@ def lower(self, call, context, *, bound, option_template): role = owner.terminal_held_role if role is None: - role = owner.released_role + role = initial_role or owner.released_role if role is None: raise ValueError( "release_on_exit requires an explicit terminal object role" @@ -203,7 +228,9 @@ def lower(self, call, context, *, bound, option_template): ), object_id=roles[role], slot_id="primary", - allow_missing_detached_baseline=owner.release_on_exit, + allow_missing_detached_baseline=( + owner.release_on_exit and initial_role is None + ), ), ), ) diff --git a/embodichain/lab/task_program/integrations/environment.py b/embodichain/lab/task_program/integrations/environment.py index 5e592f3744..33d6ee1bd8 100644 --- a/embodichain/lab/task_program/integrations/environment.py +++ b/embodichain/lab/task_program/integrations/environment.py @@ -900,6 +900,8 @@ def create_traced_configured_sources( self, program: CompiledTaskProgram, collision_guard: TabletopCollisionGuardFactory | None = None, + *, + decorate_demo_source: Callable | None = None, ) -> TracingTaskProgramNominalSources: """Bind configured prototypes to a paired host collision guard. @@ -924,6 +926,7 @@ def create_traced_configured_sources( program, self._configured_nominal_sources, host_collision_guard=collision_guard, + decorate_demo_source=decorate_demo_source, ) return TracingTaskProgramNominalSources(program, delegate) diff --git a/embodichain/lab/task_program/integrations/expansion.py b/embodichain/lab/task_program/integrations/expansion.py index 543c680ff6..e0b4100f95 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,64 @@ 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.reject_nominal_source_overlap = True + 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 +367,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 +385,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 +938,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/nominal.py b/embodichain/lab/task_program/integrations/nominal.py index 152726a502..175882b034 100644 --- a/embodichain/lab/task_program/integrations/nominal.py +++ b/embodichain/lab/task_program/integrations/nominal.py @@ -154,6 +154,7 @@ def __init__( ], *, host_collision_guard: object | None = None, + decorate_demo_source=None, ) -> None: if host_collision_guard is not None: from .simulation.real2sim_collision import TabletopCollisionGuardFactory @@ -166,6 +167,9 @@ def __init__( self._fingerprint = _fingerprint(program) self._declarations = declarations self._host_collision_guard = host_collision_guard + if decorate_demo_source is not None and not callable(decorate_demo_source): + raise TypeError("decorate_demo_source must be callable.") + self._decorate_demo_source = decorate_demo_source self._sources = {item.call_index: item for item in declarations} if len(self._sources) != len(declarations): raise ValueError("configured nominal source call indices must be unique") @@ -247,7 +251,14 @@ def create_nominal_source( if isinstance(declaration, _ConfiguredDemoSkillSource): from .simulation.demo_skill import _create_demo_skill_source - return _create_demo_skill_source(declaration, request, engine) + source = _create_demo_skill_source(declaration, request, engine) + if self._decorate_demo_source is not None: + source = self._decorate_demo_source(source) + if not isinstance(source, NominalMotionSource): + raise TypeError( + "A DemoSkill decorator must return a nominal source." + ) + return source prototype = load_prototype(declaration.prototype_manifest) return PrototypeMotionProvider( prototype, 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/executor.py b/embodichain/lab/task_program/runtime/executor.py index c0e0686cda..c21a7d4d00 100644 --- a/embodichain/lab/task_program/runtime/executor.py +++ b/embodichain/lab/task_program/runtime/executor.py @@ -1203,6 +1203,16 @@ def _prepare_grounded_call( nominal_source = self._nominal_source_factory.create_nominal_source( plan_request, engine=self._engine ) + if ( + nominal_source is not None + and plan_transform is not None + and getattr( + self._plan_transform_factory, "reject_nominal_source_overlap", False + ) + ): + raise ValueError( + "A native expansion and a declared nominal source select the same call." + ) if effect_guards: external_plan_transform = plan_transform 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/toolkits/skill_fitting/__init__.py b/embodichain/toolkits/skill_fitting/__init__.py new file mode 100644 index 0000000000..6de86881d7 --- /dev/null +++ b/embodichain/toolkits/skill_fitting/__init__.py @@ -0,0 +1,21 @@ +# ---------------------------------------------------------------------------- +# Copyright (c) 2021-2026 DexForce Technology Co., Ltd. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# ---------------------------------------------------------------------------- + +"""Video-selected, object-free atomic motion reconstruction command.""" + +from __future__ import annotations + +__all__: list[str] = [] diff --git a/embodichain/toolkits/skill_fitting/__main__.py b/embodichain/toolkits/skill_fitting/__main__.py new file mode 100644 index 0000000000..ccb993c975 --- /dev/null +++ b/embodichain/toolkits/skill_fitting/__main__.py @@ -0,0 +1,22 @@ +# ---------------------------------------------------------------------------- +# 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 .cli import main + +if __name__ == "__main__": + main() diff --git a/embodichain/toolkits/skill_fitting/_bundle.py b/embodichain/toolkits/skill_fitting/_bundle.py new file mode 100644 index 0000000000..7f941fb2de --- /dev/null +++ b/embodichain/toolkits/skill_fitting/_bundle.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. +# ---------------------------------------------------------------------------- + +"""Minimal portable fitting inputs, with optional video for web annotation.""" + +from __future__ import annotations + +from copy import deepcopy +from dataclasses import asdict +import json +from pathlib import Path +import shutil +import tempfile +from typing import Any + +import numpy as np +import yaml + +from embodichain.data_pipeline.skill_fitting import file_digest, load_joint_recording +from ._pipeline import FitInputs +from ._robot import RobotProfile + +__all__: list[str] = [] + + +def _contained(root: Path, value: str) -> Path: + path = (root / value).resolve(strict=True) + if Path(value).is_absolute() or not path.is_relative_to(root): + raise ValueError("Bundle resources must use contained relative paths.") + return path + + +def pack_inputs( + directory: Path, + inputs: FitInputs, + document: dict[str, Any], + clock: np.ndarray, + durations: np.ndarray, + *, + video: Path | None = None, + video_stream: int | None = None, +) -> Path: + """Publish self-contained robot, scene, joint and annotation inputs.""" + from ._collision import prepare_scene_collision, resolve_scene_path + from ._export import _copy_urdf + from ._scene import load_scene + + directory = directory.resolve() + if directory.exists(): + raise FileExistsError(directory) + profile = RobotProfile.load(inputs.robot_profile) + models = {Path(s["urdf_path"]) for s in profile.solvers.values()} + if len(models) != 1: + raise ValueError("A portable fitting bundle requires one complete robot URDF.") + scene = resolve_scene_path(inputs.scene, inputs.joints) + if scene is None: + raise ValueError("A reconstructed scene is required for an input bundle.") + objects, scene_files = load_scene(scene, include_ungraspable=True) + collisions = prepare_scene_collision(scene) + scene_files.update(collisions["source_files_sha256"]) + source_yaml = scene / "scene_assets.yaml" if scene.is_dir() else scene + raw = yaml.safe_load(source_yaml.read_text()) + recording = load_joint_recording( + inputs.joints, + joint_names=profile.joint_names, + semantics=inputs.qpos_semantics, + qpos_key=inputs.qpos_key, + timestamps_key=inputs.timestamps_key, + sample_dt=inputs.joint_dt, + env_index=inputs.env_index, + ) + original_files = { + str(Path(inputs.robot_profile).resolve()): file_digest(inputs.robot_profile), + str(Path(inputs.joints).resolve()): file_digest(inputs.joints), + **scene_files, + } + if video_stream is not None and ( + video is None or type(video_stream) is not int or video_stream < 0 + ): + raise ValueError("video_stream requires a video and a non-negative integer.") + if video is not None: + video = video.expanduser().resolve(strict=True) + original_files[str(video)] = file_digest(video) + directory.parent.mkdir(parents=True, exist_ok=True) + with tempfile.TemporaryDirectory( + prefix=".fitting-inputs-", dir=directory.parent + ) as tmp: + root = Path(tmp) / "bundle" + root.mkdir() + _copy_urdf(models.pop(), root) + portable = deepcopy(profile.metadata()) + for solver in portable["solvers"].values(): + solver["urdf_path"] = "robot/robot.urdf" + (root / "robot_profile.json").write_text(json.dumps(portable, indent=2)) + (root / "scene").mkdir() + for index, obj in enumerate(objects): + declaration = raw["objects"][obj.identifier] + declaration["pose"] = obj.pose.tolist() + target = root / "scene" / (f"{index:03d}" + obj.mesh_path.suffix) + shutil.copyfile(obj.mesh_path, target) + declaration["shape"]["fpath"] = target.name + if obj.identifier in collisions["objects"]: + cached = collisions["objects"][obj.identifier] + if file_digest(target) != cached["sha256"]: + target = target.with_name(f"{index:03d}.convex.obj") + shutil.copyfile(cached["fpath"], target) + declaration["shape"]["collision"]["decomposition_fpath"] = target.name + # The contract deliberately excludes exporter reports and Blender metadata. + scene_data = {"objects": raw["objects"]} + if "skill_fitting" in raw: + scene_data["skill_fitting"] = raw["skill_fitting"] + (root / "scene" / "scene_assets.yaml").write_text( + yaml.safe_dump(scene_data, sort_keys=False) + ) + np.savez_compressed( + root / "joints.npz", + qpos=recording.qpos, + timestamps=recording.timestamps, + joint_names=np.asarray(profile.joint_names), + ) + np.savez_compressed( + root / "frame_clock.npz", timestamps=clock, durations=durations + ) + (root / "annotations.json").write_text( + json.dumps(document, ensure_ascii=False, indent=2) + ) + frame_source = None + if video is not None: + from embodichain.toolkits.trajectory_annotator._cfg import AnnotationCfg + from embodichain.toolkits.trajectory_annotator._storage import ( + AnnotationStore, + ) + from embodichain.toolkits.trajectory_annotator.video import VideoFrameSource + import threading + + # Validate content, selected stream and image timing against the + # annotation seed before publishing a portable web project. + source = VideoFrameSource(video, stream_index=video_stream) + store = AnnotationStore( + root / "annotations.json", AnnotationCfg().document_bytes + ) + try: + info = source.prepare(threading.Event(), lambda **_: None) + store.initialize(info) + source.check_unchanged() + finally: + source.close() + (root / "video").mkdir() + target = root / "video" / video.name + shutil.copyfile(video, target) + # Refresh descriptive provenance; no original source directory is + # needed to reopen the seed. The content identity is unchanged. + document = store.snapshot()["document"] + document["source"]["path"] = target.relative_to(root).as_posix() + (root / "annotations.json").write_text( + json.dumps(document, ensure_ascii=False, indent=2) + ) + frame_source = dict( + kind="video", + path=target.relative_to(root).as_posix(), + stream_index=video_stream, + ) + settings = asdict(inputs) + settings.update( + joints="joints.npz", + robot_profile="robot_profile.json", + scene="scene/scene_assets.yaml", + qpos_key="qpos", + timestamps_key="timestamps", + env_index=0, + joint_dt=None, + device="cpu", + ) + manifest = dict( + schema="scene-skill-fitting-inputs/v1", + inputs=settings, + annotations="annotations.json", + frame_clock="frame_clock.npz", + files={ + p.relative_to(root).as_posix(): file_digest(p) + for p in sorted(root.rglob("*")) + if p.is_file() + }, + ) + if frame_source is not None: + manifest["frame_source"] = frame_source + (root / "fitting.json").write_text( + json.dumps(manifest, indent=2, allow_nan=False) + ) + (root / "README.md").write_text( + "# Portable scene fitting inputs\n\n" + "All lengths are metres, angles radians; poses are world-from-object 4x4 matrices. " + "Mesh vertices are object-local. Frame bounds are inclusive; joint clocks are seconds.\n\n" + "```bash\nconda activate py312_bk\n" + "embodichain fit-trajectory --bundle . --output fit.npz --run-task-output run-task\n```\n\n" + "`fitting.json` pins input resources by SHA-256. `frame_clock.npz` supports offline fitting. " + "`joints.npz` is the original full-joint recording; `annotations.json` defines skill intervals. " + "`robot_profile.json` owns control parts, solver/TCP calibration and explicit simulation settings. " + "`scene/scene_assets.yaml` owns object meshes, poses, dynamics and fitting defaults; " + "convex-part OBJ files are prepared colliders, so run-task does not decompose again. " + "The installed EmbodiChain/py312_bk runtime is required. No original source directory, " + "Blender project, export report, USDA or prior fit is required.\n" + + ( + "\nThe original encoded video is included with its recorded frame timing. Start the web workflow with:\n\n" + "```bash\nembodichain annotate-trajectory --bundle . --port 8898 --no-browser\n```\n\n" + "The web editor saves `annotations.edited.json`, independently of the sealed annotation seed. " + "Fit the full sequence and download its run-task ZIP in the webpage. The runtime bundle includes " + "`expansion.yaml`; execute it with `--skill-sequence skills.yaml --expansion-profile expansion.yaml`. " + "No original recording directory is required.\n" + if frame_source is not None + else "Video pixels are unnecessary for this offline-only bundle.\n" + ) + ) + for path, digest in original_files.items(): + if file_digest(path) != digest: + raise ValueError("Source resources changed during packaging.") + load_inputs(root) + root.rename(directory) + return directory + + +def load_inputs( + directory: Path, +) -> tuple[FitInputs, dict[str, Any], np.ndarray, np.ndarray]: + """Validate file identity and resolve a moved folder without source lookups.""" + root = directory.resolve(strict=True) + manifest = json.loads((root / "fitting.json").read_text()) + required = {"schema", "inputs", "annotations", "frame_clock", "files"} + schema = manifest.get("schema") + if schema == "scene-skill-fitting-inputs/v2": + required.add("execution_context") + if ( + not required <= manifest.keys() + or manifest.keys() - (required | {"frame_source"}) + or schema + not in {"scene-skill-fitting-inputs/v1", "scene-skill-fitting-inputs/v2"} + ): + raise ValueError("Unsupported fitting input bundle.") + for name, digest in manifest["files"].items(): + if file_digest(_contained(root, name)) != digest: + raise ValueError(f"Bundle resource changed: {name}") + settings = deepcopy(manifest["inputs"]) + if settings.keys() - FitInputs.__dataclass_fields__.keys(): + raise ValueError("Unknown fitting bundle input field.") + for key in ("joints", "robot_profile", "scene"): + settings[key] = str(_contained(root, settings[key])) + inputs = FitInputs(**settings) + inputs.check() + if ( + schema == "scene-skill-fitting-inputs/v2" + and inputs.qpos_semantics != "design_reference" + ): + raise ValueError("Version 2 fitting requires an explicit design reference.") + # Robot/scene dependencies are also confined to the sealed file inventory. + profile = RobotProfile.load(inputs.robot_profile) + from ._scene import load_scene + + _, sources = load_scene(Path(inputs.scene), include_ungraspable=True) + dependencies = set(sources) | {s["urdf_path"] for s in profile.solvers.values()} + import xml.etree.ElementTree as ET + import yourdfpy + + for solver in profile.solvers.values(): + model_path = Path(solver["urdf_path"]) + tree = ET.parse(model_path) + for element in tree.findall(".//mesh") + tree.findall(".//texture"): + uri = element.get("filename") + if uri: + dependencies.add( + str( + Path( + yourdfpy.filename_handler_magic( + uri, dir=str(model_path.parent) + ) + ).resolve(strict=True) + ) + ) + scene_data = yaml.safe_load(Path(inputs.scene).read_text()) + for obj in scene_data["objects"].values(): + decomposition = ( + obj.get("shape", {}).get("collision", {}).get("decomposition_fpath") + ) + if decomposition: + dependencies.add( + str((Path(inputs.scene).parent / decomposition).resolve(strict=True)) + ) + dependencies.update( + { + inputs.joints, + inputs.robot_profile, + str(_contained(root, manifest["annotations"])), + str(_contained(root, manifest["frame_clock"])), + } + ) + visual = _video_source(root, manifest) + if visual is not None: + dependencies.add(str(visual[0])) + if schema == "scene-skill-fitting-inputs/v2": + dependencies.add(str(_contained(root, manifest["execution_context"]))) + inventory = {str(_contained(root, p)) for p in manifest["files"]} + if dependencies - inventory: + raise ValueError("Bundle has unlisted or external robot/scene dependencies.") + document = json.loads(_contained(root, manifest["annotations"]).read_text()) + with np.load(_contained(root, manifest["frame_clock"]), allow_pickle=False) as data: + clock, durations = data["timestamps"].copy(), data["durations"].copy() + if ( + clock.ndim != 1 + or len(clock) != document["source"]["frame_count"] + or durations.shape != clock.shape + or not np.isfinite(clock).all() + or not np.isfinite(durations).all() + or np.any(np.diff(clock) <= 0) + or np.any(durations <= 0) + ): + raise ValueError("Invalid frame clock in fitting bundle.") + return inputs, document, clock, durations + + +def _video_source( + root: Path, manifest: dict[str, Any] +) -> tuple[Path, int | None] | None: + """Decode the optional closed, contained web frame-source declaration.""" + if "frame_source" not in manifest: + return None + source = manifest["frame_source"] + if ( + not isinstance(source, dict) + or source.keys() != {"kind", "path", "stream_index"} + or source["kind"] != "video" + or not isinstance(source["path"], str) + or ( + source["stream_index"] is not None + and (type(source["stream_index"]) is not int or source["stream_index"] < 0) + ) + ): + raise ValueError("Bundle frame_source requires a video path and stream_index.") + return _contained(root, source["path"]), source["stream_index"] + + +def load_annotation_inputs( + directory: Path, annotations: Path | None = None +) -> tuple[Path, int | None, Path, dict[str, Any]]: + """Resolve a web project and isolate mutable edits from sealed input files.""" + root = directory.expanduser().resolve(strict=True) + inputs, document, _, _ = load_inputs(root) + manifest = json.loads((root / "fitting.json").read_text()) + visual = _video_source(root, manifest) + if visual is None: + raise ValueError( + "This offline-only bundle has no video. Repackage with --package-video