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.

Author: grandixximo

docs/src/motion/switchkins.adoc

@@ -45,10 +45,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]
 ----
@@ -57,6 +57,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

src/emc/kinematics/genhexkins.c

@@ -693,18 +693,31 @@ 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;
+    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;
+    } 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;
+    }
 
     *kset2 = userkKinematicsSetup;
     *kfwd2 = userkKinematicsForward;

src/emc/kinematics/genserkins.c

@@ -63,13 +63,25 @@ 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;
+    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;
+    } else {
+        rtapi_print("\n!!! switchkins-type 0 is %s\n",kp->kinsname);
+        *kset0 = genserKinematicsSetup;
+        *kfwd0 = genserKinematicsForward;
+        *kinv0 = genserKinematicsInverse;
+
+        *kset1 = identityKinematicsSetup;
+        *kfwd1 = identityKinematicsForward;
+        *kinv1 = identityKinematicsInverse;
+    }
 
     *kset2 = userkKinematicsSetup;
     *kfwd2 = userkKinematicsForward;

src/emc/kinematics/pumakins.c

@@ -367,14 +367,25 @@ 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;
-
-    *kset1 = identityKinematicsSetup;
-    *kfwd1 = identityKinematicsForward;
-    *kinv1 = identityKinematicsInverse;
+    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;
+    } else {
+        rtapi_print("\n!!! switchkins-type 0 is %s\n",kp->kinsname);
+        *kset0 = pumaKinematicsSetup;
+        *kfwd0 = pumaKinematicsForward;
+        *kinv0 = pumaKinematicsInverse;
+
+        *kset1 = identityKinematicsSetup;
+        *kfwd1 = identityKinematicsForward;
+        *kinv1 = identityKinematicsInverse;
+    }
 
     *kset2 = userkKinematicsSetup;
     *kfwd2 = userkKinematicsForward;

src/emc/kinematics/scarakins.c

@@ -221,14 +221,25 @@ 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;
+    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;
+    } else {
+        rtapi_print("\n!!! switchkins-type 0 is %s\n",kp->kinsname);
+        *kset0 = scaraKinematicsSetup;
+        *kfwd0 = scaraKinematicsForward;
+        *kinv0 = scaraKinematicsInverse;
+
+        *kset1 = identityKinematicsSetup;
+        *kfwd1 = identityKinematicsForward;
+        *kinv1 = identityKinematicsInverse;
+    }
 
     *kset2 = userkKinematicsSetup;
     *kfwd2 = userkKinematicsForward;

src/emc/kinematics/three21kins.c

@@ -377,13 +377,25 @@ 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;
+    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;
+    } else {
+        rtapi_print("\n!!! switchkins-type 0 is %s\n",kp->kinsname);
+        *kset0 = three21KinematicsSetup;
+        *kfwd0 = three21KinematicsForward;
+        *kinv0 = three21KinematicsInverse;
+
+        *kset1 = identityKinematicsSetup;
+        *kfwd1 = identityKinematicsForward;
+        *kinv1 = identityKinematicsInverse;
+    }
 
     *kset2 = userkKinematicsSetup;
     *kfwd2 = userkKinematicsForward;