feat: initial commit — orchestratore presa-mano con AI assistant + dual-robot

Pipeline completa per calibrazione hand-eye e grasp planning su robot
Unitree G1/H1 con camera depth Nuwa-HP60C. Esposta come FastAPI + GUI vanilla
JS, con assistente AI (OpenRouter + tool calling curato) per pilotare il
sistema in linguaggio naturale.

Architettura:
- FastAPI orchestrator (porta da .env, default 8890) che coordina 4
  microservizi upstream: Nuwa-HP60C, marker-finder, YOLO, robot bridge.
- Dual-robot: services.g1 e services.h1 in config.yaml; switchabili a
  runtime via PUT /api/home/robot.
- Calibrazione hand-eye eye-to-hand split per-side (right/left).
- Marker pose: marker-finder riceve gli intrinseci RGB della Nuwa al boot,
  PnP server-side con quei dati. Nessun re-PnP locale richiesto.
- Depth-to-RGB: registrazione iterativa IR↔RGB per back-projection 3D
  corretta (Nuwa /api/measure usa coords IR; il nostro /api/perception/
  measure_rgb prende pixel RGB e ritorna 3D nel frame RGB OpenCV).
- Correzione affine sul depth bias (calibrabile via marker come
  ground-truth) — riduce errore Z da ~6% a ~1.5% nei test.
- AI assistant: 16 tool curati (move_wrist_*, hand_action, get_marker[_3d],
  detect_object, calibration_*, set_active_robot…), system prompt
  dinamico che riflette robot attivo + marker config + addendum
  customizzabile da GUI. Temperature di default 0.1 per evitare invenzioni.

GUI (7 tab):
- Home: stato comunicazione device + selettore robot attivo G1/H1
- Assistente AI: chat con OpenRouter, history persistita lato browser,
  task riutilizzabili (prompt salvati), trace tool calls
- Impostazioni AI: modello da combo curata, temperature, max iterations,
  prompt addendum, anteprima system prompt
- Calibrazione, Auto-Calibrazione, TrovaUtensile, Servo Visuale, Vista 3D,
  Movimento EE — invariate dalle versioni precedenti

Setup:
- Variabili in presa-mano/.env (template in .env.example)
- OPENROUTER_API_KEY richiesta solo per il tab Assistente AI
- PRESA_MANO_PORT configurabile

Co-Authored-By: Claude Opus 4.7 (1M context) <noreply@anthropic.com>

src/presa_mano/geometry.py

@@ -0,0 +1,111 @@
+"""SE(3) helpers: compose, invert, conversions rvec/quat/euler/matrix."""
+
+from __future__ import annotations
+
+import numpy as np
+
+
+def rvec_to_R(rvec: np.ndarray) -> np.ndarray:
+    import cv2
+    R, _ = cv2.Rodrigues(np.asarray(rvec, dtype=np.float64).reshape(3, 1))
+    return R
+
+
+def R_to_rvec(R: np.ndarray) -> np.ndarray:
+    import cv2
+    rvec, _ = cv2.Rodrigues(np.asarray(R, dtype=np.float64))
+    return rvec.flatten()
+
+
+def quat_xyzw_to_R(q: np.ndarray) -> np.ndarray:
+    x, y, z, w = [float(v) for v in q]
+    n = x * x + y * y + z * z + w * w
+    if n == 0.0:
+        return np.eye(3)
+    s = 2.0 / n
+    return np.array([
+        [1 - s * (y * y + z * z), s * (x * y - z * w),     s * (x * z + y * w)],
+        [s * (x * y + z * w),     1 - s * (x * x + z * z), s * (y * z - x * w)],
+        [s * (x * z - y * w),     s * (y * z + x * w),     1 - s * (x * x + y * y)],
+    ])
+
+
+def R_to_quat_xyzw(R: np.ndarray) -> np.ndarray:
+    m = np.asarray(R, dtype=np.float64)
+    t = m[0, 0] + m[1, 1] + m[2, 2]
+    if t > 0:
+        s = np.sqrt(t + 1.0) * 2
+        w = 0.25 * s
+        x = (m[2, 1] - m[1, 2]) / s
+        y = (m[0, 2] - m[2, 0]) / s
+        z = (m[1, 0] - m[0, 1]) / s
+    elif m[0, 0] > m[1, 1] and m[0, 0] > m[2, 2]:
+        s = np.sqrt(1.0 + m[0, 0] - m[1, 1] - m[2, 2]) * 2
+        w = (m[2, 1] - m[1, 2]) / s
+        x = 0.25 * s
+        y = (m[0, 1] + m[1, 0]) / s
+        z = (m[0, 2] + m[2, 0]) / s
+    elif m[1, 1] > m[2, 2]:
+        s = np.sqrt(1.0 + m[1, 1] - m[0, 0] - m[2, 2]) * 2
+        w = (m[0, 2] - m[2, 0]) / s
+        x = (m[0, 1] + m[1, 0]) / s
+        y = 0.25 * s
+        z = (m[1, 2] + m[2, 1]) / s
+    else:
+        s = np.sqrt(1.0 + m[2, 2] - m[0, 0] - m[1, 1]) * 2
+        w = (m[1, 0] - m[0, 1]) / s
+        x = (m[0, 2] + m[2, 0]) / s
+        y = (m[1, 2] + m[2, 1]) / s
+        z = 0.25 * s
+    return np.array([x, y, z, w])
+
+
+def make_T(R: np.ndarray, t: np.ndarray) -> np.ndarray:
+    T = np.eye(4)
+    T[:3, :3] = R
+    T[:3, 3] = np.asarray(t, dtype=np.float64).flatten()
+    return T
+
+
+def invert_T(T: np.ndarray) -> np.ndarray:
+    R = T[:3, :3]
+    t = T[:3, 3]
+    Ti = np.eye(4)
+    Ti[:3, :3] = R.T
+    Ti[:3, 3] = -R.T @ t
+    return Ti
+
+
+def T_from_pos_quat(pos: np.ndarray, quat_xyzw: np.ndarray) -> np.ndarray:
+    return make_T(quat_xyzw_to_R(np.asarray(quat_xyzw)), np.asarray(pos))
+
+
+def pos_quat_from_T(T: np.ndarray) -> tuple[np.ndarray, np.ndarray]:
+    return T[:3, 3].copy(), R_to_quat_xyzw(T[:3, :3])
+
+
+def T_from_rvec_tvec(rvec: np.ndarray, tvec: np.ndarray) -> np.ndarray:
+    return make_T(rvec_to_R(rvec), np.asarray(tvec).flatten())
+
+
+def R_to_euler_xyz(R: np.ndarray) -> np.ndarray:
+    """Extrinsic XYZ euler (roll, pitch, yaw) in radians."""
+    sy = np.sqrt(R[0, 0] ** 2 + R[1, 0] ** 2)
+    singular = sy < 1e-6
+    if not singular:
+        x = np.arctan2(R[2, 1], R[2, 2])
+        y = np.arctan2(-R[2, 0], sy)
+        z = np.arctan2(R[1, 0], R[0, 0])
+    else:
+        x = np.arctan2(-R[1, 2], R[1, 1])
+        y = np.arctan2(-R[2, 0], sy)
+        z = 0.0
+    return np.array([x, y, z])
+
+
+def transform_points(T: np.ndarray, pts: np.ndarray) -> np.ndarray:
+    """Apply T (4x4) to Nx3 points."""
+    pts = np.asarray(pts, dtype=np.float64)
+    R = T[:3, :3]
+    t = T[:3, 3]
+    return pts @ R.T + t