Use manipulability to diversify trajectory augmentation

[Yuan-Xinyi]

Description

Expert trajectory collection currently samples joint residuals uniformly around one reference path, so the accepted rollouts tend to cluster around the same few comfortable arm postures. This change lets manipulability guide both how candidates are proposed and which ones are kept, so a run covers a wider range of postures, including tighter ones that still complete the task.

The work stays inside embodichain/lab/sim/motion/expansion/ and reuses the existing Yoshikawa helpers in embodichain.compute.kinematics; no new numerical algorithm is introduced.

New module expansion/manipulability.py

• describe_manipulability() scores caller-supplied Jacobians and reports the per-sample values together with the lowest value inside each trajectory phase. Scoring runs in float64: the determinant of a float32 Jacobian underflows to exactly zero at small scales and would report a healthy posture as singular.
• ManipulabilityBands groups scores into ordered ranges expressed as ratios against a per-case reference value, usually the reference trajectory's lowest score. One set of boundaries therefore transfers across robots and tasks.
• manipulability_guided_residual() draws several joint_residual proposals from one local generator, scores each, and keeps the first one that lands in the requested range, falling back to the nearest reachable range. Proposals that leave the joint limits are skipped; if every proposal fails, the last error is raised rather than swallowed.

Selection side

CoverageIndex gains an optional per-range quota. Once a range is full, further rollouts in it are rejected even when their measured geometry is new, so one well-conditioned posture cannot consume the whole collection budget while tighter postures still have room. GenerationSession classifies each rollout from the manipulability observation measured during execution, never from a planned estimate, and reports per-range commit counts in snapshot().

Compatibility

Everything is opt-in through augmentation.factors.manipulability, which is disabled by default. Existing jobs decode, validate, and run exactly as before; register_case() accepts a reference value only when the factor is enabled, and CoverageIndex keeps its current behaviour when no quota is configured. Jacobians are supplied by the caller, so the package still imports no robot, solver, or simulation code. Manipulability only ranks postures: path, dynamic, and task validation are unchanged and remain the sole source of feasibility evidence.

Not included: a coverage or downstream-training comparison. Measuring the benefit needs real scene collection plus policy training, which is outside this algorithm package; it is a reasonable follow-up once a host integration adopts the option.

No new dependencies.

Fixes #634

Type of change

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

Validation

• Focused augmentation and manipulability tests: python -m pytest tests/sim/motion/expansion tests/compute/test_manipulability.py — 214 passed. 29 of them are new cases in tests/sim/motion/expansion/test_manipulability.py, plus added cases for the range quota in test_coverage.py, the new configuration fields in test_cfg.py, and the session's use of measured evidence in test_session.py.
• black . with Black 26.3.1: 1050 files unchanged.
• python docs/scripts/check_api_docs.py: 2085/2085 exports documented.
• context.py check: agent context map ok.
• The full test suite and any simulation-backed tests were not run; the change is confined to one package with no runtime or backend behaviour outside it.

Checklist

• I have run the black . command to format the code base.
• I have made corresponding changes to the documentation (expansion API reference, sim overview, motion-planning and simulation-system project context).
• Public API changes are reflected in the API docs (python docs/scripts/check_api_docs.py).
• I have added tests that prove the feature works.
• Dependencies have been reviewed; no updates are required.

🤖 Generated with Claude Code

[greptile-apps]

Confidence Score: 5/5

The PR appears safe to merge; the previously reported per-initial-state normalization issue is fixed and no new actionable failures were identified.

Summary

This PR adds opt-in manipulability-guided trajectory augmentation and measured per-band coverage quotas.

• Adds float64 manipulability profiling, reference-normalized bands, and guided residual proposal selection.
• Adds per-band reservation and commit accounting to coverage.
• Classifies executed episodes using measured manipulability normalized against each initial state's reference.
• Preserves shared scene-level coverage while allowing references to differ between initial states.
• Updates configuration, public exports, documentation, project context, and focused tests.

Diagram

%%{init: {'theme': 'neutral'}}%%
flowchart LR
  R[Reference trajectory per initial state] --> N[Per-state manipulability reference]
  P[Residual proposals] --> J[Caller-supplied Jacobians]
  J --> S[Float64 manipulability profile]
  N --> B[Reference-normalized band]
  S --> B
  B --> V[Independent path, dynamic, and task validation]
  V --> E[Executed episode]
  E --> M[Measured manipulability observation]
  M --> C[Shared scene-case CoverageIndex]
  C --> Q[Per-band quota and commit counts]

Loading

_{Reviews (2) · Last reviewed commit: "Keep manipulability references per initi..."}