feat(right_arm): math puro per delta-toward (sign flips + z-floor)

src/presa_mano/right_arm/__init__.py

@@ -0,0 +1 @@
+"""Logica della tab Braccio DX: math puro + endpoint."""

src/presa_mano/right_arm/move.py

@@ -0,0 +1,56 @@
+"""Pure math: delta wrist a partire da centroide camera + offset + sign flips."""
+from __future__ import annotations
+
+import numpy as np
+
+
+class DeltaOversize(ValueError):
+    """Sollevata quando ||delta|| supera max_step_m e force=False."""
+
+    def __init__(self, delta_norm_m: float, max_step_m: float):
+        super().__init__(f"delta {delta_norm_m*1000:.1f}mm > max {max_step_m*1000:.1f}mm")
+        self.delta_norm_m = float(delta_norm_m)
+        self.max_step_m = float(max_step_m)
+
+
+def compute_delta_wrist(
+    centroid_camera_m: np.ndarray,
+    offset_camera_m: np.ndarray,
+    axis_signs: tuple[int, int, int],
+    max_step_m: float | None = None,
+    force: bool = False,
+) -> np.ndarray:
+    """Delta da applicare al polso (frame polso) per portare la camera sull'offset.
+
+    Modello: si assume assi camera ≈ assi polso a meno dei segni.
+        delta_wrist = sign * (centroid_camera - offset_camera)
+
+    Se `max_step_m` è settato e ||delta|| > max_step_m e `force` è False, solleva
+    DeltaOversize.
+    """
+    centroid = np.asarray(centroid_camera_m, dtype=np.float64).reshape(3)
+    offset = np.asarray(offset_camera_m, dtype=np.float64).reshape(3)
+    sign = np.asarray(axis_signs, dtype=np.float64).reshape(3)
+    delta = sign * (centroid - offset)
+    if max_step_m is not None and not force:
+        n = float(np.linalg.norm(delta))
+        if n > max_step_m:
+            raise DeltaOversize(n, max_step_m)
+    return delta
+
+
+def apply_z_floor(
+    wrist_z_m: float,
+    delta: np.ndarray,
+    z_floor_m: float | None,
+) -> tuple[np.ndarray, bool]:
+    """Se la Z target finirebbe sotto `z_floor_m`, clippa il delta.Z.
+    Ritorna (nuovo_delta, was_clipped). `z_floor_m=None` disabilita il clip."""
+    delta = np.asarray(delta, dtype=np.float64).reshape(3).copy()
+    if z_floor_m is None:
+        return delta, False
+    target_z = float(wrist_z_m) + float(delta[2])
+    if target_z >= float(z_floor_m):
+        return delta, False
+    delta[2] = float(z_floor_m) - float(wrist_z_m)
+    return delta, True

tests/test_right_arm_move.py

@@ -0,0 +1,96 @@
+"""Unit test della logica pura del move-toward (no I/O)."""
+from __future__ import annotations
+
+import numpy as np
+import pytest
+
+from presa_mano.right_arm.move import (
+    DeltaOversize,
+    apply_z_floor,
+    compute_delta_wrist,
+)
+
+
+def test_zero_offset_returns_centroid_with_signs():
+    delta = compute_delta_wrist(
+        centroid_camera_m=np.array([0.0, 0.0, 1.0]),
+        offset_camera_m=np.array([0.0, 0.0, 0.0]),
+        axis_signs=(1, 1, 1),
+    )
+    assert np.allclose(delta, [0.0, 0.0, 1.0])
+
+
+def test_offset_subtracted_before_signs():
+    # centroid=(0,0,1) offset=(0,0,0.1) sign=(1,-1,-1) → (0, 0, -0.9)
+    delta = compute_delta_wrist(
+        centroid_camera_m=np.array([0.0, 0.0, 1.0]),
+        offset_camera_m=np.array([0.0, 0.0, 0.1]),
+        axis_signs=(1, -1, -1),
+    )
+    assert np.allclose(delta, [0.0, 0.0, -0.9])
+
+
+def test_signs_applied_componentwise():
+    delta = compute_delta_wrist(
+        centroid_camera_m=np.array([0.05, 0.10, 0.20]),
+        offset_camera_m=np.array([0.00, 0.00, 0.00]),
+        axis_signs=(1, -1, -1),
+    )
+    assert np.allclose(delta, [0.05, -0.10, -0.20])
+
+
+def test_oversize_raises_when_not_forced():
+    with pytest.raises(DeltaOversize):
+        compute_delta_wrist(
+            centroid_camera_m=np.array([0.0, 0.0, 0.5]),
+            offset_camera_m=np.array([0.0, 0.0, 0.0]),
+            axis_signs=(1, 1, 1),
+            max_step_m=0.15,
+            force=False,
+        )
+
+
+def test_oversize_passes_when_forced():
+    delta = compute_delta_wrist(
+        centroid_camera_m=np.array([0.0, 0.0, 0.5]),
+        offset_camera_m=np.array([0.0, 0.0, 0.0]),
+        axis_signs=(1, 1, 1),
+        max_step_m=0.15,
+        force=True,
+    )
+    assert np.allclose(delta, [0.0, 0.0, 0.5])
+
+
+def test_z_floor_clips_when_target_below():
+    # wrist_before.z=0.10, delta.z=-0.30, target=-0.20, floor=0.05 → clip a 0.05
+    # → delta.z = 0.05 - 0.10 = -0.05
+    new_delta, clipped = apply_z_floor(
+        wrist_z_m=0.10,
+        delta=np.array([0.01, -0.02, -0.30]),
+        z_floor_m=0.05,
+    )
+    assert clipped is True
+    assert new_delta[2] == pytest.approx(-0.05)
+    # X/Y invariati
+    assert new_delta[0] == pytest.approx(0.01)
+    assert new_delta[1] == pytest.approx(-0.02)
+
+
+def test_z_floor_noop_when_target_above():
+    new_delta, clipped = apply_z_floor(
+        wrist_z_m=0.30,
+        delta=np.array([0.0, 0.0, -0.10]),
+        z_floor_m=0.05,
+    )
+    assert clipped is False
+    assert np.allclose(new_delta, [0.0, 0.0, -0.10])
+
+
+def test_z_floor_none_disables_clip():
+    new_delta, clipped = apply_z_floor(
+        wrist_z_m=0.10,
+        delta=np.array([0.0, 0.0, -0.50]),
+        z_floor_m=None,
+    )
+    assert clipped is False
+    assert np.allclose(new_delta, [0.0, 0.0, -0.50])