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>
This commit is contained in:
Adriano Dal PastroandClaude Opus 4.7 committed
commit
ac573505b7
121 files changed
+67199
No files matched your search
@@ -0,0 +1,11 @@
|
||||
"""HTTP clients for upstream microservices."""
|
||||
|
||||
from .g1 import G1Client, G1StatusError, G1Unavailable
|
||||
from .marker import MarkerClient
|
||||
from .nuwa import NuwaClient, NuwaError
|
||||
from .yolo import YoloClient
|
||||
|
||||
__all__ = [
|
||||
"G1Client", "G1StatusError", "G1Unavailable",
|
||||
"MarkerClient", "NuwaClient", "NuwaError", "YoloClient",
|
||||
]
|
||||
@@ -0,0 +1,197 @@
|
||||
"""Client for the G1 API Server (port 8765).
|
||||
|
||||
Supports dry-run: when enabled, mutating calls return a mock response
|
||||
and the actual HTTP request is not performed. Useful while DDS is crashing.
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import logging
|
||||
from typing import Any
|
||||
|
||||
import httpx
|
||||
|
||||
log = logging.getLogger(__name__)
|
||||
|
||||
|
||||
class G1Unavailable(RuntimeError):
|
||||
"""Upstream G1 bridge is unreachable or returned a non-actionable error.
|
||||
Maps to 502 Bad Gateway by default."""
|
||||
pass
|
||||
|
||||
|
||||
class G1StatusError(RuntimeError):
|
||||
"""Upstream G1 returned an HTTP error with a semantic status (e.g. 409
|
||||
'arms already moving'). Preserve the code so the caller can forward it."""
|
||||
def __init__(self, status: int, detail: str, path: str):
|
||||
super().__init__(f"G1 {path}: {status} {detail}")
|
||||
self.status = status
|
||||
self.detail = detail
|
||||
self.path = path
|
||||
|
||||
|
||||
class G1Client:
|
||||
def __init__(self, base_url: str, timeout_s: float = 5.0, dry_run: bool = False,
|
||||
write_timeout_s: float | None = None):
|
||||
"""timeout_s applies to GETs (fast reads). write_timeout_s applies to
|
||||
POST/DELETE where the G1 server blocks waiting for motion to complete
|
||||
(FSM transitions, go_neutral, stand, etc.). Defaults to max(30, read)."""
|
||||
self.base_url = base_url.rstrip("/")
|
||||
self.timeout_s = timeout_s
|
||||
self.write_timeout_s = write_timeout_s if write_timeout_s is not None else max(30.0, timeout_s)
|
||||
self.dry_run = dry_run
|
||||
|
||||
# ----- read-only -----
|
||||
|
||||
def health(self) -> dict[str, Any]:
|
||||
return self._get("/health")
|
||||
|
||||
def wrist(self) -> dict[str, Any]:
|
||||
"""Current wrist poses. Server shape: {'left_wrist': {'position','quaternion'}, 'right_wrist': {...}}."""
|
||||
return self._get("/wrist")
|
||||
|
||||
def state(self) -> dict[str, Any]:
|
||||
"""Full live snapshot: arms (current+target+status), hands, locked, dds_ready."""
|
||||
return self._get("/state")
|
||||
|
||||
def hand(self) -> dict[str, Any]:
|
||||
"""Hand finger state. Shape: {'left': {'fingers': {pinky,ring,middle,index,thumb_bend,thumb_rotation}, 'age_s'}, 'right': {...}}."""
|
||||
return self._get("/hand")
|
||||
|
||||
# ----- mutating -----
|
||||
|
||||
def wrist_absolute(self, side: str, position: list[float], quaternion: list[float],
|
||||
wait: bool = True, timeout_s: float = 5.0, precise: bool = False) -> dict[str, Any]:
|
||||
body = {
|
||||
"side": side, "position": position, "quaternion": quaternion,
|
||||
"wait": wait, "timeout_s": timeout_s, "precise": precise,
|
||||
}
|
||||
return self._post("/wrist/absolute", body)
|
||||
|
||||
def wrist_relative(self, side: str, delta_position: list[float] | None = None,
|
||||
delta_quaternion: list[float] | None = None, wait: bool = True,
|
||||
timeout_s: float = 5.0) -> dict[str, Any]:
|
||||
body: dict[str, Any] = {"side": side, "wait": wait, "timeout_s": timeout_s}
|
||||
if delta_position is not None:
|
||||
body["delta_position"] = delta_position
|
||||
if delta_quaternion is not None:
|
||||
body["delta_quaternion"] = delta_quaternion
|
||||
return self._post("/wrist/relative", body)
|
||||
|
||||
def hand_action(self, action: str) -> dict[str, Any]:
|
||||
"""action ∈ {LEFT_OPEN, LEFT_CLOSE, RIGHT_OPEN, RIGHT_CLOSE}."""
|
||||
return self._post("/hand/action", {"action": action})
|
||||
|
||||
def hand_control(self, side: str, fingers: dict[str, float]) -> dict[str, Any]:
|
||||
"""Per-finger control (normalized 0..1). Unspecified fingers keep their last value."""
|
||||
return self._post("/hand/control", {"side": side, "fingers": fingers})
|
||||
|
||||
def emergency_stop(self) -> dict[str, Any]:
|
||||
return self._post("/emergency_stop", {})
|
||||
|
||||
def unlock(self) -> dict[str, Any]:
|
||||
return self._post("/unlock", {})
|
||||
|
||||
# ----- robot FSM / locomotion -----
|
||||
|
||||
def robot_state_get(self) -> dict[str, Any]:
|
||||
return self._get("/robot/state")
|
||||
|
||||
def robot_state_set(self, state: str) -> dict[str, Any]:
|
||||
return self._post("/robot/state", {"state": state})
|
||||
|
||||
def robot_arm_mode(self, mode: str) -> dict[str, Any]:
|
||||
return self._post("/robot/arm_mode", {"mode": mode})
|
||||
|
||||
def robot_stand(self, mode: str) -> dict[str, Any]:
|
||||
return self._post("/robot/stand", {"mode": mode})
|
||||
|
||||
def robot_move(self, vx: float, vy: float, vyaw: float, continuous: bool = False) -> dict[str, Any]:
|
||||
return self._post("/robot/move", {"vx": vx, "vy": vy, "vyaw": vyaw, "continuous": continuous})
|
||||
|
||||
def robot_stopmove(self) -> dict[str, Any]:
|
||||
return self._post("/robot/stopmove", {})
|
||||
|
||||
def arms_compliance(self) -> dict[str, Any]:
|
||||
return self._get("/arms/compliance")
|
||||
|
||||
def arms_compliance_start(self) -> dict[str, Any]:
|
||||
return self._post("/arms/compliance/start", {})
|
||||
|
||||
def arms_compliance_stop(self) -> dict[str, Any]:
|
||||
return self._post("/arms/compliance/stop", {})
|
||||
|
||||
def arms_go_neutral(self) -> dict[str, Any]:
|
||||
return self._post("/arms/go_neutral", {})
|
||||
|
||||
def arms_loco_reset(self) -> dict[str, Any]:
|
||||
return self._post("/arms/loco_reset", {})
|
||||
|
||||
def diagnostics(self) -> dict[str, Any]:
|
||||
"""Motor temperatures / fault codes / enabled flags extracted from the
|
||||
G1 lowstate. Optional on the upstream — a 404 here means the G1 API
|
||||
version in use does not expose diagnostics yet."""
|
||||
return self._get("/diagnostics")
|
||||
|
||||
# ----- presets (server-side persistence of pose+fingers snapshots) -----
|
||||
|
||||
def presets(self) -> dict[str, Any]:
|
||||
return self._get("/presets")
|
||||
|
||||
def preset_get(self, name: str) -> dict[str, Any]:
|
||||
return self._get(f"/presets/{name}")
|
||||
|
||||
def preset_save(self, name: str) -> dict[str, Any]:
|
||||
return self._post("/presets/save", {"name": name})
|
||||
|
||||
def preset_execute(self, name: str) -> dict[str, Any]:
|
||||
return self._post(f"/presets/{name}/execute", {})
|
||||
|
||||
def preset_delete(self, name: str) -> dict[str, Any]:
|
||||
return self._delete(f"/presets/{name}")
|
||||
|
||||
# ----- internals -----
|
||||
|
||||
def _check_status(self, r: httpx.Response, verb: str, path: str) -> None:
|
||||
if r.is_success:
|
||||
return
|
||||
# Forward semantic client-side errors (409, 412, 422, …) with detail.
|
||||
try:
|
||||
body = r.json()
|
||||
detail = body.get("detail", r.text) if isinstance(body, dict) else r.text
|
||||
except Exception:
|
||||
detail = r.text
|
||||
raise G1StatusError(r.status_code, str(detail), f"{verb} {path}")
|
||||
|
||||
def _get(self, path: str) -> dict[str, Any]:
|
||||
try:
|
||||
with httpx.Client(timeout=self.timeout_s) as c:
|
||||
r = c.get(f"{self.base_url}{path}")
|
||||
self._check_status(r, "GET", path)
|
||||
return r.json()
|
||||
except (httpx.HTTPError, OSError) as e:
|
||||
raise G1Unavailable(f"G1 GET {path} failed: {e}") from e
|
||||
|
||||
def _post(self, path: str, body: dict[str, Any]) -> dict[str, Any]:
|
||||
if self.dry_run:
|
||||
log.info("[DRY-RUN] G1 POST %s body=%s", path, body)
|
||||
return {"ok": True, "dry_run": True, "path": path, "body": body}
|
||||
try:
|
||||
with httpx.Client(timeout=self.write_timeout_s) as c:
|
||||
r = c.post(f"{self.base_url}{path}", json=body)
|
||||
self._check_status(r, "POST", path)
|
||||
return r.json()
|
||||
except (httpx.HTTPError, OSError) as e:
|
||||
raise G1Unavailable(f"G1 POST {path} failed: {e}") from e
|
||||
|
||||
def _delete(self, path: str) -> dict[str, Any]:
|
||||
if self.dry_run:
|
||||
log.info("[DRY-RUN] G1 DELETE %s", path)
|
||||
return {"ok": True, "dry_run": True, "path": path}
|
||||
try:
|
||||
with httpx.Client(timeout=self.write_timeout_s) as c:
|
||||
r = c.delete(f"{self.base_url}{path}")
|
||||
self._check_status(r, "DELETE", path)
|
||||
return r.json()
|
||||
except (httpx.HTTPError, OSError) as e:
|
||||
raise G1Unavailable(f"G1 DELETE {path} failed: {e}") from e
|
||||
@@ -0,0 +1,68 @@
|
||||
"""Client for the marker-finder service (port 8082).
|
||||
|
||||
Marker-finder accepts OpenCV intrinsics (`camera_matrix` 3x3 + `dist_coeffs`)
|
||||
either as a per-request `intrinsics` form field on `/detect`, or as a stored
|
||||
default via `POST /intrinsics`. We push the live Nuwa intrinsics at startup
|
||||
(see main.py) so every subsequent `/detect` produces correct poses with no
|
||||
extra work, and we still expose a per-call override for unusual cases.
|
||||
"""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import json
|
||||
from typing import Any
|
||||
|
||||
import httpx
|
||||
|
||||
|
||||
class MarkerClient:
|
||||
def __init__(self, base_url: str, timeout_s: float = 5.0):
|
||||
self.base_url = base_url.rstrip("/")
|
||||
self.timeout_s = timeout_s
|
||||
|
||||
def detect_url(self, markers: list[dict[str, Any]], image_url: str,
|
||||
intrinsics: dict[str, Any] | None = None) -> dict[str, Any]:
|
||||
"""Returns {'found': [...], 'not_found': [...]}.
|
||||
|
||||
Each found item includes 'corners' (Nx2), 'id', 'size_mm', 'position_m',
|
||||
'rotation_matrix', 'quaternion_xyzw'. Pose is computed by marker-finder
|
||||
with the active default intrinsics (or the per-request override).
|
||||
|
||||
Args:
|
||||
intrinsics: optional per-call override, OpenCV format
|
||||
`{"camera_matrix": [[fx,0,cx],[0,fy,cy],[0,0,1]],
|
||||
"dist_coeffs": [k1,k2,p1,p2,k3]}`
|
||||
(mostly unused now that we push the Nuwa defaults at startup;
|
||||
kept for tools that want to detect against a custom camera).
|
||||
"""
|
||||
data = {"markers": json.dumps(markers), "image_url": image_url}
|
||||
if intrinsics is not None:
|
||||
data["intrinsics"] = json.dumps(intrinsics)
|
||||
with httpx.Client(timeout=self.timeout_s) as c:
|
||||
r = c.post(f"{self.base_url}/detect", data=data)
|
||||
r.raise_for_status()
|
||||
return r.json()
|
||||
|
||||
def get_intrinsics(self) -> dict[str, Any]:
|
||||
"""Currently-active default intrinsics on marker-finder."""
|
||||
with httpx.Client(timeout=self.timeout_s) as c:
|
||||
r = c.get(f"{self.base_url}/intrinsics")
|
||||
r.raise_for_status()
|
||||
return r.json()
|
||||
|
||||
def set_intrinsics(self, camera_matrix: list[list[float]],
|
||||
dist_coeffs: list[float],
|
||||
persist: bool = False) -> dict[str, Any]:
|
||||
"""Replace marker-finder's default intrinsics.
|
||||
|
||||
`persist=True` writes them into marker-finder's config.yaml so they
|
||||
survive a restart. We default to in-memory only — presa-mano repushes
|
||||
them at every startup, so persisting would just create config drift.
|
||||
"""
|
||||
body = {"camera_matrix": camera_matrix, "dist_coeffs": dist_coeffs}
|
||||
with httpx.Client(timeout=self.timeout_s) as c:
|
||||
r = c.post(f"{self.base_url}/intrinsics",
|
||||
params={"persist": "true" if persist else "false"},
|
||||
json=body)
|
||||
r.raise_for_status()
|
||||
return r.json()
|
||||
@@ -0,0 +1,105 @@
|
||||
"""Client for Nuwa-HP60C depth camera (port 8877)."""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
import io
|
||||
from typing import Any
|
||||
|
||||
import httpx
|
||||
import numpy as np
|
||||
|
||||
|
||||
class NuwaError(RuntimeError):
|
||||
pass
|
||||
|
||||
|
||||
class NuwaClient:
|
||||
def __init__(self, base_url: str, timeout_s: float = 5.0, api_key: str | None = None):
|
||||
self.base_url = base_url.rstrip("/")
|
||||
self.timeout_s = timeout_s
|
||||
self._headers = {"x-api-key": api_key} if api_key else {}
|
||||
|
||||
def _get(self, path: str, **kwargs: Any) -> httpx.Response:
|
||||
with httpx.Client(timeout=self.timeout_s, headers=self._headers) as c:
|
||||
r = c.get(f"{self.base_url}{path}", **kwargs)
|
||||
r.raise_for_status()
|
||||
return r
|
||||
|
||||
def health(self) -> dict[str, Any]:
|
||||
return self._get("/api/health").json()
|
||||
|
||||
def status(self) -> dict[str, Any]:
|
||||
return self._get("/api/status").json()
|
||||
|
||||
def rgb_url(self) -> str:
|
||||
"""URL that upstream services (marker-finder, yolo) can use to pull the current RGB frame."""
|
||||
return f"{self.base_url}/api/rgb"
|
||||
|
||||
def rgb_jpeg(self) -> bytes:
|
||||
return self._get("/api/rgb").content
|
||||
|
||||
def intrinsics(self) -> dict[str, Any]:
|
||||
"""Returns {fx, fy, cx, cy, width, height, dist_coeffs?}."""
|
||||
data = self._get("/api/intrinsics").json()
|
||||
if isinstance(data, dict) and "data" in data:
|
||||
payload = data.get("data") or {}
|
||||
if not payload:
|
||||
raise NuwaError(data.get("error") or "intrinsics unavailable")
|
||||
return payload
|
||||
return data
|
||||
|
||||
def depth_png(self) -> bytes:
|
||||
"""16-bit depth PNG (values in mm)."""
|
||||
return self._get("/api/depth").content
|
||||
|
||||
def pointcloud(self, step: int = 4, max_depth_mm: int = 4000) -> np.ndarray:
|
||||
"""Returns Nx6 array of [x,y,z,r,g,b] in camera frame (meters, uint8)."""
|
||||
points, _ = self.pointcloud_with_meta(step=step, max_depth_mm=max_depth_mm)
|
||||
return points
|
||||
|
||||
def pointcloud_with_meta(self, step: int = 4, max_depth_mm: int = 4000
|
||||
) -> tuple[np.ndarray, dict[str, Any]]:
|
||||
"""Full-frame point cloud in Nuwa native frame (X-right, Y-up, Z-back).
|
||||
Payload: {"data": {"points": [[x,y,z,r,g,b],...], "intrinsics":{...}}}.
|
||||
"""
|
||||
return self._fetch_pointcloud("/api/pointcloud",
|
||||
{"step": step, "max_depth_mm": max_depth_mm})
|
||||
|
||||
def pointcloud_roi(self, u_min: int, v_min: int, u_max: int, v_max: int,
|
||||
step: int = 1, max_depth_mm: int = 4000
|
||||
) -> tuple[np.ndarray, dict[str, Any]]:
|
||||
"""Crop-and-fetch: Nuwa computes only the points inside the pixel ROI.
|
||||
|
||||
IMPORTANT: points come back in `[x, -y, -z, r, g, b]` — i.e. OpenCV
|
||||
camera frame (X-right, Y-down, Z-forward). Compatible with the
|
||||
hand-eye matrix `T_base_camera` (also OpenCV-frame).
|
||||
"""
|
||||
return self._fetch_pointcloud("/api/pointcloud/roi", {
|
||||
"u_min": int(u_min), "v_min": int(v_min),
|
||||
"u_max": int(u_max), "v_max": int(v_max),
|
||||
"step": int(step), "max_depth_mm": int(max_depth_mm),
|
||||
})
|
||||
|
||||
def _fetch_pointcloud(self, path: str, params: dict[str, Any]
|
||||
) -> tuple[np.ndarray, dict[str, Any]]:
|
||||
r = self._get(path, params=params)
|
||||
payload = r.json()
|
||||
data = payload.get("data", payload) if isinstance(payload, dict) else payload
|
||||
if isinstance(data, dict):
|
||||
points_raw = data.get("points") or []
|
||||
meta = {k: v for k, v in data.items() if k != "points"}
|
||||
else:
|
||||
points_raw = data
|
||||
meta = {}
|
||||
if not points_raw:
|
||||
return np.zeros((0, 6), dtype=np.float64), meta
|
||||
arr = np.asarray(points_raw, dtype=np.float64)
|
||||
if arr.ndim != 2 or arr.shape[1] < 3:
|
||||
raise NuwaError(f"unexpected pointcloud shape: {arr.shape}")
|
||||
return arr, meta
|
||||
|
||||
def measure(self, u: int, v: int, radius: int = 6) -> dict[str, Any]:
|
||||
"""Back-project pixel (u,v) to 3D in camera frame."""
|
||||
r = self._get("/api/measure", params={"u": u, "v": v, "radius": radius})
|
||||
data = r.json()
|
||||
return data.get("data", data) if isinstance(data, dict) else data
|
||||
@@ -0,0 +1,49 @@
|
||||
"""Client for the YOLO server (port 8879)."""
|
||||
|
||||
from __future__ import annotations
|
||||
|
||||
from typing import Any
|
||||
|
||||
import httpx
|
||||
|
||||
|
||||
class YoloClient:
|
||||
def __init__(self, base_url: str, timeout_s: float = 10.0):
|
||||
self.base_url = base_url.rstrip("/")
|
||||
self.timeout_s = timeout_s
|
||||
|
||||
def detect_url(self, image_url: str, conf: float | None = None) -> dict[str, Any]:
|
||||
data: dict[str, Any] = {"url": image_url}
|
||||
if conf is not None:
|
||||
data["conf"] = str(conf)
|
||||
with httpx.Client(timeout=self.timeout_s) as c:
|
||||
r = c.post(f"{self.base_url}/detect", data=data)
|
||||
r.raise_for_status()
|
||||
return r.json()
|
||||
|
||||
def classes(self) -> list[dict[str, Any]]:
|
||||
"""Returns [{id, name}, ...]. Empty list if yolo-server doesn't expose /classes
|
||||
yet or the model carries no names. Accepts both flat and {success,data} shapes."""
|
||||
with httpx.Client(timeout=self.timeout_s) as c:
|
||||
r = c.get(f"{self.base_url}/classes")
|
||||
if r.status_code == 404:
|
||||
return []
|
||||
r.raise_for_status()
|
||||
payload: Any = r.json()
|
||||
if isinstance(payload, dict) and isinstance(payload.get("data"), dict):
|
||||
payload = payload["data"]
|
||||
items: Any = payload.get("classes") if isinstance(payload, dict) else payload
|
||||
out: list[dict[str, Any]] = []
|
||||
if isinstance(items, dict):
|
||||
# {0: "Base", 1: "Mortaio", ...}
|
||||
items = [{"id": int(k), "name": str(v)} for k, v in items.items()]
|
||||
for i, it in enumerate(items or []):
|
||||
if isinstance(it, str):
|
||||
out.append({"id": i, "name": it})
|
||||
elif isinstance(it, dict):
|
||||
name = it.get("name") or it.get("label")
|
||||
if name is None:
|
||||
continue
|
||||
out.append({"id": int(it.get("id", i)), "name": str(name)})
|
||||
out.sort(key=lambda x: x["id"])
|
||||
return out
|
||||
Reference in new issue
Block a user