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 previously reported cache and isotropy contract defects are fixed, and no actionable new regression was established.

Summary

• Centralizes manipulability and condition-number calculations under embodichain.compute.kinematics.
• Computes, caches, repairs, gates, and aggregates per-point workspace manipulability.
• Adds configurable manipulability-based candidate selection to the PyTorch IK solver.
• Expands batched Robot/solver paths and analytic IK scratch-buffer reuse.
• Adds constrained-domain sampling, compact cache outputs, and Sobol defaults.
• Covers numerical helpers, cache behavior, result alignment, solver selection, and analytic batching.

Diagram

%%{init: {'theme': 'neutral'}}%%
flowchart LR
    A[Workspace samples] --> B[FK or IK analysis]
    B --> C[Aligned joint configurations]
    C --> D[Active solver Jacobians]
    D --> E[Yoshikawa scores]
    D --> F[Condition numbers]
    E --> G[Result arrays and cache]
    F --> H[Manipulability aggregates]
    G --> H
    I[Multi-start IK candidates] --> J[Candidate Jacobians]
    J --> K[Yoshikawa ranking]
    K --> L[Selected IK posture]

Loading

_{Reviews (5) · Last reviewed commit: "fix(workspace): restore per-matrix condi..."}

[yuecideng]

Changes requested

The core direction is useful, but I recommend addressing the following before merging.

1. [P2] Keep the public ManipulabilityMetric contract and its documentation consistent

manipulability_metric.py:90-100 now returns {} when neither Jacobians nor precomputed scores are supplied. However, the public metrics guide still documents ManipulabilityMetric().compute(workspace_points) as the basic usage and immediately indexes mean_manipulability (metrics.md:43-59). That documented example now raises KeyError.

Please either update the public documentation/examples to require Jacobians or precomputed scores, or provide an explicit opt-in heuristic mode with a clear deprecation path.

2. [P2] Report zero valid points correctly

The new analyzer path routes every computed score through _apply_manipulability. If a robot is at a singular posture, or the configured threshold is higher than every score, valid_scores becomes empty and is replaced with [0.0]. The result then reports num_valid_points == 1 although no point passed the threshold.

Please preserve a zero valid-point count and define an explicit empty-statistics behavior. Add a regression test with an all-zero or all-below-threshold score array.

3. Add a focused manipulability visualization

Please add a minimal workspace visualization path that colors reachable points/voxels by manipulability, keeps unreachable points visually distinct, uses robust/log normalization with a visible color bar, and exposes the raw score range. A selected point should be inspectable with its w, condition number, joint configuration, and (on demand) a translational manipulability ellipsoid. Avoid rendering an ellipsoid for every point; compute it for selected/top/bottom points only.

The visualization should preserve alignment for joint-space results and for reachable_points in Cartesian/plane results. Please add a focused example or test covering the mapping and normalization.

4. Add one application-oriented example

Please include a concise example showing a real decision driven by manipulability, rather than only printing aggregate statistics. The recommended first example is multi-seed IK re-ranking for one target pose: compare the first successful/nearest solution with the solution selected by manipulability, and show the resulting posture/ellipsoid and score.

For this to be meaningful, the candidate solutions must be scored before they are collapsed to the current “first successful seed” best_configs. A plane-sampling surface task (inspection, spraying, or polishing) would be a good second example if scope permits.

5. Extract the numerical calculation into a public compute module

The Jacobian-to-score calculation currently lives inside WorkspaceAnalyzer, which prevents IK solvers from reusing the same implementation to rank candidate solutions. Please move the pure, batched numerical helpers into a public module under the compute layer, e.g. embodichain.compute.kinematics.manipulability, and make both the analyzer and IK candidate-selection path call it.

The public API should cover at least:

• Yoshikawa score sqrt(det(J @ J.T));
• condition number / isotropy calculation;
• batched Torch tensors with explicit dtype/device behavior;
• optional translational or row-subset Jacobian selection for task-specific use.

Keep aggregation and workspace-specific result assembly in the workspace layer. Add focused tests for numerical correctness, singular/near-singular inputs, batched CPU execution, and deterministic candidate ranking. Export and document the new public API.

[Yuan-Xinyi]

Thanks for the detailed review — every point checked out. Five of the six items are addressed in the three new commits; the visualization (item 3) will follow as a separate PR.

1. Public metric contract (c94fb02) — the metrics guide examples now pass Jacobians (the old heuristic examples would raise KeyError), and the empty-return / zero-valid-point behaviour is documented. We did not add an opt-in heuristic mode: the removed centroid-distance placeholder measured negatively correlated with true manipulability (corr ≈ −0.37 on Franka), so there is no deprecation-worthy behaviour to preserve.

2. Zero valid points (c94fb02) — confirmed as a real bug: an empty valid set was replaced by [0.0] and reported num_valid_points == 1. It now reports num_valid_points == 0 with NaN statistics; regression tests cover all-below-threshold and all-zero score arrays. NaN round-trips safely through the results cache (Python JSON both ends).

3. Visualization — agreed on the full scope (manipulability-colored points with robust/log normalization and colorbar, distinct unreachable rendering, per-point inspection with w/condition/qpos, on-demand ellipsoids for selected/top/bottom only, joint-space and Cartesian alignment). It is the largest item, so it will come as a separate follow-up PR stacked on this one rather than growing this diff further.

4. Application example (8771437) — scripts/tutorials/sim/ik_manipulability_selection.py runs the recommended multi-seed IK re-ranking on the DexforceW1 left arm (50 targets × 30 seeds, identical RNG so all variants rank the same candidate pool). As suggested, candidates are scored before the collapse to best_configs, via a new opt-in PytorchSolverCfg.ik_solution_selection="manipulability" (default "nearest" unchanged):

variant	success	mean w	mean cond	time
nearest (default)	100%	0.0125	165.2	180.6 ms
manipulability re-rank	100%	0.0162 (+30.1%)	75.2	176.9 ms
iksel + nearest	100%	0.0127 (+1.9%)	164.8	132.4 ms
iksel + manipulability	100%	0.0163 (+30.8%)	78.9	122.5 ms

Re-ranking lifts mean manipulability by ~30% and halves the mean condition number at no measurable cost. The iksel comparison separates the two effects cleanly: seed selection improves which candidates exist (speed), re-ranking improves which candidate is kept (posture quality) — they compose.

5. Compute extraction (c94fb02 + 8771437) — embodichain.compute.kinematics.manipulability now provides yoshikawa_manipulability, condition_number, and select_jacobian_rows (translational/rotational/custom row subsets) as pure batched Torch helpers preserving dtype/device. The analyzer, the metric class, and the new IK candidate-selection path all call this single implementation. 14 focused numerical tests (closed-form checks, singular inputs yield 0/large-finite rather than NaN, batched CPU, deterministic ranking); exported and documented (API coverage gate 1859/1859).

8e5c276 restores the pre-existing per-matrix LinAlgError fallback in _compute_condition_numbers that the refactor had dropped — no unnecessary behaviour changes beyond what the review asked for. Existing workspace (14/14) and solver (14/14) suites pass unchanged.