Mano Robot por Gestos

import socket
import cv2
import mediapipe as mp
import sys

PUERTO = 1001
TIMEOUT_CONEXION = 5
MODEL_PATH = "hand_landmarker.task"
UMBRAL_PULGAR = 0.06

COMANDOS = {
    "menique": {"up": "1", "down": "01"},
    "corazon": {"up": "2", "down": "02"},
    "indice": {"up": "3", "down": "03"},
    "pulgar": {"up": "4", "down": "04"},
}

NOMBRES = {
    "menique": "Menique",
    "corazon": "Corazon",
    "indice": "Indice",
    "pulgar": "Pulgar",
}


def conectar(ip):
    try:
        sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
        sock.settimeout(TIMEOUT_CONEXION)
        sock.connect((ip, PUERTO))
        return sock
    except socket.error:
        return None


def enviar_comando(sock, comando):
    try:
        sock.send(comando.encode())
        return True
    except socket.error:
        return False


def detectar_dedos(landmarks):
    dx = landmarks[4].x - landmarks[5].x
    dy = landmarks[4].y - landmarks[5].y
    pulgar_extendido = (dx * dx + dy * dy) ** 0.5 > UMBRAL_PULGAR
    indice_extendido = landmarks[8].y < landmarks[6].y
    corazon_extendido = landmarks[12].y < landmarks[10].y
    menique_extendido = landmarks[20].y < landmarks[18].y
    return {
        "menique": menique_extendido,
        "corazon": corazon_extendido,
        "indice": indice_extendido,
        "pulgar": pulgar_extendido,
    }


def mostrar_ayuda():
    print("Controles por gesto:")
    print("  Menique arriba/abajo  -> Servo 1 a 180/0")
    print("  Corazon arriba/abajo  -> Servo 2 a 180/0")
    print("  Indice arriba/abajo   -> Servo 3 a 180/0")
    print("  Pulgar arriba/abajo   -> Servo 4 a 180/0")
    print("  Presiona 'q' para salir")


def main():
    print("===== Control de Mano Robot por Gesto =====")

    if len(sys.argv) > 1:
        ip = sys.argv[1]
    else:
        ip = input("Ingrese la IP de la ESP32: ").strip()

    if not ip:
        print("Error: La IP no puede estar vacia")
        return

    print(f"Conectando a {ip}:{PUERTO}...")
    sock = conectar(ip)

    if not sock:
        print("Error: No se pudo conectar a la ESP32")
        return

    print("Conexion exitosa!")
    mostrar_ayuda()

    BaseOptions = mp.tasks.BaseOptions
    HandLandmarker = mp.tasks.vision.HandLandmarker
    HandLandmarkerOptions = mp.tasks.vision.HandLandmarkerOptions
    VisionRunningMode = mp.tasks.vision.RunningMode

    options = HandLandmarkerOptions(
        base_options=BaseOptions(model_asset_path=MODEL_PATH),
        running_mode=VisionRunningMode.IMAGE,
        num_hands=1,
    )

    landmarker = HandLandmarker.create_from_options(options)

    cap = cv2.VideoCapture(0)
    estado_anterior = None

    try:
        while True:
            ret, frame = cap.read()
            if not ret:
                break

            frame = cv2.flip(frame, 1)
            rgb = cv2.cvtColor(frame, cv2.COLOR_BGR2RGB)
            mp_image = mp.Image(image_format=mp.ImageFormat.SRGB, data=rgb)

            results = landmarker.detect(mp_image)

            if results.hand_landmarks:
                for hand_landmarks in results.hand_landmarks:
                    h, w, _ = frame.shape

                    for landmark in hand_landmarks:
                        x = int(landmark.x * w)
                        y = int(landmark.y * h)
                        cv2.circle(frame, (x, y), 5, (0, 255, 0), -1)

                    for connection in mp.tasks.vision.HandLandmarksConnections.HAND_CONNECTIONS:
                        start = hand_landmarks[connection.start]
                        end = hand_landmarks[connection.end]
                        cv2.line(
                            frame,
                            (int(start.x * w), int(start.y * h)),
                            (int(end.x * w), int(end.y * h)),
                            (0, 255, 0),
                            2,
                        )

                    dedos = detectar_dedos(hand_landmarks)

                    if estado_anterior is not None:
                        for dedo, extendido in dedos.items():
                            if extendido != estado_anterior[dedo]:
                                comando = COMANDOS[dedo]["up"] if extendido else COMANDOS[dedo]["down"]
                                if enviar_comando(sock, comando):
                                    accion = "arriba" if extendido else "abajo"
                                    print(f"{NOMBRES[dedo]} {accion} - Servo enviado")
                                else:
                                    print("Error: Conexion perdida")
                                    cap.release()
                                    cv2.destroyAllWindows()
                                    sock.close()
                                    return

                    estado_anterior = dedos.copy()

                    y_offset = 30
                    for dedo in ["menique", "corazon", "indice", "pulgar"]:
                        extendido = dedos[dedo]
                        color = (0, 255, 0) if extendido else (0, 0, 255)
                        estado = "Arriba" if extendido else "Abajo"
                        cv2.putText(
                            frame,
                            f"{NOMBRES[dedo]}: {estado}",
                            (10, y_offset),
                            cv2.FONT_HERSHEY_SIMPLEX,
                            0.7,
                            color,
                            2,
                        )
                        y_offset += 30
            else:
                estado_anterior = None

            cv2.imshow("Control Mano Robot por Gesto", frame)

            if cv2.waitKey(1) & 0xFF == ord("q"):
                break
    except KeyboardInterrupt:
        print("\nInterrumpido por Ctrl+C")
    finally:
        enviar_comando(sock, "x")
        cap.release()
        cv2.destroyAllWindows()
        sock.close()
        print("Conexion cerrada")


if __name__ == "__main__":
    main()