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

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;

(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 