From 80a947ab87db8ef3ecfb6959599033ac542cb4e9 Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Wed, 21 Oct 2026 23:38:02 +1100 Subject: [PATCH] pumakins: keep every joint within a half turn of the previous value f5311b8b07 unwrapped the wrist, joints 4 and 6, which came straight from atan2() and jumped a whole turn when they crossed +-180. The arm joints were left as they were. Joint 1 is atan2(py, px) less another atan2, joint 3 a difference of two, and joint 2 the difference of joint 3 from a third, so each can come back a turn away from the joint that made the pose, not only at the boundary: the waist at 150 degrees returned -210. Motion takes that as a whole turn of the joint in one servo cycle. On the puma sim, with the waist jogged to 169 in joint mode, switching to world mode set joint 0 to -191, past the -170 soft limit, and every world move after it was refused. Every joint now goes through the same angleDiff() step as the wrist, onto the value within a half turn of the joint passed in. The wrist pitch, joint 5, as well, for one rule; at +-180 the wrist is folded back on itself and the test leaves it out. tests/pumakins-unwrap walks the waist, the shoulder and the elbow through +-180 each way as well as the wrist, the pose of each step from the forward and the inverse handed the joints of the step before. Without the change 120 of the 200 poses fail, on joints 1, 2 and 3. --- src/emc/kinematics/pumakins.c | 12 +++-- tests/pumakins-unwrap/checkresult | 2 +- tests/pumakins-unwrap/unwrapcheck.c | 76 +++++++++++++++++++---------- 3 files changed, 61 insertions(+), 29 deletions(-) diff --git a/src/emc/kinematics/pumakins.c b/src/emc/kinematics/pumakins.c index 8b17fae351e..19583c59ea5 100644 --- a/src/emc/kinematics/pumakins.c +++ b/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 */ diff --git a/tests/pumakins-unwrap/checkresult b/tests/pumakins-unwrap/checkresult index a9707f13f02..a5c05ae9144 100755 --- a/tests/pumakins-unwrap/checkresult +++ b/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" diff --git a/tests/pumakins-unwrap/unwrapcheck.c b/tests/pumakins-unwrap/unwrapcheck.c index 1c9830d4d21..4f003ff0301 100644 --- a/tests/pumakins-unwrap/unwrapcheck.c +++ b/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 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,28 +70,51 @@ 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", @@ -96,7 +122,7 @@ int rtapi_app_main(void) 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; }