Adriano Dal PastroandClaude Opus 4.7
57954b5c8d
Lavoro accumulato nella sessione 2026-05-21 prima del brainstorming
della tab Pose 3D:
- compute_delta_wrist generalizzato con cam_to_wrist_perm: la mappa
cam->polso non e' solo sign-flip ma una permutazione (default
identita' per backward compat). DeltaOversize ora espone X/Y/Z
nella stringa d'errore. Test aggiornati.
- G1Client.wrist_absolute / wrist_relative: precise=True di default
(era False). Stessa cosa per wrist_routes.AbsoluteReq Pydantic
e per agent.tools move_wrist_*. Movimenti reali ora chiedono
controllo accurato salvo override esplicito.
- Tab Braccio DX:
- /verify_tag endpoint: snapshot wrist FK + tag id=1 dalla front
camera. Per verifica relativa moveto vs marker (no calibrazione).
- GUI: pannello "Verifica tag id=1" + campi camera_offset_wrist_mm.
- markUserTouchedOnInput esteso a raOff/raSgn/raCamOff.
Co-Authored-By: Claude Opus 4.7 (1M context) <noreply@anthropic.com>
173 lines
5.5 KiB
Python
173 lines
5.5 KiB
Python
"""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_permutation_maps_cam_axes_to_wrist_axes():
|
|
# Mounting: cam guarda avanti = wrist_X. perm=(2,0,1): wrist_X<-cam_Z,
|
|
# wrist_Y<-cam_X, wrist_Z<-cam_Y. signs=(+1,-1,-1).
|
|
# centroid=(0.10, 0.05, 0.30) → wrist=(+0.30, -0.10, -0.05) prima offset/t_wc
|
|
delta = compute_delta_wrist(
|
|
centroid_camera_m=np.array([0.10, 0.05, 0.30]),
|
|
offset_camera_m=np.array([0.0, 0.0, 0.0]),
|
|
axis_signs=(1, -1, -1),
|
|
cam_to_wrist_perm=(2, 0, 1),
|
|
)
|
|
assert np.allclose(delta, [0.30, -0.10, -0.05])
|
|
|
|
|
|
def test_permutation_default_is_identity_back_compat():
|
|
# Senza perm esplicito, comportamento = sign-only (modello vecchio).
|
|
delta_new = compute_delta_wrist(
|
|
centroid_camera_m=np.array([0.10, 0.20, 0.30]),
|
|
offset_camera_m=np.array([0.0, 0.0, 0.0]),
|
|
axis_signs=(1, -1, -1),
|
|
)
|
|
assert np.allclose(delta_new, [0.10, -0.20, -0.30])
|
|
|
|
|
|
def test_invalid_permutation_raises():
|
|
import pytest as _pt
|
|
with _pt.raises(ValueError, match="permutation"):
|
|
compute_delta_wrist(
|
|
centroid_camera_m=np.zeros(3),
|
|
offset_camera_m=np.zeros(3),
|
|
axis_signs=(1, 1, 1),
|
|
cam_to_wrist_perm=(0, 0, 1),
|
|
)
|
|
|
|
|
|
def test_camera_offset_wrist_added_after_sign_flip():
|
|
# centroid=(0,0,1) offset=(0,0,0) sign=(1,1,1) t_wc=(0.05,-0.02,0.08)
|
|
# → sign*(centroid-offset) = (0,0,1)
|
|
# → + t_wc = (0.05, -0.02, 1.08)
|
|
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),
|
|
camera_offset_wrist_m=np.array([0.05, -0.02, 0.08]),
|
|
)
|
|
assert np.allclose(delta, [0.05, -0.02, 1.08])
|
|
|
|
|
|
def test_camera_offset_wrist_default_zero_back_compat():
|
|
# Senza camera_offset_wrist_m la formula vecchia resta invariata.
|
|
delta = compute_delta_wrist(
|
|
centroid_camera_m=np.array([0.10, 0.20, 0.30]),
|
|
offset_camera_m=np.array([0.00, 0.00, 0.10]),
|
|
axis_signs=(1, -1, -1),
|
|
)
|
|
assert np.allclose(delta, [0.10, -0.20, -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_message_includes_per_axis_breakdown():
|
|
# delta = (1,-1,-1) * ((0.10, -0.02, 0.50) - 0) = (0.10, 0.02, -0.50)
|
|
# ||delta|| = sqrt(0.01+0.0004+0.25) = ~0.510m, max=0.15
|
|
with pytest.raises(DeltaOversize) as exc_info:
|
|
compute_delta_wrist(
|
|
centroid_camera_m=np.array([0.10, -0.02, 0.50]),
|
|
offset_camera_m=np.array([0.0, 0.0, 0.0]),
|
|
axis_signs=(1, -1, -1),
|
|
max_step_m=0.15,
|
|
force=False,
|
|
)
|
|
msg = str(exc_info.value)
|
|
assert "X=100.0" in msg
|
|
assert "Y=20.0" in msg
|
|
assert "Z=-500.0" in msg
|
|
# E l'oggetto espone il vettore strutturato
|
|
assert np.allclose(exc_info.value.delta_m, [0.10, 0.02, -0.50])
|
|
|
|
|
|
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])
|