Merge pull request #4546 from grandixximo/kins-fixes

kinematics: six fixes found by round-tripping every module

Author: BsAtHome

src/emc/kinematics/maxkins.c

@@ -50,19 +50,25 @@ int kinematicsForward(const double *joints,
     // B correction
     const double zb = (pivot_length + joints[8] + tool_length) * cos(d2r(joints[4]));
     const double xb = (pivot_length + joints[8] + tool_length) * sin(d2r(joints[4]));
-        
-    // C correction
-    const double xyr = hypot(joints[0], joints[1]);
-    const double xytheta = atan2(joints[1], joints[0]) + d2r(joints[5]);
 
     // U correction
     const double zv = joints[6] * sin(d2r(joints[4]));
     const double xv = joints[6] * cos(d2r(joints[4]));
 
     // V correction is always in joint 1 only
 
-    pos->tran.x = xyr * cos(xytheta) - (con * xb) - xv;
-    pos->tran.y = xyr * sin(xytheta) - joints[7];
+    // B, U and V are all machine frame: the head hangs off the Z slide and
+    // does not turn with the C table, so they apply before the rotation into
+    // the workpiece frame rather than after it.
+    const double mx = joints[0] - (con * xb) - xv;
+    const double my = joints[1] - joints[7];
+
+    // C correction
+    const double xyr = hypot(mx, my);
+    const double xytheta = atan2(my, mx) + d2r(joints[5]);
+
+    pos->tran.x = xyr * cos(xytheta);
+    pos->tran.y = xyr * sin(xytheta);
     pos->tran.z = joints[2] - zb - (con * zv) + pivot_length + tool_length;
 
     pos->a = joints[3];
@@ -103,7 +109,7 @@ int kinematicsInverse(const EmcPose * pos,
 
     joints[0] = xyr * cos(xytheta) + (con * xb) + xv;
     joints[1] = xyr * sin(xytheta) + pos->v;
-    joints[2] = pos->tran.z + zb - (con * zv) - pivot_length - tool_length;
+    joints[2] = pos->tran.z + zb + (con * zv) - pivot_length - tool_length;
 
     joints[3] = pos->a;
     joints[4] = pos->b;

src/emc/kinematics/pumakins.c

@@ -29,6 +29,16 @@ struct haldata {
     hal_real_t a2, a3, d3, d4, d6;
 } *haldata = NULL;
 
+/* the difference of two angles, brought into (-pi, pi] so that a joint a
+   whole turn from the formula still matches it */
+static double angleDiff(double a, double b)
+{
+   double d = a - b;
+   while (d > PM_PI) { d -= 2*PM_PI; }
+   while (d <= -PM_PI) { d += 2*PM_PI; }
+   return d;
+}
+
 /* The flange orientation for a joint set: the ISO 9787 mechanical interface
    frame, whose z points out of the interface towards the work.  Shared by the
    forward kinematics and the tool frame so the two cannot drift apart. */
@@ -152,16 +162,16 @@ static int pumaKinematicsForward(const double * joint,
    *iflags = 0;
 
    /* Set shoulder-up flag if necessary */
-   if (fabs(joint[0]*PM_PI/180 - atan2(hom.tran.y, hom.tran.x) +
-       atan2(PUMA_D3, -sqrt(sumSq))) < FLAG_FUZZ)
+   if (fabs(angleDiff(joint[0]*PM_PI/180, atan2(hom.tran.y, hom.tran.x) -
+       atan2(PUMA_D3, -sqrt(sumSq)))) < FLAG_FUZZ)
    {
      *iflags |= PUMA_SHOULDER_RIGHT;
    }
 
    /* Set elbow down flag if necessary */
-   if (fabs(joint[2]*PM_PI/180 - atan2(PUMA_A3, PUMA_D4) +
+   if (fabs(angleDiff(joint[2]*PM_PI/180, atan2(PUMA_A3, PUMA_D4) -
        atan2(k, -sqrt(PUMA_A3 * PUMA_A3 +
-       PUMA_D4 * PUMA_D4 - k * k))) < FLAG_FUZZ)
+       PUMA_D4 * PUMA_D4 - k * k)))) < FLAG_FUZZ)
    {
       *iflags |= PUMA_ELBOW_DOWN;
    }
@@ -177,7 +187,7 @@ static int pumaKinematicsForward(const double * joint,
 
    /* if not singular set wrist flip flag if necessary */
    else{
-     if (! (fabs(joint[3]*PM_PI/180 - atan2(t1, t2)) < FLAG_FUZZ))
+     if (! (fabs(angleDiff(joint[3]*PM_PI/180, atan2(t1, t2))) < FLAG_FUZZ))
      {
        *iflags |= PUMA_WRIST_FLIP;
      }

src/emc/kinematics/scarakins.c

@@ -96,8 +96,9 @@ int scaraKinematicsForward(const double * joint,
     z = D1 + D3 - joint[2] - D5;
     c = a3;
 
+    // the elbow flag: which sign the inverse gives the acos of joint 1
     *iflags = 0;
-    if (joint[1] < 90)
+    if (joint[1] < 0)
         *iflags = 1;
 
     world->tran.x = x;

src/emc/kinematics/three21kins.c

@@ -32,6 +32,16 @@ struct haldata {
     hal_real_t a1, a2, a3, d1, d2, d3, d4, d6;
 } *haldata = NULL;
 
+/* the difference of two angles, brought into (-pi, pi] so that a joint a
+   whole turn from the formula still matches it */
+static double angleDiff(double a, double b)
+{
+   double d = a - b;
+   while (d > PM_PI) { d -= 2*PM_PI; }
+   while (d <= -PM_PI) { d += 2*PM_PI; }
+   return d;
+}
+
 static int three21KinematicsForward(const double * joint,
                                     EmcPose * world,
                                     const KINEMATICS_FORWARD_FLAGS * fflags,
@@ -132,8 +142,8 @@ static int three21KinematicsForward(const double * joint,
    *iflags = 0;
 
    /* set shoulder flag */
-   if (fabs(joint[0]*PM_PI/180 - atan2(hom.tran.y, hom.tran.x) +
-       atan2(d23, -sqrt(sumSq))) < FLAG_FUZZ)
+   if (fabs(angleDiff(joint[0]*PM_PI/180, atan2(hom.tran.y, hom.tran.x) -
+       atan2(d23, -sqrt(sumSq)))) < FLAG_FUZZ)
    {
      *iflags |= THREE21_SHOULDER_RIGHT;
    }
@@ -143,8 +153,8 @@ static int three21KinematicsForward(const double * joint,
    if (discr < 0.0) {
        discr = 0.0;
    }
-   if (fabs(joint[2]*PM_PI/180 - atan2(a3, d4) +
-       atan2(k, -sqrt(discr))) < FLAG_FUZZ)
+   if (fabs(angleDiff(joint[2]*PM_PI/180, atan2(a3, d4) -
+       atan2(k, -sqrt(discr)))) < FLAG_FUZZ)
    {
       *iflags |= THREE21_ELBOW_DOWN;
    }
@@ -158,7 +168,7 @@ static int three21KinematicsForward(const double * joint,
    }
    else
    {
-     if (! (fabs(joint[3]*PM_PI/180 - atan2(t1, t2)) < FLAG_FUZZ))
+     if (! (fabs(angleDiff(joint[3]*PM_PI/180, atan2(t1, t2))) < FLAG_FUZZ))
      {
        *iflags |= THREE21_WRIST_FLIP;
      }

src/emc/kinematics/trtfuncs.c

@@ -362,11 +362,14 @@ int xyzbcKinematicsInverse(const EmcPose * pos,
     const double          dz = hal_get_real(haldata->z_offset) + dt;
     const double       b_rad = pos->b*TO_RAD;
     const double       c_rad = pos->c*TO_RAD;
-    const double         dpx = -cos(b_rad)*dx + sin(b_rad)*dz + dx;
-    const double         dpz = -sin(b_rad)*dx - cos(b_rad)*dz + dz;
 
     rtapi_real con = hal_get_bool(haldata->conventional_directions) ? 1.0 : -1.0;
 
+    // the offsets seen from the tilted table: the same rotation the
+    // forward applies to them, in the same sense
+    const double         dpx = -cos(b_rad)*dx + con * sin(b_rad)*dz + dx;
+    const double         dpz = -con * sin(b_rad)*dx - cos(b_rad)*dz + dz;
+
     EmcPose P; // computed position
 
     P.tran.x   = +       cos(c_rad) * cos(b_rad) * (pos->tran.x - x_rot_point)

src/emc/motion/control.c

@@ -363,10 +363,6 @@ static void handle_kinematicsSwitch(void) {
         return; // the kinematics in force is unchanged
     }
 
-    switchkins_type = requested_type;
-    hal_set_real(emcmot_hal_data->kins_type, (double)switchkins_type);
-    emcmotStatus->switchkins_type = switchkins_type;
-
     KINEMATICS_FORWARD_FLAGS tmpFFlags = fflags;
     KINEMATICS_INVERSE_FLAGS tmpIFlags = iflags;
 #ifdef SWITCHKINS_DEBUG
@@ -376,15 +372,25 @@ static void handle_kinematicsSwitch(void) {
         beforePose[anum] = *pcmd_p[anum];
     }
 #endif
+    /* the joints stay where they are, so a kinematics whose forward cannot
+       solve them is one the machine cannot run in from here: put the old
+       one back, or the inverse would run the joints to wherever the pose
+       we know lands in the new one */
     EmcPose poseKinsSwitch = emcmotStatus->carte_pos_cmd;
     if (kinematicsForward(joint_posKinsSwitch, &poseKinsSwitch,
                           &tmpFFlags, &tmpIFlags)) {
-        reportError(_("kinematicsForward failed for kinematics type %d"),
-                    switchkins_type);
+        kinematicsSwitch(switchkins_type);
+        reportError(_("kinematicsForward failed for kinematics type %d,"
+                      " type %d is still in force"),
+                    requested_type, switchkins_type);
         SET_MOTION_ERROR_FLAG(1);  // abort
-        return; // keep the position we know rather than an unsolved one
+        return; // the kinematics in force and the position are unchanged
     }
     emcmotStatus->carte_pos_cmd = poseKinsSwitch;
+
+    switchkins_type = requested_type;
+    hal_set_real(emcmot_hal_data->kins_type, (double)switchkins_type);
+    emcmotStatus->switchkins_type = switchkins_type;
 #ifdef SWITCHKINS_DEBUG
     fprintf(stderr,"kswitch type=%d (%s:%d)\n",switchkins_type,__FUNCTION__,__LINE__);
     for (anum = 0; anum < EMCMOT_MAX_AXIS; anum++) {

src/hal/components/matrixkins.comp

@@ -176,6 +176,7 @@ the adjustment values should be added to the old values instead of replacing the
 """;
 see_also "kins(9)";
 pin out bool dummy=1; // halcompile requires at least one pin
+option extra_setup;
 license "GPL";
 ;;
 
@@ -191,15 +192,15 @@ static struct haldata {
     hal_real_t C_zz;
 } *haldata;
 
-static int matrixkins_setup(void) {
+EXTRA_SETUP() {
+    (void)__comp_inst;
+    (void)prefix;
+    (void)extra_arg;
     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;
-
     haldata = hal_malloc(sizeof(struct haldata));
     if (!haldata) goto error;
 
@@ -215,9 +216,6 @@ static int matrixkins_setup(void) {
 
     if (res) goto error;
 
-    res = hal_ready(comp_id);
-    if (res) goto error;
-
     rtapi_print("*** %s setup ok\n",__FILE__);
     return 0;
 error:
@@ -235,8 +233,6 @@ EXPORT_SYMBOL(kinematicsForward);
 
 KINEMATICS_TYPE kinematicsType()
 {
-    static bool is_setup=0;
-    if (!is_setup) matrixkins_setup();
     return KINEMATICS_BOTH;
 }
 

src/hal/components/millturn.comp

@@ -27,9 +27,10 @@ chapter (docs/src/motion/switchkins.txt)
 
 """;
 // The fpin pin is not accessible in kinematics functions.
-// Use the *_setup() function for pins and params used by kinematics.
+// Use EXTRA_SETUP() for pins and params used by kinematics.
 pin out si32 fpin=0"pin to demonstrate use of a conventional (non-kinematics) function fdemo";
 option period no;
+option extra_setup;
 function fdemo;
 license "GPL";
 author "David Mueller";
@@ -59,14 +60,15 @@ FUNCTION(fdemo) {
    fpin_set(fpin + 1);
 }
 
-static int millturn_setup(void) {
+EXTRA_SETUP() {
+    (void)__comp_inst;
+    (void)prefix;
+    (void)extra_arg;
 #define HAL_PREFIX "millturn"
     int res=0;
 
     // inherit comp_id from rtapi_main()
     if (comp_id < 0) goto error;
-    // set unready to allow creation of pins
-    if (hal_set_unready(comp_id)) goto error;
 
     haldata = hal_malloc(sizeof(*haldata));
     if (!haldata) goto error;
@@ -85,7 +87,6 @@ static int millturn_setup(void) {
     res += hal_pin_new_bool(comp_id, HAL_OUT, &haldata->kinstype_is_1, 0, "kinstype.is-1");
 
     if (res) goto error;
-    hal_ready(comp_id);
     rtapi_print("*** %s setup ok\n",__FILE__);
     return 0;
 error:
@@ -142,8 +143,6 @@ int kinematicsSwitch(int new_switchkins_type)
 
 KINEMATICS_TYPE kinematicsType()
 {
-static bool is_setup=0;
-    if (!is_setup) millturn_setup();
     return KINEMATICS_BOTH; // set as required
            // Note: If kinematics are identity, using KINEMATICS_BOTH
            //       may be used in order to allow a gui to display

src/hal/components/userkins.comp

@@ -53,14 +53,16 @@ change all instances of `userkins` to `mykins`.
 * The *fpin* pin is included to satisfy the requirements of the halcompile
   utility but it is not accessible to kinematics functions.
 * HAL pins and parameters needed in kinematics functions (kinematicsForward(),
-  kinematicsInverse()) must be setup in a function (*userkins_setup()*) invoked
-  by the initial motion module call to kinematicsType().
+  kinematicsInverse()) must be setup in the *EXTRA_SETUP()* function, which
+  halcompile runs once when the module is loaded, before the component is
+  made ready.
 
 """;
 // The fpin pin is not accessible in kinematics functions.
-// Use the *_setup() function for pins and params used by kinematics.
+// Use EXTRA_SETUP() for pins and params used by kinematics.
 pin out si32 fpin=0"pin to demonstrate use of a conventional (non-kinematics) function fdemo";
 option period no;
+option extra_setup;
 function fdemo;
 license "GPL";
 author "Dewey Garrett";
@@ -91,14 +93,15 @@ FUNCTION(fdemo) {
    fpin_set(fpin + 1);
 }
 
-static int userkins_setup(void) {
+EXTRA_SETUP() {
+    (void)__comp_inst;
+    (void)prefix;
+    (void)extra_arg;
 #define HAL_PREFIX "userkins"
     int res=0;
 
     // inherit comp_id from rtapi_main()
     if (comp_id < 0) goto error;
-    // set unready to allow creation of pins
-    if (hal_set_unready(comp_id)) goto error;
 
     haldata = hal_malloc(sizeof(struct haldata));
     if (!haldata) goto error;
@@ -112,7 +115,6 @@ static int userkins_setup(void) {
     res += hal_param_new_real(comp_id, HAL_RO, &haldata->param_ro, 0.0, "%s.param-ro", HAL_PREFIX);
 
     if (res) goto error;
-    hal_ready(comp_id);
     rtapi_print("*** %s setup ok\n",__FILE__);
     return 0;
 error:
@@ -130,8 +132,6 @@ EXPORT_SYMBOL(kinematicsForward);
 
 KINEMATICS_TYPE kinematicsType()
 {
-static bool is_setup=0;
-    if (!is_setup) userkins_setup();
     return KINEMATICS_IDENTITY; // set as required
            // Note: If kinematics are identity, using KINEMATICS_BOTH
            //       may be used in order to allow a gui to display

src/hal/components/xyzab_tdr_kins.comp

@@ -31,6 +31,7 @@ chapter (docs/src/motion/switchkins.txt)
 """;
 
 pin out si32 dummy=0"one pin needed to satisfy halcompile requirement";
+option extra_setup;
 
 license "GPL";
 author "David Mueller";
@@ -54,13 +55,14 @@ static struct haldata {
   hal_bool_t kinstype_is_1;
 } *haldata;
 
-static int xyzab_tdr_setup(void) {
+EXTRA_SETUP() {
+    (void)__comp_inst;
+    (void)prefix;
+    (void)extra_arg;
 #define HAL_PREFIX "xyzab_tdr_kins"
     int res=0;
     // inherit comp_id from rtapi_main()
     if (comp_id < 0) goto error;
-    // set unready to allow creation of pins
-    if (hal_set_unready(comp_id)) goto error;
 
     haldata = hal_malloc(sizeof(*haldata));
     if (!haldata) goto error;
@@ -80,7 +82,6 @@ static int xyzab_tdr_setup(void) {
     res += hal_pin_new_bool(comp_id, HAL_OUT, &haldata->kinstype_is_1, 0, "kinstype.is-1");
 
     if (res) goto error;
-    hal_ready(comp_id);
     rtapi_print("*** %s setup ok\n",__FILE__);
     return 0;
 error:
@@ -137,8 +138,6 @@ int kinematicsSwitch(int new_switchkins_type)
 
 KINEMATICS_TYPE kinematicsType()
 {
-static bool is_setup=0;
-    if (!is_setup)  xyzab_tdr_setup();
     return KINEMATICS_BOTH; // set as required
            // Note: If kinematics are identity, using KINEMATICS_BOTH
            //       may be used in order to allow a gui to display