Pages: [1]   Go Down
Author Topic: Hexapod InverseKinemetics Problem  (Read 1245 times)
0 Members and 1 Guest are viewing this topic.
Holland
Offline Offline
Newbie
*
Karma: 0
Posts: 11
Just a no life Programmer
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

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 smiley-cry i was expecting degrees. (I feel a little stupid now)
Can some one still give some critics ?

Code Deleted.

* wisFORlive.docx (20.86 KB - downloaded 11 times.)
« Last Edit: April 07, 2012, 08:04:12 am by MangaValk » Logged

Just a no life Programmer

Seattle, WA USA
Online Online
Brattain Member
*****
Karma: 549
Posts: 46077
Seattle, WA USA
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

Code:
pow((float)DataLine->ServosPosition[0+(i*3)], 2
Using pow to square a number if like hunting flies with an elephant gun.
Logged

Global Moderator
Netherlands
Offline Offline
Shannon Member
*****
Karma: 169
Posts: 12448
In theory there is no difference between theory and practice, however in practice there are many...
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

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

Rob Tillaart

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

Holland
Offline Offline
Newbie
*
Karma: 0
Posts: 11
Just a no life Programmer
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

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.  smiley-mad

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

See The Code on first post.
« Last Edit: April 03, 2012, 01:39:05 pm by MangaValk » Logged

Just a no life Programmer

Global Moderator
Netherlands
Offline Offline
Shannon Member
*****
Karma: 169
Posts: 12448
In theory there is no difference between theory and practice, however in practice there are many...
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

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)


Logged

Rob Tillaart

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

Holland
Offline Offline
Newbie
*
Karma: 0
Posts: 11
Just a no life Programmer
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

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. smiley-cry
Logged

Just a no life Programmer

Global Moderator
Netherlands
Offline Offline
Shannon Member
*****
Karma: 169
Posts: 12448
In theory there is no difference between theory and practice, however in practice there are many...
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

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

Rob Tillaart

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

Holland
Offline Offline
Newbie
*
Karma: 0
Posts: 11
Just a no life Programmer
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

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.
« Last Edit: April 03, 2012, 02:12:52 pm by MangaValk » Logged

Just a no life Programmer

Global Moderator
Netherlands
Offline Offline
Shannon Member
*****
Karma: 169
Posts: 12448
In theory there is no difference between theory and practice, however in practice there are many...
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

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

Logged

Rob Tillaart

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

Holland
Offline Offline
Newbie
*
Karma: 0
Posts: 11
Just a no life Programmer
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

Invalid.
« Last Edit: April 06, 2012, 08:10:34 am by MangaValk » Logged

Just a no life Programmer

Vancouver, Canada
Offline Offline
Jr. Member
**
Karma: 0
Posts: 73
I want to make a living designing robots
View Profile
WWW
 Bigger Bigger  Smaller Smaller  Reset Reset

http://visual-hexapod.sourceforge.net

If you want other hexapod code to compare with.
Logged

Holland
Offline Offline
Newbie
*
Karma: 0
Posts: 11
Just a no life Programmer
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

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.
« Last Edit: April 07, 2012, 08:03:15 am by MangaValk » Logged

Just a no life Programmer

Holland
Offline Offline
Newbie
*
Karma: 0
Posts: 11
Just a no life Programmer
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

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

Just a no life Programmer

Global Moderator
Netherlands
Offline Offline
Shannon Member
*****
Karma: 169
Posts: 12448
In theory there is no difference between theory and practice, however in practice there are many...
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

Please post your final code for future reference,

Thanks,
Logged

Rob Tillaart

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

Holland
Offline Offline
Newbie
*
Karma: 0
Posts: 11
Just a no life Programmer
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

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

Just a no life Programmer

Pages: [1]   Go Up
Jump to: