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