## Complete overhaul of my IK functions in C

Post code examples here

### Complete overhaul of my IK functions in C

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
`voidadd_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;}voidsub_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;}voidscalar_mult_vector3d(vector3d *v1, double s, vector3d *result) {   result->x = v1->x * s;   result->y = v1->y * s;   result->z = v1->z * s;}voidrot_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);}voidrot_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);}voidrot_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 6typedef 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;} 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
`voidik(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=-atan2(v.x,v.y);      } else {         b->leg[i].ik_angle=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=M_PI/2 - (acos(acos_1) + atan2(r,v.z));         b->leg[i].ik_angle=-acos(acos_2)+M_PI/2;      } else {         b->leg[i].ik_angle=-M_PI/2 + (acos(acos_1) + atan2(r,v.z));         b->leg[i].ik_angle=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: 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

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

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

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

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

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 