 # Inverse kinematics processing

hey, I am now strugling for a long time to make that work, but it simply doesn't. I have this processing code and I want to control some servos with it, but I need two different arm sizes. The problem is that as soon I make the lenght "a" and "b" unequal it doesn't work anymore..... Can someone help me please.

``````int sx,sy,ex,ey,hx,hy,hxo,hyo;
int armLength,ua,la;
void setup(){
size(500,500);
background(255, 224, 150);
sx = width/2;
sy = height/2;
armLength = int(width/5);
}

void draw(){
fill(255);
rect(0,0,width,height);
upperArm();
}

void upperArm(){
int dx = mouseX - sx;
int dy = mouseY - sy;
float distance = sqrt(dx*dx+dy*dy);

int a = armLength;                                      //here is the problem.
int b = armLength;
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;
print("UpperArm Angle=  "+degrees(E)+"    ");

hx = int((cos(D+B) * b)) + ex;
hy = int((sin(D+B) * b)) + ey;
println("LowerArm Angle=  "+degrees((D+B)));

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);
``````

The problem is that as soon I make the lenght "a" and "b" unequal it doesn't work anymore

In that sentence, it is a pronoun with no referent. What doesn't work anymore?

Sorry, the tip of the "arm" is not anymore at the coordinate where the mouse tip is... Seems like the angle calculatoin goes wrong if you change the values of the arm segments into something not equal to eachother. I translated it into arduino code but that code had the same problems...

Here I changed the armlength into 130 and 170. The tip will be now somewhere else but not at the given mouse position.

I hope I can explain myself enouth. English isn't my foreign language.

``````int sx,sy,ex,ey,hx,hy,hxo,hyo;
int armLength,ua,la;
void setup(){
size(500,500);
background(255, 224, 150);
sx = width/2;
sy = height/2;
armLength = int(width/5);
}

void draw(){
fill(255);
rect(0,0,width,height);
upperArm();
}

void upperArm(){
int dx = mouseX - sx;
int dy = mouseY - sy;
float distance = sqrt(dx*dx+dy*dy);

int a = 130;                                      //here is the problem.
int b = 170;
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;
print("UpperArm Angle=  "+degrees(E)+"    ");

hx = int((cos(D+B) * b)) + ex;
hy = int((sin(D+B) * b)) + ey;
println("LowerArm Angle=  "+degrees((D+B)));

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);
}
``````

I'm missing what this has to do with Arduino?

It isn't really even a Processing issue, in that Processing is doing exactly what you tell it to do.

It looks more like a failure to translate some algorithm into correct Processing syntax. So, the obvious question is what algorithm are you trying to implement?

I am right now building an ardiuno based octapod, controlled by processing. I need to translate koordinates into angles so that the legs move exactly to the koordinates I give them. I know that I have done a mistake somewhere and that this isn't really a arduino or processing problem, but I hoped someone could help me find it.

:-[

Open loop kinematics is a very complex subject. Just guessing at some equations isn't going to work, so I presume that this is not what you are doing.

What algorithm are you trying to translate into working code?

I got the mathmatics from that page here:http://www.learnaboutrobots.com/inverseKinematics.htm

At the bottom of that page there are 6 equations. Since you are not (presumably) trying to orient a hand (or foot), knowing Øhand and solving for Ø3 are not necessary.

None of the other equations involve Øhand.

So, that leaves:
B2 = Xhand2 + Yhand2 (by the Pythagorean theorem)
q1 = ATan2(Yhand/Xhand)
q2 = acos[(l12 - l22 + B2)/2l1B] (by the law of cosines)
Ø1 = q1 + q2 (I know you can handle addition)
Ø2 = acos[(l12 + l22 - B2)/2l1l2]

Which of the values in these equations does your c represent?

the c represents the B.

I think I got it now! Hopefully it will work under the conditions I need it to work, but I think it will

I wrote everything again and with a lot of sweat I got this here: (there are so many little mistakes to be aware of, like "int" instead of "long"...)

``````void setup() {

Serial.begin(9600);
}

void loop() {
// put your main code here, to run repeatedly:

IK(0,6,246,6);
}

int IK(long Xhip, long Yhip, long Xfoot, long Yfoot){

//B: length of imaginary line
//q1: angle between X-axis and imaginary line
//q2: interior angle between imaginary line and link l1

float Braw;
float B;
float q1;
float q2;
float Q1;
float Q2;
long l1 = 124;
long l2 = 175;
long Xpos;
long Ypos;

Xpos = abs(Xhip-Xfoot);
Ypos = abs(Yhip-Yfoot);

Braw = sqrt(Xpos*Xpos + Ypos*Ypos);                    //(by the Pythagorean theorem)
B = min(Braw, (Xpos+Ypos));                              // so that it is working even if Xhand and Yhand out of reach
q1 = atan2(Ypos,Xpos);
q2 = acos((l1*l1 - l2*l2 + B*B)/(2*l1*B));             //(by the law of cosines)
Q1 = (degrees(q1) + degrees(q2));                                     //(I know you can handle addition)
Q2 = acos((l1*l1 + l2*l2 - B*B)/(2*l1*l2));             //(by the law of cosines)

Serial.println(l1);
Serial.println(l2);
Serial.println(Q1);
Serial.println(degrees(Q2));
Serial.println(B);
Serial.println(degrees(q1));
Serial.println(degrees(q2));
Serial.println(Xpos);
Serial.println(Ypos);
Serial.println();

delay(5000);
}
``````

the c represents the B.

``````  float c = min(distance, a + b);
``````

How does the minimum of the position to reach or the total arm length represent the position of the hand (foot)?

There is no minimum involved anywhere in the set of equations from that page.

Even your revised code propagates this error.

Here's some Arduino IK code -> http://www.circuitsathome.com/mcu/robotic-arm-inverse-kinematics-on-arduino It is written for the arm but foot is similar. The article only describes specific implementation but it has links to sources with formulas too.

Hey I did it now, after a lot of confusion ;D thank you ! hopefully that may help someone else.

``````void setup() {

Serial.begin(9600);
}

void loop() {

IK(0,60,250,60);  // (Xhip,Yhip,Xfoot,Yfoot)
}

int IK(long Xhip, long Yhip, long Xfoot, long Yfoot){

float B;                                    //B: length of imaginary line
float q1;                                   //q1: angle between X-axis and imaginary line
float q2;                                   //q2: interior angle between imaginary line and link l1
float Q1;                                   //Q1: angle between x-axis and "l1"
float Q2;                                   //Q2: angle between "l1" and "l2"
long l1 = 124;                              //l1: length of the upper leg in mm
long l2 = 175;                            //l2: length of the lower leg in mm
long Xpos;                                  //Xpos: relative X-position out of Xhip and Xfoot where the leg should move to
long Ypos;                                  //Ypos: relative Y-position out of Yhip and Yfoot where the leg should move to

Xpos = abs(Xhip-Xfoot);                                 //relative distance of Xhip and Xfoot
Ypos = abs(Yhip-Yfoot);                                 //relative distance of Yhip and Yfoot

B = sqrt(Xpos*Xpos + Ypos*Ypos);                        //the Pythagorean theorem
q1 = atan2(Ypos,Xpos);
q2 = acos((l1*l1 - l2*l2 + B*B)/(2*l1*B));              //the law of cosines
Q1 = degrees(q2) - degrees(q1) + 90;
Q2 = degrees(acos((l1*l1 + l2*l2 - B*B)/(2*l1*l2)));    //the law of cosines

//---------------------------------------------------------------------------------------------------------------------------
//-----------------------------------------------| trouble shooting| --------------------------------------------------------
//---------------------------------------------------------------------------------------------------------------------------

Serial.println(Q1);
Serial.println(Q2);
Serial.println();
Serial.println(B);
Serial.println();
Serial.println(q1);
Serial.println(q2);
Serial.println(degrees(q1));
Serial.println(degrees(q2));
Serial.println();
Serial.println(Xpos);
Serial.println(Ypos);
Serial.println();
Serial.println("------------------------------------------------------");
Serial.println();

delay(5000);
}
``````