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