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
 


Please enter a valid email to subscribe

Confirm your email address

We need to confirm your email address.
To complete the subscription, please click the link in the email we just sent you.

Thank you for subscribing!

Arduino
via Egeo 16
Torino, 10131
Italy