Go Down

### Topic: Inverse Kinematics nightmare (Read 1 time)previous topic - next topic

#### harveymoon

##### Apr 23, 2011, 09:45 pm
I have been working on an articulating plotting robot arm
video here: http://vimeo.com/22716815

Essentially I would like to get the Inverse kinematics figured out on the arduino, so that i can just send over a XY coord and it will move the angles of each joint to get to that position.

Things were working fine until I started playing with the arm lengths, then the data would start acting erratic.

I have essentially the same mathematics running on a processing sketch and the arduino. The processing sketch will visualize where it expects it to be, when you click, it sends over a XY coords to the arduino where it should do the same math with the coords and spit back what it thinks the two angles should be.

In the processing print window, you can see the correct values on the processing side, and the sometimes correct values from the machine side.

At some places, it matches completely with the processing sketch, sometimes though it will go haywire and not do what I want at all.

Code: [Select]
`//processing side sketchint sx,sy,ex,ey,hx,hy;int ua,la;int UpperArmLength = 100;int LowerArmLength = UpperArmLength;import processing.serial.*;  // serial communicationfloat degreesA;float degreesB;boolean serial = true;Serial myPort; //declare serial portfloat uad, lad;void setup(){size(600,600);background(255, 224, 150);sx = 300;sy = 300;   println(Serial.list());   String portName = Serial.list()[0];               // Change this Value to fit which Serial Port it is on.  myPort = new Serial(this, portName, 9600);}int counter = 0;void draw(){fill(255);rect(0,0,width,height);upperArm(mouseX,mouseY);if (counter == 0){  println("start Bot : ");}    while (myPort.available() > 0) {    char inByte = myPort.readChar();   print(inByte);counter = 0;}    if (counter == 2){ println();   // println("----end Bot-----");   }    counter++;  } void upperArm(int gox, int goy){int dx = gox - sx;int dy = goy - sy;float distance = sqrt(dx*dx+dy*dy);int a = UpperArmLength;int b = LowerArmLength;float c = min(distance, a + b);float B = acos((b*b-a*a-c*c)/(-2*a*c));float C = acos((c*c-a*a-b*b)/(-2*a*b));float D = atan2(dy,dx);float E = D + B + PI + C;ex = int((cos(E) * a)) + sx;ey = int((sin(E) * a)) + sy;degreesA = degrees(E)-360;hx = int((cos(D+B) * b)) + ex;hy = int((sin(D+B) * b)) + ey;degreesB = degrees((D+B));degreesB = degreesA - degreesB;stroke(255,0,0,100);fill(240,0,0,200);ellipse(sx,sy,10,10);ellipse(ex,ey,8,8);ellipse(hx,hy,6,6);stroke(0);line(sx,sy,ex,ey);line(ex,ey,hx,hy);//delay(15000);}void mousePressed(){    println("Processing Side ____________");  println("mouse= " +mouseX +", " + mouseY);  println("Pros UpperArm Angle= "+ degreesA +" ");  println("Pros LowerArm Angle= "+ degreesB); println("lengths : " +  UpperArmLength + " " + LowerArmLength); println("Processing Side ____________");    if(serial == true){    myPort.write("*");int  xtoGo = mouseX * 100000;String xy = nf(xtoGo + mouseY, 10);delay(250);myPort.write(xy);} }void keyPressed() {     if((key == ',')||(key == '.')||(key == '<')||(key == '>')||(key == '-')||(key == '+')||(key == '[')||(key == ']')){  DirectCommand(str(key));    if(key == ']'){   UpperArmLength = UpperArmLength+=1;LowerArmLength = LowerArmLength+=1; } if(key == '['){   UpperArmLength = UpperArmLength-=1;LowerArmLength = LowerArmLength-=1; }   } // matches a correct command  } void DirectCommand(String j){  myPort.write(j); }`

#### harveymoon

#1
##### Apr 23, 2011, 09:45 pmLast Edit: Apr 23, 2011, 09:50 pm by harveymoon Reason: 1
Code: [Select]
`//arduino side#include <AFMotor.h> //Adafruit Motor ShieldAF_Stepper motor1(400, 1);AF_Stepper motor2(400, 2);#include <Servo.h>  //Servo ControllerServo myservo;  // create servo object to control a servo boolean motors = true;//functions for IKint sx,sy,ex,ey,hx,hy;int ua,la;float uad, lad;int UpperArmLength = 100;int LowerArmLength = UpperArmLength;//IK//Current and Go locationsfloat cx = 400;float cy = 400;float gx = cx;float gy = cy;//Current and Go//stuff for serial parsingint incomingByte;int digit;int xval;int yval;int dig = 0;byte PenOn = '-';//serial//starting angles of armsint a1 = 2000; // starting degreesint b1 = 4000;//starting pointint x1 = cx;int y1 = cy;void setup() {   Serial.begin(9600); Serial.println("DrawingMachine II says hello."); sx = 300; // middle point, sholder position.sy = 300;  delay(2000); motor1.setSpeed(30);     motor2.setSpeed(30);                                                                                                                                                            delay(2000);        }void loop(){ if (Serial.available() > 0) {  dig = dig +1;// read the incoming byte:incomingByte = Serial.read();//This is how the single bytes are recieved seperately and recombined.if (incomingByte == ']'){    dig = 0;UpperArmLength = UpperArmLength+=1;LowerArmLength = LowerArmLength+=1;}if (incomingByte == '['){    dig = 0;UpperArmLength = UpperArmLength-=1;LowerArmLength = LowerArmLength-=1;}if (incomingByte == '.'){    dig = 0;motor1.step(50, BACKWARD, INTERLEAVE);}if (incomingByte == ','){    dig = 0;  motor1.step(50, FORWARD, INTERLEAVE);}if (incomingByte == '>'){    dig = 0;motor2.step(50, BACKWARD, INTERLEAVE);}if (incomingByte == '<'){    dig = 0;  motor2.step(50, FORWARD, INTERLEAVE);}  if(PenOn != incomingByte){ if (incomingByte == '+'){  delay(1000);  myservo.write(50);              // tell servo to go to position in variable 'pos' delay(1000);       dig = 0;             motor1.setSpeed(6); //moove slowly for active points       motor2.setSpeed(6); PenOn = incomingByte;       }if (incomingByte == '-'){   delay(1000);   myservo.write(80);              // tell servo to go to position in variable 'pos'         dig = 0;  delay(500);        motor1.setSpeed(30);  // move quickly for non points    motor2.setSpeed(30);PenOn = incomingByte;}} // if penStatus changed// asterisk inbetween coords resetsif (incomingByte == '*'){   xval = 0;  yval = 0;    dig = 0;}  digit = int(incomingByte) - '0';  // - 0, or ascii value -30 from the incoming digit.  // this is confusing,  //if you take the ascii value of digits 0-9 it equels 30 - 39, if you take the value of the ascii value and - "0"  //you get the orignal digits. an easy way of converting ascii serial messages  if(dig == 1){   xval += digit * 10000;}if(dig == 2){ xval += digit * 1000; }if(dig == 3){  xval +=  digit * 100;}if(dig == 4){  xval += digit *10;}  if(dig == 5){   xval += digit;}if(dig == 6){ yval += digit * 10000; }if(dig == 7){  yval +=  digit * 1000;}  if(dig == 8){  yval += digit * 100;  }  if(dig == 9){  yval += digit * 10;  }  if(dig == 10){  yval += digit;Serial.print(xval, DEC);Serial.print(',');Serial.println(yval, DEC); //straightLine (xval, yval);upperArm(xval,yval);  } }    }void print(char* str, float n1, float n2) {  Serial.print(str);  Serial.print(n1);  Serial.print(", ");  Serial.print(n2);  Serial.println();  }void upperArm(int gox,int goy){ Serial.println("------from Machine------");   print("coords = ", gox, goy);    int dx = gox - sx;int dy = goy - sy;float distance = sqrt(dx*dx+dy*dy);int a = UpperArmLength;int b = LowerArmLength;float c = min(distance, a + b); print("lengths", a, b);float B = acos((b*b-a*a-c*c)/(-2*a*c)); float C = acos((c*c-a*a-b*b)/(-2*a*b));print("C = ", C , 0); print("B , C = ", B, C);float D = atan2(dy,dx);float E = D + B + PI + C;print("D E =  ", D, E);ex = int((cos(E) * a)) + sx;ey = int((sin(E) * a)) + sy;float degreesA = degrees(E)-360;print("UpperArm Angle= ", degreesA, 0);hx = int((cos(D+B) * b)) + ex;hy = int((sin(D+B) * b)) + ey;float degreesB = degrees((D+B));print("LowerArm Angle= ", degreesB,0);degreesB = degreesA - degreesB;print("LowerArm Angle= ", degreesB,0);DegToStp(degreesA, degreesB);Serial.println("----------------------------");      }void DegToStp(int degA, int degB){ int stepsA = map(abs(degA), 0, 360, 0, 8000); int stepsB = map(abs(degB), 0, 360, 0, 8000);      print("steps go to= ", stepsA,stepsB);  print("current step location = ", a1, b1 );       Serial.println("--------- ");//  moveTo(stepsA, stepsB); //comment this out if you have no motors connects, just check the math}void moveTo(int angle2in, int angle1in) {    // a2 and b2 are the final lengths of the left and right strings  int a2 = angle1in;  int b2 = angle2in;  print("steps to take= ", a1-a2,b1-b2);  int stepA;  int stepB;  if (a2>a1) {    stepA=1;  }  if (a1>a2) {    stepA=-1;  }  if (a2==a1) {    stepA=0;  }  if (b2>b1) {    stepB=1;  }  if (b1>b2) {    stepB=-1;  }  if (b2==b1) {    stepB=0;  }  // Change the angle of a1 and b1 until they are equal to the desired angle  while ((a1!=a2) || (b1!=b2)) {    if (a1!=a2) {      a1 += stepA;     if (stepA == -1){        if(motors == true){    motor1.step(1, BACKWARD, INTERLEAVE);        }  }      if (stepA == 1){      if(motors == true){       motor1.step(1, FORWARD, INTERLEAVE);      }    }    }      //delay(1);            if (b1!=b2) {      if(motors == true){      b1 += stepB;      }          if (stepB == -1){      if(motors == true){    motor2.step(1, BACKWARD, INTERLEAVE);      }    }         if (stepB == 1){        if(motors == true){       motor2.step(1, FORWARD, INTERLEAVE);     }     }    // delay(1);        }    }// location not equel to current}`

#### harveymoon

#2
##### Apr 23, 2011, 10:02 pm
Here is some data straight from the processing print window.

You can see, Processing first sends a coordinate at the mouse location of 469,366
this correctly sees that the angles should be 3.55 and 49.77
the arduino spits out 21° and 0 °

moving the point a little bit, to 460, 365 means that two suddenly link up again. Both processing and arduino give the same degree angle.
UpperArm Angle= -8.18, 0.00
LowerArm Angle= -60.58, 0.00

Print window contents below:

Processing Side ____________
mouse= 469, 366
Pros UpperArm Angle= -3.553009
Pros LowerArm Angle= -49.770573
lengths : 100 100
Processing Side ____________
469,366
------from Machine------
coords = 469.00, 366.00
lengths100.00, 100.00
C = 3.14, 0.00
B , C = 0.00, 3.14
D E =  0.37, 6.66
UpperArm Angle= 21.33, 0.00
LowerArm Angle= 0.00, 0.00
steps go to= 466.00, 0.00
current step location = 2000.00, 4000.00
---------
----------------------------

Processing Side ____________
mouse= 460, 365
Pros UpperArm Angle= -8.179199
Pros LowerArm Angle= -60.577255
lengths : 100 100
Processing Side ____________
460,365
------from Machine------
coords = 460.00, 365.00
lengths100.00, 100.00
C = 2.08, 0.00
B , C = 0.53, 2.08
D E =  0.39, 6.14
UpperArm Angle= -8.18, 0.00
LowerArm Angle= -60.58, 0.00
steps go to= 177.00, 1333.00
current step location = 2000.00, 4000.00
---------

#### MarkT

#3
##### Apr 24, 2011, 12:10 am
int is 16 bit signed on Arduino.   I suspect Processing has 32 bit int?
[ I will NOT respond to personal messages, I WILL delete them, use the forum please ]

#### harveymoon

#4
##### Apr 24, 2011, 12:52 am
WOOP! I think that did it!

changed these from int to long.
long dx = gox - sx;
long dy = goy - sy;

thanks big time!

#### felis

#5
##### Apr 24, 2011, 06:16 pm
Here's working IK code for Arduino -> http://www.circuitsathome.com/mcu/robotic-arm-inverse-kinematics-on-arduino

#### harveymoon

#6
##### Apr 24, 2011, 11:38 pm
Felis:
I was looking at that one too. but because there was a Z azix and more joints. it seemed more complicated.

I got it figured out anyway, thanks.

Go Up