Complete overhaul of my IK functions in C

Post code examples here

Complete overhaul of my IK functions in C

Postby wuselfuzz » Sat Aug 08, 2009 7:48 pm

This might be useful for others starting from scratch. All math is double precision float.

At first, I'm using a small set of linear algebra functions and a datatype vector3d consisting of 3 doubles x,y,z:

Code: Select all
typedef struct {
   double x,y,z;
} vector3d;


Then there's some math functions:

Code: Select all
void
add_vector3d(vector3d *v1, vector3d *v2, vector3d *result) {
   result->x = v1->x + v2->x;
   result->y = v1->y + v2->y;
   result->z = v1->z + v2->z;
}

void
sub_vector3d(vector3d *v1, vector3d *v2, vector3d *result) {
   result->x = v1->x - v2->x;
   result->y = v1->y - v2->y;
   result->z = v1->z - v2->z;
}

void
scalar_mult_vector3d(vector3d *v1, double s, vector3d *result) {
   result->x = v1->x * s;
   result->y = v1->y * s;
   result->z = v1->z * s;
}

void
rot_x_vector3d(vector3d *v, double a, vector3d *result) {
   result->x = v->x;
   result->y = v->y * cos(a) - v->z * sin(a);
   result->z = v->y * sin(a) + v->z * cos(a);
}

void
rot_y_vector3d(vector3d *v, double a, vector3d *result) {
   result->x =  v->x * cos(a) - v->z * sin(a);
   result->y =  v->y;
   result->z = v->z * cos(a) + v->x * sin(a);
}

void
rot_z_vector3d(vector3d *v, double a, vector3d *result) {
   result->x = v->x * cos(a) - v->y * sin(a);
   result->y = v->x * sin(a) + v->y * cos(a);
   result->z = v->z;
}


For the representation of the hexapod's mechanical structure, I'm using a structure currently only containing the leg offsets. Coxa, femur and tibia lengths would fit here, too.

Code: Select all
#define NUM_LEGS 6

typedef struct {
   vector3d leg_offset[NUM_LEGS];
} bot_configuration;


Then I'm using another structure to represent a leg's end effector and angles produced by the ik function:

Code: Select all
typedef struct {
   vector3d position;
   double   ik_angle[3];
} leg;


Using the leg datatype, the struct for a leg configuration of the running bot is defined:

Code: Select all
typedef struct {
   vector3d position;    // body position
   vector3d orientation; // body rotation

   leg      leg[NUM_LEGS];
} bot;


The leg position structure for the MSR-H01 is defined as following:

Code: Select all
bot_configuration conf = {
 {
   { 80, 40,  0},
   {  0, 60,  0},
   {-80, 40,  0},
   {-80,-40,  0},
   {  0,-60,  0},
   { 80,-40,  0}
 }
};


As previously mentioned, this should include coxa, femur and tibia lengths and should be const. :)

And finally, this is the inverse kinematics magic:

Code: Select all
void
ik(bot *b) {
   int i;

   vector3d v;

   double r;
   double r2z2,sr2z2;
   double acos_1,acos_2;

   const double c=20;
   const double f=80;
   const double t=130;

   const double f2=f*f;
   const double t2=t*t;

   for(i=0;i<NUM_LEGS;i++) {
      rot_x_vector3d(&b->leg[i].position,b->orientation.x,&v);
      rot_y_vector3d(&v,b->orientation.y,&v);
      rot_z_vector3d(&v,b->orientation.z,&v);

      sub_vector3d(&v,&conf.leg_offset[i],&v);
      sub_vector3d(&v,&b->position,&v);

      if(i<NUM_LEGS/2) {
         b->leg[i].ik_angle[0]=-atan2(v.x,v.y);
      } else {
         b->leg[i].ik_angle[0]=atan2(v.x,-v.y);
      }

      r=sqrt(v.x*v.x+v.y*v.y)-c;

      r2z2=r*r+v.z*v.z;
      sr2z2=sqrt(r2z2);

      acos_1=(r2z2+f2-t2)/(2*f*sr2z2);
      acos_2=(r2z2-f2-t2)/(2*f*t);
      if(i<NUM_LEGS/2) {
         b->leg[i].ik_angle[1]=M_PI/2 - (acos(acos_1) + atan2(r,v.z));
         b->leg[i].ik_angle[2]=-acos(acos_2)+M_PI/2;
      } else {
         b->leg[i].ik_angle[1]=-M_PI/2 + (acos(acos_1) + atan2(r,v.z));
         b->leg[i].ik_angle[2]=acos(acos_2)-M_PI/2;
      }
   }
}


This reads the positions from the bot structure and calculates servo angles. The if basically decides if it's a left or right leg and fixes some signs.

This is, how it's used.

Code: Select all
static bot idle_position = {
   {0,0,-130},  // body position
   {0,0,0},     // body rotation

   { // leg positions
      {{ 166, 110,   0},}, // leg 0
      {{   0, 160,   0},}, // leg 1       
      {{-166, 110,   0},}, // ...
      {{-166,-110,   0},},
      {{   0,-160,   0},},
      {{ 166,-110,   0},}
   }
};


This defines the following leg setup:

Image

I just call ik():

Code: Select all
   ik(&idle_position);


and copy the result to the pwm values transmit buffer servo[NUM_SERVOS]:

Code: Select all
   for(i=0;i<NUM_LEGS;i++) {
      for(j=0;j<3;j++) {
         if(!isnan(idle_position.leg[i].ik_angle[j])) { // if we hit singularities or a target point is out of reach, ik will produce NaNs
            servo[i*3+j]=(idle_position.leg[i].ik_angle[j]*8.5*1800/M_PI)+servo_offsets[i*3+j];
        }
      }
   }
   spi_update_servos();


8.5*1800/M_PI scales the angle to servo PWM values. Note that my PWM generator uses a 1usec/8.5 unit (this is an indirect result from a 66 MHz chip clock of my FPGA), for a controller using microseconds for PWM length, just 1800/M_PI would be suitable.
servo_offsets is the servo calibration table.

Source tarball: http://wuselfuzz.de/hexapod/files/licks-current.tgz
wuselfuzz
 
Posts: 112
Joined: Thu Dec 11, 2008 3:32 pm

Re: Complete overhaul of my IK functions in C

Postby Matt Denton » Sun Aug 09, 2009 9:10 pm

nice work! I'm sure this is going to be useful for many!
Matt Denton
AKA: Winchy_Matt

micromagic systems ltd
Matt Denton
Site Admin
 
Posts: 1622
Joined: Tue May 20, 2008 9:15 pm
Location: Winchester UK

Re: Complete overhaul of my IK functions in C

Postby wuselfuzz » Mon Aug 10, 2009 7:30 pm

There's a serious issue in the rotation functions, though. Noticed that today. If you do

Code: Select all
vector3d a,b;

a.x=1; a.y=0; a.z=0;
b.x=1; b.y=0; b.z=0;

add_vector3d(&a,&b,&a);


(a = a+b)

then all is fine. However, if you want to rotate a vector and store the result in the same vector

Code: Select all
rot_z_vector3d(&a, M_PI/2, &a);


it goes horribly wrong. The result for a.x gets assigned immediately before a.y is calculated, but the original a.x is needed for that calculation.

The fix is left as an exercise (or you can grab a new version of my code in a few days).
wuselfuzz
 
Posts: 112
Joined: Thu Dec 11, 2008 3:32 pm

Re: Complete overhaul of my IK functions in C

Postby wuselfuzz » Wed Aug 12, 2009 8:57 pm

Code update, tarball at http://wuselfuzz.de/hexapod/files/licks-current.tgz, unpacked files at http://wuselfuzz.de/hexapod/files/licks/.

- hexapod_gl: fixed femur and tibia direction
- linalg: fix for the problem described above
- dynamic sequencer: test code added, check out include/dynamic_sequencer.h, liblicks/dynamic_sequencer.c, sequencertest/main.c
- servooff added: this just turns all servos off. I intend to put this on my initrd, so if you interrupt a running program, you can shut off servo power fast from the command line. Yay, linux. :)
wuselfuzz
 
Posts: 112
Joined: Thu Dec 11, 2008 3:32 pm

Re: Complete overhaul of my IK functions in C

Postby wuselfuzz » Thu Aug 13, 2009 9:27 am

I uploaded the wrong version of hexapod_gl. -.-
wuselfuzz
 
Posts: 112
Joined: Thu Dec 11, 2008 3:32 pm

Re: Complete overhaul of my IK functions in C

Postby GlitchTech Science » Fri Apr 23, 2010 5:00 pm

Interesting work. I'm currently learning kinematics and inverse kinematics so this will be very helpful in getting my bot up and running while helping me to learn more about it. :)
GlitchTech Science
 
Posts: 1
Joined: Fri Apr 23, 2010 4:58 pm


Return to Code Examples

Who is online

Users browsing this forum: No registered users and 1 guest

cron