From 54a868c694db296b1f7229933c8b9bec13962904 Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Mon, 26 Oct 2026 15:15:15 +1100 Subject: [PATCH] genhexkins: take the forward's Newton step in roll, pitch and yaw rates The forward kinematics is a Newton iteration on the platform pose that steps roll, pitch and yaw by the solution of J delta = strut errors, but it built the rotational columns of J as the strut moments (R a) x u. Those are the rates per angular velocity, which equal the rates per roll, pitch and yaw rate only at zero orientation. With the platform tilted the step was off, and the iteration converged linearly instead of quadratically. Motion runs this forward on the joint feedback every servo cycle, seeded with the pose of the cycle before. From such a seed, one cycle of motion away, on the hexapod-sim geometry tilted to 20 degrees and turned to 30, it took 12.6 iterations on average and 23 at most instead of three; tilted to 40 and turned to 60, one pose in ten did not converge within the 120 iteration limit. Running the sim through MDI tilts to A35 B35 C55, the most a feedback forward needed in the session was 117 iterations. Take the rows against the rates, w = E [dr dp dy], with the E that genhexKinematicsJacobian() uses. The same poses converge in three iterations at a quarter of the cost, none fails, and the sim session needs three at most. tests/genhexkins-forward walks 600 tilted poses with genhexkins.limit-iterations at 8, each seeded a cycle away; before this change 480 of them did not converge. --- src/emc/kinematics/genhexkins.c | 18 ++- tests/genhexkins-forward/checkresult | 2 + tests/genhexkins-forward/hexfwdcheck.c | 137 +++++++++++++++++++++++ tests/genhexkins-forward/hexfwdcheck.hal | 4 + tests/genhexkins-forward/skip | 4 + tests/genhexkins-forward/test.sh | 5 + 6 files changed, 167 insertions(+), 3 deletions(-) create mode 100755 tests/genhexkins-forward/checkresult create mode 100644 tests/genhexkins-forward/hexfwdcheck.c create mode 100644 tests/genhexkins-forward/hexfwdcheck.hal create mode 100755 tests/genhexkins-forward/skip create mode 100755 tests/genhexkins-forward/test.sh diff --git a/src/emc/kinematics/genhexkins.c b/src/emc/kinematics/genhexkins.c index 0964fa7d088..ae1a11df8b4 100644 --- a/src/emc/kinematics/genhexkins.c +++ b/src/emc/kinematics/genhexkins.c @@ -345,6 +345,8 @@ static int genhexKinematicsForward(const double * joints, PmRotationMatrix RMatrix; PmRpy q_RPY; + PmCartesian E[3]; + double sp, cp, sy, cy; int iterate = 1; int i; @@ -398,6 +400,16 @@ static int genhexKinematicsForward(const double * joints, /* Convert q_RPY to Rotation Matrix */ pmRpyMatConvert(&q_RPY, &RMatrix); + /* The update steps roll, pitch and yaw, so the rows take their rates, + w = E [dr dp dy] with the E of the Jacobian below. Rows in the + angular velocity w are right only at zero orientation; tilted, the + iteration then converges slowly, or not at all. */ + sp = sin(q_RPY.p); cp = cos(q_RPY.p); + sy = sin(q_RPY.y); cy = cos(q_RPY.y); + E[0].x = cp*cy; E[0].y = cp*sy; E[0].z = -sp; + E[1].x = -sy; E[1].y = cy; E[1].z = 0; + E[2].x = 0; E[2].y = 0; E[2].z = 1; + /* compute StrutLengthDiff[] by running inverse kins on Cartesian estimate to get joint estimate, subtract joints to get joint deltas, and compute inv J while we're at it */ @@ -427,9 +439,9 @@ static int genhexKinematicsForward(const double * joints, InverseJacobian[i][0] = InvKinStrutVectUnit.x; InverseJacobian[i][1] = InvKinStrutVectUnit.y; InverseJacobian[i][2] = InvKinStrutVectUnit.z; - InverseJacobian[i][3] = RMatrix_a_cross_Strut.x; - InverseJacobian[i][4] = RMatrix_a_cross_Strut.y; - InverseJacobian[i][5] = RMatrix_a_cross_Strut.z; + pmCartCartDot(&RMatrix_a_cross_Strut, &E[0], &InverseJacobian[i][3]); + pmCartCartDot(&RMatrix_a_cross_Strut, &E[1], &InverseJacobian[i][4]); + pmCartCartDot(&RMatrix_a_cross_Strut, &E[2], &InverseJacobian[i][5]); } /* invert Inverse Jacobian */ diff --git a/tests/genhexkins-forward/checkresult b/tests/genhexkins-forward/checkresult new file mode 100755 index 00000000000..92f4048e92a --- /dev/null +++ b/tests/genhexkins-forward/checkresult @@ -0,0 +1,2 @@ +#!/bin/sh +grep -q "converged within the limit" "$1" && ! grep -q "FAIL" "$1" diff --git a/tests/genhexkins-forward/hexfwdcheck.c b/tests/genhexkins-forward/hexfwdcheck.c new file mode 100644 index 00000000000..62d42a614f4 --- /dev/null +++ b/tests/genhexkins-forward/hexfwdcheck.c @@ -0,0 +1,137 @@ +/* Check that genhexkins' forward kinematics converges like Newton's method + * from where motion seeds it. + * + * Loaded after genhexkins, so kinematicsForward() and kinematicsInverse() + * resolve to it; the hal file sets genhexkins.limit-iterations to 8. A + * failed check fails the load, and a failed load fails the test. + * + * Motion runs the forward on the joint feedback every servo cycle, seeded + * with the pose of the cycle before. The iteration stepped roll, pitch + * and yaw with Jacobian rows taken in the angular velocity, which agree + * with their rates only at zero orientation, so with the platform tilted + * it converged linearly: a dozen iterations and more where Newton takes + * three or four, and at large tilts not at all. Over a grid of tilted + * poses, seed the forward a little off each, as one cycle of motion + * leaves it, and require the pose back within the limit. + * + * Author: LinuxCNC + * License: GPL Version 2 + * System: Linux + * + * Copyright (c) 2026 All rights reserved. + */ + +#include +#include +#include +#include +#include +#include +#include +#include + +MODULE_LICENSE("GPL"); +MODULE_DESCRIPTION("genhexkins forward convergence checker"); + +/* the pose must come back to this, in length units and degrees; the + convergence criterion on the struts is far tighter */ +#define TOL 1e-6 + +#define NELEM(x) (sizeof(x) / sizeof(*(x))) + +/* the coordinates of a pose the check compares: x, y, z, a, b and c */ +#define POSE_COORDS 6 + +static int comp_id = -1; +static int failures; +static int poses; + +static double coord(const EmcPose *p, int i) +{ + const double c[POSE_COORDS] = { p->tran.x, p->tran.y, p->tran.z, p->a, p->b, p->c }; + return c[i]; +} + +static void check(const EmcPose *pose) +{ + /* one servo cycle of motion away, in every coordinate */ + static const double off[POSE_COORDS] = { .01, -.01, .01, .01, -.01, .01 }; + KINEMATICS_FORWARD_FLAGS ff = 0; + KINEMATICS_INVERSE_FLAGS inf = 0; + EmcPose seed = *pose; + double joints[EMCMOT_MAX_JOINTS]; + double worst = 0; + + poses++; + if (kinematicsInverse(pose, joints, &inf, &ff)) { + rtapi_print_msg(RTAPI_MSG_ERR, "hexfwdcheck: FAIL inverse at %g %g %g %g %g %g\n", + pose->tran.x, pose->tran.y, pose->tran.z, pose->a, pose->b, pose->c); + failures++; + return; + } + seed.tran.x += off[0]; seed.tran.y += off[1]; seed.tran.z += off[2]; + seed.a += off[3]; seed.b += off[4]; seed.c += off[5]; + if (kinematicsForward(joints, &seed, &ff, &inf)) { + rtapi_print_msg(RTAPI_MSG_ERR, + "hexfwdcheck: FAIL forward did not converge at %g %g %g %g %g %g\n", + pose->tran.x, pose->tran.y, pose->tran.z, pose->a, pose->b, pose->c); + failures++; + return; + } + for (int i = 0; i < POSE_COORDS; i++) { + double d = fabs(coord(&seed, i) - coord(pose, i)); + if (d > worst) { worst = d; } + } + if (worst > TOL) { + rtapi_print_msg(RTAPI_MSG_ERR, + "hexfwdcheck: FAIL at %g %g %g %g %g %g: worst coordinate error %g\n", + pose->tran.x, pose->tran.y, pose->tran.z, pose->a, pose->b, pose->c, worst); + failures++; + } +} + +/* the hexapod-sim geometry, the module's defaults: home Z 20, struts near + 30; tilts to 30 degrees and the platform turned 45 */ +static const double tilt[] = { -30, -15, 0, 15, 30 }; +static const double turn[] = { -45, 0, 45 }; +static const double across[] = { -5, 5 }; +static const double height[] = { 15, 25 }; + +/* every position of the grid at one orientation */ +static void check_orientation(double a, double b, double c) +{ + for (size_t ix = 0; ix < NELEM(across); ix++) { + for (size_t iy = 0; iy < NELEM(across); iy++) { + for (size_t iz = 0; iz < NELEM(height); iz++) { + EmcPose pose = {{across[ix], across[iy], height[iz]}, a, b, c, 0, 0, 0}; + check(&pose); + } + } + } +} + +int rtapi_app_main(void) +{ + comp_id = hal_init("hexfwdcheck"); + if (comp_id < 0) { return comp_id; } + + for (size_t ia = 0; ia < NELEM(tilt); ia++) { + for (size_t ib = 0; ib < NELEM(tilt); ib++) { + for (size_t ic = 0; ic < NELEM(turn); ic++) { + check_orientation(tilt[ia], tilt[ib], turn[ic]); + } + } + } + + if (failures) { + rtapi_print_msg(RTAPI_MSG_ERR, "hexfwdcheck: %d of %d pose(s) failed\n", + failures, poses); + hal_exit(comp_id); + return -ERANGE; + } + rtapi_print("hexfwdcheck: converged within the limit over %d pose(s)\n", poses); + hal_ready(comp_id); + return 0; +} + +void rtapi_app_exit(void) { hal_exit(comp_id); } diff --git a/tests/genhexkins-forward/hexfwdcheck.hal b/tests/genhexkins-forward/hexfwdcheck.hal new file mode 100644 index 00000000000..89a4c60ea0b --- /dev/null +++ b/tests/genhexkins-forward/hexfwdcheck.hal @@ -0,0 +1,4 @@ +loadrt genhexkins +# Newton's method takes three or four iterations from a one-cycle seed +setp genhexkins.limit-iterations 8 +loadrt hexfwdcheck diff --git a/tests/genhexkins-forward/skip b/tests/genhexkins-forward/skip new file mode 100755 index 00000000000..a12f31a77c2 --- /dev/null +++ b/tests/genhexkins-forward/skip @@ -0,0 +1,4 @@ +#!/bin/sh +# Builds a realtime component with halcompile, which needs the build +# tools present. Skip when testing installed packages. +[ -z "$SYSTEM_BUILD" ] diff --git a/tests/genhexkins-forward/test.sh b/tests/genhexkins-forward/test.sh new file mode 100755 index 00000000000..9fe65252e05 --- /dev/null +++ b/tests/genhexkins-forward/test.sh @@ -0,0 +1,5 @@ +#!/bin/bash +set -e + +halcompile --install hexfwdcheck.c >/dev/null +halrun -f hexfwdcheck.hal