Go Down

Topic: Hexapod InverseKinemetics Problem (Read 1 time) previous topic - next topic

MangaValk

Apr 02, 2012, 04:29 pm Last 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 :smiley-eek-blue: 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

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

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 pm Last Edit: Apr 03, 2012, 08:39 pm by MangaValk Reason: 1
Thank you, indeed i made a typing error  :smiley-sad-blue:
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

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

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

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 pm Last 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?

I think this one. http://www.learnaboutrobots.com/inverseKinematics.htm

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

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 am Last Edit: Apr 06, 2012, 03:10 pm by MangaValk Reason: 1
Invalid.
Just a no life Programmer

aggrav8d

http://visual-hexapod.sourceforge.net

If you want other hexapod code to compare with.

MangaValk

#11
Apr 06, 2012, 03:12 pm Last 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

Thanks everyone, whit the atan2 function and some debugging the code works correct now.
Just a no life Programmer

robtillaart

Please post your final code for future reference,

Thanks,
Rob Tillaart

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

MangaValk

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 15


void 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