Skip to content

Integrate gantry (XYYZ) into matrixkins.comp #3952

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
hdiethelm wants to merge 1 commit into
base: master
Choose a base branch
from
+139 −46
Open

Integrate gantry (XYYZ) into matrixkins.comp #3952

Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
185 changes: 139 additions & 46 deletions src/hal/components/matrixkins.comp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -19,6 +19,7 @@ component matrixkins "Calibrated kinematics for 3-axis machines";
description
"""
The matrixkins component implements custom kinematics for 3-axis
Use coordinates=XYZ for standard and coordinates=XYYZ for gantry
Copy link
Copy Markdown
Collaborator

@andypugh andypugh Apr 20, 2026

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

This sentence seems to be out of place.

Cartesian machines that allows compensating minor alignment inaccuracies
in software.

Expand All @@ -30,6 +31,11 @@ By default identity matrix is used, which is equal to trivial kinematics:
| X_joint | | 1 0 0 | | X_axis |
| Y_joint | = | 0 1 0 | * | Y_axis |
| Z_joint | | 0 0 1 | | Z_axis |
or
| X_joint | | 1 0 0 | | X_axis |
| Y1_joint | = | 0 1 0 | * | Y_axis |
| Y2_joint | = | 0 1 0 | * | Y_axis |
| Z_joint | | 0 0 1 | | Z_axis |
....

Adjusting the calibration matrix allows compensating out many
Expand All @@ -47,6 +53,11 @@ For 3 axis machine, the equations become:
X_joint = C_xx * X_axis + C_xy * Y_axis + C_xz * Z_axis
Y_joint = C_yx * X_axis + C_yy * Y_axis + C_yz * Z_axis
Z_joint = C_zx * X_axis + C_zy * Y_axis + C_zz * Z_axis
or:
X_joint = C_xx * X_axis + C_xy * Y_axis + C_xz * Z_axis
Y1_joint = C_yx * X_axis + C_yy * Y_axis + C_yz * Z_axis
Y2_joint = C_yx * X_axis + C_yy * Y_axis + C_yz * Z_axis
Z_joint = C_zx * X_axis + C_zy * Y_axis + C_zz * Z_axis
....

If the machine has more than 3 axes, the rest are passed through
Expand Down Expand Up @@ -176,9 +187,22 @@ the adjustment values should be added to the old values instead of replacing the
""";
see_also "kins(9)";
pin out bit dummy=1; // halcompile requires at least one pin
option extra_setup;
license "GPL";
;;

#include "kinematics.h"
#include "emcmotcfg.h"

static char *coordinates = "XYZ";
RTAPI_MP_STRING(coordinates, "Existing Axes");

static char *kinstype = "b"; // use KINEMATICS_BOTH
RTAPI_MP_STRING(kinstype, "Kinematics Type (Both)");

bool is_gantry=false;
int num_joints=0;

static struct haldata {
hal_float_t C_xx;
hal_float_t C_xy;
Expand All @@ -191,14 +215,48 @@ static struct haldata {
hal_float_t C_zz;
} *haldata;

static int matrixkins_setup(void) {
bool checkCoordinates(int off){
char* allowed = "ABCUVW";
int len_allowed = strlen(allowed);
int len_coordinates = strlen(coordinates);
int i;
for(i=0; i < len_allowed && i < len_coordinates - off; i++){
if(coordinates[i+off] != allowed[i]){
return false;
}
}
return i == len_coordinates - off;
}

// EXTRA_SETUP is executed before rtapi_app_main()
EXTRA_SETUP() {
(void)prefix;
(void)extra_arg;
(void)__comp_inst;
int res=0;

// inherit comp_id from rtapi_main()
if (comp_id < 0) goto error;

res = hal_set_unready(comp_id);
if (res) goto error;
if(*kinstype != 'b' && *kinstype != 'B') {
rtapi_print("Error: Not supported kinstype %s\n", kinstype);
goto error;
}

num_joints = strlen(coordinates);
if (num_joints > EMCMOT_MAX_JOINTS) {
rtapi_print("Error: To many coordinates %s\n", coordinates);
goto error;
}

if(strncmp(coordinates, "XYZ", 3) == 0 && checkCoordinates(3)) {
is_gantry=false;
}else if(strncmp(coordinates, "XYYZ", 4) == 0 && checkCoordinates(4)) {
is_gantry=true;
}else{
rtapi_print("Error: Not supported coordinates %s\n", coordinates);
goto error;
}

haldata = hal_malloc(sizeof(struct haldata));
if (!haldata) goto error;
Expand All @@ -220,28 +278,24 @@ static int matrixkins_setup(void) {

if (res) goto error;

res = hal_ready(comp_id);
Copy link
Copy Markdown
Collaborator

@andypugh andypugh Apr 20, 2026

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I don't see a replacement for hal_ready() but .comp files don't normally need one. Is this now automatically included with the EXTRA_SETUP macro?

Copy link
Copy Markdown
Contributor Author

@hdiethelm hdiethelm Apr 20, 2026

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I removed also a hal_set_unready(). I think so but i have to check the generated c code.

Copy link
Copy Markdown
Contributor Author

@hdiethelm hdiethelm Apr 20, 2026 •
edited
Loading

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Yes, hal_ready() is called in rtapi_app_main() after EXTRA_SETUP(). Otherwise, it probably would not have worked anyway.
The previous implementation used hal_set_unready(), additional init, hal_ready() at the first call of kinematicsType() which seamed a bit odd to me.

if (res) goto error;

rtapi_print("*** %s setup ok\n",__FILE__);
if(is_gantry){
rtapi_print("%s gantry %s setup ok\n",__FILE__, coordinates);
}else{
rtapi_print("%s standard %s setup ok\n",__FILE__, coordinates);
}
return 0;
error:
rtapi_print("\n!!! %s setup failed res=%d\n\n",__FILE__,res);
rtapi_print("\n %s setup failed res=%d\n\n",__FILE__,res);
return -1;
}

#include "kinematics.h"
#include "emcmotcfg.h"

KINS_NOT_SWITCHABLE
EXPORT_SYMBOL(kinematicsType);
EXPORT_SYMBOL(kinematicsInverse);
EXPORT_SYMBOL(kinematicsForward);

KINEMATICS_TYPE kinematicsType()
{
static bool is_setup=0;
if (!is_setup) matrixkins_setup();
return KINEMATICS_BOTH;
}

Expand Down Expand Up @@ -274,27 +328,49 @@ int kinematicsForward(const double *j,
+ c * (d * h - e * g);
double invdet = 1.0 / det;

// Apply inverse matrix transform to the 3 cartesian coordinates
pos->tran.x = invdet * ( (e * i - f * h) * j[0]
+(c * h - b * i) * j[1]
+(b * f - c * e) * j[2]);

pos->tran.y = invdet * ( (f * g - d * i) * j[0]
+(a * i - c * g) * j[1]
+(c * d - a * f) * j[2]);

pos->tran.z = invdet * ( (d * h - e * g) * j[0]
+(b * g - a * h) * j[1]
+(a * e - b * d) * j[2]);

// Pass rest of axes as identity
if (EMCMOT_MAX_JOINTS > 3) pos->a = j[3];
if (EMCMOT_MAX_JOINTS > 4) pos->b = j[4];
if (EMCMOT_MAX_JOINTS > 5) pos->c = j[5];
if (EMCMOT_MAX_JOINTS > 6) pos->u = j[6];
if (EMCMOT_MAX_JOINTS > 7) pos->v = j[7];
if (EMCMOT_MAX_JOINTS > 8) pos->w = j[8];

if(is_gantry){
// Apply inverse matrix transform to the 3 cartesian coordinates
pos->tran.x = invdet * ( (e * i - f * h) * j[0]
+(c * h - b * i) * (j[1] + j[2])/2
+(b * f - c * e) * j[3]);

pos->tran.y = invdet * ( (f * g - d * i) * j[0]
+(a * i - c * g) * (j[1] + j[2])/2
+(c * d - a * f) * j[3]);

pos->tran.z = invdet * ( (d * h - e * g) * j[0]
+(b * g - a * h) * (j[1] + j[2])/2
+(a * e - b * d) * j[3]);

// Pass rest of axes as identity
if (num_joints > 4) pos->a = j[4];
if (num_joints > 5) pos->b = j[5];
if (num_joints > 6) pos->c = j[6];
if (num_joints > 7) pos->u = j[7];
if (num_joints > 8) pos->v = j[8];
if (num_joints > 9) pos->w = j[9];
}else{
// Apply inverse matrix transform to the 3 cartesian coordinates
pos->tran.x = invdet * ( (e * i - f * h) * j[0]
+(c * h - b * i) * j[1]
+(b * f - c * e) * j[2]);

pos->tran.y = invdet * ( (f * g - d * i) * j[0]
+(a * i - c * g) * j[1]
+(c * d - a * f) * j[2]);

pos->tran.z = invdet * ( (d * h - e * g) * j[0]
+(b * g - a * h) * j[1]
+(a * e - b * d) * j[2]);

// Pass rest of axes as identity
if (num_joints > 3) pos->a = j[3];
if (num_joints > 4) pos->b = j[4];
if (num_joints > 5) pos->c = j[5];
if (num_joints > 6) pos->u = j[6];
if (num_joints > 7) pos->v = j[7];
if (num_joints > 8) pos->w = j[8];
}
return 0;
}

Expand All @@ -315,18 +391,35 @@ int kinematicsInverse(const EmcPose * pos,
double h = haldata->C_zy;
double i = haldata->C_zz;

// Apply matrix transform to the 3 cartesian coordinates
j[0] = pos->tran.x * a + pos->tran.y * b + pos->tran.z * c;
j[1] = pos->tran.x * d + pos->tran.y * e + pos->tran.z * f;
j[2] = pos->tran.x * g + pos->tran.y * h + pos->tran.z * i;

// Pass rest of axes as identity
if (EMCMOT_MAX_JOINTS > 3) j[3] = pos->a;
if (EMCMOT_MAX_JOINTS > 4) j[4] = pos->b;
if (EMCMOT_MAX_JOINTS > 5) j[5] = pos->c;
if (EMCMOT_MAX_JOINTS > 6) j[6] = pos->u;
if (EMCMOT_MAX_JOINTS > 7) j[7] = pos->v;
if (EMCMOT_MAX_JOINTS > 8) j[8] = pos->w;

if(is_gantry){
// Apply matrix transform to the 3 cartesian coordinates
j[0] = pos->tran.x * a + pos->tran.y * b + pos->tran.z * c;
j[1] = pos->tran.x * d + pos->tran.y * e + pos->tran.z * f;
j[2] = j[1];
j[3] = pos->tran.x * g + pos->tran.y * h + pos->tran.z * i;

// Pass rest of axes as identity
if (num_joints > 4) j[4] = pos->a;
if (num_joints > 5) j[5] = pos->b;
if (num_joints > 6) j[6] = pos->c;
if (num_joints > 7) j[7] = pos->u;
if (num_joints > 8) j[8] = pos->v;
if (num_joints > 9) j[9] = pos->w;
}else{
// Apply matrix transform to the 3 cartesian coordinates
j[0] = pos->tran.x * a + pos->tran.y * b + pos->tran.z * c;
j[1] = pos->tran.x * d + pos->tran.y * e + pos->tran.z * f;
j[2] = pos->tran.x * g + pos->tran.y * h + pos->tran.z * i;

// Pass rest of axes as identity
if (num_joints > 3) j[3] = pos->a;
if (num_joints > 4) j[4] = pos->b;
if (num_joints > 5) j[5] = pos->c;
if (num_joints > 6) j[6] = pos->u;
if (num_joints > 7) j[7] = pos->v;
if (num_joints > 8) j[8] = pos->w;
}

return 0;
}
Loading