From fa59eb7d12ec388aefbce3434031f80cff8d5a5a Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Fri, 28 Aug 2026 18:18:42 +1000 Subject: [PATCH 1/3] switchkins: seed iterative forward kinematics from the caller's estimate A type with iterative forward kinematics restarts from the pose it last produced. That pose only accumulates while the type is running, so a type that has never run has nothing to start from. With identity kinematics selected first, genhexkins reported the hexapod platform at the origin instead of the place it is at, and switching to it adopted that position. Track whether a stored pose exists. Without one, start from the estimate the caller supplies, which motion seeds from the [TRAJ]HOME world home, and keep the result only when the solve succeeds. A gui type that cannot solve no longer fails the running type either. Those pins drive a vismach display, and a stale reading there is not a reason to report that the kinematics motion is running have failed. --- src/emc/kinematics/switchkins.c | 26 ++++++++++++++++++++------ 1 file changed, 20 insertions(+), 6 deletions(-) diff --git a/src/emc/kinematics/switchkins.c b/src/emc/kinematics/switchkins.c index bf87adb60ce..e4d35bb535e 100644 --- a/src/emc/kinematics/switchkins.c +++ b/src/emc/kinematics/switchkins.c @@ -74,6 +74,7 @@ static struct swdata { // then save/use the lastpose static int fwd_iterates[SWITCHKINS_MAX_TYPES] = {0}; static bool use_lastpose[SWITCHKINS_MAX_TYPES] = {0}; +static bool lastpose_ok[SWITCHKINS_MAX_TYPES] = {0}; static EmcPose lastpose[SWITCHKINS_MAX_TYPES]; static void save_lastpose(int ktype, EmcPose* pos) @@ -102,7 +103,7 @@ static void get_lastpose(int ktype, EmcPose* pos) pos->w = lastpose[ktype].w; } // get_lastpose() -static int gui_forward_kins(const double *joints) +static int gui_forward_kins(const double *joints, const EmcPose* estimate) { // the hexapod vismach gui uses these hal pins to // display platform position/orientation in both @@ -119,8 +120,13 @@ static int gui_forward_kins(const double *joints) kp.gui_kinstype); return -1; } + if (!lastpose_ok[kp.gui_kinstype]) { + // no pose of our own yet, start from the caller's + lastpose[kp.gui_kinstype] = *estimate; + } res = kfwds[kp.gui_kinstype](joints, &lastpose[kp.gui_kinstype], &fflags, &iflags); + lastpose_ok[kp.gui_kinstype] = (res == 0); 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); @@ -155,7 +161,7 @@ int kinematicsSwitch(int new_switchkins_type) hal_set_bool(swdata->kinstype_is[k], k == switchkins_type); } - if (fwd_iterates[switchkins_type]) { + if (fwd_iterates[switchkins_type] && lastpose_ok[switchkins_type]) { use_lastpose[switchkins_type] = 1; // restarting a kins types } return 0; // 0==> no error @@ -167,8 +173,11 @@ int kinematicsForward(const double *joint, KINEMATICS_INVERSE_FLAGS * iflags) { int r; + EmcPose estimate = *pos; // the caller's guess, the only one we get - if (fwd_iterates[switchkins_type] && use_lastpose[switchkins_type]) { + if ( fwd_iterates[switchkins_type] + && use_lastpose[switchkins_type] + && lastpose_ok[switchkins_type]) { // initialize iterative forward kins (ok for identity too) get_lastpose(switchkins_type,pos); use_lastpose[switchkins_type] = 0; @@ -183,7 +192,10 @@ int kinematicsForward(const double *joint, return -1; } r = kfwds[switchkins_type](joint, pos, fflags, iflags); - if (fwd_iterates[switchkins_type]) {save_lastpose(switchkins_type,pos);} + if (fwd_iterates[switchkins_type]) { + save_lastpose(switchkins_type,pos); + lastpose_ok[switchkins_type] = (r == 0); + } if (r) return r; // gui.* pins created only if gui_kinstype>=0 @@ -195,10 +207,12 @@ int kinematicsForward(const double *joint, // currently the skgui pins are only needed for // the hexagui vismach program (as it needs // world coords for switchkin-types - r = gui_forward_kins(joint); + // display only: a gui type that cannot solve leaves its pins + // where they were and does not fail the running type + gui_forward_kins(joint, &estimate); } - return r; + return 0; } // kinematicsForward() int kinematicsInverse(const EmcPose * pos, From 39209a4bf0363a868739d07577756b58fed48f9a Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Fri, 28 Aug 2026 18:18:42 +1000 Subject: [PATCH 2/3] motion: keep the known position when a kinematics switch cannot solve handle_kinematicsSwitch() ran the forward kinematics of the new type straight into carte_pos_cmd and ignored the result. A type whose forward kinematics fail at the current pose therefore left whatever the failed solver had written in carte_pos_cmd, and tpSetPos() made that the machine's idea of where it is. The next move ran from a position the machine is not at. Solve into a local pose and commit it only on success, otherwise report the failure and set the motion error flag. --- src/emc/motion/control.c | 12 +++++++++--- 1 file changed, 9 insertions(+), 3 deletions(-) diff --git a/src/emc/motion/control.c b/src/emc/motion/control.c index 6c135e7625e..50ddadc6c07 100644 --- a/src/emc/motion/control.c +++ b/src/emc/motion/control.c @@ -376,9 +376,15 @@ static void handle_kinematicsSwitch(void) { beforePose[anum] = *pcmd_p[anum]; } #endif - kinematicsForward(joint_posKinsSwitch, - &emcmotStatus->carte_pos_cmd, - &tmpFFlags, &tmpIFlags); + EmcPose poseKinsSwitch = emcmotStatus->carte_pos_cmd; + if (kinematicsForward(joint_posKinsSwitch, &poseKinsSwitch, + &tmpFFlags, &tmpIFlags)) { + reportError(_("kinematicsForward failed for kinematics type %d"), + switchkins_type); + SET_MOTION_ERROR_FLAG(1); // abort + return; // keep the position we know rather than an unsolved one + } + emcmotStatus->carte_pos_cmd = poseKinsSwitch; #ifdef SWITCHKINS_DEBUG fprintf(stderr,"kswitch type=%d (%s:%d)\n",switchkins_type,__FUNCTION__,__LINE__); for (anum = 0; anum < EMCMOT_MAX_AXIS; anum++) { From 76c16f96732220fba02bd7883770852aea92e69d Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Fri, 28 Aug 2026 18:18:42 +1000 Subject: [PATCH 3/3] kinematics: offer identityfirst in the remaining switchkins modules xyzac-trt-kins, xyzbc-trt-kins and 5axiskins accept the module string parameter sparm=identityfirst, which makes switchkins-type 0 identity kinematics and switchkins-type 1 the module's own. genserkins, pumakins, scarakins, three21kins and genhexkins did not, so those machines always start in the kinematics that can fail. A failing model kinematics is exactly where a machine needs identity kinematics: motion reports the error and disables the machine, and the operator needs a mode that solves at the current pose to move clear. motion.switchkins-type rests at zero and the shipped configs drive it from motion.analog-out-03, which is also zero until an M68 runs, so type 0 is both the type a machine starts in and the type it falls back to. --- docs/src/motion/switchkins.adoc | 22 ++++++++++++--- src/emc/kinematics/genhexkins.c | 43 ++++++++++++++++++---------- src/emc/kinematics/genserkins.c | 32 +++++++++++++++------ src/emc/kinematics/pumakins.c | 48 ++++++++++++++++++++++---------- src/emc/kinematics/scarakins.c | 33 +++++++++++++++------- src/emc/kinematics/three21kins.c | 32 +++++++++++++++------ 6 files changed, 149 insertions(+), 61 deletions(-) diff --git a/docs/src/motion/switchkins.adoc b/docs/src/motion/switchkins.adoc index 7484c50b51b..8c1fea13e24 100644 --- a/docs/src/motion/switchkins.adoc +++ b/docs/src/motion/switchkins.adoc @@ -47,10 +47,10 @@ The following kinematics modules support switchable kinematics: . *scarakins* (type0:scarakins type1:identity) . *5axiskins* (type0:5axiskins type1:identity) (bridgemill) -The xyz[ab]c-trt-kins modules by default use type0==xyz[ab]c-trt-kins -for backwards compatibility. The provided sim configs alter the -type0/type1 convention by forcing type0==identity kinematics using the -module string parameter 'sparm' with an INI file setting like: +Every module listed above uses its own kinematics for type0 and +identity kinematics for type1. Each accepts the module string +parameter 'sparm' to swap the two, so that the machine starts in +identity kinematics and the module kinematics are selected on demand: [source,ini] ---- @@ -59,6 +59,20 @@ KINEMATICS = xyzac-trt-kins sparm=identityfirst # ... ---- +Starting in identity kinematics leaves a way out of poses the module +kinematics cannot solve. A module kinematics failure (near a +singularity, for instance) reports an error and disables the machine; +type0 is then reached without running the failing kinematics again. + +[NOTE] +The 'sparm' setting exchanges type0 and type1, so any G-code or HAL +logic that selects a kinematics type by number must match. + +Kinematics that solve the forward direction iteratively, genhexkins +among them, need a pose to start from. While another type is running +they take the estimate the caller supplies, which motion seeds from +the '[TRAJ]HOME' world home, so they are ready to be switched to. + === Identity letter assignments When using an *identity* kinematics type, the module parameter diff --git a/src/emc/kinematics/genhexkins.c b/src/emc/kinematics/genhexkins.c index 1e9412a04b6..2966c377852 100644 --- a/src/emc/kinematics/genhexkins.c +++ b/src/emc/kinematics/genhexkins.c @@ -693,20 +693,35 @@ int switchkinsSetup(kparms* kp, kp->required_coordinates = "xyzabc"; kp->max_joints = strlen(kp->required_coordinates); kp->allow_duplicates = 0; - kp->fwd_iterates_mask = 0x1; //genhexkins switchkins_type==0 - kp->gui_kinstype = 0; //vismach gui for switchkins_type==0 - - // switchkins_type==0 is startup default - // kins with iterative forward algorithm should be switchkins_type==0 - *kset0 = genhexKinematicsSetup; - *kfwd0 = genhexKinematicsForward; - *kinv0 = genhexKinematicsInverse; - - *kset1 = identityKinematicsSetup; - *kfwd1 = identityKinematicsForward; - *kinv1 = identityKinematicsInverse; - switchkinsDeclare(0, KINSTYPE_PRIMARY); - switchkinsDeclare(1, KINSTYPE_IDENTITY); + if (kp->sparm && strstr(kp->sparm,"identityfirst")) { + rtapi_print("\n!!! switchkins-type 0 is IDENTITY\n"); + kp->fwd_iterates_mask = 0x2; //genhexkins switchkins_type==1 + kp->gui_kinstype = 1; //vismach gui for switchkins_type==1 + + *kset0 = identityKinematicsSetup; + *kfwd0 = identityKinematicsForward; + *kinv0 = identityKinematicsInverse; + + *kset1 = genhexKinematicsSetup; + *kfwd1 = genhexKinematicsForward; + *kinv1 = genhexKinematicsInverse; + switchkinsDeclare(0, KINSTYPE_IDENTITY); + switchkinsDeclare(1, KINSTYPE_PRIMARY); + } else { + rtapi_print("\n!!! switchkins-type 0 is %s\n",kp->kinsname); + kp->fwd_iterates_mask = 0x1; //genhexkins switchkins_type==0 + kp->gui_kinstype = 0; //vismach gui for switchkins_type==0 + + *kset0 = genhexKinematicsSetup; + *kfwd0 = genhexKinematicsForward; + *kinv0 = genhexKinematicsInverse; + + *kset1 = identityKinematicsSetup; + *kfwd1 = identityKinematicsForward; + *kinv1 = identityKinematicsInverse; + switchkinsDeclare(0, KINSTYPE_PRIMARY); + switchkinsDeclare(1, KINSTYPE_IDENTITY); + } *kset2 = userkKinematicsSetup; *kfwd2 = userkKinematicsForward; diff --git a/src/emc/kinematics/genserkins.c b/src/emc/kinematics/genserkins.c index cc111c1a3c4..fa8d30598dc 100644 --- a/src/emc/kinematics/genserkins.c +++ b/src/emc/kinematics/genserkins.c @@ -63,15 +63,29 @@ int switchkinsSetup(kparms* kp, kp->max_joints = strlen(kp->required_coordinates); kp->allow_duplicates = 0; - *kset0 = genserKinematicsSetup; - *kfwd0 = genserKinematicsForward; - *kinv0 = genserKinematicsInverse; - - *kset1 = identityKinematicsSetup; - *kfwd1 = identityKinematicsForward; - *kinv1 = identityKinematicsInverse; - switchkinsDeclare(0, KINSTYPE_PRIMARY); - switchkinsDeclare(1, KINSTYPE_IDENTITY); + if (kp->sparm && strstr(kp->sparm,"identityfirst")) { + rtapi_print("\n!!! switchkins-type 0 is IDENTITY\n"); + *kset0 = identityKinematicsSetup; + *kfwd0 = identityKinematicsForward; + *kinv0 = identityKinematicsInverse; + + *kset1 = genserKinematicsSetup; + *kfwd1 = genserKinematicsForward; + *kinv1 = genserKinematicsInverse; + switchkinsDeclare(0, KINSTYPE_IDENTITY); + switchkinsDeclare(1, KINSTYPE_PRIMARY); + } else { + rtapi_print("\n!!! switchkins-type 0 is %s\n",kp->kinsname); + *kset0 = genserKinematicsSetup; + *kfwd0 = genserKinematicsForward; + *kinv0 = genserKinematicsInverse; + + *kset1 = identityKinematicsSetup; + *kfwd1 = identityKinematicsForward; + *kinv1 = identityKinematicsInverse; + switchkinsDeclare(0, KINSTYPE_PRIMARY); + switchkinsDeclare(1, KINSTYPE_IDENTITY); + } *kset2 = userkKinematicsSetup; *kfwd2 = userkKinematicsForward; diff --git a/src/emc/kinematics/pumakins.c b/src/emc/kinematics/pumakins.c index b1dfcf9921c..98dc83aa4c5 100644 --- a/src/emc/kinematics/pumakins.c +++ b/src/emc/kinematics/pumakins.c @@ -415,21 +415,39 @@ int switchkinsSetup(kparms* kp, kp->allow_duplicates = 0; kp->max_joints = strlen(kp->required_coordinates); - rtapi_print("\n!!! switchkins-type 0 is %s\n",kp->kinsname); - *kset0 = pumaKinematicsSetup; - *kfwd0 = pumaKinematicsForward; - *kinv0 = pumaKinematicsInverse; - // the maths is the ISO 9787 flange frame, so the tool axis it produces - // runs holder towards tip, the opposite of the convention - switchkinsRegisterFrames(0, pumaKinematicsWorkFrame, - pumaKinematicsToolFrame, - &TOOL_FRAME_FLANGE); - - *kset1 = identityKinematicsSetup; - *kfwd1 = identityKinematicsForward; - *kinv1 = identityKinematicsInverse; - switchkinsDeclare(0, KINSTYPE_PRIMARY); - switchkinsDeclare(1, KINSTYPE_IDENTITY); + if (kp->sparm && strstr(kp->sparm,"identityfirst")) { + rtapi_print("\n!!! switchkins-type 0 is IDENTITY\n"); + *kset0 = identityKinematicsSetup; + *kfwd0 = identityKinematicsForward; + *kinv0 = identityKinematicsInverse; + + *kset1 = pumaKinematicsSetup; + *kfwd1 = pumaKinematicsForward; + *kinv1 = pumaKinematicsInverse; + // the maths is the ISO 9787 flange frame, so the tool axis it produces + // runs holder towards tip, the opposite of the convention + switchkinsRegisterFrames(1, pumaKinematicsWorkFrame, + pumaKinematicsToolFrame, + &TOOL_FRAME_FLANGE); + switchkinsDeclare(0, KINSTYPE_IDENTITY); + switchkinsDeclare(1, KINSTYPE_PRIMARY); + } else { + rtapi_print("\n!!! switchkins-type 0 is %s\n",kp->kinsname); + *kset0 = pumaKinematicsSetup; + *kfwd0 = pumaKinematicsForward; + *kinv0 = pumaKinematicsInverse; + // the maths is the ISO 9787 flange frame, so the tool axis it produces + // runs holder towards tip, the opposite of the convention + switchkinsRegisterFrames(0, pumaKinematicsWorkFrame, + pumaKinematicsToolFrame, + &TOOL_FRAME_FLANGE); + + *kset1 = identityKinematicsSetup; + *kfwd1 = identityKinematicsForward; + *kinv1 = identityKinematicsInverse; + switchkinsDeclare(0, KINSTYPE_PRIMARY); + switchkinsDeclare(1, KINSTYPE_IDENTITY); + } *kset2 = userkKinematicsSetup; *kfwd2 = userkKinematicsForward; diff --git a/src/emc/kinematics/scarakins.c b/src/emc/kinematics/scarakins.c index f2716fdd509..8db57eb7b0c 100644 --- a/src/emc/kinematics/scarakins.c +++ b/src/emc/kinematics/scarakins.c @@ -221,16 +221,29 @@ int switchkinsSetup(kparms* kp, kp->allow_duplicates = 0; kp->max_joints = strlen(kp->required_coordinates); - rtapi_print("\n!!! switchkins-type 0 is %s\n",kp->kinsname); - *kset0 = scaraKinematicsSetup; - *kfwd0 = scaraKinematicsForward; - *kinv0 = scaraKinematicsInverse; - - *kset1 = identityKinematicsSetup; - *kfwd1 = identityKinematicsForward; - *kinv1 = identityKinematicsInverse; - switchkinsDeclare(0, KINSTYPE_PRIMARY); - switchkinsDeclare(1, KINSTYPE_IDENTITY); + if (kp->sparm && strstr(kp->sparm,"identityfirst")) { + rtapi_print("\n!!! switchkins-type 0 is IDENTITY\n"); + *kset0 = identityKinematicsSetup; + *kfwd0 = identityKinematicsForward; + *kinv0 = identityKinematicsInverse; + + *kset1 = scaraKinematicsSetup; + *kfwd1 = scaraKinematicsForward; + *kinv1 = scaraKinematicsInverse; + switchkinsDeclare(0, KINSTYPE_IDENTITY); + switchkinsDeclare(1, KINSTYPE_PRIMARY); + } else { + rtapi_print("\n!!! switchkins-type 0 is %s\n",kp->kinsname); + *kset0 = scaraKinematicsSetup; + *kfwd0 = scaraKinematicsForward; + *kinv0 = scaraKinematicsInverse; + + *kset1 = identityKinematicsSetup; + *kfwd1 = identityKinematicsForward; + *kinv1 = identityKinematicsInverse; + switchkinsDeclare(0, KINSTYPE_PRIMARY); + switchkinsDeclare(1, KINSTYPE_IDENTITY); + } *kset2 = userkKinematicsSetup; *kfwd2 = userkKinematicsForward; diff --git a/src/emc/kinematics/three21kins.c b/src/emc/kinematics/three21kins.c index 22494c2f400..a5a7b5dfa8f 100644 --- a/src/emc/kinematics/three21kins.c +++ b/src/emc/kinematics/three21kins.c @@ -377,15 +377,29 @@ int switchkinsSetup(kparms* kp, kp->allow_duplicates = 0; kp->max_joints = strlen(kp->required_coordinates); - *kset0 = three21KinematicsSetup; - *kfwd0 = three21KinematicsForward; - *kinv0 = three21KinematicsInverse; - - *kset1 = identityKinematicsSetup; - *kfwd1 = identityKinematicsForward; - *kinv1 = identityKinematicsInverse; - switchkinsDeclare(0, KINSTYPE_PRIMARY); - switchkinsDeclare(1, KINSTYPE_IDENTITY); + if (kp->sparm && strstr(kp->sparm,"identityfirst")) { + rtapi_print("\n!!! switchkins-type 0 is IDENTITY\n"); + *kset0 = identityKinematicsSetup; + *kfwd0 = identityKinematicsForward; + *kinv0 = identityKinematicsInverse; + + *kset1 = three21KinematicsSetup; + *kfwd1 = three21KinematicsForward; + *kinv1 = three21KinematicsInverse; + switchkinsDeclare(0, KINSTYPE_IDENTITY); + switchkinsDeclare(1, KINSTYPE_PRIMARY); + } else { + rtapi_print("\n!!! switchkins-type 0 is %s\n",kp->kinsname); + *kset0 = three21KinematicsSetup; + *kfwd0 = three21KinematicsForward; + *kinv0 = three21KinematicsInverse; + + *kset1 = identityKinematicsSetup; + *kfwd1 = identityKinematicsForward; + *kinv1 = identityKinematicsInverse; + switchkinsDeclare(0, KINSTYPE_PRIMARY); + switchkinsDeclare(1, KINSTYPE_IDENTITY); + } *kset2 = userkKinematicsSetup; *kfwd2 = userkKinematicsForward;