Add per-point Yoshikawa manipulability to workspace analysis

[Yuan-Xinyi]

Stack

• Layer: 2/2
• Base: perf/workspace-batching
• Depends on: Improve workspace batching and analytic IK buffer reuse #606 (builds on its batched IK/result-assembly paths)
• Followed by: none

Description

This PR wires true per-point Yoshikawa manipulability into workspace analysis and documents a measured, configuration-only speedup of Cartesian/plane reachability analysis via the seed-selection sampler from #599.

1. Per-point manipulability (new)

The metrics/ subpackage was never invoked by the analyzer (_compute_metrics carried a TODO), and ManipulabilityMetric without Jacobians fell back to a centroid-distance placeholder. We measured that placeholder against ground truth on Franka (470 reachable points):

	placeholder	true Yoshikawa
mean	0.4196	0.0644
correlation with truth	−0.366	1.0

The placeholder is not merely inaccurate — it is anti-correlated with true manipulability, so any consumer ranking by it preferred worse configurations. This PR:

• computes w = sqrt(det(J J^T)) from the active solver's Jacobian over the stored joint_configurations after analysis (all three modes), row-aligned with the configurations and, in Cartesian/plane modes, with the reachable points;
• stores manipulability_scores in the results dict and results.npz, restores it on cache hits, and reports aggregates under metrics["manipulability"] (closing the _compute_metrics TODO via the metrics module);
• gates computation on MetricConfig.enabled_metrics (default ALL — on);
• removes the placeholder: ManipulabilityMetric without Jacobians or precomputed scores now warns and returns no statistics — fabricated numbers are worse than none;
• cost is negligible: batched Jacobian + determinant is ~10.5 ms for 470 configurations (GPU), well under 0.5% of the analysis.

Downstream, the aligned scores enable score-weighted runtime sampling (RobotWorkspace already supports weights) and manipulability-aware seed re-ranking as follow-ups; both are out of scope here.

2. Seed-selection speedup for Cartesian/plane analysis (documentation + measurement)

Cartesian/plane reachability runs through the solver's multi-start get_ik, where each analyzer seed occupies slot 0 of the solver-internal multi-start — so PytorchSolverCfg.enable_seed_selection (#599) applies with zero analyzer changes. Measured on Franka, 4000 identical sampled points (fixed RNG), warm timings (compile excluded), 1 seed/point:

solver config	reachable detected	wall time	speedup
num_samples=30, random (default)	470 (11.8%)	3.3 s	1×
enable_seed_selection, num_samples=30	475 (11.9%)	1.0 s	3.3×
enable_seed_selection, num_samples=8	466 (11.7%)	0.8 s	4.1×
enable_seed_selection, num_samples=4	460 (11.5%)	0.5 s	6.6×

At unchanged num_samples=30 the seeded configuration is strictly better: more reachable points detected and 3.3× faster (good seeds converge in fewer DLS iterations under early_stopping_any_converged). The recipe and numbers are recorded in the robot-workspace context docs; analytic solvers (OPW/SRS/UR) are unaffected.

Dependencies: none beyond #606.

Type of change

• Enhancement (non-breaking change which improves an existing functionality)

Screenshots

N/A

Checklist

• I have run the black . command to format the code base.
• I have made corresponding changes to the documentation (agent_context robot-workspace topic: manipulability contract, seed-selection recipe; MAP keywords)
• Public API changes are reflected in the API docs (python docs/scripts/check_api_docs.py: 1853/1853)
• I have added tests that prove my feature works (tests/sim/motion/workspace/test_manipulability.py: exact Yoshikawa on synthetic Jacobians, precomputed-score precedence, no-fabricated-statistics guard, end-to-end alignment on CobotMagic, metric gating, cache serialization round-trip)
• Dependencies have been updated, if applicable (none)

Validation

pytest tests/sim/motion/workspace/test_manipulability.py  -> 7 passed
pytest tests/sim/motion/workspace/                        -> full suite (regression)
python docs/scripts/check_api_docs.py                     -> 1853/1853
black . / context.py check / agent-context map tests      -> clean

Note: one behavioural change is intentional — ManipulabilityMetric.compute() without Jacobians/scores now returns {} with a warning instead of placeholder statistics. Given the measured anti-correlation, silent consumers of the old numbers were being misled; failing loudly is the safer contract.

[greptile-apps]

Confidence Score: 5/5

The PR appears safe to merge; the previous cache and isotropy issues are fully addressed and no new actionable failures remain.

Summary

• Computes manipulability and optional Jacobian condition numbers in chunks.
• Stores and restores per-configuration scores through results.npz.
• Repairs or strips cached metric fields according to the active metric configuration.
• Adds focused metric, analyzer, configuration-gating, cache-repair, and serialization tests.
• Documents the manipulability contract and seed-selection measurements.

Diagram

%%{init: {'theme': 'neutral'}}%%
flowchart LR
    A[Workspace analysis] --> B[Aligned joint configurations]
    B --> C[Chunked solver Jacobians]
    C --> D[Yoshikawa scores]
    C --> E[Optional condition numbers]
    D --> F[Current-config aggregates]
    E --> F
    D --> G[results.npz]
    F --> H[Result metrics]
    G --> I[Cache hit]
    I --> J{Metric enabled?}
    J -->|Yes, solver available| C
    J -->|Yes, no solver| K[Reuse scores and rebuild aggregates]
    J -->|No| L[Strip manipulability fields]

Loading

_{Reviews (4) · Last reviewed commit: "fix(workspace): recompute cached aggrega..."}