Adriano Dal PastroandClaude Opus 4.7
ac573505b7
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>
66 lines
2.2 KiB
Python
66 lines
2.2 KiB
Python
"""Point cloud utilities: crop to bbox in image space + outlier filtering.
|
|
|
|
Nuwa /api/pointcloud returns points in camera frame; we need to project them
|
|
back to pixel space to keep only those inside a YOLO bbox.
|
|
|
|
Frame note:
|
|
Nuwa publishes points in X-right / Y-up / Z-back (Z<0 in front of camera).
|
|
`cv2.solvePnP` (used by marker-finder to produce T_cam_marker, hence
|
|
T_base_camera after hand-eye) uses X-right / Y-down / Z-forward (Z>0 in front).
|
|
Before applying T_base_camera we flip Y and Z via `nuwa_to_cv_frame`.
|
|
"""
|
|
|
|
from __future__ import annotations
|
|
|
|
import numpy as np
|
|
|
|
|
|
def nuwa_to_cv_frame(points_xyz: np.ndarray) -> np.ndarray:
|
|
"""Flip Y and Z to convert Nuwa camera points to OpenCV convention."""
|
|
pts = np.asarray(points_xyz, dtype=np.float64).copy()
|
|
pts[:, 1] *= -1.0
|
|
pts[:, 2] *= -1.0
|
|
return pts
|
|
|
|
|
|
def project(points_cam: np.ndarray, K: np.ndarray) -> np.ndarray:
|
|
"""Project Nx3 camera-frame points to Nx2 pixels via pinhole."""
|
|
pts = np.asarray(points_cam, dtype=np.float64)
|
|
z = pts[:, 2]
|
|
z = np.where(z == 0, 1e-9, z)
|
|
u = K[0, 0] * pts[:, 0] / z + K[0, 2]
|
|
v = K[1, 1] * pts[:, 1] / z + K[1, 2]
|
|
return np.stack([u, v], axis=1)
|
|
|
|
|
|
def crop_bbox(points_cam: np.ndarray, bbox_xyxy: tuple[float, float, float, float],
|
|
K: np.ndarray) -> np.ndarray:
|
|
"""Keep only 3D points whose projection falls inside bbox (in pixel coords).
|
|
|
|
Works regardless of depth-sign convention (OpenCV: Z>0 in front;
|
|
Nuwa/OpenGL-like: Z<0 in front) — the server-provided point cloud already
|
|
obeys u = fx·x/z + cx with its own convention.
|
|
"""
|
|
if points_cam.size == 0:
|
|
return points_cam[:0, :]
|
|
xyz = points_cam[:, :3]
|
|
uv = project(xyz, K)
|
|
x1, y1, x2, y2 = bbox_xyxy
|
|
mask = (
|
|
(uv[:, 0] >= x1) & (uv[:, 0] <= x2) &
|
|
(uv[:, 1] >= y1) & (uv[:, 1] <= y2) &
|
|
(np.abs(xyz[:, 2]) > 1e-6)
|
|
)
|
|
return points_cam[mask]
|
|
|
|
|
|
def filter_outliers(points: np.ndarray, n_std: float = 2.0) -> np.ndarray:
|
|
"""Remove points whose distance from centroid exceeds n_std * std."""
|
|
if len(points) < 10:
|
|
return points
|
|
xyz = points[:, :3]
|
|
c = xyz.mean(axis=0)
|
|
d = np.linalg.norm(xyz - c, axis=1)
|
|
keep = d < (d.mean() + n_std * d.std())
|
|
return points[keep]
|