Visualize per-point manipulability with inspection ellipsoids

[Yuan-Xinyi]

Description

Adds the manipulability visualization requested as item 3 of the review on #614. That PR made WorkspaceAnalyzer produce a true per-point Yoshikawa score; this one makes the score readable.

Follow-up to #614 (now merged); this PR contains only the visualization.

The module keeps a pure mapping layer separate from a thin matplotlib renderer, so normalization and alignment are unit-testable without a display:

Function	Responsibility
align_manipulability_scores	Tie the compact score vector to the drawn points
normalize_manipulability	Percentile clipping, optional log10, reports raw and clipped ranges
map_manipulability_colors	Color reachable points, keep unreachable ones distinct
select_inspection_indices / inspect_points	Pick top/bottom/explicit points and gather their diagnostics
translational_manipulability_ellipsoid	Velocity ellipsoid from the translational Jacobian rows

How each review point is met

• Alignment. Joint-space scores map 1:1 onto workspace_points; Cartesian and plane scores map 1:1 onto reachable_points and are scattered onto all_points through reachability_mask. The scatter verifies all_points[mask] == reachable_points and raises rather than drawing a mis-colored figure if that ordering assumption is ever broken.
• Robust / log normalization with an honest color bar. Percentile clipping (default 2–98) optionally on log10. clip_range is converted back into raw w units, so the bar, the array, and the footer annotation always agree. The untouched raw_range and the saturated-point count are printed next to every figure.
• Unreachable points stay distinct — small translucent grey crosses, not dropped and not blended into the ramp.
• Ellipsoids only where asked. inspect_points() calls jacobian_fn once, with only the selected rows, and uses select_jacobian_rows(J, "translational"). A 6000-point workspace costs a handful of Jacobians, not 6000.
• Degenerate inputs are explicit: empty, fully-invalid, and constant score vectors are handled instead of dividing by a zero span.

Type of change

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

Screenshots

All three are produced by scripts/tutorials/sim/workspace_manipulability_visualization.py on a UR5 and are the figures embedded in the docs page.

1. Cartesian workspace colored by manipulability — 6000 sampled points: the 981 reachable ones carry the viridis ramp, the 5019 unreachable ones remain grey crosses that outline the sampling box. The footer reports raw w range [0, 0.1036] against the percentile-clipped bar span [0, 0.09719] and the 20 saturated points, so the clipping is visible rather than silent. Red circles mark the two inspected points.

2. Joint-space workspace on a log color scale — 3541 FK samples with decade ticks on the bar. This is the figure that justifies offering log normalization: the raw range starts at 0 while the clipped span starts at 5.07e-05, and on a log ramp the near-singular shell stays separable from the dexterous mid-range instead of collapsing into one dark color.

3. Translational manipulability ellipsoids at the best and worst points — each panel prints w, cond(J), the position, the semi-axes, the anisotropy and the full qpos. The best point (w = 0.104, cond = 10.5) is a near-round ellipsoid; the worst (w = 0, cond = 2.06e9) is a flat disc whose shortest semi-axis is 0.13 m/s against 0.87.

Worth noting from that last figure: the w = 0 configuration still has a three-dimensional translational ellipsoid. Its rank deficiency lives in the rotational rows, so reading w = 0 alone would suggest the end-effector cannot move at all, when in fact only the orientation directions are degenerate. That is a concrete argument for showing the ellipsoid next to the scalar, and it is called out in the docs page.

Checklist

• I have run the black . command to format the code base.
• I have made corresponding changes to the documentation
• Public API changes are reflected in the API docs
• I have added tests that prove my feature works
• Dependencies have been updated, if applicable (none)

Validation

pytest tests/sim/motion/workspace/ tests/compute/test_manipulability.py  -> 162 passed
python scripts/tutorials/sim/workspace_manipulability_visualization.py    -> 3 figures written
black --fast --check .                                                     -> 982 files unchanged
python docs/scripts/check_api_docs.py                                      -> 2103/2103 documented
context.py check                                                           -> agent context map: ok

69 of those tests are new and cover the mapping and normalization directly: percentile clipping, log scaling, empty / single-point / all-equal inputs, the unreachable mask, index alignment in both result modes, and that the ellipsoid is computed only for the selected subset.

Known limitations

• The color bar and the range annotation are matplotlib-only. The Viser and sim_manager backends forward RGB alone, so an in-scene legend would be a separate change.
• ManipulabilityVisualizer deliberately lets scores override a colors array passed by the generic analyzer.visualize() path, which hands reachability colors to every visualizer. Documented and covered by a test.

[greptile-apps]

Confidence Score: 5/5

The PR appears safe to merge, with two non-blocking inspection-helper issues still tracked in existing review threads.

Findings

1.  Top ties reverse index order ▶
2.  Inspection arrays lack validation ▶

Fix with agent prompt

### Issue 1
embodichain/lab/sim/motion/workspace/visualizers/manipulability_visualizer.py:undefined-732
When several points share the highest score, reversing the ascending array also reverses their index order. If `top_k` truncates that tied group, this selects the highest-index point instead of the lowest-index point promised by the public docstring, so a different point and Jacobian are inspected.

```suggestion
        for index in valid_indices[
            np.lexsort((valid_indices, -values[valid_indices]))
        ][:top_k]:
```

Note: If this suggestion doesn't match your team's coding style, reply to this and let me know. I'll remember it for next time!

### Issue 2
embodichain/lab/sim/motion/workspace/visualizers/manipulability_visualizer.py:822-838
This public helper accepts independently supplied point, score, and score-index arrays, but it checks selected indices only against `points`. A shorter `scores` or `score_indices` vector therefore causes an incidental `IndexError`, while arrays that are long enough but misaligned can pair a point with the wrong score or joint configuration. Validate all point-aligned lengths and require one label per selected index before indexing.

```suggestion
    point_array = _as_points(points)
    score_array = _as_float_vector(scores)
    if len(score_array) != len(point_array):
        raise ValueError(
            f"scores has {len(score_array)} entries but points has "
            f"{len(point_array)} rows."
        )
    if len(labels) != len(indices):
        raise ValueError(
            f"selection has {len(indices)} indices but {len(labels)} labels."
        )
    if indices.size and (indices.min() < 0 or indices.max() >= len(point_array)):
        raise ValueError(
            f"inspection indices out of range for {len(point_array)} points."
        )

    qpos = (
        None
        if joint_configurations is None
        else np.asarray(_to_numpy(joint_configurations), dtype=np.float64)
    )
    if score_indices is None:
        rows = indices
    else:
        row_map = np.asarray(_to_numpy(score_indices), dtype=np.int64).reshape(-1)
        if len(row_map) != len(point_array):
            raise ValueError(
                f"score_indices has {len(row_map)} entries but points has "
                f"{len(point_array)} rows."
            )
        rows = row_map[indices] if indices.size else indices
```

---

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

Summary

Adds manipulability-aware workspace visualization with score alignment, percentile/log normalization, distinct unreachable-point styling, and selected-point translational ellipsoids.

• Registers the new visualizer with WorkspaceAnalyzer and VisualizerFactory.
• Adds a headless tutorial, generated figures, public API documentation, and focused numerical tests.
• Changes since the previous review only regenerate the three documented PNG artifacts.

Diagram

%%{init: {'theme': 'neutral'}}%%
flowchart LR
    A[WorkspaceAnalyzer results] --> B[align_manipulability_scores]
    B --> C[Aligned points, scores, mask]
    C --> D[normalize_manipulability]
    D --> E[map_manipulability_colors]
    E --> F[Workspace visualization]
    C --> G[select_inspection_indices]
    G --> H[inspect_points]
    H --> I[Selected Jacobians]
    I --> J[Translational ellipsoids]
    J --> K[Inspection visualization]

Loading

_{Reviews (2) · Last reviewed commit: "docs(workspace): track the manipulabilit..."}

[greptile-apps]

Top ties reverse index order

When several points share the highest score, reversing the ascending array also reverses their index order. If top_k truncates that tied group, this selects the highest-index point instead of the lowest-index point promised by the public docstring, so a different point and Jacobian are inspected.

Suggested change

	for index in ascending[::-1][:top_k]:
	for index in valid_indices[
	np.lexsort((valid_indices, -values[valid_indices]))
	][:top_k]:

Prompt To Fix With AI

This is a comment left during a code review.
Path: embodichain/lab/sim/motion/workspace/visualizers/manipulability_visualizer.py
Line: 732

Comment:
**Top ties reverse index order**

When several points share the highest score, reversing the ascending array also reverses their index order. If `top_k` truncates that tied group, this selects the highest-index point instead of the lowest-index point promised by the public docstring, so a different point and Jacobian are inspected.

```suggestion
        for index in valid_indices[
            np.lexsort((valid_indices, -values[valid_indices]))
        ][:top_k]:
```

---

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

Note: If this suggestion doesn't match your team's coding style, reply to this and let me know. I'll remember it for next time!

[greptile-apps]

Inspection arrays lack validation

This public helper accepts independently supplied point, score, and score-index arrays, but it checks selected indices only against points. A shorter scores or score_indices vector therefore causes an incidental IndexError, while arrays that are long enough but misaligned can pair a point with the wrong score or joint configuration. Validate all point-aligned lengths and require one label per selected index before indexing.

Suggested change

	point_array = _as_points(points)
	score_array = _as_float_vector(scores)
	if indices.size and (indices.min() < 0 or indices.max() >= len(point_array)):
	raise ValueError(
	f"inspection indices out of range for {len(point_array)} points."
	)
	qpos = (
	None
	if joint_configurations is None
	else np.asarray(_to_numpy(joint_configurations), dtype=np.float64)
	)
	if score_indices is None:
	rows = indices
	else:
	row_map = np.asarray(_to_numpy(score_indices), dtype=np.int64).reshape(-1)
	rows = row_map[indices] if indices.size else indices
	point_array = _as_points(points)
	score_array = _as_float_vector(scores)
	if len(score_array) != len(point_array):
	raise ValueError(
	f"scores has {len(score_array)} entries but points has "
	f"{len(point_array)} rows."
	)
	if len(labels) != len(indices):
	raise ValueError(
	f"selection has {len(indices)} indices but {len(labels)} labels."
	)
	if indices.size and (indices.min() < 0 or indices.max() >= len(point_array)):
	raise ValueError(
	f"inspection indices out of range for {len(point_array)} points."
	)
	qpos = (
	None
	if joint_configurations is None
	else np.asarray(_to_numpy(joint_configurations), dtype=np.float64)
	)
	if score_indices is None:
	rows = indices
	else:
	row_map = np.asarray(_to_numpy(score_indices), dtype=np.int64).reshape(-1)
	if len(row_map) != len(point_array):
	raise ValueError(
	f"score_indices has {len(row_map)} entries but points has "
	f"{len(point_array)} rows."
	)
	rows = row_map[indices] if indices.size else indices

Prompt To Fix With AI

This is a comment left during a code review.
Path: embodichain/lab/sim/motion/workspace/visualizers/manipulability_visualizer.py
Line: 822-838

Comment:
**Inspection arrays lack validation**

This public helper accepts independently supplied point, score, and score-index arrays, but it checks selected indices only against `points`. A shorter `scores` or `score_indices` vector therefore causes an incidental `IndexError`, while arrays that are long enough but misaligned can pair a point with the wrong score or joint configuration. Validate all point-aligned lengths and require one label per selected index before indexing.

```suggestion
    point_array = _as_points(points)
    score_array = _as_float_vector(scores)
    if len(score_array) != len(point_array):
        raise ValueError(
            f"scores has {len(score_array)} entries but points has "
            f"{len(point_array)} rows."
        )
    if len(labels) != len(indices):
        raise ValueError(
            f"selection has {len(indices)} indices but {len(labels)} labels."
        )
    if indices.size and (indices.min() < 0 or indices.max() >= len(point_array)):
        raise ValueError(
            f"inspection indices out of range for {len(point_array)} points."
        )

    qpos = (
        None
        if joint_configurations is None
        else np.asarray(_to_numpy(joint_configurations), dtype=np.float64)
    )
    if score_indices is None:
        rows = indices
    else:
        row_map = np.asarray(_to_numpy(score_indices), dtype=np.int64).reshape(-1)
        if len(row_map) != len(point_array):
            raise ValueError(
                f"score_indices has {len(row_map)} entries but points has "
                f"{len(point_array)} rows."
            )
        rows = row_map[indices] if indices.size else indices
```

---

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