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/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, 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; 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++) {