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.

Operator permission check

manipulability_guided_residual() stays in manipulability.py rather than moving next to the primitive operators, because it composes joint_residual() with scoring and the two modules would otherwise import each other. It still rejects a template that does not authorize joint_residual before entering the proposal loop, so a permission error is never caught by the retry handler and reported as a rejected sample. That check previously reached into operators._allowed_phases, the only cross-module use of a private name in the package; it is now the public operators.allowed_phases(), so an operator implemented outside that module has a supported way to validate template permissions. No behaviour changes.

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.

Figures

scripts/tutorials/sim/planner/manipulability_augmentation_plot.py overlays twelve guided variants on one UR5 reference, four steered at each band, and writes two views of that same set in one run so they cannot drift apart. The robot is an asset-backed kinematic chain and solver, so the figure needs no simulation manager, renderer, physics backend or display, and it is byte-identical across runs.

Three things are readable from it. Every variant meets the reference exactly at both phase endpoints, which is the contract joint_residual enforces and the reason contacts and grasps stay untouched. The interiors separate by target band rather than scattering uniformly: nine of twelve reached the band they asked for, and the variant bottlenecks span [0.0301, 0.0534] around a reference bottleneck of 0.0392. The three that missed are drawn dashed and keep the color of the band they actually reached, since reporting the nearest reachable band is the defined fallback rather than a failure.

The reference deliberately passes through an interior waypoint near the wrist singularity. A reference whose bottleneck sits on an endpoint cannot have that bottleneck moved by any residual, so every variant would report the reference score and the figure would show nothing.

The joint paths show that targeting moved something. The companion figure shows what it moved it to: manipulability measured along those same paths, with the band edges drawn as ratios of the reference bottleneck.

Each curve carries a marker at its own bottleneck, which is the single value its band membership is read from. All thirteen curves meet at both ends because the endpoints are shared, and the dip is the interior waypoint. The markers separate across the 0.8x and 1.2x edges rather than clustering at the reference, which is band targeting working. Scores are the profile each variant was selected with, not a fresh evaluation, so the curves are exactly the evidence the operator ranked.

Relationship to #651

This is the lower layer of a stack. It contributes the scoring, the reference-normalized bands and the guided sampler, but it deliberately adds no orchestration: in this package the host drives propose() and calls the operator itself, so manipulability_guided_residual() has no in-package caller here by design.

#651 introduces that orchestration (expand_trajectory_variants()), already takes per-sample Jacobians, and enumerates joint-path methods from a closed SPATIAL_METHODS registry. Wiring this operator into that registry belongs there, on top of this change, so each layer stays one logical change. The two touch different fields of factors and different members of CoverageIndex, so they compose without redesign.

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.

Merged with main

The agent-context streamlining change rewrote both context pages this branch had edited, so their old structure was gone. simulation-system.md is taken from main unchanged: its per-subsystem inventory table no longer exists, and augmentation now routes to the motion topic through main's own tables plus this branch's MAP.yaml keywords. The expansion/manipulability.py contract was re-applied onto main's "Trajectory augmentation boundary" section, keeping every constraint it stated. No source file conflicted.

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 — 215 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: 1118 files unchanged.
• python docs/scripts/check_api_docs.py: 2187/2187 exports documented.
• python scripts/tutorials/sim/planner/manipulability_augmentation_plot.py: both figures written, byte-identical on re-run.
• 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 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 with no outstanding correctness or repository-rule issues.

Summary

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

• Introduces float64 manipulability profiling, reference-normalized bands, and guided residual proposal selection.
• Adds per-band coverage quotas and classifies executed episodes using each initial state’s registered reference.
• Exposes operator permission checks publicly and adds configuration, API documentation, focused tests, and tutorial figures.
• The previous concern about requiring one reference across all initial states is fixed by storing bands per (scene_case_id, initial_state_id) while retaining scene-level shared quotas.

Diagram

%%{init: {'theme': 'neutral'}}%%
flowchart LR
    R[Reference trajectory] --> P[Residual proposals]
    P --> J[Caller-supplied Jacobians]
    J --> M[Manipulability profiles]
    M --> B[Reference-normalized bands]
    B --> S[Nearest target-band proposal]
    S --> V[Independent path, dynamic, and task validation]
    V --> E[Executed episode]
    E --> O[Measured manipulability observation]
    O --> Q[Shared scene-level band quota]
    Q --> C[Confirmed commit]

Loading

_{Reviews (6) · Last reviewed commit: "docs(expansion): plot the manipulability..."}