C code in <any>_kins.c for an XYZABC machine

Let's talk about the machine language "C"...

prologue:

When attempting to write a kins module for a 6-axis gantry style machine, I am unable to recreate the results my spreadsheet returns for X-axis compensation when rotating about axis C.
please note-- I am NOT a C programmer

discussion:

Discrepancies in syntax exist between the various examples of <your>kins.c available from various sources, to wit:

Example 1 (from www.linuxcnc.org/docs/html/motion/kinematics.htm) --

1 int kinematicsForward(const double *joint, EmcPose *world,
2 const KINEMATICS_FORWARD_FLAGS *fflags,
3 KINEMATICS_INVERSE_FLAGS *iflags)

and

4 int kinematicsInverse(const EmcPose * world, double *joints,
5 const KINEMATICS_INVERSE_FLAGS *iflags,
6 KINEMATICS_FORWARD_FLAGS *fflags)

please notice that in lines 1 thru 3 of example 1, "*joint" , "*world", "*fflags", and "*iflags" all indicate pointers whereas in lines 4 thru 6 of example 1 "*joints" is a pointer to a different label and "* world" indicates multiplication.

Example 2 (from wiki.linuxcnc.org/uploads/5axiskins.c)--

1 int kinematicsForward(const double *joints,
2 EmcPose * pos,
3 const KINEMATICS_FORWARD_FLAGS * fflags,
4 KINEMATICS_INVERSE_FLAGS * iflags)

and

5 int kinematicsInverse(const EmcPose * pos,
6 double *joints,
7 const KINEMATICS_INVERSE_FLAGS * iflags,
8 KINEMATICS_FORWARD_FLAGS * fflags)

In Example 2, "*joints" indicates a pointer while "* pos", "* fflags" and "* fflags" all indicate multiplication. Further, "* joints" is used in both methods.

specifics:

In writing the kinematics for an existing machine detailed in the attached .png files, the problem is that when I home all joints in joint mode (using the Axis UI), the machine coordinates reflect the correct offset of

X: 7.0625
Y: 0.0000
Z: 25.0000
A: 0.0000
B: 0.0000
C: 0.0000

but when I enter the following G-code into the MDI:

g0 c30

the manual display in joint mode shows

X: 0.9462
Y: -3.5312
Z: 0.0000
A: 0.0000
B: 0.0000
C: 30.0000

This result indicates (to me) that the head of my machine will be translated along the X axis FURTHER from 0. The trig in my equation ie:

adj.x = ((C_len * cos(C_rad)) * cos(B_rad)) ...

where rotating the C axis SHOULD yield a result for X CLOSER to 0 (as the identical equation entered into a spreadsheet does).

I have attempted various modifications of this statement including:

adj.x = C_len - ((C_len * cos(C_rad)) * cos(B_rad))) ...,

double C_reverse = deg2rad(C_deg + 180);
adj.x = C_len + ((C_len * cos(C_reverse)) * cos(B_rad))) ...,
etc...

but ALL equations result in a translation amount FURTHER away from 0X. This result is unexpected and incorrect.
Also, rotations of the A and B axes result in expected translations of X, Y, and Z (identical to the results achieved in my spreadsheet).

Comments:

This machine is complete and ready to start cutting. The only thing stopping me is this error in the translation results, and I have been attempting to achieve the correct results for about two weeks, to no avail. I certainly hope you guys can help solve this.

Regards,

niftyEnt


Attachments:

the relevant section of my .hal code is:

setp XYZABCkins.A_length 21
setp XYZABCkins.B_length 4
setp XYZABCkins.C_length 7.0625

the relevant sections of my XYZABCkins.c code based on a work under
* Copyright (c) 2007 Chris Radek are:

#define deg2rad(d) ((d)*PM_PI/180.0)
#define rad2deg(r) ((r)*180.0/PM_PI)

struct haldata_2 {
hal_float_t *A_length;
hal_float_t *B_length;
hal_float_t *C_length;
} *haldata_2;

static PmCartesian fwd_adj(double A_len, double B_len, double C_len, double A_deg, double B_deg, double C_deg) {
PmCartesian adj;
double A_rad = deg2rad(A_deg);
double B_rad = deg2rad(B_deg);
double C_rad = deg2rad(C_deg);

adj.x = ((C_len * cos(C_rad)) * cos(B_rad)) + ((A_len * cos(A_rad) * sin(B_rad)) + (B_len * sin(B_rad)));
adj.y = (A_len * sin(A_rad)) + (C_len * sin(C_rad));
adj.z = (A_len * cos(A_rad) * cos(B_rad)) + (B_len * cos(B_rad)) + (C_len * cos(C_rad) * sin(B_rad));

return adj;
}

int kinematicsForward(const double *joints,
EmcPose * pos,
const KINEMATICS_FORWARD_FLAGS * fflags,
KINEMATICS_INVERSE_FLAGS * iflags)
{
PmCartesian adj_fwd = fwd_adj(*(haldata_2->A_length), *(haldata_2->B_length), *(haldata_2->C_length), joints[3], joints[4], joints[5]);

pos->tran.x = joints[0] + adj_fwd.x;
pos->tran.y = joints[1] + adj_fwd.y;
pos->tran.z = joints[2] + adj_fwd.z;
pos->a = joints[3];
pos->b = joints[4];
pos->c = joints[5];
pos->u = joints[6];
pos->v = joints[7];
pos->w = joints[8];

return 0;
}

int kinematicsInverse(const EmcPose * pos,
double *joints,
const KINEMATICS_INVERSE_FLAGS * iflags,
KINEMATICS_FORWARD_FLAGS * fflags)
{

PmCartesian adj_inv = fwd_adj(*(haldata_2->A_length), *(haldata_2->B_length), *(haldata_2->C_length), joints[3], joints[4], joints[5]);

joints[0] = pos->tran.x - adj_inv.x;
joints[1] = pos->tran.y - adj_inv.y;
joints[2] = pos->tran.z - adj_inv.z;
joints[3] = pos->a;
joints[4] = pos->b;
joints[5] = pos->c;
joints[6] = pos->u;
joints[7] = pos->v;
joints[8] = pos->w;

return 0;
}

by the way...

I'm running version 2.5.1 on Ubuntu 10.04 straight from the distro disc.

Cool.

So the methods as written are not the problem. That just leaves the trig.

Any suggestions?

My apologies.

I should have written:

0: 0.9462
1: -3.5312
2: 0.0000
3: 0.0000
4: 0.0000
5: 30.0000

as the joints instead of axes.

However, all is moot. I reinstalled linuxCNC and everything is now working as expected.

Thanks for the clarification about C. Understanding always precludes the need for defense.

And thanks for your timely response.

niftyEnt