Bras robotique avec une kinect, un arduino et 4 servos.

Bonjour.

Je suis la communauté arduino depuis peu de temps mais je trouve le concept de l’open-source formidable. J’ai précédemment ouvert quelques topics afin de poser des questions et vos réponses ont toujours été promptes et enrichissantes. Du coup pour vous rendre justice cette fois si, j’ouvre ce topic afin de partager à propos d’un petit prototype sur lequel je bosse. Attention pas de méprise : je suis un noob. Beaucoup d’entre vous comprennent la programmation beaucoup mieux que moi et il leur sera facile de modifier et d’améliorer les codes que je vais partager. Néanmoins Le proto marche pas trop mal comme vous pourrez le voir sur cette vidéo ; Bras Robotique on Vimeo. Voila sans plus attendre je vous fille les deux codes qui font marcher l’engin en deux post différent :

Code Arduino :

// Kinect Robot Arduino Program 
 #include <Servo.h> // declare both servos Servo shoulder; 
 Servo elbow; // setup the array of servo positions 2 
 Servo shoulder;
 Servo ArmRoll;
 Servo Base;
 int nextServo = 0; 
 int servoAngles[] = {0,0,0,0}; 
 /*const int ANGLE_MIN=-90; // angle position MIN en degrés
const int POS_MIN=550; // largeur impulsion pour position ANGLE_MIN degrés du servomoteur
const int ANGLE_MAX=90; // angle position MAX en degrés
const int POS_MAX=2550; // largeur impulsion pour position ANGLE_MAX degrés du servomoteur*/
const int PIN_SHOULDER = 9;
const int PIN_ELBOW = 11;
const int PIN_ARMROLL = 10;
const int PIN_BASE = 6;
int elbowAngle ;
int shoulderAngle ;
int BaseAngle;
int rollArm;
byte buffin[5];

/*int ServoImpulse(int ang) {
int impulse;
impulse=POS_MIN+(POS_MAX-POS_MIN)/(ANGLE_MAX-ANGLE_MIN)*((ang)-ANGLE_MIN);
if(impulse < POS_MIN) impulse = POS_MIN;
if(impulse > POS_MAX) impulse = POS_MAX;
return impulse;
}*/

void setup() { 
  pinMode(PIN_SHOULDER,OUTPUT);
  pinMode(PIN_ELBOW,OUTPUT);
  pinMode(PIN_ARMROLL,OUTPUT);
  pinMode(PIN_BASE,OUTPUT);
  // attach servos to their pins 3 
  shoulder.attach(PIN_SHOULDER); 
  elbow.attach(PIN_ELBOW); 
  ArmRoll.attach(PIN_ARMROLL);
  Base.attach(PIN_BASE);
  
  Base.write(0);
  elbow.write(90);
  shoulder.write(90);
  ArmRoll.write(130);
  Serial.begin(9600); 
  while (!Serial) {
    ; // wait for serial port to connect. Needed for native USB port only
  }  
} 
  
  
  void loop() { 
        
    if( Serial.available()){ 
      //Serial.println("port available");
      Serial.readBytes(buffin,5);
       //delay(100);
      while( Serial.available()) Serial.read();
      if (buffin[0] == 'A' ) {
        elbowAngle = buffin[1];
        shoulderAngle = buffin[2];
        BaseAngle = buffin[3];
        rollArm = buffin[4];

        
        shoulder.write(shoulderAngle);
        elbow.write(elbowAngle);
        Base.write(BaseAngle);
        ArmRoll.write(rollArm);

//-----------------------------------------------------test routine to calibrate servo -------------------------//
        /*shoulder.write(90);
        elbow.write(90);
        Base.write(0);
        ArmRoll.write(30);/*


        }
      }
 }

Code Processing (part 1/2) :

/*************************************************
 * prog:  Max Rheiner / Interaction Design / Zhdk / http://iad.zhdk.ch/
 * date:  12/12/2012 (m/d/y)
 * ----------------------------------------------------------------------------
 */

import SimpleOpenNI.*;
SimpleOpenNI context;

import processing.serial.*; 
Serial Port;

// min,max input angle
int MININ= 10;
int MAXIN=190;
//min,max output angle
int MINOUT=-90;
int MAXOUT= 90;

int loopcpt;
PMatrix3D  orientation = new PMatrix3D();

int          angleValue = 120;
float        zoomF =0.5f;
float        rotX = radians(180);  // by default rotate the hole scene 180deg around the x-axis, 
                                   // the data from openni comes upside down
float        rotY = radians(0);
boolean      autoCalib=true;
int          MAXLOOP = 4;
int          cptLoop = 0;

PVector      bodyCenter = new PVector();
PVector      bodyDir = new PVector();
PVector      com = new PVector();                                   
PVector      com2d = new PVector();                                   
color[]       userClr = new color[]{ color(255,0,0),
                                     color(0,255,0),
                                     color(0,0,255),
                                     color(255,255,0),
                                     color(255,0,255),
                                     color(0,255,255)
                                   }; 
                                
byte[] save=new byte[6]; 
int nochar=6;
void setup()
{
  size(1024,768,P3D);  // strange, get drawing error in the cameraFrustum if i use P3D, in opengl there is no problem
  context = new SimpleOpenNI(this);
  if(context.isInit() == false)
  {
     println("Can't init SimpleOpenNI, maybe the camera is not connected!"); 
     exit();
     return;  
  }

  // disable mirror
  context.setMirror(false);

  // enable depthMap generation 
  context.enableDepth();

  // enable skeleton generation for all joints
  context.enableUser();

  stroke(255,255,255);
  smooth();  
  perspective(radians(45),
              float(width)/float(height),
              10,150000);
              
  // Serial port connexion
  println(Serial.list()); 
  String portName = Serial.list()[0];
  Port = new Serial(this, "com4", 9600);

 }

void draw()
{
  // update the cam
  context.update();

  background(0,0,0);
  
  // set the scene pos
  translate(width/2, height/2, 0);
  rotateX(rotX);
  rotateY(rotY);
  scale(zoomF);
  
  int[]   depthMap = context.depthMap();
  int[]   userMap = context.userMap();
  int     steps   = 3;  // to speed up the drawing, draw every third point
  int     index;
  PVector realWorldPoint;
 
  translate(0,0,-1000);  // set the rotation center of the scene 1000 infront of the camera

  // draw the pointcloud
  /*beginShape(POINTS);
  for(int y=0;y < context.depthHeight();y+=steps)
  {
    for(int x=0;x < context.depthWidth();x+=steps)
    {
      index = x + y * context.depthWidth();
      if(depthMap[index] > 0)
      { 
        // draw the projected point
        realWorldPoint = context.depthMapRealWorld()[index];
        if(userMap[index] == 0)
          stroke(100); 
        else
          stroke(userClr[ (userMap[index] - 1) % userClr.length ]);        
        
        point(realWorldPoint.x,realWorldPoint.y,realWorldPoint.z);
      }
    } 
  } 
  endShape();*/
  
  // draw the skeleton if it's available
  int[] userList = context.getUsers();
  for(int i=0;i<userList.length;i++)
  {
    if(context.isTrackingSkeleton(userList[i]))
      drawSkeleton(userList[i]);
    
    // draw the center of mass
    /*if(context.getCoM(userList[i],com))
    {
      stroke(100,255,0);
      strokeWeight(1);
      beginShape(LINES);
        vertex(com.x - 15,com.y,com.z);
        vertex(com.x + 15,com.y,com.z);
        
        vertex(com.x,com.y - 15,com.z);
        vertex(com.x,com.y + 15,com.z);

        vertex(com.x,com.y,com.z - 15);
        vertex(com.x,com.y,com.z + 15);
      endShape();
      
      fill(0,255,100);
      text(Integer.toString(userList[i]),com.x,com.y,com.z);
    }*/
  }    
 
  // draw the kinect cam field
  context.drawCamFrustum();
}

// draw the skeleton with the selected joints
void drawSkeleton(int userId)
{
  strokeWeight(8);

  // to get the 3d joint data
  PVector Head = new PVector(); 
  PVector Neck = new PVector(); 
  drawLimb(userId, SimpleOpenNI.SKEL_HEAD, SimpleOpenNI.SKEL_NECK,Head,Neck);
  
  PVector LeftShoulder = new PVector();
  drawLimb(userId, SimpleOpenNI.SKEL_NECK, SimpleOpenNI.SKEL_LEFT_SHOULDER,Neck,LeftShoulder);
  
  PVector LeftElbow = new PVector();
  drawLimb(userId, SimpleOpenNI.SKEL_LEFT_SHOULDER, SimpleOpenNI.SKEL_LEFT_ELBOW,LeftShoulder,LeftElbow);
  
  PVector LeftHand = new PVector();
  drawLimb(userId, SimpleOpenNI.SKEL_LEFT_ELBOW, SimpleOpenNI.SKEL_LEFT_HAND,LeftElbow,LeftHand);
  
  PVector RightShoulder = new PVector();
  drawLimb(userId, SimpleOpenNI.SKEL_NECK, SimpleOpenNI.SKEL_RIGHT_SHOULDER,Neck,RightShoulder);
  
  PVector RightElbow = new PVector();
  drawLimb(userId, SimpleOpenNI.SKEL_RIGHT_SHOULDER, SimpleOpenNI.SKEL_RIGHT_ELBOW,RightShoulder,RightElbow);
  
  PVector RightHand = new PVector();
  drawLimb(userId, SimpleOpenNI.SKEL_RIGHT_ELBOW, SimpleOpenNI.SKEL_RIGHT_HAND,RightElbow,RightHand);
  
  PVector Torso = new PVector();
  drawLimb(userId, SimpleOpenNI.SKEL_LEFT_SHOULDER, SimpleOpenNI.SKEL_TORSO,LeftShoulder,Torso);
  drawLimb(userId, SimpleOpenNI.SKEL_RIGHT_SHOULDER, SimpleOpenNI.SKEL_TORSO,RightShoulder,Torso);
  
  PVector LeftHip = new PVector();
  drawLimb(userId, SimpleOpenNI.SKEL_TORSO, SimpleOpenNI.SKEL_LEFT_HIP,Torso,LeftHip);
  //drawLimb(userId, SimpleOpenNI.SKEL_LEFT_HIP, SimpleOpenNI.SKEL_LEFT_KNEE);
  //drawLimb(userId, SimpleOpenNI.SKEL_LEFT_KNEE, SimpleOpenNI.SKEL_LEFT_FOOT);

Code processing (part 2/2 a coller directement a la suite du premier) :

 PVector RightHip = new PVector();
  drawLimb(userId, SimpleOpenNI.SKEL_TORSO, SimpleOpenNI.SKEL_RIGHT_HIP,Torso,RightHip);
  //drawLimb(userId, SimpleOpenNI.SKEL_RIGHT_HIP, SimpleOpenNI.SKEL_RIGHT_KNEE);
  //drawLimb(userId, SimpleOpenNI.SKEL_RIGHT_KNEE, SimpleOpenNI.SKEL_RIGHT_FOOT);  
  // calculate the axes against which we want to measure our angles 
  PVector torsoOrientation = PVector.sub(RightShoulder, RightHip); 
  //torsoOrientation.z = 0;
  PVector upperArmOrientation = PVector.sub(RightElbow, RightShoulder); 
  //upperArmOrientation.z = 0;
  // calculate the angles between our joints 
  float shoulderAngle = angleOf(RightElbow, RightShoulder, torsoOrientation); 
  float elbowAngle = (-angleOf(RightHand, RightElbow, upperArmOrientation));
  //float elbowAngle = angleOf(RightHand, RightElbow, upperArmOrientation);
  // calculate the axes against which we want to measure our angles 
  PVector ShoulderDir = PVector.sub(LeftShoulder, RightShoulder);
  PVector torsoOrientation1 = PVector.sub(LeftShoulder, LeftHip);
  //torsoOrientation1.z = 0;
  PVector upperArmOrientation1 = PVector.sub(LeftElbow, LeftShoulder); 
  //upperArmOrientation1.z = 0;
  // calculate the angles between our joints 
  float shoulderAngle1 = angleOf(LeftElbow, LeftShoulder, torsoOrientation1); 
  float elbowAngle1 = angleOf(LeftHand, LeftElbow, upperArmOrientation1);
  float BaseAngle = angleOf(RightElbow, RightShoulder, ShoulderDir);
  float rollArm = (-angleOf(RightHand,RightElbow,ShoulderDir));
  PVector rotElbow = VectorAngle(RightElbow, RightShoulder);
  // draw body direction
  getBodyDirection(userId,bodyCenter,bodyDir);
  
  bodyDir.mult(200);  // 200mm length
  bodyDir.add(bodyCenter);
  
  stroke(255,200,200);
  line(bodyCenter.x,bodyCenter.y,bodyCenter.z,
       bodyDir.x ,bodyDir.y,bodyDir.z);

  strokeWeight(1);
 
  //fill the buffer which will be send to the serial port
  byte out[] = new byte[5]; 
  out[0] = 'A';
  out[1] = byte(int(elbowAngle)); 
  out[2] = byte(int(shoulderAngle));
  out[3] = byte(int(BaseAngle)); 
  out[4] = byte(int(rollArm));
  // println(elbowAngle,out[1]);
  //delay(100);
  //send the buffer to the seral port
  Port.write(65);
  Port.write(out[1]);
  Port.write(out[2]);
  Port.write(out[3]);
  Port.write(out[4]);
  
}
PVector VectorAngle(PVector one, PVector two){
  PVector rot = one;
  PVector lamb = PVector.sub(two, one); 
  
  if (lamb.x != 0.0) {
     rot.y = atan(lamb.z/lamb.x)*180./PI; 
  } else { 
     rot.y = 90.;
  }
  if (lamb.y != 0.0) {
     rot.x = atan(lamb.z/lamb.y)*180./PI;
     rot.z = atan(lamb.x/lamb.y)*180./PI;
   } else { 
     rot.x = 90.;
     rot.z = 90.;
   }
   println(rot);
   return rot;
}
float angleOf(PVector one, PVector two, PVector axis) { 
   //gives the angle between the vector (two-one) and the vector axis
   //the angles are defined in the plane perpendicular to the z direction
   //the calculated angles are in the range [MININ,MAXIN]
   float ang;
   PVector limb = PVector.sub(two, one); 
   ang = degrees(PVector.angleBetween(limb, axis));
   //ang = ang + 180;
   //println(ang);
   return ang;
} 

void drawLimb(int userId,int jointType1,int jointType2,PVector jointPos1,PVector jointPos2)
{
  //PVector jointPos1 = new PVector();
  //PVector jointPos2 = new PVector();
  float  confidence;
  
  // draw the joint position
  confidence = context.getJointPositionSkeleton(userId,jointType1,jointPos1);
  confidence = context.getJointPositionSkeleton(userId,jointType2,jointPos2);

  stroke(255,0,0,confidence * 200 + 55);
  line(jointPos1.x,jointPos1.y,jointPos1.z,
       jointPos2.x,jointPos2.y,jointPos2.z);
  
  drawJointOrientation(userId,jointType1,jointPos1,50);
  
}

void drawJointOrientation(int userId,int jointType,PVector pos,float length)
{
  // draw the joint orientation  
  //PMatrix3D  orientation = new PMatrix3D();
  float confidence = context.getJointOrientationSkeleton(userId,jointType,orientation);
  if(confidence < 0.001f) 
    // nothing to draw, orientation data is useless
    return;
    
  pushMatrix();
    translate(pos.x,pos.y,pos.z);
    
    // set the local coordsys
    applyMatrix(orientation);
    
    // coordsys lines are 100mm long
    // x - r
    stroke(255,255,0,confidence * 200 + 55);
    line(0,0,0,
         length,0,0);
    // y - g
    stroke(0,255,0,confidence * 200 + 55);
    line(0,0,0,
         0,length,0);
    // z - b    
    stroke(0,0,255,confidence * 200 + 55);
    line(0,0,0,
         0,0,length);
    //println(matrixToString(orientation));
  popMatrix();
}

// -----------------------------------------------------------------
// SimpleOpenNI user events

void onNewUser(SimpleOpenNI curContext,int userId)
{
  println("onNewUser - userId: " + userId);
  println("\tstart tracking skeleton");
  
  context.startTrackingSkeleton(userId);
}

void onLostUser(SimpleOpenNI curContext,int userId)
{
  println("onLostUser - userId: " + userId);
}

void onVisibleUser(SimpleOpenNI curContext,int userId)
{
  //println("onVisibleUser - userId: " + userId);
}


// -----------------------------------------------------------------
// Keyboard events

void keyPressed()
{
  switch(key)
  {
  case ' ':
    context.setMirror(!context.mirror());
    break;
  }
    
  switch(keyCode)
  {
    case LEFT:
      rotY += 0.1f;
      break;
    case RIGHT:
      // zoom out
      rotY -= 0.1f;
      break;
    case UP:
      if(keyEvent.isShiftDown())
        zoomF += 0.01f;
      else
        rotX += 0.1f;
      break;
    case DOWN:
      if(keyEvent.isShiftDown())
      {
        zoomF -= 0.01f;
        if(zoomF < 0.01)
          zoomF = 0.01;
      }
      else
        rotX -= 0.1f;
      break;
  }
}


void getBodyDirection(int userId,PVector centerPoint,PVector dir)
{
  PVector jointL = new PVector();
  PVector jointH = new PVector();
  PVector jointR = new PVector();
  float  confidence;
  
  // draw the joint position
  confidence = context.getJointPositionSkeleton(userId,SimpleOpenNI.SKEL_LEFT_SHOULDER,jointL);
  confidence = context.getJointPositionSkeleton(userId,SimpleOpenNI.SKEL_HEAD,jointH);
  confidence = context.getJointPositionSkeleton(userId,SimpleOpenNI.SKEL_RIGHT_SHOULDER,jointR);
  
  // take the neck as the center point
  confidence = context.getJointPositionSkeleton(userId,SimpleOpenNI.SKEL_NECK,centerPoint);
  
  /*  // manually calc the centerPoint
  PVector shoulderDist = PVector.sub(jointL,jointR);
  centerPoint.set(PVector.mult(shoulderDist,.5));
  centerPoint.add(jointR);
  */
  
  PVector up = PVector.sub(jointH,centerPoint);
  PVector left = PVector.sub(jointR,centerPoint);
    
  dir.set(up.cross(left));
  dir.normalize();
}

Voila! Mème si c’est ultra perfectible, j’espère que ça pourra en aider certain. Encore une fois pas de méprise c’est un bout de code que j’ai récupéré et modifié, je n’en suis pas l’auteur à 100%. Peace!

P.S : J’espère que c’est le bon endroit pour ce genre de partage, je voudrai pas faire chi** les modos…