Usando o LeapMotion para controlar um robô meArm I

Abaixo, o vídeo dos meus primeiros resultados controlando o meArm a partir do sensor LeapMotion.

Esse sensor é semelhante ao Kinect, ou seja, detecta movimentos do nosso corpo (no caso, somente as mãos e braços) e constrói, usando modelos matemáticos bastante complexos, um modelo 3D que descreve cada parte do que está em seu "campo de visão". Essa detecção é feita com emissão e captação de radiação infravermelha.

Como a matemática por trás do troço é muito complexa, ele precisa do PC, ou seja, ainda não é possível ter uma plaquinha das que a gente usa (Arduino, RPi, Edison etc) para processar diretamente os sinais enviados pelo sensor.

Dito isso, se quisermos comandar algum device com o LM, devemos estabelecer a comunicação entre o PC onde o LM está conectado e o dispositivo a ser comandado.

No caso, a ideia é estabelecer uma maneira de comandar o meArm (pequeno robô visto no vídeo acima) de forma total, ou seja, todos os movimentos, a partir de movimentos captados pelo LM em uma ou duas mãos.

Como o robô pode ser controlado com um Arduino, pensei em ligar o Arduino no PC, captar os movimentos do meArm com uma aplicação em Python, processar esses comandos para facilitar o comando pelo Arduino e enviar pro Arduino via porta serial.

Os primeiros testes, me pareceu óbvio, seriam mais fáceis com a pinça, ou seja, fechando e abrindo a pinça do robô com movimentos em pinça do polegar contra o indicador.

Esse post vai tratar de como isso foi feito.

Primeiro, vamos analisar a aplicação Python:

###############################################################################
#                                                                             #
# This program tests the tweezers movement in meArm controlled by Leap Motion #
# You can see more in http://automatobr.blogspot.com.br                       #
#                                                                             #
# assismauro@hotmail.com                                                      #
#                                                                             #
###############################################################################
import sys
# Bring serial stuff
import serial
import thread, time

# Add path to LeapMotion libs directory 
sys.path.insert(0,"D:\Atrium\Projects\Arduino\LeapMotion\lib")

# import LM stuff
import Leap
from Leap import CircleGesture, KeyTapGesture, ScreenTapGesture, SwipeGesture

# initialize Arduino/PC communication. You must set COM port accordingly your Arduino connection
arduino = serial.Serial('COM45', 9600, timeout=.1)

# This listener is called anytime some data is available form LM
class SampleListener(Leap.Listener):

# some readable names to use later
    finger_names = ['Thumb', 'Index', 'Middle', 'Ring', 'Pinky']
    bone_names = ['Metacarpal', 'Proximal', 'Intermediate', 'Distal']

    def on_init(self, controller):
        print "Initialized"

    def on_connect(self, controller):
        print "Connected"

    def on_frame(self, controller):

        global arduino

        # Get the most recent frame, that contains data sent from LM engine
        frame = controller.frame()

        # Get hands
        if len(frame.hands) == 0:
            return
        
        hand = frame.hands[0] # only one hand

        # Get fingers: we only need Thumb and Index data, for a while
        for finger in hand.fingers:
            if (self.finger_names[finger.type()] == "Thumb") or (self.finger_names[finger.type()] == "Index"):
            # Get bones
                for b in range(0, 4):
                    bone = finger.bone(b)

                    d=0.0
                    i=0.0
                    tshMin = 10.0
                    tshMax = 100.0
                    # The idea is to process only distal bone data, that corresponds to te tip of our fingers.
                    # The LM coordinate system is a X,Y,Z based in the center of the sensor (see picture in my
                    # blog). As we need only the distance between tips, in a tweezers movement, I transform it
                    # in a "percent distance", that is, 0 corresponds to tweezer closed, and 100, tweezer
                    # oppened.
                    if (self.bone_names[bone.type] == "Distal"):
                        if (self.finger_names[finger.type()] == "Thumb"):
                            t=bone.next_joint[0]
                        else:
                            i=bone.next_joint[0]
                            d=abs(t-i)
                            o=100 if d >= tshMax else (0 if (d <= tshMin) else (d-tshMin)/tshMax*100.0)
                            print o
                            # here we send data to Arduino
                            toSend = str(o)+"\n"
                            arduino.write(toSend)
                            # and... that´s it.

def main():
    # Create a sample listener and controller
    listener = SampleListener()
    controller = Leap.Controller()
     
    # Have the sample listener receive events from the controller
    controller.add_listener(listener)

    # Keep this process running until Enter is pressed
    print "Press Enter to quit..."
    try:
        sys.stdin.readline()
    except KeyboardInterrupt:
        pass
    finally:
        # Remove the sample listener when done
        controller.remove_listener(listener)

if __name__ == "__main__":
    main()

O código acima, devidamente comentado, é responsável por enviar ao Arduino os comandos do motor da pinça (garra) do meArm.

Abaixo, o programa que roda no Arduino:

/*
  Program to command meArm gripper from LeapMotion (Arduino side)
  More about that: http://automatobr.blogspot.com.br
  assismauro@hotmail.com
*/

#include <Servo.h>
#include <SoftwareSerial.h>

// Debug communication, if you think is necessary
SoftwareSerial mySerial(7,8);

// meArm servo pins
int basePin = 11;
int shoulderPin = 10;
int elbowPin = 9;
int gripperPin = 6;

Servo base;
Servo shoulder;
Servo elbow;
Servo gripper;

void setup() {
  /*
  base.attach(basePin);
  shoulder.attach(shoulderPin);
  elbow.attach(elbowPin);
  */
  // Gripper test
  gripper.attach(gripperPin);
  Serial.begin(9600);
  mySerial.begin(9600);
  gripper.write(90);
  delay(500);
  gripper.write(120);
  delay(500);
  
}

// Stores the last cmd received
int cmdOld=-1;

// Threshold
int limiar = 5;

void loop() 
{
  String cmdStr = "";
  // Get command from Pyton PC app
  if(Serial.available())
     cmdStr=Serial.readStringUntil(10);
  
  if(cmdStr != "")
  {
     int cmd=cmdStr.toInt();
     //Check threshold
     if((cmd > 100) || (abs(cmd - cmdOld) < limiar))
        return;
     cmdOld=cmd;
     // Map LeapMotion command to gripper servo angle   
     int gripAngle=map(cmd,0,100,140,90);
     gripper.write(gripAngle);
     delay(30);
     // Send data to debug Arduino
     mySerial.print(cmd);
     mySerial.print("  -  ");
     mySerial.println(gripAngle);
  }
}

O programa é bem simples: recebe o comando que vem do Python e o trata, acionando o motor da garra.

Dois detalhes importantes:

1) Vc pode observar que tem a definição de uma segunda porta serial. Eu fiz isso para depurar o meu programa, porque como a porta serial que normalmente a gente usa para se comunicar com o Arduino está ocupada pelo próprio. Aí eu liguei um segundo Arduino a outro PC e conectei os dois criando uma porta serial usando os pinos 7 e 8. Aí deu para monitorar os comandos que estavam chegando enviando-os ao outro Arduino.

2) Outro pronto é a variável limiar.  Ela serve para suavizar os movimentos do meArm, porque os comandos recebidos variam muito. Isso evita a "tremedeira" da garra.

É isso.