Go Down

### Topic: Hexapod InverseKinemetics Problem (Read 2010 times)previous topic - next topic

#### MangaValk

##### Apr 02, 2012, 04:29 pmLast Edit: Apr 07, 2012, 03:04 pm by MangaValk Reason: 1
i have written a program for my hexapod whit some InverseKinemetics.
the InverseKinemetics formula's are already debugd.
Only i still doesn't get the right output.
i will post a little piece of the software, can some one tell me if they see a problem ?
O Will writing this i thought of one of the errors, RADIANS =( i was expecting degrees. (I feel a little stupid now)
Can some one still give some critics ?

Code Deleted.
Just a no life Programmer

#### PaulS

#1
##### Apr 02, 2012, 04:49 pm
Code: [Select]
`pow((float)DataLine->ServosPosition[0+(i*3)], 2`
Using pow to square a number if like hunting flies with an elephant gun.

#### robtillaart

#2
##### Apr 02, 2012, 07:02 pm
a 2 minute refactor gave me 2 places where the code was doubtful, indicated with comments

I advice to give the functions a more meaningful name if possible to improve readabililty

Th ehypot function comes from - http://www.nongnu.org/avr-libc/user-manual/modules.html =  math.h  (all can be used in Arduino)

Code: [Select]
`#include "InverseKinemetics.h"void RL3 (int i, Data *DataLine){        int t = i*3; long x = DataLine->ServosPosition[2+t]; if (x != 0) DataLine->l3 = hypot(DataLine->ServosPosition[0+t], x); else DataLine->l3 = DataLine->ServosPosition[0+t];}float ZX (int i, Data *DataLine){  int t = i*3;  RL3(i, DataLine);  long x = DataLine->ServosPosition[2+t];  long y = DataLine->ServosPosition[0+t];  if ((x != 0) && (y != 0))                                // <<<<<<<<<<    return atan(float(x) / float(y));  else    return 90; // Math Error Protection.}float TT (int i, Data *DataLine){  int t = i*3;  long x = DataLine->ServosPosition[0+t];  long y = DataLine->ServosPosition[2+t];  float x2 = x * x;  float y2 = x * x;                         ////  you might meant index DataLine->ServosPosition[1+(i*3)] here ????  float z2 = y * y;  float s2 = DataLine->s * DataLine->s ;  float l32 = DataLine->l3 * DataLine->l3;    DataLine->s = sqrt(x2 + y2 + z2);  float t1 = 0;  if (x != 0) t1 = acos((s2 + y2 - l32) / (2 * DataLine->s * x));  else t1 = acos(0.0);  // Math Error Protection.      float a = (s2 + l1 * l1 - l2*l2);  float b = (2 * DataLine->s * l1);  float t2 = acos(a / b);  return t1 + t2;}float LL (Data *DataLine){  return acos(l1*l1 + l2*l2 - DataLine->s*DataLine->s) / (2 * l1 * l2));}void setleg (int i, Data *DataLine){  int t = i*3;  if (i >= 3)  {    DataLine->ServosAngles[0+t] = ((float)180 - ZX(i, DataLine));    DataLine->ServosAngles[1+t] = ((float)180 - TT(i, DataLine));    DataLine->ServosAngles[2+t] = LL(DataLine);  }  else  {    DataLine->ServosAngles[0+t] = ZX(i, DataLine);    DataLine->ServosAngles[1+t] = TT(i, DataLine);    DataLine->ServosAngles[2+t] = ((float)180 - LL(DataLine));  }}void InverseKinemetics::IK(Data *DataLine)  {#ifdef DEBUG  Serial.println("### InverseKinemetics->IK ###");#endif  for (int i=0; i<6; i++)    setleg(i, DataLine);#ifdef DEBUG  for (int i=0; i<6; i++)  {    if (i != 0)      Serial.print(", ");    else      Serial.print("### InverseKinemetics->ServosPosition's are: ");    Serial.print(DataLine->ServosPosition[0+(i*3)]);    Serial.print(", ");    Serial.print(DataLine->ServosPosition[1+(i*3)]);    Serial.print(", ");    Serial.print(DataLine->ServosPosition[2+(i*3)]);  }  Serial.println(";");#endif#ifdef DEBUG  for (int i=0; i<6; i++)  {    if (i != 0)      Serial.print(", ");    else      Serial.print("### InverseKinemetics->Angles are: ");    Serial.print(DataLine->ServosAngles[0+(i*3)]);    Serial.print(", ");    Serial.print(DataLine->ServosAngles[1+(i*3)]);    Serial.print(", ");    Serial.print(DataLine->ServosAngles[2+(i*3)]);  }  Serial.println(";");#endif}`
Rob Tillaart

Nederlandse sectie - http://arduino.cc/forum/index.php/board,77.0.html -
(Please do not PM for private consultancy)

#### MangaValk

#3
##### Apr 02, 2012, 08:06 pmLast Edit: Apr 03, 2012, 08:39 pm by MangaValk Reason: 1
Thank you, indeed i made a typing error
i am looking at the code now and it looks a lot better, i am still learning c++ for arduino.
There are some difference's whit VS 2010 C++ that made me confused. ( some weird behavior with the functions of the math library )

Thank you so much, Only one error, on input :
### InverseKinemetics->ServosPosition's are: 10, 15, 0, 10, 15, 0, 10, 15, 0, 10, 15, 0, 10, 15, 0, 10, 15, 0;
I get :
### InverseKinemetics->Angles are: 90.00, 0.00, 90.00, 90.00, 0.00, 90.00, 90.00, 0.00, 90.00, 90.00, 0.00, 90.00, 90.00, 0.00, 90.00, 90.00, 0.00, 90.00;
correct me if i am wrong but i thought at x 10 y 15 z 0, i must get 90, 90, 90.

Already solved, another devide by ZERO protection.  :0

Still an ERROR, now the second angel wont get under 90, only above

See The Code on first post.
Just a no life Programmer

#### robtillaart

#4
##### Apr 03, 2012, 08:06 pm
don't understand how the code works in detail but I see atan(x/y) on some places. SOmetimes one need atan2(x,y) instead. See the link I send earlier. (2 posts ago)

Rob Tillaart

Nederlandse sectie - http://arduino.cc/forum/index.php/board,77.0.html -
(Please do not PM for private consultancy)

#### MangaValk

#5
##### Apr 03, 2012, 08:38 pm
I have replaced the atan whit atan2, but the problem is whit the T1 Function i think.
T1 returns 0, and it must be 90 degrees at x 10 y 15.
The strange think is that the math part worked without an problem in my VisualStudio C++ Application.
I am getting a bit desperate. =(
Just a no life Programmer

#### robtillaart

#6
##### Apr 03, 2012, 08:41 pm
Quote
The strange think is that the math part worked without an problem in my VisualStudio C++ Application.

Can you post that code to compare?

Do you have a link to some site describing the math behind IK?
Where did you get the math from ?
Rob Tillaart

Nederlandse sectie - http://arduino.cc/forum/index.php/board,77.0.html -
(Please do not PM for private consultancy)

#### MangaValk

#7
##### Apr 03, 2012, 08:49 pmLast Edit: Apr 03, 2012, 09:12 pm by MangaValk Reason: 1
Quote
Can you post that code to compare?

it is the same code only the variables are chanced.

Quote
Do you have a link to some site describing the math behind IK?

Quote
Where did you get the math from ?

See the attached word file, I have written the math by myself.
The file in the first post.
Just a no life Programmer

#### robtillaart

#8
##### Apr 03, 2012, 08:59 pm
no attached word file ...

If it was the same code I was wondering how the VS version handled the 2 bugs I found in the refactor (post 3 or so)?

Rob Tillaart

Nederlandse sectie - http://arduino.cc/forum/index.php/board,77.0.html -
(Please do not PM for private consultancy)

#### MangaValk

#9
##### Apr 04, 2012, 12:56 amLast Edit: Apr 06, 2012, 03:10 pm by MangaValk Reason: 1
Invalid.
Just a no life Programmer

#### aggrav8d

#10
##### Apr 04, 2012, 07:00 pm
http://visual-hexapod.sourceforge.net

If you want other hexapod code to compare with.
Dance as if no one is watching; post as if it is going to be read aloud as evidence in court.

#### MangaValk

#11
##### Apr 06, 2012, 03:12 pmLast Edit: Apr 07, 2012, 03:03 pm by MangaValk Reason: 1
A new simple piece of code, the greatest error's are out of it.
but one bug i still have is, if z isn't Zero then the left qaudrant isn't correct calculated.

Code Deleted.
Just a no life Programmer

#### MangaValk

#12
##### Apr 07, 2012, 03:03 pm
Thanks everyone, whit the atan2 function and some debugging the code works correct now.
Just a no life Programmer

#### robtillaart

#13
##### Apr 08, 2012, 10:16 am

Thanks,
Rob Tillaart

Nederlandse sectie - http://arduino.cc/forum/index.php/board,77.0.html -
(Please do not PM for private consultancy)

#### MangaValk

#14
##### Apr 08, 2012, 10:24 am
I will post the simplified version that you must run on a pc, but the mathematical part is the same.
Code: [Select]
`#include "stdafx.h"#include <math.h>#include <iostream>#include <fstream>const double pi = 3.14159265358979;using namespace std;float s;float l3;#define l1 10#define l2 15void RL3 (int x, int y, int z){  if ((x == 0) && (z == 0))   l3 = 0;  else l3 = hypot(z, x);  if (x < 0)   l3 = l3 * -1;}float O1 (int x, int y, int z){ float t = 0;  RL3(x, y, z);  if ((x != 0) & (z != 0))    t = atan((float) z / x);  else    t =  atan(0.0); // Math Error Protection.  return t;}float T1 (int x, int y, int z){  float x2 = x * x;  float y2 = y * y;  float z2 = z * z;  float l32 = l3 * l3;  s = sqrt(x2 + y2 + z2);  float t1 = 0;  float s2 = s * s;  if ((y == 0) && (l3 == 0))    t1 = 0; // Math Error Protection.  else    t1 = (atan2(float(l3), float(y)));  float a = (l1 * l1) - (l2*l2) + s2;  float b = 2 * l1 * s;  float t2 = acos(a / b);  float ret = 0;  ret = t1 + abs(t2);  return ret;}float T3 (int x, int y, int z){  return (acos(((l1*l1) + (l2*l2) - (s*s)) / (2 * l1 * l2)));}const float r = (180 / pi);#define l (l1 + l2)int _tmain(int argc, _TCHAR* argv[]){ /* Mine!!!!! */ ofstream myfile; ofstream log; myfile.open ("c:\\IK.txt"); log.open("c:\\log.txt"); for (int z=-l; z<=l; z++){ myfile << "Line: " << z << endl; for (int y=-l; y<=l; y++){ for (int x=-l; x<=l; x++) { float r0 = 90  - (O1(x, y, z) * r); float r1 = 180 - (T1(x, y, z) * r); float r2 = T3(x, y, z) * r; //cout << "X = " << x << "\tY = " << y << "\tZ = " << z << "\tR0 = " << float(r0) << "\tR1 = " <<  float(r1) << "\tR2 = " <<  float(r2) << endl; log << "X = " << x << "\tY = " << y << "\tZ = " << z << "\tR0 = " <<  float(r0) << "\tR1 = " <<  float(r1) << "\tR2 = " <<  float(r2) << endl; if (((r0 <= 180) & (r0 >= 0)) & ((r1 <= 180) & (r1 >= 0)) & (r2 <= 180) & (r2 >= 0)){ myfile << "|"; } else myfile << "_"; if (x == 0) myfile << " "; } myfile << endl; if (y == 0) myfile << endl; } myfile << endl << endl << endl << endl << endl; } myfile.close(); log.close(); system("c:\\IK.txt"); //system("c:\\log.txt"); return 0;}`
Just a no life Programmer

Go Up

Please enter a valid email to subscribe