PR5071/03 — Robot UR3e amb Visió OAK-D Pro-W

Objectius

Al finalitzar aquesta pràctica hauràs de ser capaç de:

• Configurar la càmera OAK-D Pro-W amb el SDK DepthAI dins d'un contenidor Docker
• Executar detecció d'objectes YOLOv8 directament al xip de la càmera (inferència on-device)
• Obtenir les coordenades 3D (X, Y, Z) d'un objecte detectat mitjançant la profunditat estèreo
• Realitzar una calibració mà-ull simplificada (transformació càmera → base del robot)
• Connectar-se al robot UR3e mitjançant la biblioteca Python ur_rtde (o al simulador URSim)
• Enviar una ordre de moviment al robot cap a la posició 3D calculada
• Personalitzar totes les sortides (fitxers, contenidors, JSON) amb el teu nom i cognom

---

Prerequisits

Requisit	Detall
Temps estimat	6 hores (3 sessions de 2h)
RAM mínima	8 GB (16 GB recomanat amb URSim actiu)
Python	3.11 o superior
Docker	Docker Desktop (Linux containers)
Hardware real	UR3e + OAK-D Pro-W (via USB 3.0)
Mode simulació	URSim e-Series + webcam o fitxer de vídeo

Sense hardware real

Si no disposes del robot UR3e físic ni de la càmera OAK-D Pro-W, pots completar la pràctica en mode simulació:

• Robot: URSim e-Series (contenidor Docker oficial)
• Càmera: webcam USB convencional o fitxer de vídeo .mp4 com a entrada simulada

---

Introducció

La manipulació automatitzada de peces és una de les operacions més comunes a la indústria manufacturera moderna. En entorns d'automatització flexible del 2025, els robots col·laboratius (cobots) han substituït en moltes línies de producció els robots industrials tradicionals, permetent treballar de forma segura al costat dels operaris humans sense necessitat de gàbies de protecció. La tasca de pick-and-place —agafar un objecte d'una posició i deixar-lo en una altra— requereix que el robot "sàpiga" on és l'objecte, i aquí és on entra la visió per computador.

La càmera Luxonis OAK-D Pro-W és un dispositiu de visió estèreo amb inferència neuronal integrada. Disposa de dos sensors de profunditat sincronitzats i un processador neural dedicat (Intel Myriad X) que permet executar models de xarxa neuronal directament al dispositiu, sense necessitat de GPU al PC amfitrió. El SDK DepthAI exposa una API de pipeline en Python que permet combinar fluxos de color, profunditat i detecció d'objectes en temps real. El resultat és que per a cada objecte detectat, podem obtenir directament les seves coordenades tridimensionals en l'espai de la càmera, expressades en mil·límetres.

El robot Universal Robots UR3e és un braç col·laboratiu de 6 eixos amb una càrrega útil de 3 kg i un radi d'acció de 500 mm. Es programa mitjançant el seu propi llenguatge URScript, però exposa una interfície de comunicació en temps real anomenada RTDE (Real-Time Data Exchange) que permet controlar-lo des d'un programa extern via xarxa TCP/IP. La biblioteca Python ur_rtde encapsula aquest protocol i ofereix funcions d'alt nivell per llegir l'estat del robot i enviar-li ordres de moviment. En mode col·laboratiu, el UR3e incorpora sensors de força a totes les articulacions que aturen el moviment si detecten una col·lisió inesperada.

La combinació de visió 3D i robòtica és un dels pilars de la Indústria 4.0. Quan la càmera és capaç de localitzar un objecte en l'espai tridimensional i el robot és capaç de moure's a aquella posició, tenim la base d'un sistema autònom de manipulació. Per fer-ho possible cal resoldre el problema de la calibració mà-ull: determinar la relació geomètrica entre el sistema de coordenades de la càmera i el sistema de coordenades de la base del robot. Aquesta pràctica aborda una versió simplificada d'aquest problema i integra tots els components en un pipeline funcional.

---

Part 1 — Configuració de l'entorn

Estructura del projecte

practica_ur3e_oakd/
├── docker-compose.yml
├── oak-detection/
│   ├── Dockerfile
│   ├── requirements.txt
│   ├── deteccio_objectes_nom_cognom.py
│   └── robot_vision_nom_cognom.py
└── resultats/
    └── resultat_nom_cognom.json

docker-compose.yml

services:

  # Simulador oficial del robot UR3e (e-Series)
  ursim:
    image: universalrobots/ursim_e-series
    container_name: ursim-nom-cognom
    stdin_open: true
    tty: true
    ports:
      - "5900:5900"    # VNC per visualitzar la interfície gràfica del simulador
      - "29999:29999"  # Dashboard Server
      - "30001:30001"  # Primary Interface
      - "30002:30002"  # Secondary Interface
      - "30003:30003"  # Real-Time Interface (RTDE)
      - "30004:30004"  # RTDE (alternatiu)
    environment:
      - ROBOT_MODEL=UR3e
    networks:
      - robot_net
    restart: unless-stopped

  # Servei de detecció d'objectes amb OAK-D i control del robot
  oak-detection:
    build:
      context: ./oak-detection
      dockerfile: Dockerfile
    container_name: oak-nom-cognom
    depends_on:
      - ursim
    environment:
      - ROBOT_IP=ursim          # Nom del servei Docker (resolució DNS interna)
      - ROBOT_PORT=30004
      - ALUMNE=nom-cognom       # Canvia per el teu nom i cognom
      - DISPLAY=${DISPLAY:-:0}  # Necessari per a finestres OpenCV (opcional)
    volumes:
      - ./resultats:/app/resultats
      - ./oak-detection:/app
    # Passthrough del dispositiu USB per a la càmera OAK-D real
    # Comenta aquestes línies si uses mode simulació
    devices:
      - /dev/bus/usb:/dev/bus/usb
    privileged: true
    networks:
      - robot_net
    restart: unless-stopped

networks:
  robot_net:
    driver: bridge

oak-detection/Dockerfile

FROM python:3.11-slim

WORKDIR /app

# Dependències del sistema per a DepthAI i OpenCV
RUN apt-get update && apt-get install -y \
    libusb-1.0-0-dev \
    udev \
    libgl1 \
    libglib2.0-0 \
    && rm -rf /var/lib/apt/lists/*

# Regles udev per a la càmera OAK-D (permisos USB)
RUN echo 'SUBSYSTEM=="usb", ATTRS{idVendor}=="03e7", MODE="0666"' \
    > /etc/udev/rules.d/80-movidius.rules

COPY requirements.txt .
RUN pip install --no-cache-dir -r requirements.txt

COPY . .

CMD ["python", "robot_vision_nom_cognom.py"]

oak-detection/requirements.txt

depthai==2.24.0
ultralytics==8.2.0
ur-rtde==1.5.7
opencv-python-headless==4.9.0.80
numpy==1.26.4

Posada en marxa

# Construir i iniciar tots els serveis
docker compose up --build -d

# Verificar que els contenidors estan en execució
docker compose ps

# Veure els logs del servei de detecció
docker compose logs -f oak-detection

# Aturar tot
docker compose down

Verificació del simulador

Un cop ursim-nom-cognom estigui en marxa, obre un client VNC (com TigerVNC) i connecta't a localhost:5900. Hauràs de veure la interfície gràfica del Polyscope (el sistema operatiu del UR3e). Activa el robot des de la pantalla d'inicialització i posa'l en mode Remote Control des de la barra superior.

---

Part 2 — Detecció d'objectes amb OAK-D i YOLOv8

Concepte del pipeline DepthAI

La OAK-D Pro-W processa tot internament: els dos sensors estèreo calculen el mapa de profunditat, el sensor RGB captura el color, i el xip neuronal executa el model YOLOv8. Tot plegat surt per USB com a fluxos sincronitzats de dades.

Sensor RGB ──────────────────────────────────────────┐
                                                      ▼
Sensor estèreo esquerra ──► Processador estèreo ──► Fusió ──► Detecció amb profunditat (x, y, z mm)
Sensor estèreo dret ──────►                          ▲
                                                      │
                        Xip Intel Myriad X ──► YOLOv8n (inferència on-device)

Script de detecció: deteccio_objectes_nom_cognom.py

#!/usr/bin/env python3
"""
Detecció d'objectes en 3D amb OAK-D Pro-W i YOLOv8
Pràctica PR5071/03 — Mòdul 5071 Models d'IA
Autor: Nom Cognom
"""

import depthai as dai
import numpy as np
import cv2
import json
import time
from pathlib import Path

# ── Configuració general ──────────────────────────────────────────────────────
ALUMNE = "nom-cognom"          # Canvia per el teu nom i cognom (sense espais)
CONFIANCA_MINIMA = 0.5         # Llindar mínim de confiança per acceptar una detecció
OBJECTE_DIANA = "bottle"       # Classe COCO a detectar (pots canviar-ho per "cup", "scissors", etc.)
AMPLADA_IMG = 416              # Resolució d'entrada del model (píxels)
ALCADA_IMG = 416


def crear_pipeline() -> dai.Pipeline:
    """
    Crea i configura el pipeline DepthAI per a:
    - Captura de vídeo RGB
    - Càlcul de profunditat estèreo
    - Detecció d'objectes amb YOLOv8n (o MobileNetSSD com a alternativa)
    Retorna: objecte dai.Pipeline configurat
    """
    pipeline = dai.Pipeline()

    # ── Nodes de càmera ──────────────────────────────────────────────────────
    # Càmera de color (RGB central)
    cam_rgb = pipeline.create(dai.node.ColorCamera)
    cam_rgb.setPreviewSize(AMPLADA_IMG, ALCADA_IMG)
    cam_rgb.setResolution(dai.ColorCameraProperties.SensorResolution.THE_1080_P)
    cam_rgb.setInterleaved(False)
    cam_rgb.setColorOrder(dai.ColorCameraProperties.ColorOrder.BGR)
    cam_rgb.setFps(30)

    # Càmeres monocromàtiques esquerda i dreta per a la profunditat
    cam_esq = pipeline.create(dai.node.MonoCamera)
    cam_esq.setResolution(dai.MonoCameraProperties.SensorResolution.THE_400_P)
    cam_esq.setBoardSocket(dai.CameraBoardSocket.CAM_B)  # Càmera esquerra

    cam_dre = pipeline.create(dai.node.MonoCamera)
    cam_dre.setResolution(dai.MonoCameraProperties.SensorResolution.THE_400_P)
    cam_dre.setBoardSocket(dai.CameraBoardSocket.CAM_C)  # Càmera dreta

    # ── Node de profunditat estèreo ───────────────────────────────────────────
    profunditat = pipeline.create(dai.node.StereoDepth)
    profunditat.setDefaultProfilePreset(dai.node.StereoDepth.PresetMode.HIGH_DENSITY)
    profunditat.setLeftRightCheck(True)    # Millora la qualitat de la profunditat
    profunditat.setSubpixel(True)          # Subpíxel per a major precisió
    profunditat.setDepthAlign(dai.CameraBoardSocket.CAM_A)  # Alinea amb RGB

    # Connectar càmeres mono al node de profunditat
    cam_esq.out.link(profunditat.left)
    cam_dre.out.link(profunditat.right)

    # ── Node de detecció d'objectes (YOLOv8n) ────────────────────────────────
    # Intentem carregar YOLOv8n; si no existeix, usem MobileNetSSD com a fallback
    detector = pipeline.create(dai.node.YoloDetectionNetwork)
    detector.setConfidenceThreshold(CONFIANCA_MINIMA)
    detector.setNumClasses(80)             # COCO: 80 classes
    detector.setCoordinateSize(4)
    detector.setIouThreshold(0.5)
    detector.setNumInferenceThreads(2)
    detector.input.setBlocking(False)

    # Intentar carregar model YOLOv8n en format .blob (optimitzat per Myriad X)
    blob_path = Path("yolov8n_coco_416x416_6shave.blob")
    if blob_path.exists():
        detector.setBlobPath(str(blob_path))
        print(f"[INFO] Carregant model YOLOv8n des de {blob_path}")
    else:
        # Fallback: MobileNetSSD (inclòs amb DepthAI)
        print("[AVÍS] No s'ha trobat yolov8n.blob. Usant MobileNetSSD com a alternativa.")
        detector_mn = pipeline.create(dai.node.MobileNetSpatialDetectionNetwork)
        detector_mn.setBlobPath(
            dai.OpenVINO.getBlobLatestVersion("face-detection-retail-0004")
        )
        detector_mn.setConfidenceThreshold(CONFIANCA_MINIMA)
        detector_mn.setBoundingBoxScaleFactor(0.5)
        detector_mn.setDepthLowerThreshold(100)    # mínim 10 cm
        detector_mn.setDepthUpperThreshold(5000)   # màxim 500 cm

        cam_rgb.preview.link(detector_mn.input)
        profunditat.depth.link(detector_mn.inputDepth)

        # Sortides al host
        sortida_deteccions = pipeline.create(dai.node.XLinkOut)
        sortida_deteccions.setStreamName("deteccions")
        detector_mn.out.link(sortida_deteccions.input)

        sortida_preview = pipeline.create(dai.node.XLinkOut)
        sortida_preview.setStreamName("preview")
        cam_rgb.preview.link(sortida_preview.input)

        return pipeline  # Retorna amb fallback MobileNetSSD

    # ── Node de profunditat espacial per a YOLOv8 ────────────────────────────
    detector_espacial = pipeline.create(dai.node.YoloSpatialDetectionNetwork)
    detector_espacial.setBlobPath(str(blob_path))
    detector_espacial.setConfidenceThreshold(CONFIANCA_MINIMA)
    detector_espacial.setNumClasses(80)
    detector_espacial.setCoordinateSize(4)
    detector_espacial.setIouThreshold(0.5)
    detector_espacial.setBoundingBoxScaleFactor(0.5)
    detector_espacial.setDepthLowerThreshold(100)    # mínim 10 cm de distància
    detector_espacial.setDepthUpperThreshold(5000)   # màxim 500 cm

    # Connectar entrades al detector espacial
    cam_rgb.preview.link(detector_espacial.input)
    profunditat.depth.link(detector_espacial.inputDepth)

    # ── Sortides al host via XLink ────────────────────────────────────────────
    sortida_deteccions = pipeline.create(dai.node.XLinkOut)
    sortida_deteccions.setStreamName("deteccions")
    detector_espacial.out.link(sortida_deteccions.input)

    sortida_profunditat = pipeline.create(dai.node.XLinkOut)
    sortida_profunditat.setStreamName("profunditat")
    detector_espacial.passthroughDepth.link(sortida_profunditat.input)

    sortida_preview = pipeline.create(dai.node.XLinkOut)
    sortida_preview.setStreamName("preview")
    detector_espacial.passthrough.link(sortida_preview.input)

    return pipeline


# Etiquetes de classes COCO (80 classes)
ETIQUETES_COCO = [
    "person", "bicycle", "car", "motorbike", "aeroplane", "bus", "train",
    "truck", "boat", "traffic light", "fire hydrant", "stop sign",
    "parking meter", "bench", "bird", "cat", "dog", "horse", "sheep", "cow",
    "elephant", "bear", "zebra", "giraffe", "backpack", "umbrella", "handbag",
    "tie", "suitcase", "frisbee", "skis", "snowboard", "sports ball", "kite",
    "baseball bat", "baseball glove", "skateboard", "surfboard", "tennis racket",
    "bottle", "wine glass", "cup", "fork", "knife", "spoon", "bowl", "banana",
    "apple", "sandwich", "orange", "broccoli", "carrot", "hot dog", "pizza",
    "donut", "cake", "chair", "sofa", "pottedplant", "bed", "diningtable",
    "toilet", "tvmonitor", "laptop", "mouse", "remote", "keyboard",
    "cell phone", "microwave", "oven", "toaster", "sink", "refrigerator",
    "book", "clock", "vase", "scissors", "teddy bear", "hair drier", "toothbrush"
]


def dibuixar_deteccio(frame: np.ndarray, deteccio, etiquetes: list) -> None:
    """
    Dibuixa el quadre de detecció i la informació 3D sobre el frame.
    frame: imatge BGR de NumPy
    deteccio: objecte de detecció DepthAI amb coordenades espacials
    etiquetes: llista de noms de classe
    """
    h, w = frame.shape[:2]

    # Coordenades del quadre delimitador (normalitzades 0–1 → píxels)
    x1 = int(deteccio.xmin * w)
    y1 = int(deteccio.ymin * h)
    x2 = int(deteccio.xmax * w)
    y2 = int(deteccio.ymax * h)

    # Coordenades 3D en mil·límetres (sistema de referència de la càmera)
    x_mm = deteccio.spatialCoordinates.x
    y_mm = deteccio.spatialCoordinates.y
    z_mm = deteccio.spatialCoordinates.z

    # Etiqueta de classe
    idx_classe = deteccio.label
    nom_classe = etiquetes[idx_classe] if idx_classe < len(etiquetes) else f"classe_{idx_classe}"
    confianca = deteccio.confidence

    # Color del quadre: verd per a l'objecte diana, blau per als altres
    color = (0, 255, 0) if nom_classe == OBJECTE_DIANA else (255, 100, 0)

    # Dibuixar quadre delimitador
    cv2.rectangle(frame, (x1, y1), (x2, y2), color, 2)

    # Text amb classe i confiança
    text_classe = f"{nom_classe}: {confianca:.2f}"
    cv2.putText(frame, text_classe, (x1, y1 - 25),
                cv2.FONT_HERSHEY_SIMPLEX, 0.5, color, 1)

    # Text amb coordenades 3D
    text_posicio = f"X:{x_mm:.0f} Y:{y_mm:.0f} Z:{z_mm:.0f} mm"
    cv2.putText(frame, text_posicio, (x1, y1 - 8),
                cv2.FONT_HERSHEY_SIMPLEX, 0.45, (0, 200, 255), 1)

    # Imprimir a consola
    print(f"  [{nom_classe}] conf={confianca:.3f} | "
          f"X={x_mm:.1f}mm  Y={y_mm:.1f}mm  Z={z_mm:.1f}mm")


def executar_deteccio() -> dict | None:
    """
    Executa el bucle de detecció en temps real.
    Retorna un diccionari amb la millor detecció de l'objecte diana, o None.
    """
    pipeline = crear_pipeline()
    millor_deteccio = None

    print(f"\n[PR5071/03] Iniciant detecció — Alumne: {ALUMNE}")
    print(f"[INFO] Cercant objecte: '{OBJECTE_DIANA}'")
    print("[INFO] Prem 'q' per sortir, 's' per guardar la detecció actual\n")

    with dai.Device(pipeline) as dispositiu:
        # Obtenir les cues de sortida
        cua_deteccions = dispositiu.getOutputQueue(
            name="deteccions", maxSize=4, blocking=False
        )
        cua_preview = dispositiu.getOutputQueue(
            name="preview", maxSize=4, blocking=False
        )

        while True:
            # Obtenir frames
            frame_dades = cua_preview.tryGet()
            deteccions_dades = cua_deteccions.tryGet()

            if frame_dades is not None:
                frame = frame_dades.getCvFrame()

                # Afegir capçalera amb nom de l'alumne
                cv2.putText(frame, f"PR5071/03 — {ALUMNE}", (10, 20),
                            cv2.FONT_HERSHEY_SIMPLEX, 0.6, (255, 255, 255), 1)

                if deteccions_dades is not None:
                    deteccions = deteccions_dades.detections

                    for det in deteccions:
                        idx = det.label
                        nom = ETIQUETES_COCO[idx] if idx < len(ETIQUETES_COCO) else "?"
                        dibuixar_deteccio(frame, det, ETIQUETES_COCO)

                        # Guardar la millor detecció de l'objecte diana
                        if nom == OBJECTE_DIANA:
                            if (millor_deteccio is None or
                                    det.confidence > millor_deteccio.confidence):
                                millor_deteccio = det

                # Mostrar el frame
                cv2.imshow(f"OAK-D Detecció — {ALUMNE}", frame)

            # Control de tecles
            tecla = cv2.waitKey(1) & 0xFF
            if tecla == ord('q'):
                break
            elif tecla == ord('s') and millor_deteccio is not None:
                print(f"\n[OK] Detecció guardada: {OBJECTE_DIANA}")
                break

    cv2.destroyAllWindows()

    if millor_deteccio is not None:
        return {
            "classe": ETIQUETES_COCO[millor_deteccio.label],
            "confianca": float(millor_deteccio.confidence),
            "x_cam_mm": float(millor_deteccio.spatialCoordinates.x),
            "y_cam_mm": float(millor_deteccio.spatialCoordinates.y),
            "z_cam_mm": float(millor_deteccio.spatialCoordinates.z),
        }
    return None


if __name__ == "__main__":
    resultat = executar_deteccio()
    if resultat:
        print(f"\n[RESULTAT] Objecte detectat: {resultat}")
    else:
        print(f"\n[AVÍS] No s'ha detectat cap '{OBJECTE_DIANA}'")

Executar amb passthrough USB (hardware real)

# Executar directament amb accés al dispositiu USB de la càmera
docker run --rm -it \
  --device=/dev/bus/usb \
  --privileged \
  -v $(pwd)/resultats:/app/resultats \
  oak-nom-cognom \
  python deteccio_objectes_nom_cognom.py

---

Part 3 — Calibració mà-ull simplificada

Concepte de calibració mà-ull

La calibració mà-ull (hand-eye calibration) determina la relació geomètrica entre el sistema de coordenades de la càmera i el sistema de coordenades del robot. Existeixen dues configuracions:

Configuració	Descripció	Quan s'usa
Eye-in-hand	Càmera muntada al canell del robot	Quan la càmera es mou amb el robot
Eye-to-hand	Càmera fixa, independent del robot	Quan la càmera observa l'espai de treball des de fora

En aquesta pràctica usem la configuració eye-to-hand: la càmera OAK-D està fixada en un suport extern i observa la taula de treball on opera el robot.

Mètode simplificat: 3 punts de calibració

En lloc d'usar el mètode complet de Tsai-Lenz (que requereix moure el robot a múltiples posicions), usarem 3 punts de calibració coneguts: posicions de l'espai de treball de les quals coneixem tant les coordenades en el sistema de la càmera com les coordenades en el sistema base del robot.

#!/usr/bin/env python3
"""
Calibració mà-ull simplificada — eye-to-hand
Transformació del sistema de coordenades de la càmera al sistema base del robot UR3e
Pràctica PR5071/03 — Mòdul 5071 Models d'IA
Autor: Nom Cognom
"""

import numpy as np


# ── Punts de calibració ───────────────────────────────────────────────────────
# Cada punt és una posició coneguda de l'espai de treball.
# Per obtenir-los: col·loca una marca (p.ex. una boleta de color) a cada posició,
# llegeix les coordenades de la càmera (x_cam, y_cam, z_cam) en mm,
# i mesura o llegeix la posició real en coordenades de la base del robot (x_rob, y_rob, z_rob) en metres.

# Coordenades en el sistema de la càmera (mm)
PUNTS_CAMERA_MM = np.array([
    [-150.0,  80.0, 520.0],   # Punt de calibració 1 (esquerra, prop)
    [  10.0,  80.0, 580.0],   # Punt de calibració 2 (centre, mig)
    [ 160.0,  80.0, 490.0],   # Punt de calibració 3 (dreta, prop)
], dtype=np.float64)

# Coordenades en el sistema base del robot (metres, convenció UR)
PUNTS_ROBOT_M = np.array([
    [0.250, -0.180, 0.050],   # Punt de calibració 1 → base robot
    [0.350,  0.000, 0.050],   # Punt de calibració 2 → base robot
    [0.250,  0.200, 0.050],   # Punt de calibració 3 → base robot
], dtype=np.float64)


def calcular_transformacio_cam_robot(
    punts_cam: np.ndarray,
    punts_robot: np.ndarray
) -> np.ndarray:
    """
    Calcula la matriu de transformació homogènia 4x4 que transforma
    coordenades del sistema de la càmera al sistema base del robot.

    Mètode: mínims quadrats via pseudo-inversa (SVD).
    punts_cam: array (N, 3) en mm
    punts_robot: array (N, 3) en metres
    Retorna: matriu 4x4 de transformació homogènia
    """
    # Convertir les coordenades de la càmera de mm a metres
    punts_cam_m = punts_cam / 1000.0

    # Calcular els centroides de cada conjunt de punts
    centroide_cam = punts_cam_m.mean(axis=0)
    centroide_robot = punts_robot.mean(axis=0)

    # Centrar els punts al voltant dels seus centroides
    cam_centrat = punts_cam_m - centroide_cam
    robot_centrat = punts_robot - centroide_robot

    # Descomposició SVD per calcular la matriu de rotació òptima
    H = cam_centrat.T @ robot_centrat
    U, S, Vt = np.linalg.svd(H)
    R = Vt.T @ U.T

    # Correcció si la matriu de rotació té reflexió (det = -1)
    if np.linalg.det(R) < 0:
        Vt[-1, :] *= -1
        R = Vt.T @ U.T

    # Calcular la translació
    t = centroide_robot - R @ centroide_cam

    # Construir la matriu homogènia 4x4
    T = np.eye(4)
    T[:3, :3] = R
    T[:3, 3] = t

    return T


def transformar_coordenades(
    pos_cam_mm: np.ndarray,
    T_cam2robot: np.ndarray
) -> np.ndarray:
    """
    Transforma una posició del sistema de la càmera al sistema base del robot.
    pos_cam_mm: array (3,) amb coordenades [x, y, z] en mm
    T_cam2robot: matriu de transformació 4x4
    Retorna: array (3,) amb coordenades [x, y, z] en metres (sistema robot)
    """
    # Convertir a metres i a coordenades homogènies
    pos_cam_m = pos_cam_mm / 1000.0
    pos_hom = np.array([pos_cam_m[0], pos_cam_m[1], pos_cam_m[2], 1.0])

    # Aplicar la transformació
    pos_robot_hom = T_cam2robot @ pos_hom

    # Retornar les coordenades cartesianes (sense el component homogeni)
    return pos_robot_hom[:3]


def validar_calibracio(
    T_cam2robot: np.ndarray,
    punts_cam: np.ndarray,
    punts_robot: np.ndarray
) -> float:
    """
    Calcula l'error de reprojeccció (RMS) de la calibració en mm.
    Retorna: error RMS en mm
    """
    errors = []
    for p_cam, p_robot_real in zip(punts_cam, punts_robot):
        p_robot_calc = transformar_coordenades(p_cam, T_cam2robot)
        error_m = np.linalg.norm(p_robot_calc - p_robot_real)
        errors.append(error_m * 1000.0)  # Convertir a mm

    rms = float(np.sqrt(np.mean(np.array(errors) ** 2)))
    return rms


# ── Execució principal ────────────────────────────────────────────────────────
if __name__ == "__main__":
    print("[PR5071/03] Calibració mà-ull simplificada (eye-to-hand)")
    print("=" * 60)

    # Calcular la matriu de transformació
    T_cam2robot = calcular_transformacio_cam_robot(
        PUNTS_CAMERA_MM, PUNTS_ROBOT_M
    )

    print("\nMatriu de transformació càmera → robot (T_cam2robot):")
    print(np.array2string(T_cam2robot, precision=6, suppress_small=True))

    # Validar la calibració
    error_rms = validar_calibracio(T_cam2robot, PUNTS_CAMERA_MM, PUNTS_ROBOT_M)
    print(f"\nError RMS de calibració: {error_rms:.2f} mm")

    if error_rms < 5.0:
        print("[OK] Calibració acceptable (error < 5 mm)")
    else:
        print("[AVÍS] Error de calibració alt. Reviseu els punts de referència.")

    # Exemple de transformació d'una posició detectada
    posicio_camera = np.array([25.0, 90.0, 550.0])  # mm (sistema càmera)
    posicio_robot = transformar_coordenades(posicio_camera, T_cam2robot)
    print(f"\nExemple de transformació:")
    print(f"  Posició càmera:  X={posicio_camera[0]:.1f}mm "
          f"Y={posicio_camera[1]:.1f}mm Z={posicio_camera[2]:.1f}mm")
    print(f"  Posició robot:   X={posicio_robot[0]:.4f}m "
          f"Y={posicio_robot[1]:.4f}m Z={posicio_robot[2]:.4f}m")

Com obtenir els punts de calibració

1. Imprimeix o col·loca 3 marques de color a posicions conegudes de la taula de treball.
2. Per cada marca: llegeix les coordenades (x, y, z) en mm que reporta el pipeline DepthAI.
3. Porta manualment el robot (mode freedrive o teach pendant) fins a tocar cada marca.
4. Llegeix la posició cartesiana del TCP del robot (via Polyscope o ur_rtde).
5. Omple els arrays PUNTS_CAMERA_MM i PUNTS_ROBOT_M amb els valors mesurats.

---

Part 4 — Connexió i control del UR3e

Prerequisits del robot

Abans de connectar-se via RTDE, assegura't que:

1. El robot (o URSim) està en marxa i inicialitzat
2. El robot està en mode Remote Control (barra superior del Polyscope → icona de l'ordinador)
3. El programa "External Control" o un programa buit està carregat i en mode play
4. La IP del robot és accessible (ping 192.168.56.101 per a URSim)

Script de control: connexió i moviment

#!/usr/bin/env python3
"""
Control del robot UR3e via ur_rtde
Pràctica PR5071/03 — Mòdul 5071 Models d'IA
Autor: Nom Cognom
"""

import rtde_control
import rtde_receive
import rtde_io
import numpy as np
import time

# ── Configuració de connexió ──────────────────────────────────────────────────
IP_ROBOT = "192.168.56.101"   # IP del robot (o contenidor URSim)
VELOCITAT = 0.1               # m/s — velocitat baixa per seguretat
ACCELERACIO = 0.05            # m/s² — acceleració suau

# Posició d'inici segura (posició "home") — [x, y, z, rx, ry, rz] en metres i radians
# rx, ry, rz: vector de rotació (eix-angle, format UR)
POSICIO_HOME = [0.300, 0.000, 0.300, np.pi, 0.0, 0.0]

# Altura de seguretat per als moviments de translació (metres)
ALCADA_SEGURA = 0.250


def connectar_robot(ip: str) -> tuple:
    """
    Estableix connexió RTDE amb el robot UR3e.
    Retorna: (rtde_control, rtde_receive) o llança excepció si falla.
    """
    print(f"[INFO] Connectant al robot: {ip} ...")

    try:
        # Interfície de control (enviar ordres)
        control = rtde_control.RTDEControlInterface(ip)
        # Interfície de recepció (llegir estat)
        recepcio = rtde_receive.RTDEReceiveInterface(ip)

        print("[OK] Connexió RTDE establerta correctament")
        print(f"     Versió del controlador: {recepcio.getRobotMode()}")

        return control, recepcio

    except Exception as e:
        print(f"[ERROR] No s'ha pogut connectar al robot: {e}")
        print("[CONSELL] Verifica que el robot és en Remote Control i que la IP és correcta.")
        raise


def llegir_estat_robot(recepcio: rtde_receive.RTDEReceiveInterface) -> dict:
    """
    Llegeix l'estat actual del robot (posició joints i TCP).
    Retorna: diccionari amb l'estat
    """
    # Posició de les 6 articulacions en radians
    angles_joints = recepcio.getActualQ()
    # Posició cartesiana del TCP [x, y, z, rx, ry, rz]
    posicio_tcp = recepcio.getActualTCPPose()
    # Velocitat actual del TCP
    velocitat_tcp = recepcio.getActualTCPSpeed()
    # Mode del robot (0=Disconnected, 7=Running, etc.)
    mode_robot = recepcio.getRobotMode()

    estat = {
        "angles_joints_rad": angles_joints,
        "posicio_tcp_m": posicio_tcp,
        "velocitat_tcp": velocitat_tcp,
        "mode": mode_robot,
    }

    print(f"\n[ESTAT ROBOT]")
    print(f"  Mode: {mode_robot}")
    print(f"  TCP:  X={posicio_tcp[0]:.4f}m  Y={posicio_tcp[1]:.4f}m  Z={posicio_tcp[2]:.4f}m")
    print(f"  Joints (º): " +
          "  ".join(f"J{i+1}={np.degrees(q):.1f}" for i, q in enumerate(angles_joints)))

    return estat


def moure_a_posicio(
    control: rtde_control.RTDEControlInterface,
    recepcio: rtde_receive.RTDEReceiveInterface,
    posicio_objectiu: list,
    velocitat: float = VELOCITAT,
    acceleracio: float = ACCELERACIO
) -> bool:
    """
    Mou el robot fins a la posició cartesiana especificada (moveL).
    posicio_objectiu: [x, y, z, rx, ry, rz] en metres i radians
    Retorna: True si el moviment s'ha completat, False si hi ha hagut error.
    """
    # Verificació de seguretat: la posició Z ha de ser >= 0 (sobre la taula)
    if posicio_objectiu[2] < 0.02:
        print(f"[SEGURETAT] La posició Z={posicio_objectiu[2]:.3f}m és massa baixa. Moviment cancel·lat.")
        return False

    # Verificació de l'espai de treball del UR3e (radi màxim ~500mm des de la base)
    dist_horitzontal = np.sqrt(posicio_objectiu[0]**2 + posicio_objectiu[1]**2)
    if dist_horitzontal > 0.48:
        print(f"[SEGURETAT] Posició fora del radi de treball ({dist_horitzontal*1000:.0f}mm > 480mm).")
        return False

    print(f"\n[MOVIMENT] Movent a: "
          f"X={posicio_objectiu[0]:.4f}m  "
          f"Y={posicio_objectiu[1]:.4f}m  "
          f"Z={posicio_objectiu[2]:.4f}m")
    print(f"  Velocitat: {velocitat} m/s  |  Acceleració: {acceleracio} m/s²")

    try:
        # moveL: moviment lineal en l'espai cartesià
        control.moveL(posicio_objectiu, velocitat, acceleracio)

        # Llegir i mostrar la posició final
        time.sleep(0.2)
        posicio_final = recepcio.getActualTCPPose()
        print(f"[OK] Posició assolida: "
              f"X={posicio_final[0]:.4f}m  "
              f"Y={posicio_final[1]:.4f}m  "
              f"Z={posicio_final[2]:.4f}m")
        return True

    except Exception as e:
        print(f"[ERROR] El moviment ha fallat: {e}")
        return False


def anar_a_home(
    control: rtde_control.RTDEControlInterface,
    recepcio: rtde_receive.RTDEReceiveInterface
) -> bool:
    """Porta el robot a la posició d'inici segura (home)."""
    print("\n[HOME] Movent a posició d'inici...")
    return moure_a_posicio(control, recepcio, POSICIO_HOME)


def apropar_i_baixar(
    control: rtde_control.RTDEControlInterface,
    recepcio: rtde_receive.RTDEReceiveInterface,
    posicio_objectiu: list,
    alcada_aproximacio: float = 0.10
) -> bool:
    """
    Moviment de seguretat en dues fases:
    1. Moure't per sobre de l'objecte (posició d'aproximació)
    2. Baixar fins a l'objectiu
    posicio_objectiu: [x, y, z, rx, ry, rz]
    alcada_aproximacio: distància per sobre de l'objectiu per a la fase d'aproximació (m)
    """
    # Fase 1: posició d'aproximació (per sobre de l'objectiu)
    posicio_aprox = posicio_objectiu.copy()
    posicio_aprox[2] = posicio_objectiu[2] + alcada_aproximacio

    print(f"\n[APROXIMACIÓ] Fase 1: moure's per sobre de l'objectiu")
    ok1 = moure_a_posicio(control, recepcio, posicio_aprox)
    if not ok1:
        return False

    # Fase 2: baixar fins a l'objectiu (velocitat reduïda)
    print(f"[APROXIMACIÓ] Fase 2: baixar fins a l'objectiu")
    ok2 = moure_a_posicio(control, recepcio, posicio_objectiu,
                          velocitat=0.05, acceleracio=0.02)
    return ok2


# ── Exemple d'ús complet ──────────────────────────────────────────────────────
if __name__ == "__main__":
    print("[PR5071/03] Control del UR3e via RTDE")
    print("=" * 60)

    # Connectar al robot
    control, recepcio = connectar_robot(IP_ROBOT)

    try:
        # Llegir estat inicial
        llegir_estat_robot(recepcio)

        # Moure a posició home
        anar_a_home(control, recepcio)
        time.sleep(1.0)

        # Exemple: moure a una posició de prova per damunt de la taula
        posicio_prova = [0.350, 0.100, 0.150, np.pi, 0.0, 0.0]
        print("\n[PROVA] Movent a posició de prova...")
        apropar_i_baixar(control, recepcio, posicio_prova)
        time.sleep(1.0)

        # Tornar a home
        anar_a_home(control, recepcio)

    finally:
        # Alliberar la connexió RTDE correctament
        control.stopScript()
        print("\n[INFO] Connexió RTDE tancada.")

---

Part 5 — Integració completa

Pipeline complet: visió → transformació → robot

flowchart TD
    A([Inici del sistema]) --> B[Inicialitzar càmera OAK-D<br/>Pipeline DepthAI + YOLOv8]
    B --> C[Connectar al robot UR3e<br/>via RTDE]
    C --> D[Moure robot a posició HOME]
    D --> E[Capturar frame de la càmera]
    E --> F{Objecte diana<br/>detectat?}
    F -- No --> E
    F -- Sí --> G[Obtenir coordenades 3D<br/>X, Y, Z en mm — sistema càmera]
    G --> H{Confiança ><br/>llindar mínim?}
    H -- No --> E
    H -- Sí --> I[Aplicar transformació<br/>T_cam2robot → coordenades robot]
    I --> J[Verificació de seguretat<br/>Z > 0.02m, dist < 480mm]
    J -- Fora de rang --> K[Registrar avís<br/>i continuar capturant]
    K --> E
    J -- OK --> L[Moure robot per sobre<br/>de l'objectiu — aproximació]
    L --> M[Baixar fins a<br/>la posició objectiu]
    M --> N[Guardar resultat JSON<br/>resultat_nom_cognom.json]
    N --> O[Tornar a HOME]
    O --> P([Fi])

Script d'integració: robot_vision_nom_cognom.py

#!/usr/bin/env python3
"""
Script d'integració completa: Visió OAK-D + Control UR3e
Pràctica PR5071/03 — Mòdul 5071 Models d'IA
Autor: Nom Cognom
Data: 2025
"""

import depthai as dai
import numpy as np
import cv2
import json
import time
import os
from pathlib import Path
from datetime import datetime

# Importar la lògica de control del robot (del script de la Part 4)
import rtde_control
import rtde_receive

# ── Paràmetres globals ────────────────────────────────────────────────────────
ALUMNE = os.environ.get("ALUMNE", "nom-cognom")   # Llegit de la variable d'entorn Docker
IP_ROBOT = os.environ.get("ROBOT_IP", "192.168.56.101")

OBJECTE_DIANA = "bottle"       # Classe COCO a detectar
CONFIANCA_MINIMA = 0.60        # Llindar de confiança (60%)
VELOCITAT_ROBOT = 0.08         # m/s — velocitat segura per a l'entorn de laboratori
ACCELERACIO_ROBOT = 0.04       # m/s²

# Orientació del TCP durant els moviments (apunta cap avall, vertical)
ORIENTACIO_TCP = [np.pi, 0.0, 0.0]

# Directori de sortida per als resultats
DIR_RESULTATS = Path("/app/resultats")
DIR_RESULTATS.mkdir(parents=True, exist_ok=True)

# Matriu de transformació càmera → robot (calculada a la Part 3)
# Substitueix amb la teva matriu calibrada!
T_CAM2ROBOT = np.array([
    [ 0.9998, -0.0105,  0.0174,  0.3512],
    [ 0.0108,  0.9998, -0.0152, -0.0023],
    [-0.0172,  0.0154,  0.9997,  0.0481],
    [ 0.0000,  0.0000,  0.0000,  1.0000],
], dtype=np.float64)


# ── Etiquetes COCO ────────────────────────────────────────────────────────────
ETIQUETES_COCO = [
    "person", "bicycle", "car", "motorbike", "aeroplane", "bus", "train",
    "truck", "boat", "traffic light", "fire hydrant", "stop sign",
    "parking meter", "bench", "bird", "cat", "dog", "horse", "sheep", "cow",
    "elephant", "bear", "zebra", "giraffe", "backpack", "umbrella", "handbag",
    "tie", "suitcase", "frisbee", "skis", "snowboard", "sports ball", "kite",
    "baseball bat", "baseball glove", "skateboard", "surfboard", "tennis racket",
    "bottle", "wine glass", "cup", "fork", "knife", "spoon", "bowl", "banana",
    "apple", "sandwich", "orange", "broccoli", "carrot", "hot dog", "pizza",
    "donut", "cake", "chair", "sofa", "pottedplant", "bed", "diningtable",
    "toilet", "tvmonitor", "laptop", "mouse", "remote", "keyboard",
    "cell phone", "microwave", "oven", "toaster", "sink", "refrigerator",
    "book", "clock", "vase", "scissors", "teddy bear", "hair drier", "toothbrush"
]


# ── Funcions auxiliars ────────────────────────────────────────────────────────

def transformar_cam_a_robot(pos_cam_mm: np.ndarray) -> np.ndarray:
    """Aplica la transformació T_CAM2ROBOT i retorna la posició en metres."""
    pos_m = pos_cam_mm / 1000.0
    pos_hom = np.array([pos_m[0], pos_m[1], pos_m[2], 1.0])
    pos_robot = T_CAM2ROBOT @ pos_hom
    return pos_robot[:3]


def verificar_posicio_segura(pos: np.ndarray) -> bool:
    """
    Comprova que la posició objectiu és dins dels límits de seguretat del UR3e.
    Retorna True si és segura, False si cal rebutjar el moviment.
    """
    # Altura mínima per sobre de la taula
    if pos[2] < 0.02:
        print(f"[SEGURETAT] Z={pos[2]:.3f}m — massa proper a la taula")
        return False
    # Radi màxim horitzontal del UR3e (500mm de rang, usem 480mm per marge)
    radi = np.sqrt(pos[0]**2 + pos[1]**2)
    if radi > 0.48:
        print(f"[SEGURETAT] Radi={radi*1000:.0f}mm — fora del rang de treball")
        return False
    return True


def crear_pipeline_deteccio() -> dai.Pipeline:
    """
    Crea el pipeline DepthAI per a detecció espacial YOLOv8.
    Versió simplificada per a la integració.
    """
    pipeline = dai.Pipeline()

    cam_rgb = pipeline.create(dai.node.ColorCamera)
    cam_rgb.setPreviewSize(416, 416)
    cam_rgb.setResolution(dai.ColorCameraProperties.SensorResolution.THE_1080_P)
    cam_rgb.setInterleaved(False)
    cam_rgb.setColorOrder(dai.ColorCameraProperties.ColorOrder.BGR)

    cam_esq = pipeline.create(dai.node.MonoCamera)
    cam_esq.setResolution(dai.MonoCameraProperties.SensorResolution.THE_400_P)
    cam_esq.setBoardSocket(dai.CameraBoardSocket.CAM_B)

    cam_dre = pipeline.create(dai.node.MonoCamera)
    cam_dre.setResolution(dai.MonoCameraProperties.SensorResolution.THE_400_P)
    cam_dre.setBoardSocket(dai.CameraBoardSocket.CAM_C)

    profunditat = pipeline.create(dai.node.StereoDepth)
    profunditat.setDefaultProfilePreset(dai.node.StereoDepth.PresetMode.HIGH_DENSITY)
    profunditat.setLeftRightCheck(True)
    profunditat.setDepthAlign(dai.CameraBoardSocket.CAM_A)

    cam_esq.out.link(profunditat.left)
    cam_dre.out.link(profunditat.right)

    detector = pipeline.create(dai.node.YoloSpatialDetectionNetwork)
    detector.setBlobPath("yolov8n_coco_416x416_6shave.blob")
    detector.setConfidenceThreshold(CONFIANCA_MINIMA)
    detector.setNumClasses(80)
    detector.setCoordinateSize(4)
    detector.setIouThreshold(0.5)
    detector.setBoundingBoxScaleFactor(0.5)
    detector.setDepthLowerThreshold(100)
    detector.setDepthUpperThreshold(5000)

    cam_rgb.preview.link(detector.input)
    profunditat.depth.link(detector.inputDepth)

    sortida_det = pipeline.create(dai.node.XLinkOut)
    sortida_det.setStreamName("deteccions")
    detector.out.link(sortida_det.input)

    sortida_prev = pipeline.create(dai.node.XLinkOut)
    sortida_prev.setStreamName("preview")
    detector.passthrough.link(sortida_prev.input)

    return pipeline


def detectar_objecte_diana(
    dispositiu: dai.Device,
    max_intents: int = 60
) -> dict | None:
    """
    Cerca l'objecte diana en els frames de la càmera.
    Espera fins a max_intents frames abans de rendir-se.
    Retorna: diccionari amb la detecció o None.
    """
    cua_det = dispositiu.getOutputQueue("deteccions", maxSize=4, blocking=False)
    cua_prev = dispositiu.getOutputQueue("preview", maxSize=4, blocking=False)

    millor = None
    intents = 0

    print(f"[DETECCIÓ] Cercant '{OBJECTE_DIANA}' (màx {max_intents} frames)...")

    while intents < max_intents:
        frame_dades = cua_prev.tryGet()
        det_dades = cua_det.tryGet()

        if det_dades is not None:
            for det in det_dades.detections:
                nom = ETIQUETES_COCO[det.label] if det.label < len(ETIQUETES_COCO) else "?"
                if nom == OBJECTE_DIANA and det.confidence >= CONFIANCA_MINIMA:
                    if millor is None or det.confidence > millor["confianca"]:
                        millor = {
                            "classe": nom,
                            "confianca": float(det.confidence),
                            "x_cam_mm": float(det.spatialCoordinates.x),
                            "y_cam_mm": float(det.spatialCoordinates.y),
                            "z_cam_mm": float(det.spatialCoordinates.z),
                        }
                        print(f"  [OK] '{nom}' detectat amb confiança {det.confidence:.3f} "
                              f"— Z={det.spatialCoordinates.z:.0f}mm")

        if frame_dades is not None:
            frame = frame_dades.getCvFrame()
            cv2.putText(frame, f"Cercant: {OBJECTE_DIANA} — {ALUMNE}",
                        (10, 20), cv2.FONT_HERSHEY_SIMPLEX, 0.6, (0, 255, 0), 1)
            cv2.imshow("PR5071/03 — Detecció", frame)
            if cv2.waitKey(1) == ord('q'):
                break

        intents += 1

    cv2.destroyAllWindows()
    return millor


def guardar_resultat(
    deteccio: dict,
    pos_cam: np.ndarray,
    pos_robot: np.ndarray,
    posicio_tcp_final: list
) -> Path:
    """
    Guarda el resultat de la pràctica en un fitxer JSON amb el nom de l'alumne.
    Retorna: ruta al fitxer guardat.
    """
    nom_fitxer = DIR_RESULTATS / f"resultat_{ALUMNE}.json"

    resultat = {
        "metadades": {
            "alumne": ALUMNE,
            "practica": "PR5071/03",
            "modul": "5071",
            "timestamp": datetime.now().isoformat(),
        },
        "deteccio": {
            "classe": deteccio["classe"],
            "confianca": deteccio["confianca"],
            "coordenades_camera_mm": {
                "x": deteccio["x_cam_mm"],
                "y": deteccio["y_cam_mm"],
                "z": deteccio["z_cam_mm"],
            },
        },
        "transformacio": {
            "coordenades_robot_m": {
                "x": float(pos_robot[0]),
                "y": float(pos_robot[1]),
                "z": float(pos_robot[2]),
            },
            "matriu_T_cam2robot": T_CAM2ROBOT.tolist(),
        },
        "robot": {
            "ip": IP_ROBOT,
            "posicio_tcp_final": posicio_tcp_final,
            "velocitat_ms": VELOCITAT_ROBOT,
            "acceleracio_ms2": ACCELERACIO_ROBOT,
        },
    }

    with open(nom_fitxer, "w", encoding="utf-8") as f:
        json.dump(resultat, f, ensure_ascii=False, indent=2)

    print(f"\n[RESULTAT] Fitxer guardat: {nom_fitxer}")
    return nom_fitxer


# ── Pipeline principal ────────────────────────────────────────────────────────

def executar_pipeline_complet() -> None:
    """
    Funció principal que orquestra tot el pipeline:
    1. Inicialitza la càmera OAK-D
    2. Connecta al robot UR3e
    3. Porta el robot a HOME
    4. Detecta l'objecte diana
    5. Transforma coordenades càmera → robot
    6. Mou el robot cap a l'objectiu
    7. Guarda el resultat JSON
    8. Torna a HOME
    """
    print(f"\n{'='*60}")
    print(f"  PR5071/03 — Integració Visió + Robòtica")
    print(f"  Alumne: {ALUMNE}")
    print(f"  Robot:  {IP_ROBOT}")
    print(f"  Objecte diana: '{OBJECTE_DIANA}'")
    print(f"{'='*60}\n")

    # ── Pas 1: Connectar al robot ─────────────────────────────────────────────
    print("[PAS 1] Connectant al robot...")
    try:
        robot_ctrl = rtde_control.RTDEControlInterface(IP_ROBOT)
        robot_recv = rtde_receive.RTDEReceiveInterface(IP_ROBOT)
        print("[OK] Robot connectat")
    except Exception as e:
        print(f"[ERROR CRÍTIC] No es pot connectar al robot: {e}")
        print("  → Verifica que URSim (o el robot) estigui en Remote Control")
        return

    try:
        # ── Pas 2: Moure a HOME ───────────────────────────────────────────────
        print("\n[PAS 2] Movent a posició HOME...")
        posicio_home = POSICIO_HOME = [0.300, 0.000, 0.300] + ORIENTACIO_TCP
        robot_ctrl.moveL(posicio_home, VELOCITAT_ROBOT, ACCELERACIO_ROBOT)
        time.sleep(0.5)
        print("[OK] Robot a HOME")

        # ── Pas 3: Inicialitzar càmera i detectar ─────────────────────────────
        print("\n[PAS 3] Inicialitzant càmera OAK-D i detectant objecte...")
        pipeline = crear_pipeline_deteccio()

        with dai.Device(pipeline) as dispositiu:
            deteccio = detectar_objecte_diana(dispositiu, max_intents=90)

        if deteccio is None:
            print(f"\n[AVÍS] No s'ha detectat '{OBJECTE_DIANA}'. Aturant el pipeline.")
            return

        print(f"\n[OK] Objecte detectat: {deteccio['classe']} "
              f"(confiança: {deteccio['confianca']:.3f})")

        # ── Pas 4: Transformar coordenades ────────────────────────────────────
        print("\n[PAS 4] Transformant coordenades càmera → robot...")
        pos_cam = np.array([
            deteccio["x_cam_mm"],
            deteccio["y_cam_mm"],
            deteccio["z_cam_mm"],
        ])
        pos_robot = transformar_cam_a_robot(pos_cam)
        print(f"  Càmera:  X={pos_cam[0]:.1f}mm  Y={pos_cam[1]:.1f}mm  Z={pos_cam[2]:.1f}mm")
        print(f"  Robot:   X={pos_robot[0]:.4f}m  Y={pos_robot[1]:.4f}m  Z={pos_robot[2]:.4f}m")

        # ── Pas 5: Verificació de seguretat i moviment ────────────────────────
        print("\n[PAS 5] Verificació de seguretat i moviment cap a l'objectiu...")

        # Afegir una petita alçada per no xocar amb l'objecte directament
        pos_objectiu = [
            pos_robot[0],
            pos_robot[1],
            pos_robot[2] + 0.05,   # 5 cm per sobre de l'objecte
        ] + ORIENTACIO_TCP

        if not verificar_posicio_segura(np.array(pos_objectiu[:3])):
            print("[AVÍS] Posició fora de rang. Moviment cancel·lat.")
            return

        # Moviment en dues fases: aproximació + descens
        pos_aprox = pos_objectiu.copy()
        pos_aprox[2] += 0.10    # 10 cm per sobre per a la fase d'aproximació

        print("  → Fase d'aproximació...")
        robot_ctrl.moveL(pos_aprox, VELOCITAT_ROBOT, ACCELERACIO_ROBOT)
        time.sleep(0.3)

        print("  → Descens fins a l'objectiu...")
        robot_ctrl.moveL(pos_objectiu, VELOCITAT_ROBOT * 0.5, ACCELERACIO_ROBOT * 0.5)
        time.sleep(0.5)

        # Llegir la posició TCP final real
        posicio_tcp_final = list(robot_recv.getActualTCPPose())
        print(f"[OK] Posició TCP final: {[f'{v:.4f}' for v in posicio_tcp_final]}")

        # ── Pas 6: Guardar resultat ───────────────────────────────────────────
        print("\n[PAS 6] Guardant resultat...")
        fitxer = guardar_resultat(deteccio, pos_cam, pos_robot, posicio_tcp_final)
        print(f"[OK] Resultat guardat a: {fitxer}")

        # ── Pas 7: Tornar a HOME ──────────────────────────────────────────────
        print("\n[PAS 7] Tornant a posició HOME...")
        time.sleep(0.5)
        robot_ctrl.moveL(posicio_home, VELOCITAT_ROBOT, ACCELERACIO_ROBOT)
        print("[OK] Robot a HOME. Pipeline completat!")

    finally:
        # Tancar la connexió RTDE de forma segura
        robot_ctrl.stopScript()
        print("\n[INFO] Connexió RTDE tancada correctament.")


if __name__ == "__main__":
    executar_pipeline_complet()

Estructura del fitxer de resultat

El fitxer resultat_nom-cognom.json tindrà aquesta estructura:

{
  "metadades": {
    "alumne": "nom-cognom",
    "practica": "PR5071/03",
    "modul": "5071",
    "timestamp": "2025-03-15T10:42:37.123456"
  },
  "deteccio": {
    "classe": "bottle",
    "confianca": 0.847,
    "coordenades_camera_mm": { "x": 25.3, "y": 91.7, "z": 548.2 }
  },
  "transformacio": {
    "coordenades_robot_m": { "x": 0.3721, "y": 0.0891, "z": 0.0523 },
    "matriu_T_cam2robot": [[...], [...], [...], [...]]
  },
  "robot": {
    "ip": "192.168.56.101",
    "posicio_tcp_final": [0.3721, 0.0891, 0.1023, 3.1416, 0.0, 0.0],
    "velocitat_ms": 0.08,
    "acceleracio_ms2": 0.04
  }
}

---

Preguntes de reflexió

Preguntes de reflexió (inclou les respostes a la memòria)

1. Fonts d'error en la calibració mà-ull

Enumera almenys tres fonts d'error que poden afectar la precisió de la matriu de transformació T_cam2robot en el mètode simplificat de 3 punts. Com podries reduir-ne l'impacte? Considera tant errors de mesura com errors sistemàtics del mètode.

2. Seguretat: robot industrial vs. cobot

Explica les diferències principals de seguretat entre un robot industrial tradicional (com l'ABB IRB 6700) i un cobot com el UR3e. Per quines raons el UR3e és adequat per treballar sense gàbia de protecció? Quins riscos residuals existeixen igualment?

3. Profunditat estèreo vs. càmera única

Per quina raó s'usa una càmera estèreo (com la OAK-D) en lloc d'una càmera monocular per obtenir coordenades 3D en aplicacions de robòtica? Quins avantatges ofereix el sistema estèreo integrat de la OAK-D respecte a l'ús de dues càmeres separades calibrades manualment?

4. Llindar de confiança de la detecció

Descriu el comportament del sistema si el llindar de confiança CONFIANCA_MINIMA es fixa molt baix (0.1) o molt alt (0.95). Quin compromís cal fer? Com implementaries una lògica que s'adapti automàticament al nivell de llum ambient del laboratori?

5. Extensió per a pick-and-place amb pinça

El pipeline actual porta el robot fins a l'objecte però no el recull. Descriu els passos addicionals necessaris per implementar una seqüència completa de pick-and-place amb una pinça (per exemple una Robotiq 2F-85). Quines accions calen abans d'agafar l'objecte? I per deixar-lo en una posició de destí?

---

Ampliació (AP5071/03)

Si has completat la pràctica i vols aprofundir, et proposem les extensions següents:

• Control de pinça Robotiq: Integra una pinça Robotiq 2F-85 via el protocol RTDE o el paquet robotiq_gripper. Implementa la seqüència completa agafar → transportar → deixar.

• Seqüència completa pick-and-place: Defineix dues zones de l'espai de treball (origen i destí). El robot ha de detectar objectes a la zona d'origen, agafar-los un per un i deixar-los a la zona de destí. Comptabilitza quants objectes s'han processats i guarda-ho al JSON.

• Calibració amb marcadors ArUco: Substitueix el mètode de 3 punts per una calibració basada en marcadors ArUco (OpenCV aruco module). Col·loca un tauler de marcadors a l'espai de treball i usa la seva posició detectada per calcular la transformació. Compara el resultat (error RMS) amb el mètode simplificat.

• Zona de seguretat dinàmica: Implementa una verificació que comprovi en temps real si cap persona s'apropa a l'espai de treball (detecta "person" amb YOLOv8) i aturi el robot si es detecta una presència humana a menys de 800mm del robot.

---

Consulta la Rúbrica PR5071/03