Controlar un brazo robótico con Arduino usando visión por computador

Ya hemos controlado Arduino y el brazo robótico desde una web y con la voz, ahora toca hacerlo con gestos usando la webcam. Eso significa que necesitaremos usar alguna forma de estimar la posición del brazo y de la mano.

Este proyecto se basa en p5.js junto con la libreria ML5js que encapsula varios modelos de redes neuronales de TensorFlow.js (¿Os acordáis cuando TensorFlow era lo más increíble de la IA?…pues no hace tanto) para usarlos de manera sencilla. Vamos a usar dos modelos distintos uno para el cuerpo posenet y otro para la mano handpose (efectivamente, los nombres no son los más originales del mundo).

En este articulo nos vamos a centrar solo en la parte de la visión por computador, para entender como funciona el software usado en Arduino o como se le envía datos de desde la web puedes consultar los enlaces: control del brazo y controlar Arduino desde la web

Modelos

Ambos modelos funcionan de una forma similar, marcando puntos clave en el esqueleto y la mano. Nosotros tomaremos solo unos pocos puntos que usaremos para calcular el movimiento del brazo y de la mano.

Mano

El modelo de la mano tiene los siguientes puntos:

Indice	Parte
0	muñeca
1,2,3,4	pulgar
5,6,7,8	índice
9,10,11,12	corazón
13,14,15,16	anular
17,18,19,20	meñique

Usaremos los puntos asociados a la punta del pulgar (4) y del corazón (12)

Si la distancia entre esos dos puntos es menor que un valor dado la mano del robot se cierra y si es mayo que ese valor la mano se abre.

//distancia entre dos puntos
function distance(x1,y1,x2,y2){
  return Math.sqrt(Math.pow(x1-x2, 2) + Math.pow(y1-y2, 2));  
}

Brazo

El modelo del cuerpo (posenet) tiene los siguientes puntos:

Indice	Parte
0	nariz
1	ojo izquierdo
2	ojo derecho
3	oreja izquierda
4	oreja derecha
5	hombro izquierdo
6	hombro derecho
7	codo izquierdo
8	codo derecho
9	muñeca izquierda
10	muñeca derecha
11	cadera izquierda
12	cadera derecha
13	rodilla izquierda
14	rodilla derecha
15	tobillo izquierdo
16	tobillo derecho

En mi caso usare los puntos asociados al hombro (6), codo (8) y muñeca (10) del brazo derecho. Con ellos trazaremos dos rectas una del hombro al codo y otra de la muñeca al codo. Usaremos el ángulo entre esas dos rectas para mover el brazo robótico. Moveremos los servos del codo y del hombro. Ignoraremos la base del brazo robótico, ese servo se va a quedar quieto

Pendiente de una recta definida por dos puntos

m = (y2-y1) / (x2-x1)

Ahora que tenemos la pendiente de cada recta (m1, m2) podemos calcular el angulo entre ellas:

g = arctg( ( m1-m2) / (1-m1m2) )

Como la función arcotangente de JS devuelve radianes tendremos que convertir a grados multiplicando por:

g * 180/PI

En código:

//pendiente de una recta definida por dos puntos
function pendiente(x1,y1, x2,y2){
  return (y1-y2)/(x1-x2);  
}

//angulo entre dos rectas  
function angulo(m1,m2){
  return (Math.atan((m1-m2)/(1-(m1*m2))) * 180/ Math.PI);  
}

Transladando movimientos

Seria genial simplemtne transladar los movimientos del brazo en la camara al robot, pero la cosa es un poco más complicada. Ya que no se parecen.

Si bien es fácil hacer una pinza con la mano es un poco más difícil adaptar el brazo humano al del robot:

Para evitar complicaciones innecesarias simplemente mediremos el angulo entre el brazo y el antebrazo y estiraremos o encogeremos el brazo robótico.

Otra cosa que es necesario hacer para que el movimiento sea estable y que no tiemble el brazo (el robótico) es que los movimientos no van a ser continuos, por ejemplo el brazo solo avanzara cuando el angulo del brazo humano se modifique en 10 grados. Y la mano tendrá un rango de distancia que no cambiara de estado si esta cerrada seguirá cerrada, si esta abierta seguirá abierta. Como ayuda visual estos cambios se indicaran con cambios de color en la imagen.

Código para la mano:

// marca los keypoints de la mano
function drawHandKeypoints() {
  if(predictions.length > 0) {
    const keypoint1 = predictions[0].landmarks[4];
    const keypoint2 = predictions[0].landmarks[12];
    
    const dist = distance(keypoint1[0], keypoint1[1], keypoint2[0], keypoint2[1]);
    //console.log(dist);
    
    //calcula la posicion del servo
    if(dist > distOpen){
      manoPos = manoPosMin;
      fill(0, 0, 255);
    } else if(dist < distOpen && dist > distClose){ 
      manoPos = manoPosMin;
      fill(0, 255, 255);
    } else {
      manoPos = manoPosMax;
      fill(255, 255, 255);
    }

    //dibuja los keypoints
    noStroke();
    ellipse(keypoint1[0], keypoint1[1], 10, 10);  
    ellipse(keypoint2[0], keypoint2[1], 10, 10);  
  }
}

Código para el brazo:

// marca los keypoints de la pose 
function drawPoseKeypoints()  {
  // Si ha detectado una pose
  if(poses.length > 0){
    let pose = poses[0].pose;
    
    //lee los keypoints de la pose que buscamos
    let manoKeypoint = pose.keypoints[6];
    let codoKeypoint = pose.keypoints[8];
    let hombroKeypoint = pose.keypoints[10];
    
    //dibuja los keypoints
    fill(255, 0, 0);
    noStroke();
    ellipse(manoKeypoint.position.x, manoKeypoint.position.y, 10, 10);
    ellipse(codoKeypoint.position.x, codoKeypoint.position.y, 10, 10);
    ellipse(hombroKeypoint.position.x, hombroKeypoint.position.y, 10, 10);
    
    let mManoCodo = pendiente(manoKeypoint.position.x, manoKeypoint.position.y, codoKeypoint.position.x, codoKeypoint.position.y);
    let mHombroCodo = pendiente(hombroKeypoint.position.x, hombroKeypoint.position.y,codoKeypoint.position.x, codoKeypoint.position.y);
    
    let ang = Math.abs(angulo(mManoCodo,mHombroCodo));
    //console.log("angulo "+ang);     
    
    //calcula la posicion de los servos
    if(ang > 80){            
      hombroPos = hombroPosMin;
      codoPos = codoPosMin;      
      stroke(255, 255, 255);
    } else if(ang > 70){  
      hombroPos = hombroPosMin+10;
      codoPos = codoPosMin+10;      
      stroke(255, 100, 100);    
    } else if(ang > 60){  
      hombroPos = hombroPosMin+20;
      codoPos = codoPosMin+20;      
      stroke(255, 50, 50);     
    } else if(ang > 50){  
      hombroPos = hombroPosMin+30;
      codoPos = codoPosMin+30;      
      stroke(200, 0, 0);         
    } else if(ang > 40){  
      hombroPos = hombroPosMin+40;
      codoPos = codoPosMin+40;      
      stroke(128, 0, 0);    
    } else if(ang > 30){  
      hombroPos = hombroPosMax;
      codoPos = codoPosMax;  
      stroke(0, 0, 0);      
    } else {
      stroke(0, 255, 0); 
    }
    
    //dibuja el esqueleto
    line(manoKeypoint.position.x, manoKeypoint.position.y, codoKeypoint.position.x, codoKeypoint.position.y);
    line(hombroKeypoint.position.x, hombroKeypoint.position.y,codoKeypoint.position.x, codoKeypoint.position.y);
  }
}

Enviar datos al Arduino

Una vez leídas las posiciones del brazo y la mano y convertidas a ángulos del brazo robótico se envían al Arduino.

//Actualizamos la posicion del brazo
function updatePosition(){  
  send("S2:"+codoPos);
  send("S3:"+hombroPos);
  send("S4:"+manoPos);
}

Enviar los datos demasiado rápido surgen dos problemas:

Saturar el puerto serie lo cual bloquearía la placa
No dar tiempo al servo a posicionarse antes de cambiar a una nueva posición lo cual hace que el brazo tiemble

Para evitar todo esto vamos a actualizar las posiciones de los servos cuarto de segundo, tiempo calculado a base de prueba y error, para ello usaremos setInterval(updatePosition, 250);

Código completo

Index.html

<html>
<head>
  <meta charset="UTF-8">
  <title>Mover brazo con CV</title>
  <script src="https://cdnjs.cloudflare.com/ajax/libs/p5.js/0.9.0/p5.min.js"></script>
  <script src="https://cdnjs.cloudflare.com/ajax/libs/p5.js/0.9.0/addons/p5.dom.min.js"></script>
  <script src="https://unpkg.com/p5-webserial@0.1.1/build/p5.webserial.js"></script>  
  <script src="https://unpkg.com/ml5@latest/dist/ml5.min.js"></script>
</head>
<body>
  <h1>CV control</h1>
  <p id='status'>Cagando modelos:</p>
  <script src="sketch.js"></script>
</head>
</body>
</html>

sketch.js:

let video;
let poseNet;
let poses = [];
let handpose;
let predictions = [];
let hombroPos = 90;
let codoPos = 120;
let manoPos = 10;
  
let hombroPosMax = 150;
let codoPosMax = 170;
let manoPosMax = 35;
let hombroPosMin = 80;
let codoPosMin = 90;
let manoPosMin = 10;
const distOpen = 70;
const distClose = 25;
let statusText = "";

//puerto serie
const serial = new p5.WebSerial();

function setup() {
  createCanvas(640, 480);
  portButton = createButton("Elegir puerto");
  portButton.position(5, 5);
  portButton.mousePressed(choosePort);
  let botonApagar = createButton("Pos. Inicial");
  botonApagar.position(300, 5);
  botonApagar.mousePressed(() => {    
    send("Q");   
    hombroPos = 90;
    codoPos = 120;
    manoPos = 10;
  });
  
  video = createCapture(VIDEO);
  video.size(width, height);
  inicializarSerial();
  setInterval(updatePosition, 250);
  // deteccion de la pose
  poseNet = ml5.poseNet(video, modelPoseReady);  
  poseNet.on('pose', function(results) {
    poses = results;
  });
  // deteccion de la mano
  handpose = ml5.handpose(video, modelHandReady);
  handpose.on("predict", results => {
    predictions = results;
  });  
  
  video.hide();
}

function modelPoseReady() {
  statusText += "Modelo para la pose cargado<br>"
  select('#status').html(statusText);
}

function modelHandReady() {
  statusText += "Modelo para la mano cargado<br>"
  select('#status').html(statusText);
}

function draw() {
  image(video, 0, 0, width, height);
  // Dibujamos los keypoints
  drawPoseKeypoints();
  drawHandKeypoints();
}

// marca los keypoints de la pose 
function drawPoseKeypoints()  {
  // Si ha detectado una pose
  if(poses.length > 0){
    let pose = poses[0].pose;
    
    //lee los keypoints de la pose que buscamos
    let manoKeypoint = pose.keypoints[6];
    let codoKeypoint = pose.keypoints[8];
    let hombroKeypoint = pose.keypoints[10];
    
    //dibuja los keypoints
    fill(255, 0, 0);
    noStroke();
    ellipse(manoKeypoint.position.x, manoKeypoint.position.y, 10, 10);
    ellipse(codoKeypoint.position.x, codoKeypoint.position.y, 10, 10);
    ellipse(hombroKeypoint.position.x, hombroKeypoint.position.y, 10, 10);
    
    let mManoCodo = pendiente(manoKeypoint.position.x, manoKeypoint.position.y, codoKeypoint.position.x, codoKeypoint.position.y);
    let mHombroCodo = pendiente(hombroKeypoint.position.x, hombroKeypoint.position.y,codoKeypoint.position.x, codoKeypoint.position.y);
    
    let ang = Math.abs(angulo(mManoCodo,mHombroCodo));
    //console.log("angulo "+ang);     
    
    //calcula la posicion de los servos
    if(ang > 80){            
      hombroPos = hombroPosMin;
      codoPos = codoPosMin;      
      stroke(255, 255, 255);
    } else if(ang > 70){  
      hombroPos = hombroPosMin+10;
      codoPos = codoPosMin+10;      
      stroke(255, 100, 100);    
    } else if(ang > 60){  
      hombroPos = hombroPosMin+20;
      codoPos = codoPosMin+20;      
      stroke(255, 50, 50);     
    } else if(ang > 50){  
      hombroPos = hombroPosMin+30;
      codoPos = codoPosMin+30;      
      stroke(200, 0, 0);         
    } else if(ang > 40){  
      hombroPos = hombroPosMin+40;
      codoPos = codoPosMin+40;      
      stroke(128, 0, 0);    
    } else if(ang > 30){  
      hombroPos = hombroPosMax;
      codoPos = codoPosMax;  
      stroke(0, 0, 0);      
    } else {
      stroke(0, 255, 0); 
    }
    
    //dibuja el esqueleto
    line(manoKeypoint.position.x, manoKeypoint.position.y, codoKeypoint.position.x, codoKeypoint.position.y);
    line(hombroKeypoint.position.x, hombroKeypoint.position.y,codoKeypoint.position.x, codoKeypoint.position.y);
  }
}

// marca los keypoints de la mano
function drawHandKeypoints() {
  if(predictions.length > 0) {
    const keypoint1 = predictions[0].landmarks[4];
    const keypoint2 = predictions[0].landmarks[12];
    
    const dist = distance(keypoint1[0], keypoint1[1], keypoint2[0], keypoint2[1]);
    //console.log(dist);
    
    //calcula la posicion del servo
    if(dist > distOpen){
      manoPos = manoPosMin;
      fill(0, 0, 255);
    } else if(dist < distOpen && dist > distClose){ 
      fill(0, 255, 255);
    } else {
      manoPos = manoPosMax;
      fill(255, 255, 255);
    }

    //dibuja los keypoints
    noStroke();
    ellipse(keypoint1[0], keypoint1[1], 10, 10);  
    ellipse(keypoint2[0], keypoint2[1], 10, 10);  
  }
}

//pendiente de una recta definida por dos puntos
function pendiente(x1,y1, x2,y2){
  return (y1-y2)/(x1-x2);  
}

//angulo entre dos rectas  
function angulo(m1,m2){
  return (Math.atan((m1-m2)/(1-(m1*m2))) * 180/ Math.PI);  
}

//distancia entre dos puntos
function distance(x1,y1,x2,y2){
  return Math.sqrt(Math.pow(x1-x2, 2) + Math.pow(y1-y2, 2));  
}

//Actualizamos la posicion del brazo
function updatePosition(){  
  send("S2:"+codoPos);
  send("S3:"+hombroPos);
  send("S4:"+manoPos);
}

//enviar datos al puerto serie
function send(cmd) {
    console.log(cmd);
    serial.write(cmd+"\n");
}

//leer datos del puerto serie
function serialEvent() {
    let readSerialStr = serial.readLine();
    trim(readSerialStr);
    if (readSerialStr) {
        console.log(readSerialStr);
    }
}

//incializar la conexion serie
function inicializarSerial() {
    if (!navigator.serial) {
        alert("WebSerial no esta sorportado en este navegador. Prueba en Chrome o Edge.");
    }
    serial.getPorts();
    serial.on("noport", showPortButton);
    serial.on("portavailable", openPort);
    serial.on("requesterror", portError);
    serial.on("data", serialEvent);
    serial.on("close", closePort);
    navigator.serial.addEventListener("connect", portConnect);
    navigator.serial.addEventListener("disconnect", portDisconnect);
    statusText += "Puerto serie incializado<br>"
    select('#status').html(statusText);
}

// Muestra la ventana de seldccion de puerto
function choosePort() {
    showPortButton();
    serial.requestPort();
}

//abrir conexion con puerto serie
function openPort() {
    console.log("Abriendo puerto serie");
    serial.open().then(initiateSerial);
    function initiateSerial() {
        console.log("Puerto serie abierto");
    }
    hidePortButton();
}

//Cerrar conexion con puerto serie
function closePort() {
    console.log("Puerto serie cerrado");
    serial.close();
    showPortButton();
}

//Error con el puerto serie
function portError(err) {
    alert("Serial port error: " + err);
    showPortButton();
}

//Evento puerto serie conectado
function portConnect() {
    console.log("Puerto serie conectado");
    serial.getPorts();
    hidePortButton()
}

//Evento puerto serie desconectado
function portDisconnect() {
    serial.close();
    console.log("Puerto serie desconectado");
    showPortButton();
}

function showPortButton() {
    portButton.show();
}

function hidePortButton() {
    portButton.hide();
}

Puede ver un vídeo sobre este tema en mi canal de Youtube:

Y si estas interesado en el tema puedes ver la lista de vídeos.