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; }