From e2f06d2624d7e8a0325a0cc5445530f31272931c Mon Sep 17 00:00:00 2001 From: Adriano Dal Pastro Date: Mon, 11 May 2026 12:42:27 +0200 Subject: [PATCH] docs: implementation plan for SAM21 segmentation in grasp pipeline MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit 7 task TDD-first: pytest scaffold, depth_align (vectorised IR→RGB), Sam21Client (httpx mock unit tests), _deps + config wiring, refactor estraendo _points_via_bbox, branch SAM21 con fallback, verifica live. Allinea config key a base_url (consistente con altri servizi). Co-Authored-By: Claude Opus 4.7 (1M context) --- .../plans/2026-05-11-sam21-segmentation.md | 1095 +++++++++++++++++ .../2026-05-11-sam21-segmentation-design.md | 4 +- 2 files changed, 1097 insertions(+), 2 deletions(-) create mode 100644 docs/superpowers/plans/2026-05-11-sam21-segmentation.md diff --git a/docs/superpowers/plans/2026-05-11-sam21-segmentation.md b/docs/superpowers/plans/2026-05-11-sam21-segmentation.md new file mode 100644 index 0000000..9daa4fb --- /dev/null +++ b/docs/superpowers/plans/2026-05-11-sam21-segmentation.md @@ -0,0 +1,1095 @@ +# SAM21 segmentation in the grasp pipeline — Implementation Plan + +> **For agentic workers:** REQUIRED SUB-SKILL: Use superpowers:subagent-driven-development (recommended) or superpowers:executing-plans to implement this plan task-by-task. Steps use checkbox (`- [ ]`) syntax for tracking. + +**Goal:** Replace bbox-only point extraction with SAM21-segmented points in `/api/grasp/plan` and `/api/grasp/execute`, with automatic fallback to the legacy path when SAM21 is unreachable. + +**Architecture:** New `Sam21Client` + `align_depth_to_rgb()` helper. The branch lives inside `_object_points_in_base()` in `grasp_routes.py`: if `grasp.use_sam21=true`, build an RGB-aligned depth image client-side, POST `(rgb, depth, intrinsics, bbox)` to SAM21, get points in OpenCV RGB-camera frame, transform to base. Any exception in this branch logs and falls back to the bbox path. + +**Tech Stack:** Python 3.10 · FastAPI · `httpx` · `numpy` · `opencv-contrib-python-headless` (already deps) · `pytest` (added as dev dep). + +**Design doc:** `docs/superpowers/specs/2026-05-11-sam21-segmentation-design.md`. + +**Critical engineering facts** (verified by reading the SAM21 source): +- SAM21 expects RGB **PNG** base64 (not JPEG); cv2 round-trips JPEG→PNG cleanly because PNG stores channels in RGB order regardless of OpenCV's BGR memory layout. +- SAM21 expects depth as 16-bit single-channel PNG, values in mm, **same shape as RGB**. +- SAM21 returns `data.points` in **millimeters**, not meters — divide by 1000 before `transform_points(T_base_camera, ...)`. +- Nuwa intrinsics is a nested dict `{rgb: {fx,fy,cx,cy,width,height}, ir: {...}, extrinsics_ir_to_rgb: {...}}`. SAM21 wants only the `rgb` block. +- `CameraRegistration.from_nuwa()` (existing, in `perception/depth_to_rgb.py`) already parses both intrinsics blocks and the IR→RGB rigid transform — reuse it. + +--- + +## File Structure + +| File | Action | Responsibility | +|---|---|---| +| `pyproject.toml` | modify | Add `pytest` as optional `[dependency-groups] dev` dependency | +| `tests/__init__.py` | create | Empty package marker | +| `tests/conftest.py` | create | Pytest path setup so `presa_mano.*` imports work without install | +| `tests/test_depth_align.py` | create | Unit tests for `align_depth_to_rgb` | +| `tests/test_sam21_client.py` | create | Unit tests for `Sam21Client` (mocks `httpx.Client.post`) | +| `src/presa_mano/perception/depth_align.py` | create | `align_depth_to_rgb(depth_ir_mm, reg) -> depth_rgb_mm` | +| `src/presa_mano/clients/sam21.py` | create | `Sam21Client.segment_pointcloud(...) -> np.ndarray` (meters), `Sam21Unavailable`, `Sam21Error` | +| `src/presa_mano/clients/__init__.py` | modify | Re-export `Sam21Client`, `Sam21Unavailable`, `Sam21Error` | +| `src/presa_mano/api/_deps.py` | modify | Add `sam21()` factory | +| `config.yaml` | modify | Add `services.sam21.{base_url,timeout_s}` and `grasp.use_sam21` | +| `src/presa_mano/api/grasp_routes.py` | modify | Extract `_points_via_bbox`, add `_points_via_sam21`, insert SAM21 branch with fallback in `_object_points_in_base` | + +--- + +## Task 0: Dev infrastructure (pytest scaffold) + +**Files:** +- Modify: `pyproject.toml` +- Create: `tests/__init__.py` +- Create: `tests/conftest.py` + +- [ ] **Step 1: Add pytest as optional dev dependency** + +Edit `pyproject.toml`. Append after the `[build-system]` section (the file ends at line 22): + +```toml +[dependency-groups] +dev = [ + "pytest>=8.0.0", +] +``` + +- [ ] **Step 2: Create tests directory with package marker** + +Create `tests/__init__.py` with empty content (`""` is fine — write a single empty line so the file exists). + +- [ ] **Step 3: Create conftest.py for src-layout import** + +Create `tests/conftest.py`: + +```python +"""Make `presa_mano.*` importable in tests without installing the package.""" +from __future__ import annotations + +import sys +from pathlib import Path + +_SRC = Path(__file__).resolve().parent.parent / "src" +if str(_SRC) not in sys.path: + sys.path.insert(0, str(_SRC)) +``` + +- [ ] **Step 4: Install dev deps and verify pytest runs** + +Run: + +```bash +uv sync --group dev +uv run pytest --collect-only -q +``` + +Expected: pytest collects 0 tests, exits 0 (no errors). If you see `ModuleNotFoundError` or `error in conftest`, fix before moving on. + +- [ ] **Step 5: Commit** + +```bash +git add pyproject.toml tests/__init__.py tests/conftest.py uv.lock +git commit -m "test: add pytest dev dep + tests scaffold + +Co-Authored-By: Claude Opus 4.7 (1M context) " +``` + +--- + +## Task 1: `align_depth_to_rgb` (TDD) + +**Files:** +- Create: `tests/test_depth_align.py` +- Create: `src/presa_mano/perception/depth_align.py` + +The function vectorises the standard "rs.align(color)" operation: forward-project every IR pixel with a depth value into the RGB pixel grid, keeping the closest point per RGB pixel. + +- [ ] **Step 1: Write the failing test** + +Create `tests/test_depth_align.py`: + +```python +"""Unit tests for align_depth_to_rgb.""" +from __future__ import annotations + +import numpy as np + +from presa_mano.perception.depth_align import align_depth_to_rgb +from presa_mano.perception.depth_to_rgb import CameraRegistration + + +def _identity_reg(W: int = 64, H: int = 48) -> CameraRegistration: + """Same K for RGB and IR, no extrinsic offset.""" + K = np.array([[60.0, 0.0, W / 2.0], + [0.0, 60.0, H / 2.0], + [0.0, 0.0, 1.0]], dtype=np.float64) + return CameraRegistration( + K_rgb=K.copy(), K_ir=K.copy(), + R_ir_to_rgb=np.eye(3, dtype=np.float64), + t_ir_to_rgb=np.zeros(3, dtype=np.float64), + rgb_width=W, rgb_height=H, + ir_width=W, ir_height=H, + ) + + +def test_identity_alignment_preserves_depth(): + """K_ir == K_rgb and no extrinsic → every IR pixel maps to itself.""" + reg = _identity_reg() + depth_ir = np.full((reg.ir_height, reg.ir_width), 1000, dtype=np.uint16) + depth_rgb = align_depth_to_rgb(depth_ir, reg) + assert depth_rgb.shape == (reg.rgb_height, reg.rgb_width) + assert depth_rgb.dtype == np.uint16 + nonzero = depth_rgb[depth_rgb > 0] + # Coverage close to full (rounding can drop a few border pixels). + assert nonzero.size >= 0.95 * depth_rgb.size, f"coverage too low: {nonzero.size}/{depth_rgb.size}" + # All filled pixels equal the input depth (±1 mm rounding). + assert np.all(np.abs(nonzero.astype(int) - 1000) <= 1) + + +def test_zero_depth_pixels_stay_zero(): + """IR pixels with depth=0 must not produce any RGB output.""" + reg = _identity_reg() + depth_ir = np.zeros((reg.ir_height, reg.ir_width), dtype=np.uint16) + depth_rgb = align_depth_to_rgb(depth_ir, reg) + assert depth_rgb.shape == (reg.rgb_height, reg.rgb_width) + assert int(depth_rgb.sum()) == 0 + + +def test_sparse_pixels_preserved(): + """Two isolated IR depths produce exactly two filled RGB pixels (identity reg).""" + reg = _identity_reg(W=64, H=48) + depth_ir = np.zeros((reg.ir_height, reg.ir_width), dtype=np.uint16) + depth_ir[0, 0] = 1000 + depth_ir[reg.ir_height - 1, reg.ir_width - 1] = 1000 + depth_rgb = align_depth_to_rgb(depth_ir, reg) + assert (depth_rgb > 0).sum() == 2 + + +def test_z_buffer_keeps_closer_point(): + """When two IR pixels collide on the same RGB pixel, the smaller depth wins. + + Force a collision with K_ir = 2 × K_rgb (same resolution), so adjacent IR + pixels round to the same RGB pixel.""" + W = H = 16 + K_rgb = np.array([[60.0, 0.0, W / 2.0], + [0.0, 60.0, H / 2.0], + [0.0, 0.0, 1.0]], dtype=np.float64) + K_ir = K_rgb.copy() + K_ir[0, 0] *= 2.0 # double focal length → two IR pixels collapse to one RGB pixel + K_ir[1, 1] *= 2.0 + reg = CameraRegistration( + K_rgb=K_rgb, K_ir=K_ir, + R_ir_to_rgb=np.eye(3, dtype=np.float64), + t_ir_to_rgb=np.zeros(3, dtype=np.float64), + rgb_width=W, rgb_height=H, ir_width=W, ir_height=H, + ) + depth_ir = np.zeros((H, W), dtype=np.uint16) + # Two IR pixels (5, 5) and (6, 5) both project to the same RGB pixel near (4, 5) + # (when projected through K_ir with depth 1m and re-projected through K_rgb). + depth_ir[5, 5] = 1500 + depth_ir[5, 6] = 800 + depth_rgb = align_depth_to_rgb(depth_ir, reg) + nonzero = depth_rgb[depth_rgb > 0] + # At least one RGB pixel must receive the closer (800) value; nothing further + # may be ≥ 1500 if a collision actually occurred. If they didn't collide, we + # see exactly two values [800, 1500]; if they did, we see only [800]. + assert nonzero.size in (1, 2) + assert int(nonzero.min()) == 800 + if nonzero.size == 1: + assert int(nonzero[0]) == 800 # collision happened, closer point won +``` + +- [ ] **Step 2: Run tests to verify they fail** + +Run: + +```bash +uv run pytest tests/test_depth_align.py -v +``` + +Expected: all 4 tests FAIL with `ModuleNotFoundError: No module named 'presa_mano.perception.depth_align'`. + +- [ ] **Step 3: Implement `align_depth_to_rgb`** + +Create `src/presa_mano/perception/depth_align.py`: + +```python +"""Forward-project an IR depth image into RGB pixel coordinates. + +Vectorised equivalent of librealsense `rs.align(stream.color)`. Each IR pixel +with valid depth is back-projected to 3D in the IR camera frame, transformed to +the RGB camera frame via the rigid extrinsics, and projected with RGB +intrinsics. On collisions the closest point wins (z-buffer). + +Output is a uint16 array in millimeters, shape (rgb_height, rgb_width), with 0 +on pixels that received no IR projection. +""" +from __future__ import annotations + +import numpy as np + +from .depth_to_rgb import CameraRegistration + +_SENTINEL = np.iinfo(np.uint32).max + + +def align_depth_to_rgb( + depth_ir_mm: np.ndarray, + reg: CameraRegistration, +) -> np.ndarray: + """Forward-project IR depth → RGB pixel grid. + + Parameters + ---------- + depth_ir_mm : (H_ir, W_ir) uint16 + Depth in millimeters, IR camera frame. Zero means "no depth". + reg : CameraRegistration + IR and RGB intrinsics + IR→RGB rigid transform (translation in meters). + + Returns + ------- + (reg.rgb_height, reg.rgb_width) uint16 + Depth in millimeters in RGB pixel coordinates; 0 where no IR pixel + projected. + """ + if depth_ir_mm.ndim != 2: + raise ValueError(f"depth_ir_mm must be 2D, got shape {depth_ir_mm.shape}") + if depth_ir_mm.shape != (reg.ir_height, reg.ir_width): + raise ValueError( + f"depth_ir_mm shape {depth_ir_mm.shape} ≠ IR resolution " + f"({reg.ir_height}, {reg.ir_width}) from registration" + ) + + H_rgb, W_rgb = reg.rgb_height, reg.rgb_width + H_ir, W_ir = reg.ir_height, reg.ir_width + + valid = depth_ir_mm > 0 + if not valid.any(): + return np.zeros((H_rgb, W_rgb), dtype=np.uint16) + + v_idx, u_idx = np.where(valid) + z_ir_m = depth_ir_mm[v_idx, u_idx].astype(np.float64) / 1000.0 + + # Back-project IR pixel → 3D point in IR camera frame. + K_ir_inv = np.linalg.inv(reg.K_ir) + pixels_h = np.vstack([u_idx.astype(np.float64), + v_idx.astype(np.float64), + np.ones_like(u_idx, dtype=np.float64)]) + rays_ir = K_ir_inv @ pixels_h + P_ir = rays_ir * z_ir_m + + # IR camera frame → RGB camera frame. + P_rgb = reg.R_ir_to_rgb @ P_ir + reg.t_ir_to_rgb.reshape(3, 1) + + z_rgb = P_rgb[2] + in_front = z_rgb > 1e-6 + P_rgb = P_rgb[:, in_front] + if P_rgb.shape[1] == 0: + return np.zeros((H_rgb, W_rgb), dtype=np.uint16) + + z_rgb = P_rgb[2] + u_rgb = reg.K_rgb[0, 0] * P_rgb[0] / z_rgb + reg.K_rgb[0, 2] + v_rgb = reg.K_rgb[1, 1] * P_rgb[1] / z_rgb + reg.K_rgb[1, 2] + + u_int = np.round(u_rgb).astype(np.int64) + v_int = np.round(v_rgb).astype(np.int64) + z_mm = np.round(z_rgb * 1000.0).astype(np.int64) + + in_bounds = ( + (u_int >= 0) & (u_int < W_rgb) + & (v_int >= 0) & (v_int < H_rgb) + & (z_mm > 0) & (z_mm < 65535) + ) + u_int = u_int[in_bounds] + v_int = v_int[in_bounds] + z_mm = z_mm[in_bounds].astype(np.uint32) + if z_mm.size == 0: + return np.zeros((H_rgb, W_rgb), dtype=np.uint16) + + # Z-buffer: initialise to sentinel, reduce-min, then sentinel→0. + zbuf = np.full(H_rgb * W_rgb, _SENTINEL, dtype=np.uint32) + flat_idx = v_int * W_rgb + u_int + np.minimum.at(zbuf, flat_idx, z_mm) + zbuf[zbuf == _SENTINEL] = 0 + return zbuf.reshape(H_rgb, W_rgb).astype(np.uint16) +``` + +- [ ] **Step 4: Run tests to verify they pass** + +Run: + +```bash +uv run pytest tests/test_depth_align.py -v +``` + +Expected: 4 PASS. + +- [ ] **Step 5: Commit** + +```bash +git add tests/test_depth_align.py src/presa_mano/perception/depth_align.py +git commit -m "feat(perception): vectorised IR→RGB depth alignment + +Forward-project IR depth pixels into the RGB pixel grid with a z-buffer. +Output uint16 mm at RGB resolution, ready to feed SAM21. + +Co-Authored-By: Claude Opus 4.7 (1M context) " +``` + +--- + +## Task 2: `Sam21Client` (TDD) + +**Files:** +- Create: `tests/test_sam21_client.py` +- Create: `src/presa_mano/clients/sam21.py` + +The client posts to `/api/segment_pointcloud`, unwraps the `{success, data, error}` envelope, normalises errors into two exception types, and returns an `(N, 3)` array in **meters**. + +- [ ] **Step 1: Write the failing test** + +Create `tests/test_sam21_client.py`: + +```python +"""Unit tests for Sam21Client.""" +from __future__ import annotations + +import json +from unittest.mock import MagicMock, patch + +import httpx +import numpy as np +import pytest + +from presa_mano.clients.sam21 import Sam21Client, Sam21Error, Sam21Unavailable + + +def _intrinsics(): + return {"fx": 600.0, "fy": 600.0, "cx": 320.0, "cy": 240.0, + "width": 640, "height": 480} + + +def _ok_response(points_mm): + body = { + "success": True, + "data": { + "n_points": len(points_mm), + "n_pixels_in_mask": len(points_mm), + "n_pixels_with_valid_depth": len(points_mm), + "elapsed_ms": {"segment": 100.0, "project": 5.0, "total": 105.0}, + "points": [list(p) + [0.0, 0.0, 0.0] for p in points_mm], + }, + "error": None, + } + return httpx.Response(200, json=body) + + +def _err_response(status: int, code: str, message: str): + body = {"success": False, "data": None, + "error": {"code": code, "message": message}} + return httpx.Response(status, json=body) + + +def test_segment_pointcloud_happy_path(): + points_mm = [(100.0, 200.0, 1000.0), (110.0, 200.0, 1010.0)] + response = _ok_response(points_mm) + client = Sam21Client("http://sam21:8002", timeout_s=5.0) + with patch("httpx.Client.post", return_value=response) as mock_post: + out = client.segment_pointcloud( + rgb_png=b"FAKE_PNG", depth_png=b"FAKE_DEPTH", + intrinsics=_intrinsics(), bbox=(10, 20, 100, 200), + ) + assert mock_post.call_count == 1 + posted_url = mock_post.call_args.args[0] + assert posted_url.endswith("/api/segment_pointcloud") + body = mock_post.call_args.kwargs["json"] + assert body["bbox"] == [10, 20, 100, 200] + assert body["depth_scale_mm"] == 1.0 + assert body["return_format"] == "json" + assert body["return_mask"] is False + assert isinstance(body["rgb_png_base64"], str) and len(body["rgb_png_base64"]) > 0 + assert isinstance(body["depth_png_base64"], str) and len(body["depth_png_base64"]) > 0 + # Output is in meters: mm/1000. + assert out.shape == (2, 3) + assert out.dtype == np.float64 + assert np.allclose(out, np.array([[0.1, 0.2, 1.0], [0.11, 0.2, 1.01]])) + + +def test_segment_pointcloud_connect_error_raises_unavailable(): + client = Sam21Client("http://sam21:8002") + with patch("httpx.Client.post", side_effect=httpx.ConnectError("refused")): + with pytest.raises(Sam21Unavailable): + client.segment_pointcloud( + rgb_png=b"x", depth_png=b"y", + intrinsics=_intrinsics(), bbox=(0, 0, 10, 10), + ) + + +def test_segment_pointcloud_timeout_raises_unavailable(): + client = Sam21Client("http://sam21:8002") + with patch("httpx.Client.post", side_effect=httpx.TimeoutException("timeout")): + with pytest.raises(Sam21Unavailable): + client.segment_pointcloud( + rgb_png=b"x", depth_png=b"y", + intrinsics=_intrinsics(), bbox=(0, 0, 10, 10), + ) + + +def test_segment_pointcloud_model_not_ready_raises_unavailable(): + """503 MODEL_NOT_READY is transient → treated as unavailable (fallback).""" + response = _err_response(503, "MODEL_NOT_READY", "loading") + client = Sam21Client("http://sam21:8002") + with patch("httpx.Client.post", return_value=response): + with pytest.raises(Sam21Unavailable): + client.segment_pointcloud( + rgb_png=b"x", depth_png=b"y", + intrinsics=_intrinsics(), bbox=(0, 0, 10, 10), + ) + + +def test_segment_pointcloud_segmentation_empty_raises_error(): + """422 SEGMENTATION_EMPTY is a semantic outcome, not transport.""" + response = _err_response(422, "SEGMENTATION_EMPTY", "no mask") + client = Sam21Client("http://sam21:8002") + with patch("httpx.Client.post", return_value=response): + with pytest.raises(Sam21Error) as info: + client.segment_pointcloud( + rgb_png=b"x", depth_png=b"y", + intrinsics=_intrinsics(), bbox=(0, 0, 10, 10), + ) + assert "SEGMENTATION_EMPTY" in str(info.value) + + +def test_segment_pointcloud_no_valid_depth_raises_error(): + response = _err_response(422, "NO_VALID_DEPTH", "mask found but no depth") + client = Sam21Client("http://sam21:8002") + with patch("httpx.Client.post", return_value=response): + with pytest.raises(Sam21Error) as info: + client.segment_pointcloud( + rgb_png=b"x", depth_png=b"y", + intrinsics=_intrinsics(), bbox=(0, 0, 10, 10), + ) + assert "NO_VALID_DEPTH" in str(info.value) + + +def test_segment_pointcloud_empty_points_returns_empty_array(): + """Service returned success but 0 points — return (0, 3) array.""" + response = _ok_response([]) + client = Sam21Client("http://sam21:8002") + with patch("httpx.Client.post", return_value=response): + out = client.segment_pointcloud( + rgb_png=b"x", depth_png=b"y", + intrinsics=_intrinsics(), bbox=(0, 0, 10, 10), + ) + assert out.shape == (0, 3) +``` + +- [ ] **Step 2: Run tests to verify they fail** + +Run: + +```bash +uv run pytest tests/test_sam21_client.py -v +``` + +Expected: all tests FAIL with `ModuleNotFoundError: No module named 'presa_mano.clients.sam21'`. + +- [ ] **Step 3: Implement `Sam21Client`** + +Create `src/presa_mano/clients/sam21.py`: + +```python +"""Client for the SAM21 segmentation service (port 8002).""" +from __future__ import annotations + +import base64 +from typing import Any + +import httpx +import numpy as np + + +class Sam21Unavailable(RuntimeError): + """Transport-level failure (connection refused, timeout, model loading).""" + + +class Sam21Error(RuntimeError): + """Semantic failure surfaced by the service (e.g. empty segmentation).""" + + def __init__(self, code: str, message: str): + super().__init__(f"{code}: {message}") + self.code = code + self.message = message + + +_TRANSIENT_CODES = {"MODEL_NOT_READY"} + + +class Sam21Client: + """Thin wrapper around POST /api/segment_pointcloud. + + Returns points in **meters**, OpenCV RGB-camera frame (X-right, Y-down, + Z-forward) — directly compatible with `T_base_camera`. + """ + + def __init__(self, base_url: str, timeout_s: float = 10.0): + self.base_url = base_url.rstrip("/") + self.timeout_s = timeout_s + + def segment_pointcloud( + self, + rgb_png: bytes, + depth_png: bytes, + intrinsics: dict[str, Any], + bbox: tuple[int, int, int, int], + depth_scale_mm: float = 1.0, + ) -> np.ndarray: + body = { + "rgb_png_base64": base64.b64encode(rgb_png).decode("ascii"), + "depth_png_base64": base64.b64encode(depth_png).decode("ascii"), + "intrinsics": { + "fx": float(intrinsics["fx"]), + "fy": float(intrinsics["fy"]), + "cx": float(intrinsics["cx"]), + "cy": float(intrinsics["cy"]), + "width": int(intrinsics["width"]), + "height": int(intrinsics["height"]), + }, + "bbox": [int(bbox[0]), int(bbox[1]), int(bbox[2]), int(bbox[3])], + "depth_scale_mm": float(depth_scale_mm), + "return_format": "json", + "return_mask": False, + } + url = f"{self.base_url}/api/segment_pointcloud" + try: + with httpx.Client(timeout=self.timeout_s) as c: + r = c.post(url, json=body) + except (httpx.ConnectError, httpx.TimeoutException, httpx.NetworkError) as e: + raise Sam21Unavailable(f"sam21 unreachable: {e}") from e + + # Try to parse the envelope even on non-2xx, because SAM21 always + # returns its `{success, data, error}` shape. + try: + payload = r.json() + except ValueError as e: + raise Sam21Unavailable(f"sam21 returned non-JSON (status {r.status_code}): {e}") from e + + if not isinstance(payload, dict): + raise Sam21Unavailable(f"sam21 returned unexpected payload type: {type(payload)}") + + if payload.get("success") is True: + data = payload.get("data") or {} + points = data.get("points") or [] + if not points: + return np.empty((0, 3), dtype=np.float64) + arr_mm = np.asarray(points, dtype=np.float64) + if arr_mm.ndim != 2 or arr_mm.shape[1] < 3: + raise Sam21Unavailable(f"unexpected points shape: {arr_mm.shape}") + return arr_mm[:, :3] / 1000.0 + + err = payload.get("error") or {} + code = str(err.get("code") or "UNKNOWN") + msg = str(err.get("message") or f"http {r.status_code}") + if code in _TRANSIENT_CODES: + raise Sam21Unavailable(f"{code}: {msg}") + raise Sam21Error(code, msg) +``` + +- [ ] **Step 4: Run tests to verify they pass** + +Run: + +```bash +uv run pytest tests/test_sam21_client.py -v +``` + +Expected: 7 PASS. + +- [ ] **Step 5: Commit** + +```bash +git add tests/test_sam21_client.py src/presa_mano/clients/sam21.py +git commit -m "feat(clients): Sam21Client wrapper for /api/segment_pointcloud + +Posts (rgb_png, depth_png, intrinsics, bbox) and returns (N,3) points in +meters in OpenCV RGB-camera frame. Normalises transport vs semantic errors +into Sam21Unavailable / Sam21Error. + +Co-Authored-By: Claude Opus 4.7 (1M context) " +``` + +--- + +## Task 3: Wire `Sam21Client` into the orchestrator + +**Files:** +- Modify: `src/presa_mano/clients/__init__.py` +- Modify: `src/presa_mano/api/_deps.py` +- Modify: `config.yaml` + +- [ ] **Step 1: Re-export Sam21 symbols** + +Edit `src/presa_mano/clients/__init__.py` — change the imports and `__all__` to: + +```python +"""HTTP clients for upstream microservices.""" + +from .g1 import G1Client, G1StatusError, G1Unavailable +from .marker import MarkerClient +from .nuwa import NuwaClient, NuwaError +from .sam21 import Sam21Client, Sam21Error, Sam21Unavailable +from .yolo import YoloClient + +__all__ = [ + "G1Client", "G1StatusError", "G1Unavailable", + "MarkerClient", "NuwaClient", "NuwaError", + "Sam21Client", "Sam21Error", "Sam21Unavailable", + "YoloClient", +] +``` + +- [ ] **Step 2: Add `sam21()` factory to `_deps.py`** + +Edit `src/presa_mano/api/_deps.py`. Update the imports at line 9: + +```python +from ..clients import G1Client, MarkerClient, NuwaClient, Sam21Client, YoloClient +``` + +Append a new factory after the existing `yolo()` function (around line 30, before `active_robot_key`): + +```python +def sam21() -> Sam21Client: + c = _svc(get_config(), "sam21") + return Sam21Client( + base_url=c.get("base_url", "http://localhost:8002"), + timeout_s=float(c.get("timeout_s", 10.0)), + ) +``` + +- [ ] **Step 3: Add config entries** + +Edit `config.yaml`. Inside the existing `services:` block (after the `h1:` entry, before `aruco:`), insert: + +```yaml + sam21: + base_url: http://localhost:8002 + timeout_s: 10.0 +``` + +Inside the existing `grasp:` block, add a new key after `pointcloud_max_depth_mm`: + +```yaml + use_sam21: true +``` + +- [ ] **Step 4: Smoke-test the factory** + +Run: + +```bash +uv run python -c "from presa_mano.api._deps import sam21; c = sam21(); print(c.base_url, c.timeout_s)" +``` + +Expected output: `http://localhost:8002 10.0`. If you get a `KeyError` or wrong values, fix before moving on. + +- [ ] **Step 5: Commit** + +```bash +git add src/presa_mano/clients/__init__.py src/presa_mano/api/_deps.py config.yaml +git commit -m "feat: wire Sam21Client into _deps and config + +Adds services.sam21 (base_url + timeout_s) and grasp.use_sam21 toggle. + +Co-Authored-By: Claude Opus 4.7 (1M context) " +``` + +--- + +## Task 4: Refactor `_object_points_in_base` (extract `_points_via_bbox`) + +Pure refactor — no behaviour change. Splits the existing body into a helper so Task 5 can add the SAM21 branch cleanly. + +**Files:** +- Modify: `src/presa_mano/api/grasp_routes.py` + +- [ ] **Step 1: Read the current function** + +Open `src/presa_mano/api/grasp_routes.py`. The current `_object_points_in_base` is at lines 43–88. Keep its public signature exactly the same. + +- [ ] **Step 2: Replace the function with the refactored split** + +Replace lines 43–88 (the whole `_object_points_in_base` function) with: + +```python +def _object_points_in_base(conf: float | None, class_name: str | None, sw: Stopwatch + ) -> tuple[np.ndarray, dict[str, Any]]: + cfg = get_config() + nuwa = _deps.nuwa() + yolo = _deps.yolo() + + side = cfg.get("robot", {}).get("wrist_side", "right") + T_base_cam = CalibrationStore(_deps.calibration_file_path(side)).T_base_camera() + if T_base_cam is None: + raise HTTPException(status_code=412, detail=f"calibration missing for side='{side}': run /api/calibration/solve first") + + detector = ObjectDetector(yolo, nuwa) + try: + with sw.step("yolo"): + dets = detector.detect(conf=conf) + except HTTPException: + raise + except Exception as e: + raise HTTPException(status_code=502, detail=f"YOLO detect failed: {e}") + if class_name: + dets = [d for d in dets if (d.get("class") or "").lower() == class_name.lower()] + if not dets: + raise HTTPException(status_code=404, detail="no object detected") + dets.sort(key=lambda d: d["confidence"], reverse=True) + top = dets[0] + bbox = _bbox_to_roi(top["bbox"]) + + pts_base = _points_via_bbox(nuwa, T_base_cam, bbox, sw) + meta = { + "detection": top, + "T_base_camera": T_base_cam.tolist(), + "n_points": int(len(pts_base)), + "source": "bbox_roi", + } + return pts_base, meta + + +def _points_via_bbox(nuwa, T_base_cam: np.ndarray, bbox: tuple[int, int, int, int], + sw: Stopwatch) -> np.ndarray: + """Legacy path: bbox → Nuwa ROI cloud → filter outliers → base frame. + + Raises HTTPException(422) on <10 valid depth points (existing behaviour). + """ + cfg = get_config() + u_min, v_min, u_max, v_max = bbox + try: + with sw.step("pointcloud_roi"): + pc, _meta = nuwa.pointcloud_roi( + u_min, v_min, u_max, v_max, + step=int(cfg.get("grasp", {}).get("pointcloud_step", 1)), + max_depth_mm=int(cfg.get("grasp", {}).get("pointcloud_max_depth_mm", 2000)), + ) + except Exception as e: + raise HTTPException(status_code=502, detail=f"Nuwa ROI pointcloud failed: {e}") + with sw.step("filter_outliers"): + cropped = filter_outliers(pc) + if len(cropped) < 10: + raise HTTPException(status_code=422, detail=f"too few valid depth points in bbox: {len(cropped)}") + with sw.step("to_base_frame"): + pts_cv = nuwa_to_cv_frame(cropped[:, :3]) + pts_base = transform_points(T_base_cam, pts_cv) + return pts_base +``` + +Note: `_bbox_to_roi` (imported from `perception_routes` at the top of the file) returns `(u_min, v_min, u_max, v_max)`; the refactor stores that tuple in `bbox` and passes it to `_points_via_bbox`. The semantics are unchanged. + +- [ ] **Step 3: Smoke-test that the orchestrator still imports cleanly** + +Run: + +```bash +uv run python -c "from presa_mano.api.grasp_routes import _object_points_in_base, _points_via_bbox; print('ok')" +``` + +Expected: `ok`. + +- [ ] **Step 4: Commit** + +```bash +git add src/presa_mano/api/grasp_routes.py +git commit -m "refactor(grasp): extract _points_via_bbox helper + +Pure refactor in preparation for the SAM21 branch. _object_points_in_base +keeps its signature and the bbox→ROI→base-frame logic moves into the +private helper. Meta now includes source='bbox_roi'. + +Co-Authored-By: Claude Opus 4.7 (1M context) " +``` + +--- + +## Task 5: Add SAM21 branch with automatic fallback + +**Files:** +- Modify: `src/presa_mano/api/grasp_routes.py` + +- [ ] **Step 1: Add imports and the fallback sentinel** + +In `src/presa_mano/api/grasp_routes.py`, near the existing imports (after the `from ..clients import G1Unavailable` line), add: + +```python +import base64 +import logging + +import cv2 + +from ..clients import Sam21Error, Sam21Unavailable +from ..perception.depth_align import align_depth_to_rgb +from ..perception.depth_to_rgb import CameraRegistration + +log = logging.getLogger(__name__) + + +class _Sam21Fallback(Exception): + """Internal sentinel: the SAM21 path could not produce a usable cloud. + Never escapes _object_points_in_base — caught and turned into a log line + + legacy bbox path.""" +``` + +Place the `_Sam21Fallback` class definition just above the `_object_points_in_base` function (so it's in scope without forward references). + +The codes that signal "the request was structurally wrong" (presa-mano bug) and deserve `ERROR` level rather than `WARNING`: + +```python +_SAM21_BUG_CODES = frozenset({"SHAPE_MISMATCH", "BBOX_OUT_OF_BOUNDS", + "IMAGE_DECODE_ERROR", "INVALID_INPUT"}) +``` + +Place this constant right after the `_Sam21Fallback` class. + +- [ ] **Step 2: Add the `_points_via_sam21` helper** + +Add this helper right after `_points_via_bbox`: + +```python +def _points_via_sam21(nuwa, bbox: tuple[int, int, int, int], sw: Stopwatch) -> np.ndarray: + """SAM21 path: bbox → segmented point cloud in RGB-camera/OpenCV frame. + + Returns points in **meters**. Raises _Sam21Fallback on any failure that + should trigger the bbox path; lets unexpected exceptions propagate as + _Sam21Fallback with repr(e). + """ + try: + with sw.step("nuwa_grab"): + rgb_jpeg = nuwa.rgb_jpeg() + depth_png_in = nuwa.depth_png() + intr_full = nuwa.intrinsics() + rgb_intr = intr_full.get("rgb") + if not rgb_intr: + raise _Sam21Fallback("nuwa intrinsics missing 'rgb' block") + + # JPEG → PNG. cv2.imdecode gives BGR; cv2.imencode(".png", bgr) writes + # PNG bytes with standard R,G,B color order (libpng handles the swap), + # which SAM21's PIL-based decoder reads as RGB. Verified by inspection. + bgr = cv2.imdecode(np.frombuffer(rgb_jpeg, np.uint8), cv2.IMREAD_COLOR) + if bgr is None: + raise _Sam21Fallback("could not decode Nuwa RGB JPEG") + ok, rgb_png_buf = cv2.imencode(".png", bgr) + if not ok: + raise _Sam21Fallback("could not re-encode RGB as PNG") + rgb_png = bytes(rgb_png_buf) + + with sw.step("depth_align"): + depth_ir = cv2.imdecode(np.frombuffer(depth_png_in, np.uint8), + cv2.IMREAD_UNCHANGED) + if depth_ir is None: + raise _Sam21Fallback("could not decode Nuwa depth PNG") + if depth_ir.dtype != np.uint16: + raise _Sam21Fallback(f"depth PNG dtype expected uint16, got {depth_ir.dtype}") + reg = CameraRegistration.from_nuwa(intr_full) + depth_aligned = align_depth_to_rgb(depth_ir, reg) + ok, depth_png_buf = cv2.imencode(".png", depth_aligned) + if not ok: + raise _Sam21Fallback("could not encode aligned depth as PNG") + depth_png_out = bytes(depth_png_buf) + except _Sam21Fallback: + raise + except Exception as e: + raise _Sam21Fallback(f"prep failed: {e!r}") from e + + with sw.step("sam21"): + try: + pts_cv = _deps.sam21().segment_pointcloud( + rgb_png=rgb_png, + depth_png=depth_png_out, + intrinsics=rgb_intr, + bbox=bbox, + depth_scale_mm=1.0, + ) + except Sam21Unavailable as e: + raise _Sam21Fallback(f"unreachable: {e}") from e + except Sam21Error as e: + if e.code in _SAM21_BUG_CODES: + log.error("SAM21 rejected payload (%s): %s", e.code, e.message) + raise _Sam21Fallback(f"{e.code}: {e.message}") from e + except Exception as e: + log.error("Unexpected SAM21 error: %r", e) + raise _Sam21Fallback(f"unexpected: {e!r}") from e + + if pts_cv.shape[0] < 10: + raise _Sam21Fallback(f"too few points: {pts_cv.shape[0]}") + return pts_cv +``` + +- [ ] **Step 3: Insert the SAM21 branch into `_object_points_in_base`** + +Replace the body of `_object_points_in_base` (the version produced in Task 4) so that the SAM21 path runs before the bbox path. The full replacement of `_object_points_in_base`: + +```python +def _object_points_in_base(conf: float | None, class_name: str | None, sw: Stopwatch + ) -> tuple[np.ndarray, dict[str, Any]]: + cfg = get_config() + nuwa = _deps.nuwa() + yolo = _deps.yolo() + + side = cfg.get("robot", {}).get("wrist_side", "right") + T_base_cam = CalibrationStore(_deps.calibration_file_path(side)).T_base_camera() + if T_base_cam is None: + raise HTTPException(status_code=412, detail=f"calibration missing for side='{side}': run /api/calibration/solve first") + + detector = ObjectDetector(yolo, nuwa) + try: + with sw.step("yolo"): + dets = detector.detect(conf=conf) + except HTTPException: + raise + except Exception as e: + raise HTTPException(status_code=502, detail=f"YOLO detect failed: {e}") + if class_name: + dets = [d for d in dets if (d.get("class") or "").lower() == class_name.lower()] + if not dets: + raise HTTPException(status_code=404, detail="no object detected") + dets.sort(key=lambda d: d["confidence"], reverse=True) + top = dets[0] + bbox = _bbox_to_roi(top["bbox"]) + meta: dict[str, Any] = {"detection": top, "T_base_camera": T_base_cam.tolist()} + + if cfg.get("grasp", {}).get("use_sam21", True): + try: + pts_cv = _points_via_sam21(nuwa, bbox, sw) + with sw.step("to_base_frame"): + pts_base = transform_points(T_base_cam, pts_cv) + meta["source"] = "sam21" + meta["n_points"] = int(len(pts_base)) + return pts_base, meta + except _Sam21Fallback as e: + log.warning("SAM21 path failed, falling back to bbox: %s", e) + meta["sam21_fallback_reason"] = str(e) + + pts_base = _points_via_bbox(nuwa, T_base_cam, bbox, sw) + meta["source"] = "bbox_roi" + meta["n_points"] = int(len(pts_base)) + return pts_base, meta +``` + +Note the **critical frame convention difference**: the SAM21 branch does NOT call `nuwa_to_cv_frame` (SAM21 already returns OpenCV-frame points), whereas `_points_via_bbox` still calls it (legacy behaviour, preserves the existing path exactly). + +- [ ] **Step 4: Smoke-test imports + happy reload** + +```bash +uv run python -c "from presa_mano.api.grasp_routes import _object_points_in_base, _points_via_sam21, _points_via_bbox, _Sam21Fallback; print('ok')" +``` + +Expected: `ok`. + +- [ ] **Step 5: Commit** + +```bash +git add src/presa_mano/api/grasp_routes.py +git commit -m "feat(grasp): SAM21 segmentation branch with bbox fallback + +When grasp.use_sam21=true (default), _object_points_in_base feeds the YOLO +bbox to SAM21 and uses the segmented cloud directly. Any failure +(transport, segmentation empty, no valid depth, payload bug) logs and falls +back to the legacy bbox path. Response meta includes source ∈ {sam21, +bbox_roi} and sam21_fallback_reason when applicable. + +Co-Authored-By: Claude Opus 4.7 (1M context) " +``` + +--- + +## Task 6: Live integration verification + +**Files:** none modified (verification only). At the end, optionally rebuild the Docker image. + +- [ ] **Step 1: Make sure SAM21 is up** + +```bash +docker compose -f /opt/docker/robotserver/docker-compose.yml up -d segmentation-sam21 +docker compose -f /opt/docker/robotserver/docker-compose.yml ps segmentation-sam21 +curl -fsS http://localhost:8002/api/health | jq . +``` + +Expected: `model_loaded: true`, `device` is `cuda` or `cpu`. + +- [ ] **Step 2: Rebuild and start presa-mano with the new code** + +```bash +docker compose -f /opt/docker/robotserver/docker-compose.yml up -d --build presa-mano +``` + +Wait for the healthcheck: + +```bash +until curl -fsS http://localhost:8890/health >/dev/null 2>&1; do sleep 2; done +echo "presa-mano healthy" +``` + +- [ ] **Step 3: Verify the SAM21 happy path** + +With an object visible to the camera: + +```bash +curl -s -X POST http://localhost:8890/api/grasp/plan \ + -H 'Content-Type: application/json' -d '{}' | jq '{source, n_points, sam21_fallback_reason, timings_ms}' +``` + +Expected: +- `source: "sam21"` +- `n_points > 100` (typical for a hand-sized object) +- `sam21_fallback_reason` absent +- `timings_ms.sam21 < 1000` + +- [ ] **Step 4: Verify the fallback path** + +```bash +docker compose -f /opt/docker/robotserver/docker-compose.yml stop segmentation-sam21 +curl -s -X POST http://localhost:8890/api/grasp/plan \ + -H 'Content-Type: application/json' -d '{}' | jq '{source, n_points, sam21_fallback_reason}' +docker compose -f /opt/docker/robotserver/docker-compose.yml start segmentation-sam21 +``` + +Expected: +- `source: "bbox_roi"` +- `sam21_fallback_reason: "unreachable: ..."` +- `n_points` still reasonable (legacy path produces ROI cloud as before) + +- [ ] **Step 5: Verify the empty-mask fallback** + +Aim the camera at a featureless surface or move the object outside the frame, then: + +```bash +curl -s -X POST http://localhost:8890/api/grasp/plan \ + -H 'Content-Type: application/json' -d '{"class_name":"hammer"}' | jq '{source, sam21_fallback_reason}' +``` + +If YOLO still detects something but SAM21 can't segment it, expect `source: "bbox_roi"`, `sam21_fallback_reason: "SEGMENTATION_EMPTY: ..."`. If YOLO detects nothing the orchestrator returns 404 (existing behaviour); that's fine, just confirm it doesn't crash. + +- [ ] **Step 6: Tail logs and confirm no ERROR-level lines** + +```bash +docker compose -f /opt/docker/robotserver/docker-compose.yml logs --since=2m presa-mano | grep -E 'ERROR|Traceback' || echo "no errors" +``` + +Expected: `no errors`. If you see any `ERROR` in SAM21-related lines (other than the explicit ones from `_SAM21_BUG_CODES`), investigate before declaring done. + +- [ ] **Step 7: Final commit (only if anything new emerged)** + +If verification revealed anything that needed a fix (e.g. config typo, missing import), commit those fixes here. Otherwise, this task ends with no commit and the work is done. + +--- + +## Done criteria + +- All pytest tests pass: `uv run pytest -v` → green. +- `/api/grasp/plan` returns `source: "sam21"` with SAM21 up. +- `/api/grasp/plan` returns `source: "bbox_roi"` + `sam21_fallback_reason` with SAM21 down. +- No `ERROR` lines in `presa-mano` logs during normal operation. +- The 7 commits from Tasks 0–6 are on the branch. diff --git a/docs/superpowers/specs/2026-05-11-sam21-segmentation-design.md b/docs/superpowers/specs/2026-05-11-sam21-segmentation-design.md index 6ba45c6..fe7d037 100644 --- a/docs/superpowers/specs/2026-05-11-sam21-segmentation-design.md +++ b/docs/superpowers/specs/2026-05-11-sam21-segmentation-design.md @@ -135,7 +135,7 @@ def sam21() -> Sam21Client: cfg = get_config() s = cfg.get("services", {}).get("sam21", {}) return Sam21Client( - base_url=s.get("url", "http://localhost:8002"), + base_url=s.get("base_url", "http://localhost:8002"), timeout_s=float(s.get("timeout_s", 10.0)), ) ``` @@ -222,7 +222,7 @@ failure of the grasp request. ```yaml services: sam21: - url: http://localhost:8002 + base_url: http://localhost:8002 timeout_s: 10.0 grasp: