Skip to content

switchkins: allow more than three kinematics types #4372

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
grandixximo wants to merge 3 commits into
base: master
Choose a base branch
from
+137 −90
Open

switchkins: allow more than three kinematics types #4372

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
708f86b
switchkins: reject a bad kinematics type before switching to it
grandixximo Aug 11, 2026
a39d478
switchkins: allow more than three kinematics types
grandixximo Aug 11, 2026
8db6fbf
switchkins: let switchkinsRegister() provide any kinematics type
grandixximo Aug 11, 2026
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
35 changes: 29 additions & 6 deletions docs/src/motion/switchkins.adoc
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -128,6 +128,9 @@ program behavior in accordance with the active kinematics type.
. *kinstype.is-1* Output (bit)
. *kinstype.is-2* Output (bit)

A module providing more than three kinematics types has one
'kinstype.is-N' pin per type.

== Usage

=== HAL Connections
Expand Down Expand Up @@ -388,13 +391,33 @@ routines and the functions for forward an inverse calculation for
each kinstype (0,1,2) and sets a number of configuration
settings.

A module can provide further kinstypes by calling
switchkinsRegister() from within switchkinsSetup(), once per
kinstype:

----
int switchkinsRegister(int ktype, KS kset, KF kfwd, KI kinv);
----

'ktype' runs from 0 to SWITCHKINS_MAX_TYPES-1 (defined in
switchkins.h). A kinstype has to come from one route or the
other, so registering one that switchkinsSetup() has already
filled in is an error, and so is leaving a gap below the highest
kinstype provided. Either mistake fails the module load and says
which kinstype is at fault.

Each kinstype gets its own 'kinstype.is-N' pin, so a module
providing the usual three keeps the pin names it always had.

After calling switchkinsSetup(), rtapi_app_main() checks the
supplied parameters, creates a HAL component, and then invokes
the setup routine identified for each kinstype (0,1,2).

Each kinstype (0,1,2) setup routine can (optionally) create HAL
pins and set them to default values. When all setup routines
finish, rtapi_app_main() issues hal_ready() for the component
to complete creation of the module.
the setup routine identified for each kinstype.

Each kinstype setup routine can (optionally) create HAL
pins and set them to default values. A setup routine is called
once per kinstype it is registered for, so a routine used for two
kinstypes must not create the same pin twice. When all setup
routines finish, rtapi_app_main() issues hal_ready() for the
component to complete creation of the module.

// vim: set syntax=asciidoc:
184 changes: 102 additions & 82 deletions src/emc/kinematics/switchkins.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -38,19 +38,18 @@
// kinematic functions (default=0 for err detection):
static kparms kp; // kinematics parms (common all types)

static KF kfwd0 = NULL; // 0==switchkins_type kinematics forward
static KF kfwd1 = NULL; // 1
static KF kfwd2 = NULL; // 2
// indexed by switchkins_type (NULL==not provided, for err detection):
static KS ksetups[SWITCHKINS_MAX_TYPES] = {NULL};
static KF kfwds[SWITCHKINS_MAX_TYPES] = {NULL};
static KI kinvs[SWITCHKINS_MAX_TYPES] = {NULL};

static KI kinv0 = NULL; // 0==switchkins_type kinematics inverse
static KI kinv1 = NULL; // 1
static KI kinv2 = NULL; // 2
// types provided, counted in rtapi_app_main() once they are all in
static int kins_count;
static int register_error;

static int switchkins_type;
static struct swdata {
hal_bool_t kinstype_is_0;
hal_bool_t kinstype_is_1;
hal_bool_t kinstype_is_2;
hal_bool_t kinstype_is[SWITCHKINS_MAX_TYPES];

hal_real_t gui_x;
hal_real_t gui_y;
Expand Down Expand Up @@ -104,15 +103,16 @@ static int gui_forward_kins(const double *joints)
int res;
KINEMATICS_FORWARD_FLAGS fflags = 0;
KINEMATICS_INVERSE_FLAGS iflags;
switch (kp.gui_kinstype) {
case 0: res = kfwd0(joints, &lastpose[0], &fflags, &iflags);break;
case 1: res = kfwd1(joints, &lastpose[1], &fflags, &iflags);break;
case 2: res = kfwd2(joints, &lastpose[2], &fflags, &iflags);break;
default: rtapi_print_msg(RTAPI_MSG_ERR,
"gui_forward_kins BAD gui_kinstype <%d>\n",
kp.gui_kinstype);
return -1;
}
if ( kp.gui_kinstype < 0
|| kp.gui_kinstype >= kins_count
|| !kfwds[kp.gui_kinstype]) {
rtapi_print_msg(RTAPI_MSG_ERR,
"gui_forward_kins BAD gui_kinstype <%d>\n",
kp.gui_kinstype);
return -1;
}
res = kfwds[kp.gui_kinstype](joints, &lastpose[kp.gui_kinstype],
&fflags, &iflags);
hal_set_real(swdata->gui_x, lastpose[kp.gui_kinstype].tran.x);
hal_set_real(swdata->gui_y, lastpose[kp.gui_kinstype].tran.y);
hal_set_real(swdata->gui_z, lastpose[kp.gui_kinstype].tran.z);
Expand All @@ -128,36 +128,25 @@ int kinematicsSwitchable() {return 1;}
int kinematicsSwitch(int new_switchkins_type)
{
int k;

// reject first, so a bad request leaves the running kinematics alone
if (new_switchkins_type < 0 || new_switchkins_type >= kins_count) {
rtapi_print_msg(RTAPI_MSG_ERR,
"kinematicsSwitch:BAD VALUE <%d>\n",
new_switchkins_type);
return -1; // FAIL
}

for (k=0; k< SWITCHKINS_MAX_TYPES; k++) { use_lastpose[k] = 0;}

switchkins_type = new_switchkins_type;
switch (switchkins_type) {
case 0: rtapi_print_msg(RTAPI_MSG_INFO,
"kinematicsSwitch:TYPE0\n");
hal_set_bool(swdata->kinstype_is_0, 1);
hal_set_bool(swdata->kinstype_is_1, 0);
hal_set_bool(swdata->kinstype_is_2, 0);
break;
case 1: rtapi_print_msg(RTAPI_MSG_INFO,
"kinematicsSwitch:TYPE1\n");
hal_set_bool(swdata->kinstype_is_0, 0);
hal_set_bool(swdata->kinstype_is_1, 1);
hal_set_bool(swdata->kinstype_is_2, 0);
break;
case 2: rtapi_print_msg(RTAPI_MSG_INFO,
"kinematicsSwitch:TYPE2\n");
hal_set_bool(swdata->kinstype_is_0, 0);
hal_set_bool(swdata->kinstype_is_1, 0);
hal_set_bool(swdata->kinstype_is_2, 1);
break;
default: rtapi_print_msg(RTAPI_MSG_ERR,
"kinematicsSwitch:BAD VALUE <%d>\n",
switchkins_type);
hal_set_bool(swdata->kinstype_is_1, 0);
hal_set_bool(swdata->kinstype_is_0, 0);
hal_set_bool(swdata->kinstype_is_2, 0);
return -1; // FAIL

rtapi_print_msg(RTAPI_MSG_INFO,
"kinematicsSwitch:TYPE%d\n", switchkins_type);
for (k=0; k < kins_count; k++) {
hal_set_bool(swdata->kinstype_is[k], k == switchkins_type);
Comment thread
BsAtHome marked this conversation as resolved.
}

if (fwd_iterates[switchkins_type]) {
use_lastpose[switchkins_type] = 1; // restarting a kins types
}
Expand All @@ -177,15 +166,15 @@ int kinematicsForward(const double *joint,
use_lastpose[switchkins_type] = 0;
}

switch (switchkins_type) {
case 0: r = kfwd0(joint, pos, fflags, iflags); break;
case 1: r = kfwd1(joint, pos, fflags, iflags); break;
case 2: r = kfwd2(joint, pos, fflags, iflags); break;
default: rtapi_print_msg(RTAPI_MSG_ERR,
"switchkins: Forward BAD switchkins_type </%d>\n",
switchkins_type);
return -1;
if ( switchkins_type < 0
|| switchkins_type >= kins_count
|| !kfwds[switchkins_type]) {
rtapi_print_msg(RTAPI_MSG_ERR,
"switchkins: Forward BAD switchkins_type </%d>\n",
switchkins_type);
return -1;
}
r = kfwds[switchkins_type](joint, pos, fflags, iflags);
if (fwd_iterates[switchkins_type]) {save_lastpose(switchkins_type,pos);}
if (r) return r;

Expand All @@ -211,15 +200,15 @@ int kinematicsInverse(const EmcPose * pos,
{
int r;

switch (switchkins_type) {
case 0: r = kinv0(pos, joint, iflags, fflags); break;
case 1: r = kinv1(pos, joint, iflags, fflags); break;
case 2: r = kinv2(pos, joint, iflags, fflags); break;
default: rtapi_print_msg(RTAPI_MSG_ERR,
"switchkins: Inverse BAD switchkins_type </%d>\n",
switchkins_type);
return -1;
if ( switchkins_type < 0
|| switchkins_type >= kins_count
|| !kinvs[switchkins_type]) {
rtapi_print_msg(RTAPI_MSG_ERR,
"switchkins: Inverse BAD switchkins_type </%d>\n",
switchkins_type);
return -1;
}
r = kinvs[switchkins_type](pos, joint, iflags, fflags);
return r;
} // kinematicsInverse()

Expand All @@ -228,6 +217,29 @@ KINEMATICS_TYPE kinematicsType()
return KINEMATICS_BOTH;
}

int switchkinsRegister(int ktype, KS kset, KF kfwd, KI kinv)
{
if (ktype < 0 || ktype >= SWITCHKINS_MAX_TYPES) {
rtapi_print_msg(RTAPI_MSG_ERR,
"switchkinsRegister: BAD switchkins_type <%d>"
" (must be 0..%d)\n",
ktype, SWITCHKINS_MAX_TYPES - 1);
register_error = 1;
return -1;
}
if (ksetups[ktype] || kfwds[ktype] || kinvs[ktype]) {
rtapi_print_msg(RTAPI_MSG_ERR,
"switchkinsRegister: switchkins-type %d"
" already provided\n", ktype);
register_error = 1;
return -1;
}
Comment on lines +220 to +236

@BsAtHome BsAtHome Aug 11, 2026

Copy link
Copy Markdown
Contributor

Choose a reason for hiding this comment

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

Wouldn't it be more consistent to allow switchkinsRegister to supplant the old switchkinsSetup?

The real issue is to deal with is empty/skipping slots. That problem will be present also for any entry larger than 3.

@grandixximo grandixximo Aug 11, 2026

Copy link
Copy Markdown
Contributor Author

Choose a reason for hiding this comment

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

Supplanting switchkinsSetup(): agreed the split is inconsistent. It cannot be replaced without making it optional, since every module defines it and rtapi_app_main() calls it unconditionally. Either convert the eight in-tree modules and drop it, which breaks out of tree modules at compile time, or declare it weak and call it only if present, which I can only test on uspace. Cheap step either way is dropping the ktype < 3 restriction so registration covers 0 to 2 as well. Which end state do you want?

Gaps: already refused at module load, not at switch time. scarakins registering type 5 without 3 and 4 gives

Switchkins FAIL scarakins:<Missing setup function>

The message is the problem, it names neither the type nor the gap. Same area: a double registration silently overwrites, and nobody checks the return of switchkinsRegister(). I will name the missing type, reject double registration, and fail the load on a rejected registration.

@BsAtHome BsAtHome Aug 11, 2026

Copy link
Copy Markdown
Contributor

Choose a reason for hiding this comment

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

The path of least resistance would be to drop the ktype < 3 condition.

In the long run, we should make it generic. I'm also confused a bit because there are examples in hal/components that do their own switch. Maybe this is the reason why the current construct feels messy and inconsistent.

@grandixximo grandixximo Aug 11, 2026

Copy link
Copy Markdown
Contributor Author

Choose a reason for hiding this comment

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

Done, in a separate commit.

ktype runs from 0 now, and the count comes from the highest type filled by either route, so a module can register all of them and leave the switchkinsSetup() arguments alone. A type has to come from one route or the other. Registering one that switchkinsSetup() already filled, leaving a gap, and an out of range type are all refused at load time, naming the type at fault.

On the components with their own switch: millturn, xyzab_tdr_kins, xyzacb_trsrn and xyzbca_trsrn in src/objects/hal/components implement kinematicsSwitch(), forward, inverse and their own kinstype.is-N pins without linking switchkins.o, so there are two implementations of the same thing in the tree. They also all have the assign before validate bug the first commit fixes. Folding them onto switchkins.c looks doable now the type count is not fixed. Do you want that in this PR or a separate one?

For context on where I am going with this: I have G12.1 and G13.1 working locally to select the kinematics from G-code, with the interpreter tracking which one is active and exposing it as #<_kins_type>. Selecting the kinematics through a HAL pin and then forcing a queue buster with M66 E0 L0 is the wrong shape: it is a state change the interpreter has to know about, and today it does not, so #<_kins_type> and anything built on it can disagree with what motion is running. The intention is to make G-code the proper way to switch and deprecate the HAL route, which is also what makes more than three types worth having.

ksetups[ktype] = kset;
kfwds[ktype] = kfwd;
kinvs[ktype] = kinv;
return 0;
} // switchkinsRegister()

//*********************************************************************
static char *coordinates;
RTAPI_MP_STRING(coordinates, "Axes-to-joints-ordering");
Expand All @@ -239,6 +251,7 @@ EXPORT_SYMBOL(kinematicsSwitch);
EXPORT_SYMBOL(kinematicsType);
EXPORT_SYMBOL(kinematicsForward);
EXPORT_SYMBOL(kinematicsInverse);
EXPORT_SYMBOL(switchkinsRegister);
MODULE_LICENSE("GPL");

static int comp_id;
Expand All @@ -259,15 +272,19 @@ int rtapi_app_main(void)

kp.sparm = sparm; // module parm passed to kins

KS ksetup0 = NULL;
KS ksetup1 = NULL;
KS ksetup2 = NULL;

// may also call switchkinsRegister()
res = switchkinsSetup(&kp,
&ksetup0, &ksetup1, &ksetup2,
&kfwd0, &kfwd1, &kfwd2,
&kinv0, &kinv1, &kinv2);
&ksetups[0], &ksetups[1], &ksetups[2],
&kfwds[0], &kfwds[1], &kfwds[2],
&kinvs[0], &kinvs[1], &kinvs[2]);
if (res) {emsg="switchkinsSetp FAIL"; goto error;}
if (register_error) {emsg="switchkinsRegister FAIL"; goto error;}

// the highest type provided by either route sets the count
for (i=0; i < SWITCHKINS_MAX_TYPES; i++) {
if (ksetups[i] || kfwds[i] || kinvs[i]) { kins_count = i + 1; }
}
if (!kins_count) { emsg = "no switchkins-types provided"; goto error; }

for (i=0; i < SWITCHKINS_MAX_TYPES; i++) {
if (kp.fwd_iterates_mask & (1<<i)) {
Expand All @@ -286,18 +303,20 @@ int rtapi_app_main(void)
if (kp.max_joints <= 0 || kp.max_joints > EMCMOT_MAX_JOINTS) {
emsg = "bogus max_joints"; goto error;
}
if (kp.gui_kinstype >= SWITCHKINS_MAX_TYPES) {
if (kp.gui_kinstype >= kins_count) {
emsg = "bogus gui_kinstype"; goto error;
}

if (!ksetup0 || !ksetup1 || !ksetup2) {
emsg = "Missing setup function"; goto error;
}
if (!kfwd0 || !kfwd1 || !kfwd2) {
emsg = "Missing fwd functionn"; goto error;
}
if (!kinv0 || !kinv1 || !kinv2) {
emsg = "Missing inv function"; goto error;
// a type left out below the highest one provided is a gap, not a count
for (i=0; i < kins_count; i++) {
if (ksetups[i] && kfwds[i] && kinvs[i]) { continue; }
rtapi_print_msg(RTAPI_MSG_ERR,
"switchkins: switchkins-type %d incomplete:%s%s%s\n",
i,
ksetups[i] ? "" : " no setup",
kfwds[i] ? "" : " no forward",
kinvs[i] ? "" : " no inverse");
emsg = "incomplete switchkins-type"; goto error;
}

comp_id = hal_init(kp.kinsname);
Expand All @@ -306,9 +325,10 @@ int rtapi_app_main(void)
swdata = hal_malloc(sizeof(struct swdata));
if (!swdata) goto error;

res += hal_pin_new_bool(comp_id, HAL_OUT, &(swdata->kinstype_is_0), 0, "kinstype.is-0");
res += hal_pin_new_bool(comp_id, HAL_OUT, &(swdata->kinstype_is_1), 0, "kinstype.is-1");
res += hal_pin_new_bool(comp_id, HAL_OUT, &(swdata->kinstype_is_2), 0, "kinstype.is-2");
for (i=0; i < kins_count; i++) {
res += hal_pin_new_bool(comp_id, HAL_OUT, &(swdata->kinstype_is[i]),
0, "kinstype.is-%d", i);
}

if (kp.gui_kinstype >=0) {
res += hal_pin_new_real(comp_id, HAL_IN, &swdata->gui_x, 0.0, "skgui.x");
Expand All @@ -325,9 +345,9 @@ int rtapi_app_main(void)

if (!coordinates) {coordinates = kp.required_coordinates;}

ksetup0(comp_id,coordinates,&kp);
ksetup1(comp_id,coordinates,&kp);
ksetup2(comp_id,coordinates,&kp);
for (i=0; i < kins_count; i++) {
ksetups[i](comp_id,coordinates,&kp);
}

hal_ready(comp_id);
return 0;
Expand Down
8 changes: 6 additions & 2 deletions src/emc/kinematics/switchkins.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -6,8 +6,8 @@

#include <kinematics.h>

//hardcoded number of switchkins types (KS,KF,KI):
#define SWITCHKINS_MAX_TYPES 3
//max number of switchkins types (KS,KF,KI) a module may provide:
#define SWITCHKINS_MAX_TYPES 9

// KinematicsFORWARD functions
typedef int (*KF)(const double *joint,
Expand All @@ -28,9 +28,13 @@ typedef int (*KS)(const int comp_id, // halpins
);

//*********************************************************************
// supplied by the using module, provides types 0,1,2
extern int switchkinsSetup(kparms* ksetup_parms,
KS* kset0, KS* kset1, KS* kset2,
KF* kfwd0, KF* kfwd1, KF* kfwd2,
KI* kinv0, KI* kinv1, KI* kinv2
);

// called from switchkinsSetup(), once per type it does not provide itself
extern int switchkinsRegister(int ktype, KS kset, KF kfwd, KI kinv);
#endif // }
Loading