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

31 May 2013 21:14 - 31 May 2013 21:25 #35020
by niftyEnt

*C code in <any>_kins.c for an XYZABC machine*was created by

*niftyEnt*

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;

}

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;

}

Last edit: 31 May 2013 21:25 by niftyEnt. Reason: correction to 1 line of code

Please Log in or Create an account to join the conversation.

01 Jun 2013 01:53 #35049
by niftyEnt

Replied by

*niftyEnt*on topic*C code in <any>_kins.c for an XYZABC machine*
by the way...

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

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

Please Log in or Create an account to join the conversation.

01 Jun 2013 10:18 #35076
by andypugh

No. * only means multiplication if it is between things that C expects to multiply. As I understand it spaces are ignored in C syntax.

So, *a and * a are the same.

Whether the * is multiplication or dereference is context sensitive. Don't ask me to defend C.

Replied by

*andypugh*on topic*C code in <any>_kins.c for an XYZABC machine*niftyEnt wrote: Let's talk about the machine language "C"...

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.

No. * only means multiplication if it is between things that C expects to multiply. As I understand it spaces are ignored in C syntax.

So, *a and * a are the same.

Whether the * is multiplication or dereference is context sensitive. Don't ask me to defend C.

Please Log in or Create an account to join the conversation.

01 Jun 2013 10:53 #35080
by niftyEnt

Replied by

*niftyEnt*on topic*C code in <any>_kins.c for an XYZABC machine*
Cool.

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

Any suggestions?

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

Any suggestions?

Please Log in or Create an account to join the conversation.

01 Jun 2013 22:20 #35098
by andypugh

I don't understand why you think that X=0.9642 is further from the origin than X=7.0625

Replied by

*andypugh*on topic*C code in <any>_kins.c for an XYZABC machine*niftyEnt wrote: Any suggestions?

I don't understand why you think that X=0.9642 is further from the origin than X=7.0625

Please Log in or Create an account to join the conversation.

02 Jun 2013 12:59 #35139
by niftyEnt

Replied by

*niftyEnt*on topic*C code in <any>_kins.c for an XYZABC machine*
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

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

Please Log in or Create an account to join the conversation.

Time to create page: 0.079 seconds