基于leap motion 的机械臂控制

用的是ABB IRB 4400 6轴关节型机器人模型

               

通过matlab计算得出：

 

px =

 20*cos(a) + 120*cos(a)*cos(b) + 20*cos(a)*cos(b)*cos(c) - 120*cos(a)*cos(b)*sin(c) - 120*cos(a)*cos(c)*sin(b) - 20*cos(a)*sin(b)*sin(c)

py =

20*sin(a) + 120*cos(b)*sin(a) + 20*cos(b)*cos(c)*sin(a) - 120*cos(b)*sin(a)*sin(c) - 120*cos(c)*sin(a)*sin(b) - 20*sin(a)*sin(b)*sin(c)

pz =

 120*sin(b)*sin(c) - 120*sin(b) - 120*cos(b)*cos(c) - 20*cos(b)*sin(c) - 20*cos(c)*sin(b) + 99

 

Processing提供反馈和交互的能力使得它已经成为一个流行的学习编程的方法，它具有对图像的强调草稿化（Sketching）和交互能力。

图3.1 Proceessing软件示意图

import processing.serial.*;
import de.voidplus.leapmotion.*;
import development.*;
Serial port;

LeapMotion leap;
float processhandx;
float processhandy;
float processhandz;
float processhandx0;
float processhandy0;
float processhandz0;
byte bDirX0;
byte bDirY0;
byte bDirZ0;
int i=0;
float m,n,q;
float a=60;
float b=60;
float c=45;
byte bDirX;
byte bDirY;
byte bDirZ;
float x,y,z;

void setup() 
 {
  size(800, 500, P3D);
  background(255);
  noStroke(); 
  fill(50);
  println(Serial.list());
  port = new Serial(this, Serial.list()[0], 9600);
  leap = new LeapMotion(this);
 }

void draw() 
{
  
  background(200);
  int fps = leap.getFrameRate();

  for (Hand hand : leap.getHands ()) 
  {
    hand.draw();
    PVector hand_position = hand.getPosition();
    PVector hand_direction = hand.getDirection();
    PVector hand_dynamics = hand.getDynamics() ;
    int finger_count= hand.countFingers();
    boolean  get_finger = hand.hasFingers();
 println(hand_position.x, hand_position.y, hand_position.z);
 processhandx=hand_position.x;
 processhandy=hand_position.y;
 processhandz=hand_position.z;
     if(i>1)
     {
       m=processhandx0-processhandx;
       n=processhandy0-processhandy;
       q=processhandz0-processhandz;
      
      if((m>10)||(m<-10)) 
           {processhandx=processhandx0;}
      if((n>10)||(n<-10)) 
           {processhandy=processhandy0;}
      if((q>10)||(q<-10)) 
           {processhandz=processhandz0;}
      if((bDirX0-bDirX>10)||(bDirX-bDirX0>10)) 
           {bDirX=bDirX0;}
      if((bDirY0-bDirY>10)||(bDirY-bDirY0>10)) 
           {bDirY=bDirY0;}
      if((bDirZ0-bDirZ>10)||(bDirZ-bDirZ0>10)) 
            {bDirZ=bDirZ0;}
     }  
    
 
   a=a*3.1415926/180; 
   b=b*3.1415926/180;
   c=c*3.1415926/180;
    
    x=20*cos(a) + 120*cos(a)*cos(b) + 20*cos(a)*cos(b)*cos(c) - 120*cos(a)*cos(b)*sin(c) - 120*cos(a)*cos(c)*sin(b) - 20*cos(a)*sin(b)*sin(c);
    y=20*sin(a) + 120*cos(b)*sin(a) + 20*cos(b)*cos(c)*sin(a) - 120*cos(b)*sin(a)*sin(c) - 120*cos(c)*sin(a)*sin(b) - 20*sin(a)*sin(b)*sin(c);
    z=120*sin(b)*sin(c) - 120*sin(b) - 120*cos(b)*cos(c) - 20*cos(b)*sin(c) - 20*cos(c)*sin(b) + 99; 
     if(hand_position.x>600)
       {x=x+0.2;}
     if(hand_position.x<240)
       {x=x-0.2;}
     if(hand_position.y<200)
       {z=z-1;}
     if(hand_position.y>400)
       {z=z+1;}
     if(hand_position.z>70)
       {y=y-1;}
     if(hand_position.z<30)
       {y=y+1;}
 
     a=atan2(y,x);
     c=0.1651-asin(((cos(a)*x+sin(y)-20)*(cos(a)*x+sin(y)-20)+(99-z)*(99-z)-29200)/(4800*sqrt(37)));
     b=asin((-20*sin(c)-120*cos(c))/sqrt((cos(a)*x+sin(a)*y-20)*(cos(a)*x+sin(a)*y-20)+(99-z)*(99-z)))+acos((cos(a)*x+sin(a)*y-20)/sqrt((cos(a)*x+sin(a)*y-20)*(cos(a)*x+sin(a)*y-20)+(99-z)*(99-z)));
     
   
    bDirX = byte(20*hand_direction.x);
    bDirY = byte(50*hand_direction.y);
    bDirZ = byte(25*hand_direction.z);
    
    a=a*180/3.1415926; 
    b=b*180/3.1415926;
    c=c*180/3.1415926;
    processhandx0=processhandx;
    processhandy0=processhandy;
    processhandz0=processhandz;
    bDirX0=bDirX;
    bDirY0=bDirY;
    bDirZ0=bDirZ;
    i++;
    if(i>3000)
       {i=2;}

 delay(100);  
   
     byte[]q={127,byte(a),byte(b),byte(c),bDirX,bDirY,bDirZ};
   
  println(q);
   port.write(q);
  
  }
}

在PC下安装Processing编译软件，并安装LeapMotionForProcessing、Opengl、nyar4psg、OBJLoader、GSVideo等第三方库。Leap Motion获取手的信息，检测到手掌中心的位置、手掌的法向量、手掌朝向的方向等信息，利用Processing将Leap Motion追踪hands, fingers,和tools取得的数据进行处理，首先通过Hand.isLeft (  ) 与 Hand.isRight(  )判断是左手还是右手，再通过Hand hand : leap.getHands  获取手掌三维坐标X、Y、Z方向的位置信息（hand.getPosition），手掌稳定（hand.getStabilizedPosition）系数，手指的运动速度（finger.getVelocity）信息，手指行动的方向（finger.getDirection）矢量信息，以及手抓（hand.getGrabStrength）的动作信息等，把processing获取的不同手势的相关信息进行严格的手势编译，用于放大倍数(scale)、沿XYZ轴的旋转（rotateX、rotateY、rotateZ）、切换（anim[h].draw）等变换，从而使虚拟物体缩放、旋转、切换等变换。

Arduino能通过各种传感器来感知外界环境，通过控制LED灯和马达等对外界进行控制。板子上的微控制器可以通过Arduino的编程语言来编写程序，编译成二进制文件，烧录进微控制器。对 Arduino的编程是利用 Arduino编程语言 (基于 Wiring)和Arduino开发环境(基于 Processing)来实现的。基于Arduino的项目，可以包含Arduino和其他一些在PC上运行的软件。

 

图3.2 Arduino软件示意图

#include <Servo.h>
Servo myservo0;
Servo myservo1;
Servo myservo2;
Servo myservo3;
Servo myservo4;
Servo myservo5;
int servo0pin=3;
int servo1pin=5;
int servo2pin=6;
int servo3pin=9;
int servo4pin=10;
int servo5pin=11;
int leaphandx = 0;
int leaphandy = 0;
int leaphandz = 0;
int leapdirx = 0;
int leapdiry = 0;
void setup(){
  Serial.begin(9600);
  myservo0.attach(servo0pin);
  myservo1.attach(servo1pin);
  myservo2.attach(servo2pin);
  myservo3.attach(servo3pin);
  myservo4.attach(servo4pin);
  myservo5.attach(servo5pin);
}
void loop(){
  if(Serial.available()){
    char buffer[6];
    Serial.readBytes(buffer,6);
    if(buffer[0]==127){
       myservo0.write(leaphandx);
       myservo1.write(leaphandy);    
       myservo2.write(leaphandz); 
       myservo3.write(leapdirx);
       myservo4.write(leapdiry); 
      leaphandx = buffer[1];
      leaphandy = buffer[2];
      leaphandz = buffer[3];
      leapdirx = buffer[4];
      leapdiry = buffer[5];
      }
  }
  Serial.flush();
}

3.3 蓝牙使Processing软件与Arduino软件的信息交互

  为了使程序能够不断的读取输入设备的输入代码，Processing软件装备了draw()函数，在draw()函数中的代码会从头到尾一直运行，直到关闭窗口，它才停止。每次执行draw()被称为一帧（默认每秒60帧）。通过draw()函数可以不断地向蓝牙传输数据。