Add registered real2sim qpos sources to Task Program

[yuecideng]

Description

Real qpos recordings can supply a complete nominal Pick plan through Task Program without first running ordinary motion planning. Integrations select an immutable expert provider ID/revision and a content-pinned source bundle. Source-independent trajectory assembly is shared with ordinary PickUp and MoveJoints planning: Skills supply semantic effects and event/dependency boundaries, and the existing plan builder produces commands and tracking.

Refs #709 and #708. This is the first B=1, sequential, fixed-qpos integration slice; these issues remain open for physical qualification, broader source modes and collection support.

• Add dataset-independent real trajectory, frame/joint/unit normalization and anchored timing contracts, with explicit gripper decoding and raw-source JSON/NPZ bundles.

• Add ragged (B,N_max,D) anchored resampling with explicit source/anchor counts, reusable device maps, exact event copies, terminal padding and float32/float64 Torch or Warp CPU/CUDA evaluation. CPU float64 clock preparation shares the single-path algorithm; registered Task Program providers remain B=1.

• Register expert factories and exact expanded call occurrences in the existing simulation catalog/fingerprint. Track live provider identity with weak references so completed calls release their recordings while cached-instance reuse is rejected. Reject unknown versions, unsupported capabilities, unverified bundles, replay-incompatible recovery and ambiguous bindings before use.

• Bind replay to the target profile and robot asset digest; validate the live joint order, restored initial state, command period, joint position limits, velocity references and every command interval’s required speed.

• Add versioned SkillTrajectoryContract, owned SkillTrajectoryInput/typed geometry, and SkillPlanSemantics. The Engine calls a framework-owned assembler and explicit Skill semantic hook; PickUp and MoveJoints use it for both ordinary and external motion. Contracts are included in the catalog fingerprint (schema 6); unsupported Skills fail before planner fallback.

• Retain assembly inputs on plan snapshots and reassemble same-grid candidates with current provenance, commands, tracking, semantic effects and monitor bounds. Reject stale geometric-event facts, preserve the original recorded-path Engine method as a generic compatibility adapter, and leave non-joint/non-opted-in Skills on their existing path. Task Program execution, effect assurance and host lifecycle remain authoritative.

• Include deterministic source-neutral residual examples, a four-panel native renderer preview, and an audit/preparation CLI for one optional original DROID HDF5. CI fixtures are synthetic and have no dependency on the earlier real2sim experiment, its reconstructed assets or local outputs.

• Add physics_preview.py using the existing fixed-cadence playback host: one estimated foreground scene, B=1 reset-isolated trials, PD robot control and gravity/contact-driven objects. Success also requires continuous measured release and stable placement for the final second; both previews expose the shared visualization CLI options.

Dependencies: no package dependency changes.

Type of change

• New feature (non-breaking change which adds functionality)

Screenshots

The reproducible four-panel previews show the real recording, nominal qpos and two seeded variants. The physical version adds an estimated table/block/open-container scene, PD control, measured finger contacts, object lift and release/settling. Commands, controller preload and calibration scope are documented in examples/real2sim/README.md; generated videos, raw traces and assets remain external runtime outputs.

Validation

• Current shared-assembly regression: Atomic Skills, full Task Program runtime/Gym integrations, real2sim data, expansion, context and API-checker tests: 2,369 passed, 2 skipped, 8 real-simulation cases deselected. Focused cases verify ordinary/external command and tracking parity, typed owned geometry, B=2/custom bindings/reordered environment IDs, phase/event/revision checks, interval speeds, ordered waypoint visits, protected geometric events, and complete candidate rebuilding with current lineage.

• Earlier compute/data and compatibility/visualization validation passed 1,938 tests, including explicit CUDA cases; four real-simulation tests were deselected. Compute kernels and the physical preview are unchanged by the shared-assembly follow-up. Dedicated proxy-scene physics trials are recorded below.

• Batch tests cover variable lengths/anchor counts, row permutations, exact signed-zero anchor copies including directly constructed maps with right-closed anchor brackets, independent interpolation references, ignored NaN padding, bounded allocation, mapping reuse, noncontiguous input, explicit backend failures and non-default CUDA stream ordering.

• Provider regressions reject the 3 rad/s reversal whose central differences stay below 2 rad/s, accept intervals at/below the limit, verify completed recordings can be collected, and reject reused or non-weak-referenceable provider instances.

• Release-evidence tests reject closed-gripper/contact cases and interrupted/short release windows. The previous held-inside-container false positive is rejected by the physical summary, while a released and stable trace passes. Historical action-utils aliases remain covered independently of new compute exports; both visualization coverage and preview CLI help checks pass.

• One optional original recording was checked at three prefix lengths on Torch/Warp CPU/CUDA in float32/float64: maximum error against the previous single-path implementation was zero. Existing normalized bundle revision and raw source digest remain unchanged.

• The reproducible scripts/benchmark/trajectory/anchored_batch.py reports preparation, cached apply, offline end-to-end transfer, memory and numerical agreement separately. For B=128, N_max=192, D=13 float64 on RTX 5090, cached Warp CUDA evaluation was about 0.12 ms versus 0.17 ms for Torch CUDA; full offline conversion was about 14 ms for both. Torch remains the default backend.

• B=1 synthetic source -> registered selector -> complete PickUp plan -> Task Program bridge regression passes with projected semantic completion. Tests also retain measured-effect failure coverage and ensure source errors never invoke the ordinary planner.

• Original HDF5 audit preserves raw gripper samples and camera calibration metadata, extracts a conservative Pick slice, writes a content-pinned diagnostic bundle, and confirms the original input digest is unchanged.

• Native previews include the 397-frame kinematic comparison and a 501-frame physical comparison (including its result card) at 30 FPS. Motion-limit and measured-contact/settling checks pass within their declared scopes; source-calibration status remains unverified.

• Black 26.3.1, API coverage (2658/2658), context routing/checks and diff whitespace checks pass.

• Sphinx dummy build completes. Repository-wide documentation warnings remain; the added release helper has no autodoc member warning. The shared-assembly API page is documented without duplicate method/geometry indexes.

• Fresh physical validation: 9/9 measured proxy-scene trials pass (nominal and two seeded variants, three resets each) under the stricter release criteria. The block rises about 10 cm, bilateral hold lasts about 2.88 s or longer, and measured opening without bilateral grasp contact persists for about 3.16 s before the end. Oriented containment and final-second translational/rotational stability also pass. Source calibration remains unverified.

• The physical CLI ran all nine fresh trials with --no-render; output traces retain full/active joint names and IDs, controller targets, calibration and release thresholds. The original recording digest is unchanged.

• The local native runtime emits an entity-balance warning at process teardown; runs return zero and the saved state traces/videos pass verification. This follow-up does not change native simulator cleanup.

Remaining scope

Real-recording formal admission remains blocked by unresolved real flange/gripper/TCP mounting, hardware jaw-width calibration, independent camera reprojection/frame alignment and final target initial-state registration. The audit retains unverified states and the configured provider rejects that sample.

Next acceptance stages are independently calibrated foreground geometry and hardware mapping, formal provider admission/verified Task Program execution for the original recording, and receipt-backed source-array attachments. The physical preview is an estimated proxy-scene diagnostic and leaves source calibration unverified. Additional Skill assemblers (Place, MoveHeldObject, HandOver and contact skills), registered-source combined expansion, batched provider execution, ActionPlan time-grid changes, eef_pose/hybrid, parallel calls and multi-slot collection are outside this slice and remain follow-ups.

Checklist

• I have run the black . command to format the code base.
• I reviewed affected documentation and agent context, updated the source/admission/Skill ownership guidance, and retained unchanged simulation lifecycle guidance.
• Public API changes are reflected in the API docs (python docs/scripts/check_api_docs.py).
• I have added tests that prove the source admission, full-plan assembly, exact call binding and bridge behavior.
• Dependencies have been reviewed; no package dependency changes are required.

[greptile-apps]

Confidence Score: 4/5

[High impact] Adds real-to-sim trajectory sources and nominal plan providers to task execution.

Fix the ordinary MoveJoints goal-tolerance mismatch before merging.

Findings

1.  Successful neural joint plans fail ▶

Fix with agent prompt

### Issue 1
embodichain/lab/sim/atomic_actions/primitives/move_joints.py:181-188
Ordinary `MoveJoints` now runs this check, which requires the final joint positions to be within `0.0001` rad of the goal. But `NeuralPlanner` supports joint moves and accepts errors below its configured `joint_eps`, which defaults to `0.02` rad. It returns the reached position without snapping it to the goal.

A successful rollout ending `0.005` rad short therefore raises here instead of producing a plan. Preserve the planner's accepted tolerance for ordinary planning, or make the stricter requirement explicit rather than applying it to every backend.

---

For each issue above, determine whether it is valid and should be fixed. If so, fix it directly.

Summary

This PR adds registered, content-pinned real joint recordings as complete nominal plans for Task Program.

• Real joint recordings can be normalized into reviewable robot trajectories.
• Anchored resampling keeps event poses exact across single and ragged batches.
• PickUp and MoveJoints can build plans from timed joint paths.
• Task Programs can load a pinned qpos source for one Pick call.
• Recording tools can prepare and preview real2sim Pick examples.

Diagram

%%{init: {'theme': 'neutral'}}%%
flowchart LR
  A[Registered source bundle] --> B[Source admission]
  B --> C[SkillTrajectoryInput]
  D[Ordinary planning] --> C
  C --> E[Skill semantic assembly]
  E --> F[Commands and tracking]
  F --> G[Task Program execution]
  G --> H[Existing effect checks]

Loading

_{Reviews (3) · Last reviewed commit: "docs(atomic): avoid duplicate trajectory..." · Reviewed by Greptile}

[greptile-apps]

Successful neural joint plans fail

Ordinary MoveJoints now runs this check, which requires the final joint positions to be within 0.0001 rad of the goal. But NeuralPlanner supports joint moves and accepts errors below its configured joint_eps, which defaults to 0.02 rad. It returns the reached position without snapping it to the goal.

A successful rollout ending 0.005 rad short therefore raises here instead of producing a plan. Preserve the planner's accepted tolerance for ordinary planning, or make the stricter requirement explicit rather than applying it to every backend.

Prompt To Fix With AI

This is a comment left during a code review.
Path: embodichain/lab/sim/atomic_actions/primitives/move_joints.py
Line: 181-188

Comment:
**Successful neural joint plans fail**

Ordinary `MoveJoints` now runs this check, which requires the final joint positions to be within `0.0001` rad of the goal. But `NeuralPlanner` supports joint moves and accepts errors below its configured `joint_eps`, which defaults to `0.02` rad. It returns the reached position without snapping it to the goal.

A successful rollout ending `0.005` rad short therefore raises here instead of producing a plan. Preserve the planner's accepted tolerance for ordinary planning, or make the stricter requirement explicit rather than applying it to every backend.

---

For each issue above, determine whether it is valid and should be fixed. If so, fix it directly.