diff --git a/src/presa_mano/right_arm/__init__.py b/src/presa_mano/right_arm/__init__.py new file mode 100644 index 0000000..863a60a --- /dev/null +++ b/src/presa_mano/right_arm/__init__.py @@ -0,0 +1 @@ +"""Logica della tab Braccio DX: math puro + endpoint.""" diff --git a/src/presa_mano/right_arm/move.py b/src/presa_mano/right_arm/move.py new file mode 100644 index 0000000..4fec373 --- /dev/null +++ b/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 diff --git a/tests/test_right_arm_move.py b/tests/test_right_arm_move.py new file mode 100644 index 0000000..51cf5a9 --- /dev/null +++ b/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])