pumakins: keep every joint within a half turn of the previous value

src/emc/kinematics/pumakins.c

@@ -396,10 +396,16 @@ static int pumaKinematicsInverse(const EmcPose * world,
      th6 = th6 + PM_PI;
    }
 
-   /* th4 and th6 come straight from atan2(), which always lands in
-      (-pi, pi], so a wrist crossing +-180 would jump a whole turn in one
-      call.  Bring them back within a half turn of the joints passed in. */
+   /* The angles come from atan2(), which always lands in (-pi, pi], or
+      from a difference of two, so a joint crossing +-180 would jump a
+      whole turn in one call, and th1, th2 and th3 can be a turn off
+      anywhere.  Bring each back within a half turn of the joint passed
+      in. */
+   th1 = joint[0]*PM_PI/180 + angleDiff(th1, joint[0]*PM_PI/180);
+   th2 = joint[1]*PM_PI/180 + angleDiff(th2, joint[1]*PM_PI/180);
+   th3 = joint[2]*PM_PI/180 + angleDiff(th3, joint[2]*PM_PI/180);
    th4 = joint[3]*PM_PI/180 + angleDiff(th4, joint[3]*PM_PI/180);
+   th5 = joint[4]*PM_PI/180 + angleDiff(th5, joint[4]*PM_PI/180);
    th6 = joint[5]*PM_PI/180 + angleDiff(th6, joint[5]*PM_PI/180);
 
    /* copy out */

tests/pumakins-unwrap/checkresult

@@ -1,2 +1,2 @@
 #!/bin/sh
-grep -q "wrist continuous" "$1" && ! grep -q "FAIL" "$1"
+grep -q "joints continuous" "$1" && ! grep -q "FAIL" "$1"

tests/pumakins-unwrap/unwrapcheck.c

@@ -1,15 +1,18 @@
-/* Check that pumakins keeps its wrist joints continuous (issue #4585).
+/* Check that pumakins keeps its joints continuous (issue #4585).
  *
  * Loaded after pumakins, so kinematicsForward() and kinematicsInverse()
  * resolve to it.  A failed check fails the load, and a failed load fails
  * the test.
  *
- * The inverse took joints 4 and 6 straight from atan2(), which always
- * lands in (-180, 180], so a wrist crossing a half turn came back a whole
- * turn away in one call.  Walk both joints through +-180 in each
- * direction, as motion does: take the pose of each step from the forward
- * and hand the inverse the joints of the step before.  The inverse has to
- * return the joints that made the pose, not ones a turn away.
+ * The inverse took its joints from atan2(), or from a difference of two,
+ * which lands in (-180, 180] or (-360, 360], so a joint crossing a half
+ * turn came back a whole turn away in one call.  Walk each joint through
+ * +-180 in each direction, as motion does: take the pose of each step from
+ * the forward and hand the inverse the joints of the step before.  The
+ * inverse has to return the joints that made the pose, not ones a turn
+ * away.  The wrist pitch, joint 5, is left out: at +-180 the wrist is
+ * folded back on itself, a singularity where joints 4 and 6 are not
+ * determined.
  *
  * Author: LinuxCNC
  * License: GPL Version 2
@@ -28,7 +31,7 @@
 #include <kinematics.h>
 
 MODULE_LICENSE("GPL");
-MODULE_DESCRIPTION("pumakins wrist unwrap checker");
+MODULE_DESCRIPTION("pumakins joint unwrap checker");
 
 /* degrees the inverse may drift from the joints that made the pose; the
    round trip is exact, this only covers floating point noise */
@@ -50,15 +53,15 @@ static void step(double *prev, const double *j)
 
     poses++;
     if (kinematicsForward(j, &world, &ff, &inf)) {
-        rtapi_print_msg(RTAPI_MSG_ERR, "unwrapcheck: FAIL forward at j4=%g j6=%g\n",
-                        j[3], j[5]);
+        rtapi_print_msg(RTAPI_MSG_ERR, "unwrapcheck: FAIL forward at %g %g %g %g %g %g\n",
+                        j[0], j[1], j[2], j[3], j[4], j[5]);
         failures++;
         return;
     }
     memcpy(out, prev, sizeof(out));
     if (kinematicsInverse(&world, out, &inf, &ff)) {
-        rtapi_print_msg(RTAPI_MSG_ERR, "unwrapcheck: FAIL inverse at j4=%g j6=%g\n",
-                        j[3], j[5]);
+        rtapi_print_msg(RTAPI_MSG_ERR, "unwrapcheck: FAIL inverse at %g %g %g %g %g %g\n",
+                        j[0], j[1], j[2], j[3], j[4], j[5]);
         failures++;
         return;
     }
@@ -67,36 +70,59 @@ static void step(double *prev, const double *j)
     }
     if (worst > TOL) {
         rtapi_print_msg(RTAPI_MSG_ERR,
-                        "unwrapcheck: FAIL at j4=%g j6=%g: got j4=%g j6=%g, worst joint error %g\n",
-                        j[3], j[5], out[3], out[5], worst);
+                        "unwrapcheck: FAIL at %g %g %g %g %g %g: got %g %g %g %g %g %g,"
+                        " worst joint error %g\n",
+                        j[0], j[1], j[2], j[3], j[4], j[5],
+                        out[0], out[1], out[2], out[3], out[4], out[5], worst);
         failures++;
     }
     memcpy(prev, out, sizeof(out));
 }
 
-int rtapi_app_main(void)
+/* Walk the joints in mask by 60 degrees from the start, past +180, and
+   back down again, the others held. */
+static void walk(const double *start, unsigned mask)
 {
-    double j[6] = { 20.0, -20.0, 100.0, 150.0, 35.0, 140.0 };
-    double prev[6];
-
-    comp_id = hal_init("unwrapcheck");
-    if (comp_id < 0) { return comp_id; }
+    double j[6], prev[6];
+    int n, i;
 
-    /* joint 4 from 150 to 210 and joint 6 from 140 to 200, through the
-       +-180 atan2 boundary on both, and back down the other way */
+    memcpy(j, start, sizeof(j));
     memcpy(prev, j, sizeof(prev));
-    for (; j[3] <= 210.0; j[3] += STEP, j[5] += STEP) { step(prev, j); }
-    for (j[3] = 210.0, j[5] = 200.0; j[3] >= 150.0; j[3] -= STEP, j[5] -= STEP) {
+    for (n = 0; n <= 24; n++) {
+        for (i = 0; i < 6; i++) {
+            if (mask & (1u << i)) { j[i] = start[i] + n * STEP; }
+        }
         step(prev, j);
     }
+    for (n = 24; n >= 0; n--) {
+        for (i = 0; i < 6; i++) {
+            if (mask & (1u << i)) { j[i] = start[i] + n * STEP; }
+        }
+        step(prev, j);
+    }
+}
+
+int rtapi_app_main(void)
+{
+    comp_id = hal_init("unwrapcheck");
+    if (comp_id < 0) { return comp_id; }
+
+    /* joints 4 and 6 together from 150 and 140 */
+    walk((const double[6]){ 20.0, -20.0, 100.0, 150.0, 35.0, 140.0 }, 0x28);
+    /* the waist, joint 1, from 150 */
+    walk((const double[6]){ 150.0, -20.0, 100.0, 30.0, 35.0, 40.0 }, 0x01);
+    /* the shoulder, joint 2, from 150 */
+    walk((const double[6]){ 20.0, 150.0, 100.0, 30.0, 35.0, 40.0 }, 0x02);
+    /* the elbow, joint 3, from 150 */
+    walk((const double[6]){ 20.0, -20.0, 150.0, 30.0, 35.0, 40.0 }, 0x04);
 
     if (failures) {
         rtapi_print_msg(RTAPI_MSG_ERR, "unwrapcheck: %d of %d pose(s) failed\n",
                         failures, poses);
         hal_exit(comp_id);
         return -ERANGE;
     }
-    rtapi_print("unwrapcheck: wrist continuous over %d pose(s)\n", poses);
+    rtapi_print("unwrapcheck: joints continuous over %d pose(s)\n", poses);
     hal_ready(comp_id);
     return 0;
 }