next topic »

[on:]

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.

[Reply #1 on:]

Code:

pow((float)DataLine->ServosPosition[0+(i*3)], 2

Using pow to square a number if like hunting flies with an elephant gun.

[Reply #2 on:]

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:

#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
}

[Reply #3 on:]

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.  

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

See The Code on first post.

[Reply #4 on:]

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)

[Reply #5 on:]

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.

[Reply #6 on:]

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 ?

[Reply #7 on:]

Can you post that code to compare?

it is the same code only the variables are chanced.

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

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

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.

[Reply #8 on:]

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)?

[Reply #9 on:]

Invalid.

[Reply #10 on:]

http://visual-hexapod.sourceforge.net

If you want other hexapod code to compare with.

[Reply #11 on:]

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.

[Reply #12 on:]

Thanks everyone, whit the atan2 function and some debugging the code works correct now.

[Reply #13 on:]

Please post your final code for future reference,

Thanks,

[Reply #14 on:]

I will post the simplified version that you must run on a pc, but the mathematical part is the same.

Code:

#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;
}