From 2ef6241496df9083824d46f62e65904a06b45ce7 Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Thu, 3 Sep 2026 18:21:24 +1000 Subject: [PATCH 1/9] kinematics: add an optional Jacobian entry point A feed is a speed in the work frame and the machine delivers a speed at each joint; on any kinematics but the identity the two are related by where the machine is, and nothing in the interface answered that, so a limit taken from the joints had nowhere to get it. kinematicsJacobian() answers it: jac[j][a] is how joint j responds to a unit rate of pose coordinate a, rows joints, columns in EmcPose order. It is the derivative of the inverse, since that is what every consumer multiplies by and every module has one, in joint units per pose unit so nothing is converted. A module supplies nothing: kinsJacobianFromInverse() takes central differences of its inverse, eighteen calls a pose. switchkins answers for every type, exactly for an identity type and by differences for one that registers nothing; switchkinsRegisterJacobian() takes a closed form. kinsJacobianFromMappedAxes() turns the derivative of a computed position into rows for the modules that finish in position_to_mapped_joints(). Nothing in motion calls it yet; that is the realtime seam of the limits work, and it waits on the closed forms. --- docs/src/motion/kinematics-conventions.adoc | 112 +++++++++++++++++- src/emc/kinematics/kinematics.h | 95 +++++++++++++++ src/emc/kinematics/kins_util.c | 123 ++++++++++++++++++++ src/emc/kinematics/switchkins.c | 40 +++++++ src/emc/kinematics/switchkins.h | 11 ++ src/emc/kinematics/trivkins.c | 9 ++ 6 files changed, 384 insertions(+), 6 deletions(-) diff --git a/docs/src/motion/kinematics-conventions.adoc b/docs/src/motion/kinematics-conventions.adoc index 063ee63e794..4596796ca03 100644 --- a/docs/src/motion/kinematics-conventions.adoc +++ b/docs/src/motion/kinematics-conventions.adoc @@ -279,12 +279,11 @@ The joint values that reach a requested orientation, through question a tilted work plane asks when it has to orient the machine. <> says what it answers. -The Jacobian, relating commanded velocity to joint velocity at a given pose, so -that a feed can be checked against the joint velocity, acceleration and limit -values it will actually demand, and so that proximity to a singularity is a -number rather than a surprise. A module with a closed form can supply it -directly. Otherwise it can be obtained by differencing `kinematicsInverse()` -about the pose, which needs no change to the module at all. +How joint motion follows world motion, through `kinematicsJacobian()`, so +that a feed can be checked against the joint velocity and acceleration it will +actually demand, and so that proximity to a singularity is a number rather +than a surprise. <> says what it answers and in +which units. All of these are functions of the joint values and the module's own geometry. None needs state carried between calls, and none needs the module to be running @@ -377,6 +376,101 @@ The search is not a realtime routine. How long it takes depends on the machine and on the request, and the callers that want it, orienting a tilted work plane and previewing a program, are not in the servo loop. +[[sec:jacobian]] +== The Jacobian + +A feed is a speed in the work frame. What the machine has to deliver is a +speed at each joint, and on any kinematics that is not the identity the two +are related by where the machine is. The Jacobian is that relation at one +pose: how each joint responds to a unit rate of each pose coordinate. + + jac[j][a] = d joint[j] / d pose[a] + +Rows are joints. Columns are the pose coordinates in `EmcPose` order, X Y Z A +B C U V W. It is the derivative of `kinematicsInverse()`: multiplied by a pose +velocity it gives the joint velocity motion will command, which is what a feed +limit compares with the joint limits. Joint `j` binds when + + |jac[j] . tangent| * F + +exceeds that joint's velocity limit, `tangent` being the direction of the move +in pose coordinates and `F` the feed along it. The acceleration limit follows +from a second Jacobian taken further along the path, with no more from the +module. A row that grows without bound is a pose approaching a singularity, +where no world speed is slow enough for the joints to follow. + +=== Units + +Each entry is in joint units per pose unit, whatever units the module's own +forward and inverse already use. Nothing is converted: a caller that feeds +pose rates in `EmcPose` units gets joint rates in the units motion already +commands, and never has to know which unit a rotary joint is in. On every +module in the tree both are degrees, so a table rotary's own row is a 1 in its +own column, and a robot's rotary rows carry degrees per millimetre against the +linear columns. + +This is why the Jacobian, unlike the orientation inverse, does not need the +interface to name the rotary joint unit. Every number in it is a ratio of +quantities that already pass through `kinematicsForward()` and +`kinematicsInverse()`, and the caller never combines it with anything measured +in another unit. + +=== Frame + +The columns are pose coordinates, so the answer lives in the work frame, where +`kinematicsForward()` reports positions. The A, B and C columns are rates of +the pose words, the wrapped linear axes the planner already treats as +coordinates, and not an angular velocity vector: on a machine that carries the +work the forward writes the rotary joint into the pose word, and that column +says exactly that, a 1 for its own joint. + +That makes this a different object from the frames of +<>, and the two rules are kept apart deliberately. A frame +is an orientation, and a renderer placing two bodies needs each against +something fixed, so frames are reported against the machine. A Jacobian is a +derivative of the pose, and everything that uses it multiplies it by a pose +rate, so it is reported where the pose is. A module whose maths produces a +twist in the machine frame, which is what the Denavit-Hartenberg modules +produce, turns it into pose word rates through the matrix of the axes each +pose word turns about, once, inside the module. `genserkins` does this, and +having it written once there is worth more than the closed form itself, since +every consumer would otherwise guess it. + +=== What a module has to supply + +Nothing. If a module registers no Jacobian, the shared code computes one by +central differences: it calls the module's inverse eighteen times, stepping +each pose coordinate a small amount to either side on the solution branch +the inverse flags select, and differences the results. The answer is as +precise as the inverse itself; behind an inverse that iterates, that is the +iteration's convergence tolerance divided by the step. Modules built on `switchkins.c` answer this way for every type that +registers nothing; an identity type answers exactly. + +This cost matters because `kinematicsJacobian()` is meant to run in the +servo thread, where checking a feed limit calls it once per cycle: each call +then costs eighteen inverses. On the closed-form inverses in the tree that +is under two microseconds. `genserkins`, the one module whose inverse +iterates, would cost near eighty microseconds per call this way, against +under two for the geometric Jacobian it registers instead. + +So a module whose inverse iterates should register a closed-form Jacobian, +as below. Nothing enforces this. A module that registers nothing still gets +a correct Jacobian, just one that costs eighteen inverses and carries the +iteration's precision, which behind an iterating inverse is too slow for the +servo thread. + +A module with a closed form registers it with `switchkinsRegisterJacobian()`. +It is exact, it costs what the inverse costs, and it knows its own singular +poses rather than discovering them as an inverse that fails a step away from +the pose. Every module in the tree whose inverse is written out supplies one. +The two arms whose inverse is a chain of arc tangents, `pumakins` and +`three21kins`, answer through the differences. + +A module reading its rotary angles from the joint argument of the inverse +rather than from the pose, which the nutating heads do, has an inverse whose +derivative about the pose is not the coupling the machine has. Such a module +supplies the closed form, taken against the pose. + [[sec:writing-a-module]] == Writing a Module @@ -406,6 +500,12 @@ Orientation inverse:: do nothing. Register a closed form only where one exists, and where it does, say which poses it treats as degenerate. +Jacobian:: + Rows are joints, columns are pose coordinates, entries in the units the + forward and inverse already use, reported where the pose is. A module with + a closed form inverse differentiates it and registers the result; one + without lets the shared code difference the inverse. + Geometry stays in the module:: Whatever a consumer needs to know about the machine's shape is answered by the module. A consumer that restates it has taken a copy that nothing keeps diff --git a/src/emc/kinematics/kinematics.h b/src/emc/kinematics/kinematics.h index ba285cb8861..900fe5aa474 100644 --- a/src/emc/kinematics/kinematics.h +++ b/src/emc/kinematics/kinematics.h @@ -16,6 +16,7 @@ #define __LINUXCNC_KINEMATICS_H #include "emcpos.h" /* EmcPose */ +#include "emcmotcfg.h" /* EMCMOT_MAX_JOINTS, EMCMOT_MAX_AXIS */ #include "rtapi_bool.h" /* @@ -360,6 +361,90 @@ extern int toolFrameSolve(kinsFrameFunc work, int *free_directions, double *tool_spin); +/* How each joint responds to a unit rate of each pose coordinate: + + jac[j][a] = d joint[j] / d pose[a] + + Rows are joints, columns are pose coordinates in EmcPose order, x y z a b + c u v w. This is the derivative of kinematicsInverse(): multiply it by a + pose velocity and the result is the joint velocity that motion will + command, which is what a feed limit checks against the joint limits. A + row that grows without bound is a pose approaching a singularity, where + the joints cannot keep up with any world speed at all. + + Each entry is in joint units per pose unit, whatever units the module's + own forward and inverse already use. Nothing is converted here: a caller + that feeds pose rates in EmcPose units gets joint rates in the units + motion already commands, and never has to know which unit a rotary joint + is in. On every module in the tree both are degrees, so a table rotary's + own row is a plain 1 in its own column. + + The columns are pose coordinates, so the answer lives in the work frame, + where kinematicsForward() reports positions. The a, b and c columns are + rates of the pose words, the wrapped linear axes the planner already + treats as coordinates, and not an angular velocity vector. That makes + this a different object from the frames above, which are orientations + and are given against the machine; see the Kinematics Conventions + chapter. + + joint and world are one pose in both descriptions: world is what + kinematicsForward() reports for joint under these flags. Both are given + because a closed form differentiates at the joints while the generic + default perturbs the pose, and iflags keeps every inverse the default + calls on the same solution branch. Rows past the module's joint count + are zero. + + Optional, like the frames. Modules built on switchkins.c export it + always and answer for every type, since it can always be obtained from + the inverse where a frame cannot; other modules need not export it, and + a caller that resolves it dynamically and finds nothing can call + kinsJacobianFromInverse() itself with the module's inverse. + + Returns 0, or -1 if the module cannot answer at this pose. */ +extern int kinematicsJacobian(const double *joint, + const EmcPose *world, + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], + const KINEMATICS_INVERSE_FLAGS *iflags); + +typedef int (*kinsInverseFunc)(const EmcPose *world, + double *joint, + const KINEMATICS_INVERSE_FLAGS *iflags, + KINEMATICS_FORWARD_FLAGS *fflags); + +/* The generic Jacobian, by central differences of an inverse about world: + two inverse calls per pose coordinate, eighteen in all, on the solution + branch iflags selects. The joint array handed to every call starts from + joint, so a module that reads its joint argument sees the machine where + it is. + + The answer is as good as the inverse: a closed form gives it to rounding, + an inverse that iterates to a tolerance gives it to that tolerance over + the step, and should supply its own. num_joints is the module's joint + count. Returns 0, or -1 if any inverse fails. */ +#define KINS_JACOBIAN_STEP 1e-3 /* pose units, either kind */ + +extern int kinsJacobianFromInverse(kinsInverseFunc inverse, + int num_joints, + const double *joint, + const EmcPose *world, + const KINEMATICS_INVERSE_FLAGS *iflags, + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS]); + +/* For a module whose inverse computes a position P and then hands it to + position_to_mapped_joints(): given dP[axis][pose], how each coordinate of + P responds to each pose coordinate, fill in jac so that every joint gets + the row of the letter it is mapped to. Duplicate letters get duplicate + rows, which is the gantry case. */ +extern int kinsJacobianFromMappedAxes(int max_joints, + const double dP[EMCMOT_MAX_AXIS][EMCMOT_MAX_AXIS], + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS]); + +/* joints are axes: a 1 per joint in the column of its letter */ +extern int identityKinematicsJacobian(const double *joint, + const EmcPose *world, + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], + const KINEMATICS_INVERSE_FLAGS *iflags); + extern int kinematicsSwitchable(void); extern int kinematicsSwitch(int switchkins_type); //NOTE: switchable kinematics may require Interp::Synch @@ -414,6 +499,11 @@ extern int xyzacKinematicsWorkFrame(const double *joints, PmRotationMatrix *rot, const KINEMATICS_FORWARD_FLAGS *fflags); +extern int xyzacKinematicsJacobian(const double *joints, + const EmcPose *pos, + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], + const KINEMATICS_INVERSE_FLAGS *iflags); + extern int xyzbcKinematicsForward(const double *joints, EmcPose * pos, @@ -433,4 +523,9 @@ extern int xyzbcKinematicsWorkFrame(const double *joints, PmRotationMatrix *rot, const KINEMATICS_FORWARD_FLAGS *fflags); +extern int xyzbcKinematicsJacobian(const double *joints, + const EmcPose *pos, + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], + const KINEMATICS_INVERSE_FLAGS *iflags); + //********************************************************************* diff --git a/src/emc/kinematics/kins_util.c b/src/emc/kinematics/kins_util.c index fa20010d1d2..324392e6864 100644 --- a/src/emc/kinematics/kins_util.c +++ b/src/emc/kinematics/kins_util.c @@ -1040,3 +1040,126 @@ int toolFrameSolve(kinsFrameFunc work, } return found; } + +//---------------------------------------------------------------------- +// The Jacobian. See kinematics.h for what it is and which way it points. +//---------------------------------------------------------------------- + +static void kj_zero(double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS]) +{ + memset(jac, 0, EMCMOT_MAX_JOINTS * EMCMOT_MAX_AXIS * sizeof(jac[0][0])); +} + +// pose coordinate a of p, in EmcPose order +static double *kj_coord(EmcPose *p, int a) +{ + switch (a) { + case 0: return &p->tran.x; + case 1: return &p->tran.y; + case 2: return &p->tran.z; + case 3: return &p->a; + case 4: return &p->b; + case 5: return &p->c; + case 6: return &p->u; + case 7: return &p->v; + default: return &p->w; + } +} + +int kinsJacobianFromInverse(kinsInverseFunc inverse, + int num_joints, + const double *joint, + const EmcPose *world, + const KINEMATICS_INVERSE_FLAGS *iflags, + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS]) +{ + double qp[EMCMOT_MAX_JOINTS], qm[EMCMOT_MAX_JOINTS]; + KINEMATICS_INVERSE_FLAGS ifl = iflags ? *iflags : 0; + KINEMATICS_FORWARD_FLAGS ffl = 0; + EmcPose p; + int j, a; + + if (!inverse || !joint || !world || !jac + || num_joints <= 0 || num_joints > EMCMOT_MAX_JOINTS) { + return -1; + } + + kj_zero(jac); + + for (a = 0; a < EMCMOT_MAX_AXIS; a++) { + p = *world; + // the joint array every call sees starts at the machine's own + // position, for a module that reads it before writing it + for (j = 0; j < EMCMOT_MAX_JOINTS; j++) { qp[j] = qm[j] = joint[j]; } + + *kj_coord(&p, a) += KINS_JACOBIAN_STEP; + if (inverse(&p, qp, &ifl, &ffl)) { return -1; } + + *kj_coord(&p, a) -= 2 * KINS_JACOBIAN_STEP; + if (inverse(&p, qm, &ifl, &ffl)) { return -1; } + + for (j = 0; j < num_joints; j++) { + jac[j][a] = (qp[j] - qm[j]) / (2 * KINS_JACOBIAN_STEP); + } + } + return 0; +} // kinsJacobianFromInverse() + +int kinsJacobianFromMappedAxes(int max_joints, + const double dP[EMCMOT_MAX_AXIS][EMCMOT_MAX_AXIS], + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS]) +{ + int jno, a; + + if (!map_initialized) { + rtapi_print_msg(RTAPI_MSG_ERR, + "kinsJacobianFromMappedAxes before map_initialized\n"); + return -1; + } + if (max_joints <= 0 || max_joints > EMCMOT_MAX_JOINTS) { return -1; } + + kj_zero(jac); + + for (jno = 0; jno < max_joints; jno++) { + int bit = 1<= kins_count) { + return -1; + } + // a closed form is exact and knows its own singular poses + if (kjacs[switchkins_type]) { + return kjacs[switchkins_type](joint, world, jac, iflags); + } + // otherwise the type's own inverse, differenced. The type function + // rather than the dispatch, so this cannot recurse through a switch. + if (!kinvs[switchkins_type]) { return -1; } + return kinsJacobianFromInverse(kinvs[switchkins_type], kp.max_joints, + joint, world, iflags, jac); +} // kinematicsJacobian() + KINEMATICS_TYPE kinematicsType() { return KINEMATICS_BOTH; @@ -354,6 +374,20 @@ int switchkinsRegisterFrames(int ktype, KT kwork, KT ktool, return 0; } // switchkinsRegisterFrames() +int switchkinsRegisterJacobian(int ktype, KJ kjac) +{ + if (ktype < 0 || ktype >= SWITCHKINS_MAX_TYPES) { + rtapi_print_msg(RTAPI_MSG_ERR, + "switchkinsRegisterJacobian: BAD switchkins_type" + " <%d> (must be 0..%d)\n", + ktype, SWITCHKINS_MAX_TYPES - 1); + register_error = 1; + return -1; + } + kjacs[ktype] = kjac; + return 0; +} // switchkinsRegisterJacobian() + int switchkinsRegisterToolFrameInverse(int ktype, KTI kinv) { if (ktype < 0 || ktype >= SWITCHKINS_MAX_TYPES) { @@ -402,11 +436,13 @@ EXPORT_SYMBOL(kinematicsInverse); EXPORT_SYMBOL(kinematicsToolFrame); EXPORT_SYMBOL(kinematicsWorkFrame); EXPORT_SYMBOL(kinematicsToolFrameInverse); +EXPORT_SYMBOL(kinematicsJacobian); EXPORT_SYMBOL(switchkinsRegister); EXPORT_SYMBOL(switchkinsRegisterFrames); EXPORT_SYMBOL(switchkinsRegisterToolFrameInverse); EXPORT_SYMBOL(switchkinsDeclare); EXPORT_SYMBOL(kinematicsTypeFlags); +EXPORT_SYMBOL(switchkinsRegisterJacobian); MODULE_LICENSE("GPL"); static int comp_id; @@ -443,6 +479,10 @@ int rtapi_app_main(void) ktools[i] = identityKinematicsToolFrame; knative[i] = TOOL_FRAME_SPINDLE; } + // and its Jacobian is exact, so do not difference for it + if (!kjacs[i] && kfwds[i] == identityKinematicsForward) { + kjacs[i] = identityKinematicsJacobian; + } } // the highest type provided by either route sets the count diff --git a/src/emc/kinematics/switchkins.h b/src/emc/kinematics/switchkins.h index d325ce75e48..77caca90629 100644 --- a/src/emc/kinematics/switchkins.h +++ b/src/emc/kinematics/switchkins.h @@ -73,4 +73,15 @@ extern int switchkinsRegisterToolFrameInverse(int ktype, KTI kinv); // never calls it leaves its types numeric-only: G12.1 P still works, // G13.1 refuses to guess which type is identity. extern int switchkinsDeclare(int ktype, int flags); + +// KinematicsJACOBIAN function (optional, see kinematics.h) +typedef int (*KJ)(const double *joint, + const EmcPose *world, + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], + const KINEMATICS_INVERSE_FLAGS *iflags); + +// called from switchkinsSetup() only by a type with a closed form. A type +// that does not gets the exact answer if it is an identity type, and +// otherwise the generic differences of its own inverse. +extern int switchkinsRegisterJacobian(int ktype, KJ kjac); #endif // } diff --git a/src/emc/kinematics/trivkins.c b/src/emc/kinematics/trivkins.c index 4b3685dc6d6..f04d9642622 100644 --- a/src/emc/kinematics/trivkins.c +++ b/src/emc/kinematics/trivkins.c @@ -52,6 +52,14 @@ int kinematicsWorkFrame(const double *joints, return identityKinematicsWorkFrame(joints, rot, fflags); } +int kinematicsJacobian(const double *joints, + const EmcPose *pos, + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], + const KINEMATICS_INVERSE_FLAGS *iflags) +{ + return identityKinematicsJacobian(joints, pos, jac, iflags); +} + static KINEMATICS_TYPE ktype = -1; KINEMATICS_TYPE kinematicsType() @@ -72,6 +80,7 @@ EXPORT_SYMBOL(kinematicsForward); EXPORT_SYMBOL(kinematicsInverse); EXPORT_SYMBOL(kinematicsToolFrame); EXPORT_SYMBOL(kinematicsWorkFrame); +EXPORT_SYMBOL(kinematicsJacobian); MODULE_LICENSE("GPL"); static int comp_id; From 058fe43186993af2c92cd7852e074008ee4c9e2e Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Thu, 3 Sep 2026 18:21:24 +1000 Subject: [PATCH 2/9] trtfuncs, 5axiskins, maxkins: supply the Jacobian Each inverse is a rotation of the pose about the table or the pivot plus offsets, so the derivative is the same rotation for the linear columns and the rotation advanced a quarter turn, times the lever, for the rotary ones. The tables and 5axiskins build the position and hand it to position_to_mapped_joints(), so they fill a matrix of the position's derivative and let kinsJacobianFromMappedAxes() place the rows, which keeps duplicate letters right. maxkins has fixed joint numbers and fills its rows directly. --- src/emc/kinematics/5axiskins.c | 42 ++++++++++++ src/emc/kinematics/maxkins.c | 44 ++++++++++++ src/emc/kinematics/trtfuncs.c | 101 ++++++++++++++++++++++++++++ src/emc/kinematics/xyzac-trt-kins.c | 2 + src/emc/kinematics/xyzbc-trt-kins.c | 2 + 5 files changed, 191 insertions(+) diff --git a/src/emc/kinematics/5axiskins.c b/src/emc/kinematics/5axiskins.c index aa0af3cfb8b..6e7923d58f4 100644 --- a/src/emc/kinematics/5axiskins.c +++ b/src/emc/kinematics/5axiskins.c @@ -162,6 +162,46 @@ static int fiveaxis_KinematicsInverse(const EmcPose * pos, return 0; } // fiveaxis_kinematicsInverse() +static int fiveaxis_KinematicsJacobian(const double *joints, + const EmcPose * pos, + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], + const KINEMATICS_INVERSE_FLAGS * iflags) +{ + (void)joints; + (void)iflags; + rtapi_real pivot_length = hal_get_real(haldata->pivot_length); + const double R = pivot_length + pos->w; + const double sb = sin(TO_RAD*pos->b), cb = cos(TO_RAD*pos->b); + const double sc = sin(TO_RAD*pos->c), cc = cos(TO_RAD*pos->c); + double dP[EMCMOT_MAX_AXIS][EMCMOT_MAX_AXIS]; + int a; + + memset(dP, 0, sizeof(dP)); + + // the computed position of the inverse is the pose less the pivot + // vector r = s2r(R, c, 180 - b), which is (R sin b cos c, R sin b sin c, + // -R cos b); each row is that coordinate differentiated + dP[0][0] = 1; + dP[0][4] = -R * cb * cc * TO_RAD; + dP[0][5] = R * sb * sc * TO_RAD; + dP[0][8] = -sb * cc; + + dP[1][1] = 1; + dP[1][4] = -R * cb * sc * TO_RAD; + dP[1][5] = -R * sb * cc * TO_RAD; + dP[1][8] = -sb * sc; + + dP[2][2] = 1; + dP[2][4] = -R * sb * TO_RAD; + dP[2][8] = cb; + + for (a = 3; a < EMCMOT_MAX_AXIS; a++) { dP[a][a] = 1; } + + return kinsJacobianFromMappedAxes(fiveaxis_max_joints, + (const double (*)[EMCMOT_MAX_AXIS])dP, + jac); +} // fiveaxis_KinematicsJacobian() + int fiveaxis_KinematicsSetup(const int comp_id, const char* coordinates, kparms* kp) @@ -264,11 +304,13 @@ int switchkinsSetup(kparms* kp, *kinv1 = fiveaxis_KinematicsInverse; switchkinsDeclare(0, KINSTYPE_IDENTITY); switchkinsDeclare(1, KINSTYPE_PRIMARY); + switchkinsRegisterJacobian(1, fiveaxis_KinematicsJacobian); } else { rtapi_print("\n!!! switchkins-type 0 is %s\n",kp->kinsname); *kset0 = fiveaxis_KinematicsSetup; *kfwd0 = fiveaxis_KinematicsForward; *kinv0 = fiveaxis_KinematicsInverse; + switchkinsRegisterJacobian(0, fiveaxis_KinematicsJacobian); *kset1 = identityKinematicsSetup; *kfwd1 = identityKinematicsForward; diff --git a/src/emc/kinematics/maxkins.c b/src/emc/kinematics/maxkins.c index d2623e0ad62..a5725480094 100644 --- a/src/emc/kinematics/maxkins.c +++ b/src/emc/kinematics/maxkins.c @@ -19,6 +19,7 @@ #include #include #include +#include #include #include /* these decls */ @@ -121,6 +122,48 @@ int kinematicsInverse(const EmcPose * pos, return 0; } +int kinematicsJacobian(const double *joints, + const EmcPose * pos, + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], + const KINEMATICS_INVERSE_FLAGS * iflags) +{ + rtapi_real con = hal_get_bool(haldata->conventional_directions) ? 1.0 : -1.0; + rtapi_real pivot_length = hal_get_real(haldata->pivot_length); + const double k = M_PI/180; + const double sb = sin(d2r(pos->b)), cb = cos(d2r(pos->b)); + const double sc = sin(d2r(pos->c)), cc = cos(d2r(pos->c)); + const double x = pos->tran.x, y = pos->tran.y; + const double R = pivot_length + pos->w; + int j; + + (void)joints; + (void)iflags; + memset(jac, 0, EMCMOT_MAX_JOINTS * EMCMOT_MAX_AXIS * sizeof(jac[0][0])); + + // kinematicsInverse() with the polar form expanded: rotating (x, y) + // by -c is x*cos(c) + y*sin(c) and y*cos(c) - x*sin(c), and the + // B and U corrections are what they are written as + jac[0][0] = cc; + jac[0][1] = sc; + jac[0][4] = (con * R * cb - pos->u * sb) * k; + jac[0][5] = (-x * sc + y * cc) * k; + jac[0][6] = cb; + jac[0][8] = con * sb; + + jac[1][0] = -sc; + jac[1][1] = cc; + jac[1][5] = (-x * cc - y * sc) * k; + jac[1][7] = 1; + + jac[2][2] = 1; + jac[2][4] = (-R * sb + con * pos->u * cb) * k; + jac[2][6] = con * sb; + jac[2][8] = cb; + + for (j = 3; j < 9; j++) { jac[j][j] = 1; } + return 0; +} + KINEMATICS_TYPE kinematicsType() { return KINEMATICS_BOTH; @@ -130,6 +173,7 @@ KINS_NOT_SWITCHABLE EXPORT_SYMBOL(kinematicsType); EXPORT_SYMBOL(kinematicsInverse); EXPORT_SYMBOL(kinematicsForward); +EXPORT_SYMBOL(kinematicsJacobian); MODULE_LICENSE("GPL"); static int comp_id; diff --git a/src/emc/kinematics/trtfuncs.c b/src/emc/kinematics/trtfuncs.c index 0cb4b5eb7aa..b445524b6e9 100644 --- a/src/emc/kinematics/trtfuncs.c +++ b/src/emc/kinematics/trtfuncs.c @@ -299,6 +299,57 @@ int xyzacKinematicsToolFrame(const double *joints, return 0; } // xyzacKinematicsToolFrame() +int xyzacKinematicsJacobian(const double *joints, + const EmcPose *pos, + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], + const KINEMATICS_INVERSE_FLAGS *iflags) +{ + (void)joints; + (void)iflags; + const double x_rot_point = hal_get_real(haldata->x_rot_point); + const double y_rot_point = hal_get_real(haldata->y_rot_point); + const double z_rot_point = hal_get_real(haldata->z_rot_point); + const double dy = hal_get_real(haldata->y_offset); + const double dt = hal_get_real(haldata->tool_offset); + const double dz = hal_get_real(haldata->z_offset) + dt; + const double sa = sin(pos->a*TO_RAD), ca = cos(pos->a*TO_RAD); + const double sc = sin(pos->c*TO_RAD), cc = cos(pos->c*TO_RAD); + const double X = pos->tran.x - x_rot_point; + const double Y = pos->tran.y - y_rot_point; + const double Z = pos->tran.z - z_rot_point; + double dP[EMCMOT_MAX_AXIS][EMCMOT_MAX_AXIS]; + int a; + + rtapi_real con = hal_get_bool(haldata->conventional_directions) ? 1.0 : -1.0; + + memset(dP, 0, sizeof(dP)); + + // the computed position P of xyzacKinematicsInverse(), differentiated: + // its coefficients for x, y and z, and the same expressions with the + // rotation taken a quarter turn on for a and for c + dP[0][0] = cc; + dP[0][1] = con * sc; + dP[0][5] = (-sc*X + con*cc*Y) * TO_RAD; + + dP[1][0] = - con * sc * ca; + dP[1][1] = cc * ca; + dP[1][2] = con * sa; + dP[1][3] = (con*sc*sa*X - cc*sa*Y + con*ca*Z + sa*dy - con*ca*dz) * TO_RAD; + dP[1][5] = (-con*cc*ca*X - sc*ca*Y) * TO_RAD; + + dP[2][0] = sc * sa; + dP[2][1] = - con * cc * sa; + dP[2][2] = ca; + dP[2][3] = (sc*ca*X - con*cc*ca*Y - sa*Z + con*ca*dy + sa*dz) * TO_RAD; + dP[2][5] = (cc*sa*X + con*sc*sa*Y) * TO_RAD; + + for (a = 3; a < EMCMOT_MAX_AXIS; a++) { dP[a][a] = 1; } + + return kinsJacobianFromMappedAxes(trtfuncs_max_joints, + (const double (*)[EMCMOT_MAX_AXIS])dP, + jac); +} // xyzacKinematicsJacobian() + int xyzbcKinematicsForward(const double *joints, EmcPose * pos, const KINEMATICS_FORWARD_FLAGS * fflags, @@ -443,3 +494,53 @@ int xyzbcKinematicsToolFrame(const double *joints, *rot = TOOL_FRAME_SPINDLE; return 0; } // xyzbcKinematicsToolFrame() + +int xyzbcKinematicsJacobian(const double *joints, + const EmcPose *pos, + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], + const KINEMATICS_INVERSE_FLAGS *iflags) +{ + (void)joints; + (void)iflags; + const double x_rot_point = hal_get_real(haldata->x_rot_point); + const double y_rot_point = hal_get_real(haldata->y_rot_point); + const double z_rot_point = hal_get_real(haldata->z_rot_point); + const double dx = hal_get_real(haldata->x_offset); + const double dt = hal_get_real(haldata->tool_offset); + const double dz = hal_get_real(haldata->z_offset) + dt; + const double sb = sin(pos->b*TO_RAD), cb = cos(pos->b*TO_RAD); + const double sc = sin(pos->c*TO_RAD), cc = cos(pos->c*TO_RAD); + const double X = pos->tran.x - x_rot_point; + const double Y = pos->tran.y - y_rot_point; + const double Z = pos->tran.z - z_rot_point; + double dP[EMCMOT_MAX_AXIS][EMCMOT_MAX_AXIS]; + int a; + + rtapi_real con = hal_get_bool(haldata->conventional_directions) ? 1.0 : -1.0; + + memset(dP, 0, sizeof(dP)); + + // see the comment in xyzacKinematicsJacobian(); dpx and dpz of the + // inverse depend on b as well + dP[0][0] = cc * cb; + dP[0][1] = con * sc * cb; + dP[0][2] = - con * sb; + dP[0][4] = (-cc*sb*X - con*sc*sb*Y - con*cb*Z + sb*dx + con*cb*dz) * TO_RAD; + dP[0][5] = (-sc*cb*X + con*cc*cb*Y) * TO_RAD; + + dP[1][0] = - con * sc; + dP[1][1] = cc; + dP[1][5] = (-con*cc*X - sc*Y) * TO_RAD; + + dP[2][0] = con * cc * sb; + dP[2][1] = sc * sb; + dP[2][2] = cb; + dP[2][4] = (con*cc*cb*X + sc*cb*Y - sb*Z - con*cb*dx + sb*dz) * TO_RAD; + dP[2][5] = (-con*sc*sb*X + cc*sb*Y) * TO_RAD; + + for (a = 3; a < EMCMOT_MAX_AXIS; a++) { dP[a][a] = 1; } + + return kinsJacobianFromMappedAxes(trtfuncs_max_joints, + (const double (*)[EMCMOT_MAX_AXIS])dP, + jac); +} // xyzbcKinematicsJacobian() diff --git a/src/emc/kinematics/xyzac-trt-kins.c b/src/emc/kinematics/xyzac-trt-kins.c index 13fd6bd79ec..b8bb47bbc1f 100644 --- a/src/emc/kinematics/xyzac-trt-kins.c +++ b/src/emc/kinematics/xyzac-trt-kins.c @@ -43,6 +43,7 @@ int switchkinsSetup(kparms* kp, &TOOL_FRAME_SPINDLE); switchkinsDeclare(0, KINSTYPE_IDENTITY); switchkinsDeclare(1, KINSTYPE_PRIMARY); + switchkinsRegisterJacobian(1, xyzacKinematicsJacobian); } else { rtapi_print("\n!!! switchkins-type 0 is %s\n",kp->kinsname); *kset0 = trtKinematicsSetup; // trt: xyzac,xyzbc @@ -51,6 +52,7 @@ int switchkinsSetup(kparms* kp, switchkinsRegisterFrames(0, xyzacKinematicsWorkFrame, xyzacKinematicsToolFrame, &TOOL_FRAME_SPINDLE); + switchkinsRegisterJacobian(0, xyzacKinematicsJacobian); *kset1 = identityKinematicsSetup; *kfwd1 = identityKinematicsForward; diff --git a/src/emc/kinematics/xyzbc-trt-kins.c b/src/emc/kinematics/xyzbc-trt-kins.c index 73ea69bf820..7b61a69e301 100644 --- a/src/emc/kinematics/xyzbc-trt-kins.c +++ b/src/emc/kinematics/xyzbc-trt-kins.c @@ -43,6 +43,7 @@ int switchkinsSetup(kparms* kp, &TOOL_FRAME_SPINDLE); switchkinsDeclare(0, KINSTYPE_IDENTITY); switchkinsDeclare(1, KINSTYPE_PRIMARY); + switchkinsRegisterJacobian(1, xyzbcKinematicsJacobian); } else { rtapi_print("\n!!! switchkins-type 0 is %s\n",kp->kinsname); *kset0 = trtKinematicsSetup; // trt: xyzac,xyzbc @@ -51,6 +52,7 @@ int switchkinsSetup(kparms* kp, switchkinsRegisterFrames(0, xyzbcKinematicsWorkFrame, xyzbcKinematicsToolFrame, &TOOL_FRAME_SPINDLE); + switchkinsRegisterJacobian(0, xyzbcKinematicsJacobian); *kset1 = identityKinematicsSetup; *kfwd1 = identityKinematicsForward; From dd2ff0a9a15e88899890216d05912c825011172e Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Thu, 3 Sep 2026 18:21:25 +1000 Subject: [PATCH 3/9] corexykins, rotatekins, rosekins, matrixkins, millturn, userkins: supply the Jacobian The belt sum and difference, the rotation and its quarter turn, the polar radius and angle, the calibration matrix itself, and the two templates' joint to axis assignments. These are the short ones; they are here so that no module in the tree answers by differencing when its inverse is a few lines. --- src/emc/kinematics/corexykins.c | 19 ++++++++++++++++++ src/emc/kinematics/rosekins.c | 22 ++++++++++++++++++++ src/emc/kinematics/rotatekins.c | 23 +++++++++++++++++++++ src/hal/components/matrixkins.comp | 28 ++++++++++++++++++++++++++ src/hal/components/millturn.comp | 32 ++++++++++++++++++++++++++++++ src/hal/components/userkins.comp | 22 ++++++++++++++++++++ 6 files changed, 146 insertions(+) diff --git a/src/emc/kinematics/corexykins.c b/src/emc/kinematics/corexykins.c index f592ff52e9f..6d3ac4e75ab 100644 --- a/src/emc/kinematics/corexykins.c +++ b/src/emc/kinematics/corexykins.c @@ -6,6 +6,7 @@ #include #include +#include #include #include @@ -49,6 +50,23 @@ int kinematicsInverse(const EmcPose *pos return 0; } +int kinematicsJacobian(const double *joints, + const EmcPose *pos, + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], + const KINEMATICS_INVERSE_FLAGS *iflags) +{ + int j; + (void)joints; + (void)pos; + (void)iflags; + memset(jac, 0, EMCMOT_MAX_JOINTS * EMCMOT_MAX_AXIS * sizeof(jac[0][0])); + // the two belt motors each carry x and y, in opposite senses for y + jac[0][0] = 1; jac[0][1] = 1; + jac[1][0] = 1; jac[1][1] = -1; + for (j = 2; j < 9; j++) { jac[j][j] = 1; } + return 0; +} + int kinematicsHome(EmcPose *world ,double *joint ,KINEMATICS_FORWARD_FLAGS *fflags @@ -65,6 +83,7 @@ KINS_NOT_SWITCHABLE EXPORT_SYMBOL(kinematicsType); EXPORT_SYMBOL(kinematicsForward); EXPORT_SYMBOL(kinematicsInverse); +EXPORT_SYMBOL(kinematicsJacobian); MODULE_LICENSE("GPL"); static int comp_id; diff --git a/src/emc/kinematics/rosekins.c b/src/emc/kinematics/rosekins.c index adfe763a33a..ac7a11159f5 100644 --- a/src/emc/kinematics/rosekins.c +++ b/src/emc/kinematics/rosekins.c @@ -18,6 +18,7 @@ #include #include +#include #include #include #include @@ -26,6 +27,7 @@ KINS_NOT_SWITCHABLE EXPORT_SYMBOL(kinematicsType); EXPORT_SYMBOL(kinematicsInverse); EXPORT_SYMBOL(kinematicsForward); +EXPORT_SYMBOL(kinematicsJacobian); MODULE_LICENSE("GPL"); #ifndef hypot @@ -112,6 +114,26 @@ int kinematicsInverse(const EmcPose * pos, return 0; } +int kinematicsJacobian(const double *joints, + const EmcPose *pos, + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], + const KINEMATICS_INVERSE_FLAGS *iflags) +{ + double x = pos->tran.x, y = pos->tran.y; + double r2 = x*x + y*y; + double r = sqrt(r2); + (void)joints; + (void)iflags; + // on the axis the angle is undefined and its rate unbounded + if (r2 <= 0) { return -1; } + memset(jac, 0, EMCMOT_MAX_JOINTS * EMCMOT_MAX_AXIS * sizeof(jac[0][0])); + jac[0][0] = x/r; jac[0][1] = y/r; + jac[1][2] = 1; + jac[2][0] = -y/r2 * TO_DEG; + jac[2][1] = x/r2 * TO_DEG; + return 0; +} + KINEMATICS_TYPE kinematicsType() { return KINEMATICS_BOTH; diff --git a/src/emc/kinematics/rotatekins.c b/src/emc/kinematics/rotatekins.c index 838c9178154..47f95bee2cd 100644 --- a/src/emc/kinematics/rotatekins.c +++ b/src/emc/kinematics/rotatekins.c @@ -15,6 +15,7 @@ #include #include #include +#include #include #include /* these decls */ @@ -60,6 +61,27 @@ int kinematicsInverse(const EmcPose * pos, return 0; } +int kinematicsJacobian(const double *joints, + const EmcPose *pos, + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], + const KINEMATICS_INVERSE_FLAGS *iflags) +{ + double c_rad = pos->c*M_PI/180; + double cc = cos(c_rad), sc = sin(c_rad); + int j; + (void)joints; + (void)iflags; + memset(jac, 0, EMCMOT_MAX_JOINTS * EMCMOT_MAX_AXIS * sizeof(jac[0][0])); + // the inverse above, differentiated: the rotation itself for x and y, + // and the rotated point turned a quarter turn for c + jac[0][0] = cc; jac[0][1] = -sc; + jac[0][5] = (-pos->tran.x*sc - pos->tran.y*cc) * (M_PI/180); + jac[1][0] = sc; jac[1][1] = cc; + jac[1][5] = ( pos->tran.x*cc - pos->tran.y*sc) * (M_PI/180); + for (j = 2; j < 9; j++) { jac[j][j] = 1; } + return 0; +} + /* implemented for these kinematics as giving joints preference */ int kinematicsHome(EmcPose * world, double *joint, @@ -81,6 +103,7 @@ KINS_NOT_SWITCHABLE EXPORT_SYMBOL(kinematicsType); EXPORT_SYMBOL(kinematicsForward); EXPORT_SYMBOL(kinematicsInverse); +EXPORT_SYMBOL(kinematicsJacobian); MODULE_LICENSE("GPL"); int comp_id; diff --git a/src/hal/components/matrixkins.comp b/src/hal/components/matrixkins.comp index aac6c04d913..8bf76899c8e 100644 --- a/src/hal/components/matrixkins.comp +++ b/src/hal/components/matrixkins.comp @@ -229,6 +229,7 @@ error: KINS_NOT_SWITCHABLE EXPORT_SYMBOL(kinematicsType); EXPORT_SYMBOL(kinematicsInverse); +EXPORT_SYMBOL(kinematicsJacobian); EXPORT_SYMBOL(kinematicsForward); KINEMATICS_TYPE kinematicsType() @@ -321,3 +322,30 @@ int kinematicsInverse(const EmcPose * pos, return 0; } + +int kinematicsJacobian(const double *j, + const EmcPose * pos, + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], + const KINEMATICS_INVERSE_FLAGS * iflags) +{ + int r, c; + (void)j; + (void)pos; + (void)iflags; + for (r = 0; r < EMCMOT_MAX_JOINTS; r++) { + for (c = 0; c < EMCMOT_MAX_AXIS; c++) { jac[r][c] = 0; } + } + // the inverse is the calibration matrix itself, so its derivative is + // that matrix, and the pass-through axes are ones + jac[0][0] = hal_get_real(haldata->C_xx); + jac[0][1] = hal_get_real(haldata->C_xy); + jac[0][2] = hal_get_real(haldata->C_xz); + jac[1][0] = hal_get_real(haldata->C_yx); + jac[1][1] = hal_get_real(haldata->C_yy); + jac[1][2] = hal_get_real(haldata->C_yz); + jac[2][0] = hal_get_real(haldata->C_zx); + jac[2][1] = hal_get_real(haldata->C_zy); + jac[2][2] = hal_get_real(haldata->C_zz); + for (r = 3; r < 9; r++) { jac[r][r] = 1; } + return 0; +} diff --git a/src/hal/components/millturn.comp b/src/hal/components/millturn.comp index 45ec650ad96..abb217f7a3a 100644 --- a/src/hal/components/millturn.comp +++ b/src/hal/components/millturn.comp @@ -100,6 +100,7 @@ EXPORT_SYMBOL(kinematicsSwitchable); EXPORT_SYMBOL(kinematicsTypeFlags); EXPORT_SYMBOL(kinematicsSwitch); EXPORT_SYMBOL(kinematicsInverse); +EXPORT_SYMBOL(kinematicsJacobian); EXPORT_SYMBOL(kinematicsForward); static rtapi_u32 switchkins_type; @@ -224,3 +225,34 @@ int kinematicsInverse(const EmcPose * pos, return 0; } // kinematicsInverse() + +int kinematicsJacobian(const double *j, + const EmcPose * pos, + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], + const KINEMATICS_INVERSE_FLAGS * iflags) +{ + int r, c; + (void)j; + (void)pos; + (void)iflags; + for (r = 0; r < EMCMOT_MAX_JOINTS; r++) { + for (c = 0; c < EMCMOT_MAX_AXIS; c++) { jac[r][c] = 0; } + } + // the derivative of kinematicsInverse() for each type: which joint + // follows which pose coordinate, and in which sense + switch (switchkins_type) { + case 0: + jac[0][0] = 1; + jac[1][1] = 1; + jac[2][2] = 1; + jac[3][3] = 1; + break; + case 1: + jac[2][0] = 1; + jac[1][1] = -1; + jac[0][2] = 1; + jac[3][3] = 1; + break; + } + return 0; +} // kinematicsJacobian() diff --git a/src/hal/components/userkins.comp b/src/hal/components/userkins.comp index a7af5d29a75..ac0c003369d 100644 --- a/src/hal/components/userkins.comp +++ b/src/hal/components/userkins.comp @@ -128,6 +128,7 @@ KINS_NOT_SWITCHABLE EXPORT_SYMBOL(kinematicsType); EXPORT_SYMBOL(kinematicsInverse); +EXPORT_SYMBOL(kinematicsJacobian); EXPORT_SYMBOL(kinematicsForward); KINEMATICS_TYPE kinematicsType() @@ -194,3 +195,24 @@ int kinematicsInverse(const EmcPose * pos, return 0; } // kinematicsInverse() + +int kinematicsJacobian(const double *j, + const EmcPose * pos, + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], + const KINEMATICS_INVERSE_FLAGS * iflags) +{ + int r, c; + (void)j; + (void)pos; + (void)iflags; + // How each joint responds to each pose coordinate, the derivative of + // kinematicsInverse(): for this template joint 0 follows x, joint 1 + // follows y and joint 2 follows z, each one for one. See kinematics.h. + for (r = 0; r < EMCMOT_MAX_JOINTS; r++) { + for (c = 0; c < EMCMOT_MAX_AXIS; c++) { jac[r][c] = 0; } + } + jac[0][0] = 1; + jac[1][1] = 1; + jac[2][2] = 1; + return 0; +} // kinematicsJacobian() From 1692344d941dcbf773c6bde8c30b212be9dba268 Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Thu, 3 Sep 2026 18:21:25 +1000 Subject: [PATCH 4/9] xyzab_tdr_kins, xyzacb_trsrn, xyzbca_trsrn: supply the Jacobian The dual rotary table is its TCP inverse differentiated: the rotation matrix for the linear columns, and each term with A or B advanced a quarter turn for the rotary ones. The nutating heads are differentiated the same way, term by term through the secondary angle's r, s and t and the primary angle's sine and cosine. Their TCP inverse reads the rotary angles from the joint argument rather than from the pose, the two being the same numbers once a move is done; the derivative is taken against the pose, which is what a consumer multiplies by, and is the coupling the machine has. The TOOL type takes its angles from pins, so its inverse is linear in the pose and its rows are the coefficients. --- src/hal/components/xyzab_tdr_kins.comp | 62 +++++++++++ src/hal/components/xyzacb_trsrn.comp | 136 +++++++++++++++++++++++++ src/hal/components/xyzbca_trsrn.comp | 136 +++++++++++++++++++++++++ 3 files changed, 334 insertions(+) diff --git a/src/hal/components/xyzab_tdr_kins.comp b/src/hal/components/xyzab_tdr_kins.comp index dd0350e44f7..2ee61e6a9b8 100644 --- a/src/hal/components/xyzab_tdr_kins.comp +++ b/src/hal/components/xyzab_tdr_kins.comp @@ -95,6 +95,7 @@ EXPORT_SYMBOL(kinematicsSwitchable); EXPORT_SYMBOL(kinematicsSwitch); EXPORT_SYMBOL(kinematicsTypeFlags); EXPORT_SYMBOL(kinematicsInverse); +EXPORT_SYMBOL(kinematicsJacobian); EXPORT_SYMBOL(kinematicsForward); static rtapi_u32 switchkins_type; @@ -265,3 +266,64 @@ int kinematicsInverse(const EmcPose * pos, return 0; } // kinematicsInverse() + +int kinematicsJacobian(const double *j, + const EmcPose * pos, + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], + const KINEMATICS_INVERSE_FLAGS * iflags) +{ + (void)j; + (void)iflags; + double x_rot_point = hal_get_real(haldata->x_rot_point); + double y_rot_point = hal_get_real(haldata->y_rot_point); + double z_rot_point = hal_get_real(haldata->z_rot_point); + double dx = hal_get_real(haldata->x_offset); + double dz = hal_get_real(haldata->z_offset); + double dt = hal_get_real(haldata->tool_offset_z); + double sa = sin(pos->a*TO_RAD); + double ca = cos(pos->a*TO_RAD); + double sb = sin(pos->b*TO_RAD); + double cb = cos(pos->b*TO_RAD); + double qx = pos->tran.x - x_rot_point - dx; + double qy = pos->tran.y - y_rot_point; + double qz = pos->tran.z - z_rot_point - dz - dt; + int r, c; + + for (r = 0; r < EMCMOT_MAX_JOINTS; r++) { + for (c = 0; c < EMCMOT_MAX_AXIS; c++) { jac[r][c] = 0; } + } + + switch (switchkins_type) { + case 0: // ====================== IDENTITY kinematics JACOBIAN ==================== + jac[0][0] = 1; + jac[1][1] = 1; + jac[2][2] = 1; + jac[3][3] = 1; + jac[4][4] = 1; + break; + case 1: // ========================= TCP kinematics JACOBIAN ====================== + // the TCP inverse above differentiated: its coefficients of + // qx, qy and qz for the linear columns, and the same terms + // with a or b advanced a quarter turn for the rotary columns + jac[0][0] = cb; + jac[0][1] = sa*sb; + jac[0][2] = -ca*sb; + jac[0][3] = ( ca*sb*qy + sa*sb*qz) * TO_RAD; + jac[0][4] = (-sb*qx + sa*cb*qy - ca*cb*qz - sb*dx - cb*dz) * TO_RAD; + + jac[1][1] = ca; + jac[1][2] = sa; + jac[1][3] = (-sa*qy + ca*qz) * TO_RAD; + + jac[2][0] = sb; + jac[2][1] = -sa*cb; + jac[2][2] = ca*cb; + jac[2][3] = (-ca*cb*qy - sa*cb*qz) * TO_RAD; + jac[2][4] = ( cb*qx + sa*sb*qy - ca*sb*qz + cb*dx - sb*dz) * TO_RAD; + + jac[3][3] = 1; + jac[4][4] = 1; + break; + } + return 0; +} // kinematicsJacobian() diff --git a/src/hal/components/xyzacb_trsrn.comp b/src/hal/components/xyzacb_trsrn.comp index 321d45584e5..67fd97715a8 100644 --- a/src/hal/components/xyzacb_trsrn.comp +++ b/src/hal/components/xyzacb_trsrn.comp @@ -91,6 +91,7 @@ EXPORT_SYMBOL(kinematicsSwitchable); EXPORT_SYMBOL(kinematicsSwitch); EXPORT_SYMBOL(kinematicsTypeFlags); EXPORT_SYMBOL(kinematicsInverse); +EXPORT_SYMBOL(kinematicsJacobian); EXPORT_SYMBOL(kinematicsForward); EXPORT_SYMBOL(kinematicsToolFrame); EXPORT_SYMBOL(kinematicsWorkFrame); @@ -555,3 +556,138 @@ int kinematicsInverse(const EmcPose * pos, return 0; } // kinematicsInverse() + +int kinematicsJacobian(const double *j, + const EmcPose * pos, + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], + const KINEMATICS_INVERSE_FLAGS * iflags) +{ + (void)j; + (void)iflags; + + // the same geometry as kinematicsInverse(), read the same way + double Ly = hal_get_real(haldata->y_pivot); + double Lz = hal_get_real(haldata->z_pivot); + double Dx = hal_get_real(haldata->x_offset); + double Dy = hal_get_real(haldata->y_offset); + double Dray = hal_get_real(haldata->y_rot_axis) - (Dy + Ly); + double Draz = hal_get_real(haldata->z_rot_axis) - Lz; + double tc = hal_get_real(haldata->pre_rot); + double nu = hal_get_real(haldata->nut_angle); // degrees + double theta_1 = hal_get_real(haldata->prim_angle); // degrees + double theta_2 = hal_get_real(haldata->sec_angle); // degrees + double Dt = hal_get_real(haldata->tool_offset_z); + + double Sv = sin(nu*TO_RAD); + double Cv = cos(nu*TO_RAD); + double Stc = sin(tc); + double Ctc = cos(tc); + + // The TCP inverse reads the rotary angles from its joint argument, + // where the machine is, and its own pose words for the same angles + // are the same numbers once the move is done. Its derivative is taken + // against the pose, which is what a consumer multiplies by. + double Sw = sin(pos->a*TO_RAD); + double Cw = cos(pos->a*TO_RAD); + double Ss = 0, Cs = 0, Sp = 0, Cp = 0; + double CvSs = 0, SvSs = 0, r = 0, s = 0, t = 0; + // derivatives of the above over the secondary angle (Ss, r, s, t, CvSs, + // SvSs) and the primary angle (Sp, Cp), per degree + double dSs = 0, dr = 0, ds = 0, dt_ = 0, dCvSs = 0, dSvSs = 0; + double dSp = 0, dCp = 0; + + double Qy = pos->tran.y; + double Qz = pos->tran.z; + double Ay, Az; // the two lever arms the table turns about + int R, C; + + for (R = 0; R < EMCMOT_MAX_JOINTS; R++) { + for (C = 0; C < EMCMOT_MAX_AXIS; C++) { jac[R][C] = 0; } + } + + switch (switchkins_type) { + + case 0: // ========================= IDENTITY kinematics JACOBIAN ==================== + for (R = 0; R < 6; R++) { jac[R][R] = 1; } + break; + + case 1: // ========================= TCP kinematics JACOBIAN + Ss = sin(pos->b*TO_RAD); + Cs = cos(pos->b*TO_RAD); + Sp = sin(pos->c*TO_RAD); + Cp = cos(pos->c*TO_RAD); + CvSs = Cv*Ss; + SvSs = Sv*Ss; + r = Cs + Sv*Sv*(1-Cs); + s = Cs + Cv*Cv*(1-Cs); + t = Sv*Cv*(1-Cs); + + dSs = Cs*TO_RAD; + dr = -Ss*Cv*Cv*TO_RAD; + ds = -Ss*Sv*Sv*TO_RAD; + dt_ = Sv*Cv*Ss*TO_RAD; + dCvSs = Cv*dSs; + dSvSs = Sv*dSs; + dSp = Cp*TO_RAD; + dCp = -Sp*TO_RAD; + + Ay = Dray + Dy + Ly - Qy; + Az = Draz + Dt + Lz - Qz; + + // j[0]: Qx plus terms in the head angles only + jac[0][0] = 1; + jac[0][4] = (Cp*dSvSs - Sp*dt_)*(Dt + Lz) - (Cp*dCvSs + Sp*dr)*Ly; + jac[0][5] = (dCp*SvSs - dSp*t)*(Dt + Lz) + dCp*Dx + - (dCp*CvSs + dSp*r)*Ly - Dy*dSp; + + // j[1]: -Cw*Ay - Az*Sw plus head terms + jac[1][1] = Cw; + jac[1][2] = Sw; + jac[1][3] = ( Sw*Ay - Az*Cw)*TO_RAD; + jac[1][4] = (Sp*dSvSs + Cp*dt_)*(Dt + Lz) - (dCvSs*Sp - Cp*dr)*Ly; + jac[1][5] = dCp*Dy + Dx*dSp + (dSp*SvSs + dCp*t)*(Dt + Lz) + - (CvSs*dSp - dCp*r)*Ly; + + // j[2]: -Cw*Az + Ay*Sw plus head terms + jac[2][1] = -Sw; + jac[2][2] = Cw; + jac[2][3] = ( Sw*Az + Ay*Cw)*TO_RAD; + jac[2][4] = (Dt + Lz)*ds + Ly*dt_; + + jac[3][3] = 1; + jac[4][4] = 1; + jac[5][5] = 1; + break; + + case 2: // ========================= TOOL kinematics JACOBIAN + // the head angles come from pins, so the inverse is linear in + // the pose and the rows are its coefficients + Ss = sin(theta_2*TO_RAD); + Cs = cos(theta_2*TO_RAD); + Sp = sin(theta_1*TO_RAD); + Cp = cos(theta_1*TO_RAD); + CvSs = Cv*Ss; + SvSs = Sv*Ss; + r = Cs + Sv*Sv*(1-Cs); + s = Cs + Cv*Cv*(1-Cs); + t = Sv*Cv*(1-Cs); + + jac[0][0] = ((Cp*Cs - CvSs*Sp)*Ctc - (Cp*CvSs + Sp*r)*Stc); + jac[0][1] = -((Cp*CvSs + Sp*r)*Ctc + (Cp*Cs - CvSs*Sp)*Stc); + jac[0][2] = (Cp*SvSs - Sp*t); + + jac[1][0] = ((Cp*CvSs + Cs*Sp)*Ctc - (CvSs*Sp - Cp*r)*Stc); + jac[1][1] = -((CvSs*Sp - Cp*r)*Ctc + (Cp*CvSs + Cs*Sp)*Stc); + jac[1][2] = (Sp*SvSs + Cp*t); + + jac[2][0] = -(Ctc*SvSs - Stc*t); + jac[2][1] = (Stc*SvSs + Ctc*t); + jac[2][2] = s; + + jac[3][3] = 1; + jac[4][4] = 1; + jac[5][5] = 1; + break; + } + return 0; +} // kinematicsJacobian() diff --git a/src/hal/components/xyzbca_trsrn.comp b/src/hal/components/xyzbca_trsrn.comp index 371349d2365..10126165eb1 100644 --- a/src/hal/components/xyzbca_trsrn.comp +++ b/src/hal/components/xyzbca_trsrn.comp @@ -93,6 +93,7 @@ EXPORT_SYMBOL(kinematicsSwitchable); EXPORT_SYMBOL(kinematicsSwitch); EXPORT_SYMBOL(kinematicsTypeFlags); EXPORT_SYMBOL(kinematicsInverse); +EXPORT_SYMBOL(kinematicsJacobian); EXPORT_SYMBOL(kinematicsForward); EXPORT_SYMBOL(kinematicsToolFrame); EXPORT_SYMBOL(kinematicsWorkFrame); @@ -560,3 +561,138 @@ int kinematicsInverse(const EmcPose * pos, return 0; } // kinematicsInverse() + +int kinematicsJacobian(const double *j, + const EmcPose * pos, + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], + const KINEMATICS_INVERSE_FLAGS * iflags) +{ + (void)j; + (void)iflags; + + // the same geometry as kinematicsInverse(), read the same way + double Lx = hal_get_real(haldata->x_pivot); + double Lz = hal_get_real(haldata->z_pivot); + double Dx = hal_get_real(haldata->x_offset); + double Dy = hal_get_real(haldata->y_offset); + double Drax = hal_get_real(haldata->x_rot_axis) - Lx - Dx; + double Draz = hal_get_real(haldata->z_rot_axis) - Lz; + double tc = hal_get_real(haldata->pre_rot); + double nu = hal_get_real(haldata->nut_angle); // degrees + double theta_1 = hal_get_real(haldata->prim_angle); // degrees + double theta_2 = hal_get_real(haldata->sec_angle); // degrees + double Dt = hal_get_real(haldata->tool_offset_z); + + double Sv = sin(nu*TO_RAD); + double Cv = cos(nu*TO_RAD); + double Stc = sin(tc); + double Ctc = cos(tc); + + // The TCP inverse reads the rotary angles from its joint argument, + // where the machine is, and its own pose words for the same angles + // are the same numbers once the move is done. Its derivative is taken + // against the pose, which is what a consumer multiplies by. + double Sw = sin(pos->b*TO_RAD); + double Cw = cos(pos->b*TO_RAD); + double Ss = 0, Cs = 0, Sp = 0, Cp = 0; + double CvSs = 0, SvSs = 0, r = 0, s = 0, t = 0; + // derivatives of the above over the secondary angle (Ss, r, s, t, CvSs, + // SvSs) and the primary angle (Sp, Cp), per degree + double dSs = 0, dr = 0, ds = 0, dt_ = 0, dCvSs = 0, dSvSs = 0; + double dSp = 0, dCp = 0; + + double Qx = pos->tran.x; + double Qz = pos->tran.z; + double Ax, Az; // the two lever arms the table turns about + int R, C; + + for (R = 0; R < EMCMOT_MAX_JOINTS; R++) { + for (C = 0; C < EMCMOT_MAX_AXIS; C++) { jac[R][C] = 0; } + } + + switch (switchkins_type) { + + case 0: // ========================= IDENTITY kinematics JACOBIAN ==================== + for (R = 0; R < 6; R++) { jac[R][R] = 1; } + break; + + case 1: // ========================= TCP kinematics JACOBIAN + Ss = sin(pos->a*TO_RAD); + Cs = cos(pos->a*TO_RAD); + Sp = sin(pos->c*TO_RAD); + Cp = cos(pos->c*TO_RAD); + CvSs = Cv*Ss; + SvSs = Sv*Ss; + r = Cs + Sv*Sv*(1-Cs); + s = Cs + Cv*Cv*(1-Cs); + t = Sv*Cv*(1-Cs); + + dSs = Cs*TO_RAD; + dr = -Ss*Cv*Cv*TO_RAD; + ds = -Ss*Sv*Sv*TO_RAD; + dt_ = Sv*Cv*Ss*TO_RAD; + dCvSs = Cv*dSs; + dSvSs = Sv*dSs; + dSp = Cp*TO_RAD; + dCp = -Sp*TO_RAD; + + Ax = Drax + Dx + Lx - Qx; + Az = Draz + Dt + Lz - Qz; + + // j[0]: -Cw*Ax + Az*Sw plus head terms + jac[0][0] = Cw; + jac[0][2] = -Sw; + jac[0][3] = (Sp*dSvSs + Cp*dt_)*(Dt + Lz) - (dCvSs*Sp - Cp*dr)*Lx; + jac[0][4] = ( Sw*Ax + Az*Cw)*TO_RAD; + jac[0][5] = dCp*Dx - Dy*dSp + (dSp*SvSs + dCp*t)*(Dt + Lz) + - (CvSs*dSp - dCp*r)*Lx; + + // j[1]: Qy plus head terms + jac[1][1] = 1; + jac[1][3] = -(Cp*dSvSs - Sp*dt_)*(Dt + Lz) + (Cp*dCvSs + Sp*dr)*Lx; + jac[1][5] = -(dCp*SvSs - dSp*t)*(Dt + Lz) + dCp*Dy + + (dCp*CvSs + dSp*r)*Lx + Dx*dSp; + + // j[2]: -Cw*Az - Ax*Sw plus head terms + jac[2][0] = Sw; + jac[2][2] = Cw; + jac[2][3] = (Dt + Lz)*ds + Lx*dt_; + jac[2][4] = ( Sw*Az - Ax*Cw)*TO_RAD; + + jac[3][3] = 1; + jac[4][4] = 1; + jac[5][5] = 1; + break; + + case 2: // ========================= TOOL kinematics JACOBIAN + // the head angles come from pins, so the inverse is linear in + // the pose and the rows are its coefficients + Ss = sin(theta_2*TO_RAD); + Cs = cos(theta_2*TO_RAD); + Sp = sin(theta_1*TO_RAD); + Cp = cos(theta_1*TO_RAD); + CvSs = Cv*Ss; + SvSs = Sv*Ss; + r = Cs + Sv*Sv*(1-Cs); + s = Cs + Cv*Cv*(1-Cs); + t = Sv*Cv*(1-Cs); + + jac[0][0] = -((CvSs*Sp - Cp*r)*Ctc + (Cp*CvSs + Cs*Sp)*Stc); + jac[0][1] = -((Cp*CvSs + Cs*Sp)*Ctc - (CvSs*Sp - Cp*r)*Stc); + jac[0][2] = (Sp*SvSs + Cp*t); + + jac[1][0] = ((Cp*CvSs + Sp*r)*Ctc + (Cp*Cs - CvSs*Sp)*Stc); + jac[1][1] = ((Cp*Cs - CvSs*Sp)*Ctc - (Cp*CvSs + Sp*r)*Stc); + jac[1][2] = -(Cp*SvSs - Sp*t); + + jac[2][0] = (Stc*SvSs + Ctc*t); + jac[2][1] = (Ctc*SvSs - Stc*t); + jac[2][2] = s; + + jac[3][3] = 1; + jac[4][4] = 1; + jac[5][5] = 1; + break; + } + return 0; +} // kinematicsJacobian() From f8b3621f64c74490ba243f22b0634e1c0bb6710b Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Thu, 3 Sep 2026 18:21:25 +1000 Subject: [PATCH 5/9] tripodkins, lineardeltakins, rotarydeltakins, genhexkins, pentakins: supply the Jacobian A strut or rod changes length by the component of its moving end's motion along it, so the rows of the parallel machines are unit vectors and moments rather than differentiated formulas. The tripod's rows are the strut directions; the linear delta's are the rod directions scaled by the rise; the rotary delta's follow from the foot staying a shin from each knee, so the foot and the knee agree along the leg. The hexapod's rows are the ones its own Newton step already builds, with the rotary columns taken through the matrix that carries roll, pitch and yaw rates to the angular velocity; with a screw lead set, whose correction is a function of the pose too, it falls back to differencing. The pentapod differentiates InvKins() the same way, in effector coordinates. --- src/emc/kinematics/genhexkins.c | 69 ++++++++++++++++++++++++++ src/emc/kinematics/lineardeltakins.c | 26 ++++++++++ src/emc/kinematics/pentakins.c | 74 ++++++++++++++++++++++++++++ src/emc/kinematics/rotarydeltakins.c | 52 +++++++++++++++++++ src/emc/kinematics/tripodkins.c | 31 ++++++++++++ 5 files changed, 252 insertions(+) diff --git a/src/emc/kinematics/genhexkins.c b/src/emc/kinematics/genhexkins.c index 2966c377852..c634611ee62 100644 --- a/src/emc/kinematics/genhexkins.c +++ b/src/emc/kinematics/genhexkins.c @@ -544,6 +544,73 @@ static int genhexKinematicsInverse(const EmcPose * pos, return 0; } //genhexKinematicsInverse() +/************************ genhexKinematicsJacobian() ***********************/ +/* A strut length changes by the component of its platform end's motion + along the strut. That end moves with the platform, dP + w x (R a), so + the row for strut i is [u_i, (R a_i x u_i) . E] with u_i the unit strut + vector and E the matrix taking the rates of the roll, pitch and yaw + words to the angular velocity w for R = Rz(c) Ry(b) Rx(a). The forward + kinematics builds the same rows for its Newton step, in radians. */ + +static int genhexKinematicsJacobian(const double * joints, + const EmcPose * pos, + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], + const KINEMATICS_INVERSE_FLAGS * iflags) +{ + PmCartesian aw, RMatrix_a, strut, u, moment; + PmRotationMatrix RMatrix; + PmRpy rpy; + PmCartesian E[3]; + double sb, cb, sc, cc; + int i, m; + + genhex_read_hal_pins(); + + /* the screw lead correction is a function of the pose too, and this + does not differentiate it; difference the inverse instead */ + if (hal_get_real(haldata->screw_lead) != 0.0) { + return kinsJacobianFromInverse(genhexKinematicsInverse, NUM_STRUTS, + joints, pos, iflags, jac); + } + + memset(jac, 0, EMCMOT_MAX_JOINTS * EMCMOT_MAX_AXIS * sizeof(jac[0][0])); + + rpy.r = pos->a * PM_PI / 180.0; + rpy.p = pos->b * PM_PI / 180.0; + rpy.y = pos->c * PM_PI / 180.0; + pmRpyMatConvert(&rpy, &RMatrix); + + /* w = E [da db dc]: the roll axis carried by pitch and yaw, the pitch + axis carried by yaw, and the yaw axis fixed */ + sb = sin(rpy.p); cb = cos(rpy.p); + sc = sin(rpy.y); cc = cos(rpy.y); + E[0].x = cb*cc; E[0].y = cb*sc; E[0].z = -sb; + E[1].x = -sc; E[1].y = cc; E[1].z = 0; + E[2].x = 0; E[2].y = 0; E[2].z = 1; + + for (i = 0; i < NUM_STRUTS; i++) { + double len; + + pmMatCartMult(&RMatrix, &a[i], &RMatrix_a); + pmCartCartAdd(&pos->tran, &RMatrix_a, &aw); + pmCartCartSub(&aw, &b[i], &strut); + pmCartMag(&strut, &len); + if (len <= 0) { return -1; } + pmCartScalMult(&strut, 1.0/len, &u); + pmCartCartCross(&RMatrix_a, &u, &moment); + + jac[i][0] = u.x; + jac[i][1] = u.y; + jac[i][2] = u.z; + for (m = 0; m < 3; m++) { + double dot; + pmCartCartDot(&moment, &E[m], &dot); + jac[i][3+m] = dot * PM_PI / 180.0; + } + } + return 0; +} // genhexKinematicsJacobian() + // HAL pin initializaion values. In small arrays so we can easily // address them in the pin creation loop. static const rtapi_real init_basex[NUM_STRUTS] = { @@ -707,6 +774,7 @@ int switchkinsSetup(kparms* kp, *kinv1 = genhexKinematicsInverse; switchkinsDeclare(0, KINSTYPE_IDENTITY); switchkinsDeclare(1, KINSTYPE_PRIMARY); + switchkinsRegisterJacobian(1, genhexKinematicsJacobian); } else { rtapi_print("\n!!! switchkins-type 0 is %s\n",kp->kinsname); kp->fwd_iterates_mask = 0x1; //genhexkins switchkins_type==0 @@ -715,6 +783,7 @@ int switchkinsSetup(kparms* kp, *kset0 = genhexKinematicsSetup; *kfwd0 = genhexKinematicsForward; *kinv0 = genhexKinematicsInverse; + switchkinsRegisterJacobian(0, genhexKinematicsJacobian); *kset1 = identityKinematicsSetup; *kfwd1 = identityKinematicsForward; diff --git a/src/emc/kinematics/lineardeltakins.c b/src/emc/kinematics/lineardeltakins.c index 353e9234562..d894bb46adb 100644 --- a/src/emc/kinematics/lineardeltakins.c +++ b/src/emc/kinematics/lineardeltakins.c @@ -16,6 +16,7 @@ #include #include +#include #include #include @@ -48,6 +49,30 @@ int kinematicsInverse(const EmcPose *pos, double *joints, return kinematics_inverse(pos, joints); } +int kinematicsJacobian(const double *joints, + const EmcPose *pos, + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], + const KINEMATICS_INVERSE_FLAGS *iflags) { + double x = pos->tran.x, y = pos->tran.y, z = pos->tran.z; + int i, j; + (void)iflags; + set_geometry(hal_get_real(haldata->r), hal_get_real(haldata->l)); + memset(jac, 0, EMCMOT_MAX_JOINTS * EMCMOT_MAX_AXIS * sizeof(jac[0][0])); + // each carriage is the platform height plus the rise of its rod, and + // the rise changes with the horizontal offset from the tower + for (i = 0; i < 3; i++) { + double tx = (i == 0) ? Ax : (i == 1) ? Bx : Cx; + double ty = (i == 0) ? Ay : (i == 1) ? By : Cy; + double rise = joints[i] - z; + if (rise <= 0) { return -1; } + jac[i][0] = (tx - x)/rise; + jac[i][1] = (ty - y)/rise; + jac[i][2] = 1; + } + for (j = 3; j < 9; j++) { jac[j][j] = 1; } + return 0; +} + KINEMATICS_TYPE kinematicsType() { return KINEMATICS_BOTH; @@ -85,4 +110,5 @@ KINS_NOT_SWITCHABLE EXPORT_SYMBOL(kinematicsType); EXPORT_SYMBOL(kinematicsForward); EXPORT_SYMBOL(kinematicsInverse); +EXPORT_SYMBOL(kinematicsJacobian); MODULE_LICENSE("GPL"); diff --git a/src/emc/kinematics/pentakins.c b/src/emc/kinematics/pentakins.c index 18487be3134..551b79a4260 100644 --- a/src/emc/kinematics/pentakins.c +++ b/src/emc/kinematics/pentakins.c @@ -50,6 +50,7 @@ #include #include #include +#include #include #include /* these decls, KINEMATICS_FORWARD_FLAGS */ @@ -399,6 +400,78 @@ int kinematicsInverse(const EmcPose * pos, return 0; } +int kinematicsJacobian(const double * joints, + const EmcPose * pos, + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], + const KINEMATICS_INVERSE_FLAGS * iflags) +{ + PmRotationMatrix R; + PmRpy rpy; + PmCartesian P, d, xyz, wa, wb, dxyz[5]; + int i, col; + + (void)joints; + (void)iflags; + pentakins_read_hal_pins(); + memset(jac, 0, EMCMOT_MAX_JOINTS * EMCMOT_MAX_AXIS * sizeof(jac[0][0])); + + /* InvKins() differentiated. The effector end of each strut is found in + effector coordinates as xyz = R^T (b - P) with R = Ry(b) Rx(a), so a + pose translation moves it by -R^T and a pose rotation about w moves + it by -R^T (w x (b - P)); the strut length is then the distance from + that point to the strut's pivot circle of radius ra at height za. */ + P = pos->tran; + rpy.r = pos->a * PM_PI / 180.0; + rpy.p = pos->b * PM_PI / 180.0; + rpy.y = 0; + pmRpyMatConvert(&rpy, &R); + + /* rotation axes for a and b, in world coordinates */ + wa.x = cos(rpy.p); wa.y = 0; wa.z = -sin(rpy.p); + wb.x = 0; wb.y = 1; wb.z = 0; + + for (i = 0; i < NUM_STRUTS; i++) { + double rho, A, B, len; + + pmCartCartSub(&b[i], &P, &d); + /* R^T d, written out since pmMatCartMult applies R */ + xyz.x = R.x.x*d.x + R.x.y*d.y + R.x.z*d.z; + xyz.y = R.y.x*d.x + R.y.y*d.y + R.y.z*d.z; + xyz.z = R.z.x*d.x + R.z.y*d.y + R.z.z*d.z; + + /* d xyz / d pose, one PmCartesian per pose column x y z a b */ + for (col = 0; col < 3; col++) { + /* -R^T e_col, which is minus row col of R^T, i.e. minus column + col of R read as a row of R^T */ + PmCartesian e = {0, 0, 0}, w; + if (col == 0) e.x = 1; else if (col == 1) e.y = 1; else e.z = 1; + w.x = -(R.x.x*e.x + R.x.y*e.y + R.x.z*e.z); + w.y = -(R.y.x*e.x + R.y.y*e.y + R.y.z*e.z); + w.z = -(R.z.x*e.x + R.z.y*e.y + R.z.z*e.z); + dxyz[col] = w; + } + for (col = 3; col < 5; col++) { + PmCartesian cr, w; + pmCartCartCross(col == 3 ? &wa : &wb, &d, &cr); + w.x = -(R.x.x*cr.x + R.x.y*cr.y + R.x.z*cr.z) * (PM_PI/180.0); + w.y = -(R.y.x*cr.x + R.y.y*cr.y + R.y.z*cr.z) * (PM_PI/180.0); + w.z = -(R.z.x*cr.x + R.z.y*cr.y + R.z.z*cr.z) * (PM_PI/180.0); + dxyz[col] = w; + } + + rho = sqrt(sqr(xyz.x) + sqr(xyz.y)); + A = xyz.z - za[i]; + B = rho - ra[i]; + len = sqrt(sqr(A) + sqr(B)); + if (len <= 0 || rho <= 0) { return -1; } + for (col = 0; col < 5; col++) { + jac[i][col] = (A*dxyz[col].z + + B*(xyz.x*dxyz[col].x + xyz.y*dxyz[col].y)/rho) / len; + } + } + return 0; +} + KINEMATICS_TYPE kinematicsType() { return KINEMATICS_BOTH; @@ -408,6 +481,7 @@ KINS_NOT_SWITCHABLE EXPORT_SYMBOL(kinematicsType); EXPORT_SYMBOL(kinematicsForward); EXPORT_SYMBOL(kinematicsInverse); +EXPORT_SYMBOL(kinematicsJacobian); MODULE_LICENSE("GPL"); diff --git a/src/emc/kinematics/rotarydeltakins.c b/src/emc/kinematics/rotarydeltakins.c index 8c83ebdec4f..92e8a763a99 100644 --- a/src/emc/kinematics/rotarydeltakins.c +++ b/src/emc/kinematics/rotarydeltakins.c @@ -17,6 +17,7 @@ #include #include +#include #include #include @@ -51,6 +52,56 @@ int kinematicsInverse(const EmcPose *pos, double *joints, return kinematics_inverse(pos, joints); } +int kinematicsJacobian(const double *joints, + const EmcPose *pos, + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], + const KINEMATICS_INVERSE_FLAGS *iflags) { + int i, j; + (void)iflags; + set_geometry(hal_get_real(haldata->pfr), hal_get_real(haldata->tl), hal_get_real(haldata->sl), hal_get_real(haldata->fr)); + memset(jac, 0, EMCMOT_MAX_JOINTS * EMCMOT_MAX_AXIS * sizeof(jac[0][0])); + // The foot stays a shin length from each knee, so along a leg the + // motion of the foot and the motion of the knee agree: + // (P - K) . dP = (P - K) . dK/dq dq + // K is the knee less the foot offset, written as kinematics_forward() + // writes it, and q the hip angle that swings it. + for (i = 0; i < 3; i++) { + double q = D2R(joints[i]); + double reach = platformradius - footradius + thighlength * cos(q); + double kx, ky, kz, dkx, dky, dkz, px, py, pz, denom; + switch (i) { + case 0: + kx = 0; ky = -reach; + dkx = 0; dky = thighlength * sin(q); + break; + case 1: + kx = reach * 0.5 * sqrt(3); ky = reach * 0.5; + dkx = -thighlength * sin(q) * 0.5 * sqrt(3); + dky = -thighlength * sin(q) * 0.5; + break; + default: + kx = -reach * 0.5 * sqrt(3); ky = reach * 0.5; + dkx = thighlength * sin(q) * 0.5 * sqrt(3); + dky = -thighlength * sin(q) * 0.5; + break; + } + kz = -thighlength * sin(q); + dkz = -thighlength * cos(q); + px = pos->tran.x - kx; + py = pos->tran.y - ky; + pz = pos->tran.z - kz; + denom = (px*dkx + py*dky + pz*dkz) * (M_PI/180.); + // the shin at right angles to the thigh's swing: the knee cannot + // move the foot, so no finite hip rate follows the foot + if (fabs(denom) < 1e-12) { return -1; } + jac[i][0] = px/denom; + jac[i][1] = py/denom; + jac[i][2] = pz/denom; + } + for (j = 3; j < 9; j++) { jac[j][j] = 1; } + return 0; +} + KINEMATICS_TYPE kinematicsType() { return KINEMATICS_BOTH; @@ -92,4 +143,5 @@ KINS_NOT_SWITCHABLE EXPORT_SYMBOL(kinematicsType); EXPORT_SYMBOL(kinematicsForward); EXPORT_SYMBOL(kinematicsInverse); +EXPORT_SYMBOL(kinematicsJacobian); MODULE_LICENSE("GPL"); diff --git a/src/emc/kinematics/tripodkins.c b/src/emc/kinematics/tripodkins.c index 990b7997297..ab5c7081b19 100644 --- a/src/emc/kinematics/tripodkins.c +++ b/src/emc/kinematics/tripodkins.c @@ -65,6 +65,7 @@ #include /* RTAPI realtime OS API */ #include /* RTAPI realtime module decls */ #include +#include #include #include /* these decls */ @@ -218,6 +219,35 @@ int kinematicsInverse(const EmcPose * pos, #undef Dz } +int kinematicsJacobian(const double * joints, + const EmcPose * pos, + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], + const KINEMATICS_INVERSE_FLAGS * iflags) +{ + rtapi_real Bx = hal_get_real(haldata->bx); + rtapi_real Cx = hal_get_real(haldata->cx); + rtapi_real Cy = hal_get_real(haldata->cy); + /* the three strut base points, in the order of the joints */ + const double base[3][2] = { {0, 0}, {Bx, 0}, {Cx, Cy} }; + int i; + + (void)iflags; + memset(jac, 0, EMCMOT_MAX_JOINTS * EMCMOT_MAX_AXIS * sizeof(jac[0][0])); + /* a strut length changes by the component of the motion along the + strut, so each row is the unit vector from base to D */ + for (i = 0; i < 3; i++) { + double dx = pos->tran.x - base[i][0]; + double dy = pos->tran.y - base[i][1]; + double dz = pos->tran.z; + double len = joints[i]; + if (len <= 0) { return -1; } + jac[i][0] = dx/len; + jac[i][1] = dy/len; + jac[i][2] = dz/len; + } + return 0; +} + KINEMATICS_TYPE kinematicsType() { return KINEMATICS_BOTH; @@ -356,6 +386,7 @@ KINS_NOT_SWITCHABLE EXPORT_SYMBOL(kinematicsType); EXPORT_SYMBOL(kinematicsForward); EXPORT_SYMBOL(kinematicsInverse); +EXPORT_SYMBOL(kinematicsJacobian); MODULE_LICENSE("GPL"); From ecc042fcecf3eb09e8d8d100a459aca9aa445522 Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Thu, 3 Sep 2026 18:21:25 +1000 Subject: [PATCH 6/9] scarakins, scorbot-kins: supply the Jacobian Both inverses are chains of a few closed form steps, and the derivative follows the chain: for the scara the squared reach fixes the elbow and the bearing less the outer arm's angle fixes the shoulder; for the scorbot the distance to the wrist fixes the isosceles triangle the shoulder and elbow make. Each declines at the poses where its own inverse has no derivative, the arm straight or folded. --- src/emc/kinematics/scarakins.c | 52 +++++++++++++++++++++++ src/emc/kinematics/scorbot-kins.c | 70 +++++++++++++++++++++++++++++++ 2 files changed, 122 insertions(+) diff --git a/src/emc/kinematics/scarakins.c b/src/emc/kinematics/scarakins.c index 3b0e69ee7e4..db3ad15b737 100644 --- a/src/emc/kinematics/scarakins.c +++ b/src/emc/kinematics/scarakins.c @@ -179,6 +179,56 @@ static int scaraKinematicsInverse(const EmcPose * world, return (0); } // scaraKinematicsInverse() +static int scaraKinematicsJacobian(const double * joint, + const EmcPose * world, + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], + const KINEMATICS_INVERSE_FLAGS * iflags) +{ + (void)iflags; + rtapi_real D2 = hal_get_real(haldata->d2); + rtapi_real D4 = hal_get_real(haldata->d4); + rtapi_real D6 = hal_get_real(haldata->d6); + const double a3 = world->c * (PM_PI / 180); + const double q1 = joint[1] * (PM_PI / 180); + const double xt = world->tran.x - D6*cos(a3); + const double yt = world->tran.y - D6*sin(a3); + const double rsq = xt*xt + yt*yt; + /* gradients over (x, y, c) of the quantities the inverse builds */ + double d_xt[3] = { 1, 0, D6*sin(a3) * (PM_PI/180) }; + double d_yt[3] = { 0, 1, -D6*cos(a3) * (PM_PI/180) }; + double d_q1[3], d_q0[3], dphi_dq1; + int i; + + if (rsq <= 0 || fabs(sin(q1)) < 1e-12) { + /* the arm folded or straight out: the elbow rate is unbounded */ + return -1; + } + memset(jac, 0, EMCMOT_MAX_JOINTS * EMCMOT_MAX_AXIS * sizeof(jac[0][0])); + + /* rsq = D2^2 + D4^2 + 2 D2 D4 cos(q1), so q1 follows rsq; q0 is the + bearing of the end effector less the angle the outer arm subtends, + whose rate over q1 is (D2 D4 cos(q1) + D4^2) / rsq */ + dphi_dq1 = (D2*D4*cos(q1) + D4*D4) / rsq; + for (i = 0; i < 3; i++) { + double d_rsq = 2*xt*d_xt[i] + 2*yt*d_yt[i]; + d_q1[i] = -d_rsq / (2*D2*D4*sin(q1)); + d_q0[i] = (xt*d_yt[i] - yt*d_xt[i]) / rsq - dphi_dq1 * d_q1[i]; + } + + /* columns x, y, c; the rest of the pose does not reach these joints */ + for (i = 0; i < 3; i++) { + int col = (i == 2) ? 5 : i; + jac[0][col] = d_q0[i] * (180 / PM_PI); + jac[1][col] = d_q1[i] * (180 / PM_PI); + jac[3][col] = -(jac[0][col] + jac[1][col]); + } + jac[3][5] += 1; + jac[2][2] = -1; + jac[4][3] = 1; + jac[5][4] = 1; + return 0; +} // scaraKinematicsJacobian() + #define DEFAULT_D1 490 #define DEFAULT_D2 340 #define DEFAULT_D3 50 @@ -233,11 +283,13 @@ int switchkinsSetup(kparms* kp, *kinv1 = scaraKinematicsInverse; switchkinsDeclare(0, KINSTYPE_IDENTITY); switchkinsDeclare(1, KINSTYPE_PRIMARY); + switchkinsRegisterJacobian(1, scaraKinematicsJacobian); } else { rtapi_print("\n!!! switchkins-type 0 is %s\n",kp->kinsname); *kset0 = scaraKinematicsSetup; *kfwd0 = scaraKinematicsForward; *kinv0 = scaraKinematicsInverse; + switchkinsRegisterJacobian(0, scaraKinematicsJacobian); *kset1 = identityKinematicsSetup; *kfwd1 = identityKinematicsForward; diff --git a/src/emc/kinematics/scorbot-kins.c b/src/emc/kinematics/scorbot-kins.c index bd8868a063d..828f7b4ec43 100644 --- a/src/emc/kinematics/scorbot-kins.c +++ b/src/emc/kinematics/scorbot-kins.c @@ -41,6 +41,7 @@ #include #include #include +#include #include #include @@ -294,6 +295,74 @@ int kinematicsInverse( } +int kinematicsJacobian( + const double *joints, + const EmcPose *pose, + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], + const KINEMATICS_INVERSE_FLAGS *iflags +) { + // kinematicsInverse() above, differentiated step by step in the same + // order, each quantity carried as its gradient over (x, y, z) + const double x = pose->tran.x, y = pose->tran.y; + const double rho2 = x*x + y*y; + const double rho = sqrt(rho2); + double r_cp, z_cp, dist, angle_to_cp, j1_angle, j1, z_j2, u; + double d_r_cp[3], d_z_cp[3], d_dist[3], d_angle[3], d_j1a[3], d_j1[3], d_j2[3]; + double q; + int i; + + (void)joints; + (void)iflags; + if (rho2 <= 0) { return -1; } + memset(jac, 0, EMCMOT_MAX_JOINTS * EMCMOT_MAX_AXIS * sizeof(jac[0][0])); + + // j0 = atan2(y, x) + jac[0][0] = -y/rho2 * TO_DEG; + jac[0][1] = x/rho2 * TO_DEG; + + r_cp = rho - L0_HORIZONTAL_DISTANCE; + z_cp = pose->tran.z - L0_VERTICAL_DISTANCE; + d_r_cp[0] = x/rho; d_r_cp[1] = y/rho; d_r_cp[2] = 0; + d_z_cp[0] = 0; d_z_cp[1] = 0; d_z_cp[2] = 1; + + dist = sqrt(r_cp*r_cp + z_cp*z_cp); + if (dist <= 0 || dist >= 2*L1_LENGTH) { return -1; } + for (i = 0; i < 3; i++) { + d_dist[i] = (r_cp*d_r_cp[i] + z_cp*d_z_cp[i]) / dist; + } + + // the signed acos in the inverse is atan2(z_cp, r_cp) + angle_to_cp = TO_DEG * atan2(z_cp, r_cp); + for (i = 0; i < 3; i++) { + d_angle[i] = TO_DEG * (r_cp*d_z_cp[i] - z_cp*d_r_cp[i]) / (dist*dist); + } + + q = dist / (2*L1_LENGTH); + j1_angle = TO_DEG * acos(q); + for (i = 0; i < 3; i++) { + d_j1a[i] = -TO_DEG / sqrt(1 - q*q) * d_dist[i] / (2*L1_LENGTH); + } + + j1 = angle_to_cp + j1_angle; + for (i = 0; i < 3; i++) { + d_j1[i] = d_angle[i] + d_j1a[i]; + jac[1][i] = d_j1[i]; + } + + z_j2 = L1_LENGTH * sin(TO_RAD * j1); + u = (z_j2 - z_cp) / L2_LENGTH; + if (fabs(u) >= 1) { return -1; } + for (i = 0; i < 3; i++) { + double d_z_j2 = L1_LENGTH * cos(TO_RAD * j1) * TO_RAD * d_j1[i]; + d_j2[i] = -TO_DEG / sqrt(1 - u*u) * (d_z_j2 - d_z_cp[i]) / L2_LENGTH; + jac[2][i] = d_j2[i]; + } + + jac[3][3] = 1; + jac[4][4] = 1; + return 0; +} + KINEMATICS_TYPE kinematicsType(void) { return KINEMATICS_BOTH; } @@ -302,6 +371,7 @@ KINS_NOT_SWITCHABLE EXPORT_SYMBOL(kinematicsType); EXPORT_SYMBOL(kinematicsForward); EXPORT_SYMBOL(kinematicsInverse); +EXPORT_SYMBOL(kinematicsJacobian); MODULE_LICENSE("GPL"); static int comp_id; From e7d9fad2ed3605664ce9006527842dc44679a8de Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Thu, 3 Sep 2026 18:21:25 +1000 Subject: [PATCH 7/9] genserkins: supply the Jacobian from its geometric one compute_jinv() already gives radians of joint per unit of base frame twist. A pose word rate is not a twist: the roll, pitch and yaw rates reach the angular velocity through the matrix of the axes each one turns about, for the RPY convention go_rpy_mat_convert() uses. The Jacobian is that product, with the unit conversions and the unrotate coupling applied in the order the inverse applies them, and the u, v, w pass-through as ones. Having the conversion written once in the module is worth more than the closed form itself, since every consumer would otherwise guess it. --- src/emc/kinematics/genserfuncs.c | 106 +++++++++++++++++++++++++++++++ src/emc/kinematics/genserkins.c | 2 + src/emc/kinematics/genserkins.h | 5 ++ 3 files changed, 113 insertions(+) diff --git a/src/emc/kinematics/genserfuncs.c b/src/emc/kinematics/genserfuncs.c index 5600ab2be1f..02703735b2a 100644 --- a/src/emc/kinematics/genserfuncs.c +++ b/src/emc/kinematics/genserfuncs.c @@ -37,6 +37,7 @@ #include #endif #include +#include #include #include "libposemath/gotypes.h" /* go_result, go_integer */ #include "libposemath/gomath.h" /* go_pose */ @@ -313,6 +314,111 @@ int genser_kin_jac_fwd(void *kins, return GO_RESULT_OK; } +/* The Jacobian in the terms of kinematics.h: joints in degrees per pose + word in EmcPose units, the derivative of genserKinematicsInverse(). + + compute_jinv() gives the geometric inverse Jacobian, radians of joint per + unit of base-frame twist. A pose word rate is not a twist: the roll, + pitch and yaw rates reach the angular velocity through E, the matrix of + the axes each one turns about, for the RPY convention of go_rpy_mat_convert, + R = Rz(yaw) Ry(pitch) Rx(roll). So + + dq/dp = unrotate . deg . Jinv . blockdiag(I, E . rad) + + with the unit conversions and the unrotate coupling applied in the order + the inverse applies them. */ +int genserKinematicsJacobian(const double *joint, + const EmcPose *world, + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], + const KINEMATICS_INVERSE_FLAGS *iflags) +{ + (void)iflags; + genser_struct *genser = KINS_PTR; + GO_MATRIX_DECLARE(Jfwd, Jfwd_stg, 6, GENSER_MAX_JOINTS); + GO_MATRIX_DECLARE(Jinv, Jinv_stg, GENSER_MAX_JOINTS, 6); + go_pose T_L_0; + go_link linkout[GENSER_MAX_JOINTS] = {}; + go_real jest[GENSER_MAX_JOINTS]; + double E[3][3]; + double sb, cb, sc, cc; + int link, i, a, m, retval; + +#ifndef ULAPI + genser_kin_init(); + if (!genser_hal_inited) { + rtapi_print_msg(RTAPI_MSG_ERR, + "genserKinematicsJacobian: not initialized\n"); + return -1; + } +#endif + + memset(jac, 0, EMCMOT_MAX_JOINTS * EMCMOT_MAX_AXIS * sizeof(jac[0][0])); + + // the kinematic joint angles, in radians and with the unrotate + // coupling removed, exactly as the forward prepares them + for (link = 0; link < genser->link_num; link++) { + rtapi_s32 unrotate = hal_get_si32(haldata->unrotate[link]); + jest[link] = joint[link] * (PM_PI / 180); + if (link && unrotate) + jest[link] -= unrotate * jest[link-1]; + } + + go_matrix_init(Jfwd, Jfwd_stg, 6, genser->link_num); + go_matrix_init(Jinv, Jinv_stg, genser->link_num, 6); + + for (link = 0; link < genser->link_num; link++) { + retval = go_link_joint_set(&genser->links[link], jest[link], &linkout[link]); + if (GO_RESULT_OK != retval) + return -1; + } + retval = compute_jfwd(linkout, genser->link_num, &Jfwd, &T_L_0); + if (GO_RESULT_OK != retval) + return -1; + retval = compute_jinv(&Jfwd, &Jinv); + if (GO_RESULT_OK != retval) + return -1; // singular: no finite joint rate follows the pose + + // E columns: the roll axis carried by pitch and yaw, the pitch axis + // carried by yaw, and the yaw axis fixed + sb = sin(world->b * PM_PI / 180); cb = cos(world->b * PM_PI / 180); + sc = sin(world->c * PM_PI / 180); cc = cos(world->c * PM_PI / 180); + E[0][0] = cb*cc; E[1][0] = cb*sc; E[2][0] = -sb; + E[0][1] = -sc; E[1][1] = cc; E[2][1] = 0; + E[0][2] = 0; E[1][2] = 0; E[2][2] = 1; + + for (i = 0; i < genser->link_num; i++) { + // linear pose words: the twist column is the pose column, and the + // joint comes out in radians + for (a = 0; a < 3; a++) { + jac[i][a] = Jinv.el[i][a] * (180 / PM_PI); + } + // angular pose words: through E, radians of pose word per degree + // of pose word and degrees of joint per radian of joint cancel + for (m = 0; m < 3; m++) { + double s = 0; + for (a = 0; a < 3; a++) { s += Jinv.el[i][3+a] * E[a][m]; } + jac[i][3+m] = s; + } + } + + // the unrotate coupling, in link order as the inverse applies it + for (link = 1; link < genser->link_num; link++) { + rtapi_s32 unrotate = hal_get_si32(haldata->unrotate[link]); + if (unrotate) { + for (a = 0; a < EMCMOT_MAX_AXIS; a++) { + jac[link][a] += unrotate * jac[link-1][a]; + } + } + } + + // uvw pass through as joints 6, 7, 8 + if (total_joints > 6) jac[6][6] = 1; + if (total_joints > 7) jac[7][7] = 1; + if (total_joints > 8) jac[8][8] = 1; + + return 0; +} // genserKinematicsJacobian() + /* main function called by emc2 for forward Kins */ int genserKinematicsForward(const double *joint, EmcPose * world, diff --git a/src/emc/kinematics/genserkins.c b/src/emc/kinematics/genserkins.c index fa8d30598dc..94a325cbcf6 100644 --- a/src/emc/kinematics/genserkins.c +++ b/src/emc/kinematics/genserkins.c @@ -74,11 +74,13 @@ int switchkinsSetup(kparms* kp, *kinv1 = genserKinematicsInverse; switchkinsDeclare(0, KINSTYPE_IDENTITY); switchkinsDeclare(1, KINSTYPE_PRIMARY); + switchkinsRegisterJacobian(1, genserKinematicsJacobian); } else { rtapi_print("\n!!! switchkins-type 0 is %s\n",kp->kinsname); *kset0 = genserKinematicsSetup; *kfwd0 = genserKinematicsForward; *kinv0 = genserKinematicsInverse; + switchkinsRegisterJacobian(0, genserKinematicsJacobian); *kset1 = identityKinematicsSetup; *kfwd1 = identityKinematicsForward; diff --git a/src/emc/kinematics/genserkins.h b/src/emc/kinematics/genserkins.h index 3aa0756fc5a..b74b826d2ec 100644 --- a/src/emc/kinematics/genserkins.h +++ b/src/emc/kinematics/genserkins.h @@ -142,6 +142,11 @@ extern int compute_jfwd(go_link * link_params, extern int compute_jinv(go_matrix * Jfwd, go_matrix * Jinv); +extern int genserKinematicsJacobian(const double *joint, + const EmcPose *world, + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], + const KINEMATICS_INVERSE_FLAGS *iflags); + extern int genserKinematicsForward(const double *joint, EmcPose * world, const KINEMATICS_FORWARD_FLAGS * fflags, From 2ce8380dbbe1ddec1798faec03acc19c28a49f75 Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Mon, 21 Sep 2026 21:35:56 +1000 Subject: [PATCH 8/9] pumakins, three21kins: supply the Jacobian from the Denavit-Hartenberg chain The rule the chapter states is that every module supplies a closed-form Jacobian, and the two arms whose inverse is a chain of arc tangents were the exception, answering through eighteen differenced inverses. That costs 3 us instead of 0.3 on this machine, which is not the point; the point is what the differences do near the wrist singularity, where the inverse flips its branch flags a step away from the pose and the difference reads two branches at once, so a consumer capping a feed or a jog sees a magnitude that means nothing. kinsJacobianFromDhArm() in kins_util.c takes the arm's table in Craig's convention, alpha, a and d per link and the tool point along the last z, composes the chain at the given joints, and builds the geometric Jacobian: each joint's axis crossed with the vector from it to the tool point for the point's rate, the axis itself for the angular rate. The six by six is inverted by Gauss-Jordan with row pivoting, -1 where a pivot is too small to trust, and the angular columns go through the matrix of the axes the roll, pitch and yaw rates turn about, as genserkins already does with its own chain. pumakins is Craig's PUMA 560 table with D6 the tool point; three21kins the same with A1 and D1 setting the shoulder off the base and D2 and D3 both along the upper arm's axis. Both register it for their arm type. tests/kins-jacobian already checks both arms against the differenced forward and the differenced inverse to 1e-6; a table entry one degree off fails the forward check, so the closed form is what the test sees. --- docs/src/motion/kinematics-conventions.adoc | 11 +- src/emc/kinematics/kinematics.h | 23 ++++ src/emc/kinematics/kins_util.c | 118 ++++++++++++++++++++ src/emc/kinematics/pumakins.c | 21 ++++ src/emc/kinematics/three21kins.c | 21 ++++ 5 files changed, 191 insertions(+), 3 deletions(-) diff --git a/docs/src/motion/kinematics-conventions.adoc b/docs/src/motion/kinematics-conventions.adoc index 4596796ca03..5740fbc7012 100644 --- a/docs/src/motion/kinematics-conventions.adoc +++ b/docs/src/motion/kinematics-conventions.adoc @@ -462,9 +462,14 @@ servo thread. A module with a closed form registers it with `switchkinsRegisterJacobian()`. It is exact, it costs what the inverse costs, and it knows its own singular poses rather than discovering them as an inverse that fails a step away from -the pose. Every module in the tree whose inverse is written out supplies one. -The two arms whose inverse is a chain of arc tangents, `pumakins` and -`three21kins`, answer through the differences. +the pose. Every module in the tree supplies one. The two arms whose inverse is +a chain of arc tangents, `pumakins` and `three21kins`, take theirs from +their Denavit-Hartenberg chain through `kinsJacobianFromDhArm()`, which any +six-joint serial arm can call with its own table, written in the modified +convention of Craig's Introduction to Robotics: each joint's axis crossed +with the vector from it to the tool point gives the point's rate, the axis +itself the angular rate, and the six by six that makes is inverted and +taken through the roll, pitch and yaw rates the pose words are. A module reading its rotary angles from the joint argument of the inverse rather than from the pose, which the nutating heads do, has an inverse whose diff --git a/src/emc/kinematics/kinematics.h b/src/emc/kinematics/kinematics.h index 900fe5aa474..c4ef1f45ba0 100644 --- a/src/emc/kinematics/kinematics.h +++ b/src/emc/kinematics/kinematics.h @@ -439,6 +439,29 @@ extern int kinsJacobianFromMappedAxes(int max_joints, const double dP[EMCMOT_MAX_AXIS][EMCMOT_MAX_AXIS], double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS]); +/* The Jacobian of a serial arm of six revolute joints from its + Denavit-Hartenberg chain. Two conventions carry that name and put the + four parameters on different links; this is the modified one of John J. + Craig, Introduction to Robotics: Mechanics and Control, where link i is + Rx(alpha[i]) Tx(a[i]) Rz(joint[i]) Tz(d[i]), the joint turning about the + z of the frame Rx and Tx leave it in. (The original 1955 convention is + Rz(theta) Tz(d) Tx(a) Rx(alpha), and a table written for it does not fit + here.) The tool point `tool` lies along the z of the last frame, and + the pose of that point is reported as X Y Z and the RPY of the last + frame, R = Rz(C) Ry(B) Rx(A), as pmMatRpyConvert() does. + Each joint's axis crossed with the vector from it to the tool point + gives the point's rate per radian of the joint, the axis itself the + angular rate; that 6x6 inverted is the joint rate per unit of twist, and + the RPY rates reach the twist through the matrix of the axes each one + turns about, which is where world's B and C come in. alpha and joint in + degrees, a, d and tool in the module's length unit. Rows 0 to 5 of jac + are filled, the rest zero. Returns 0, or -1 at a singular pose, where no + finite joint rate follows the pose. */ +extern int kinsJacobianFromDhArm(const double alpha[6], const double a[6], + const double d[6], const double *joint, + double tool, const EmcPose *world, + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS]); + /* joints are axes: a 1 per joint in the column of its letter */ extern int identityKinematicsJacobian(const double *joint, const EmcPose *world, diff --git a/src/emc/kinematics/kins_util.c b/src/emc/kinematics/kins_util.c index 324392e6864..27773c9edb1 100644 --- a/src/emc/kinematics/kins_util.c +++ b/src/emc/kinematics/kins_util.c @@ -1138,6 +1138,124 @@ int kinsJacobianFromMappedAxes(int max_joints, return 0; } // kinsJacobianFromMappedAxes() +/* r = r * Rx(angle), r = r * Rz(angle): a rotation composed on the right */ +static void kj_rot_x(double r[3][3], double angle) +{ + double c = cos(angle), s = sin(angle); + int i; + for (i = 0; i < 3; i++) { + double y = r[i][1], z = r[i][2]; + r[i][1] = y * c + z * s; + r[i][2] = -y * s + z * c; + } +} + +static void kj_rot_z(double r[3][3], double angle) +{ + double c = cos(angle), s = sin(angle); + int i; + for (i = 0; i < 3; i++) { + double x = r[i][0], y = r[i][1]; + r[i][0] = x * c + y * s; + r[i][1] = -x * s + y * c; + } +} + +/* m = inverse of the 6x6 m, by Gauss-Jordan with row pivoting; -1 where a + pivot is too small to trust, the arm singular */ +static int kj_invert6(double m[6][6]) +{ + double inv[6][6]; + int i, j, k, piv; + + for (i = 0; i < 6; i++) { + for (j = 0; j < 6; j++) { inv[i][j] = (i == j) ? 1.0 : 0.0; } + } + for (k = 0; k < 6; k++) { + double big = fabs(m[k][k]); + piv = k; + for (i = k + 1; i < 6; i++) { + if (fabs(m[i][k]) > big) { big = fabs(m[i][k]); piv = i; } + } + if (big < 1e-9) { return -1; } + if (piv != k) { + for (j = 0; j < 6; j++) { + double t = m[k][j]; m[k][j] = m[piv][j]; m[piv][j] = t; + t = inv[k][j]; inv[k][j] = inv[piv][j]; inv[piv][j] = t; + } + } + { + double f = 1.0 / m[k][k]; + for (j = 0; j < 6; j++) { m[k][j] *= f; inv[k][j] *= f; } + } + for (i = 0; i < 6; i++) { + double f = m[i][k]; + if (i == k || f == 0.0) { continue; } + for (j = 0; j < 6; j++) { m[i][j] -= f * m[k][j]; inv[i][j] -= f * inv[k][j]; } + } + } + memcpy(m, inv, sizeof(inv)); + return 0; +} + +int kinsJacobianFromDhArm(const double alpha[6], const double a[6], + const double d[6], const double *joint, + double tool, const EmcPose *world, + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS]) +{ + double r[3][3] = { {1, 0, 0}, {0, 1, 0}, {0, 0, 1} }; + double o[3] = { 0, 0, 0 }; + double z[6][3], p[6][3], e[3]; + double jfwd[6][6], E[3][3]; + double sb, cb, sc, cc; + int i, j, m; + + if (!alpha || !a || !d || !joint || !world || !jac) { return -1; } + kj_zero(jac); + + /* the chain: after Rx(alpha) Tx(a) the frame's z is joint i's axis and + its origin a point on it; Rz(joint) Tz(d) then carry on to the next */ + for (i = 0; i < 6; i++) { + kj_rot_x(r, alpha[i] * PM_PI / 180); + for (j = 0; j < 3; j++) { o[j] += r[j][0] * a[i]; } + for (j = 0; j < 3; j++) { z[i][j] = r[j][2]; p[i][j] = o[j]; } + kj_rot_z(r, joint[i] * PM_PI / 180); + for (j = 0; j < 3; j++) { o[j] += r[j][2] * d[i]; } + } + /* the tool point, along the last z */ + for (j = 0; j < 3; j++) { e[j] = o[j] + r[j][2] * tool; } + + /* the point's rate and the angular rate per radian of each joint */ + for (i = 0; i < 6; i++) { + double v[3] = { e[0] - p[i][0], e[1] - p[i][1], e[2] - p[i][2] }; + jfwd[0][i] = z[i][1] * v[2] - z[i][2] * v[1]; + jfwd[1][i] = z[i][2] * v[0] - z[i][0] * v[2]; + jfwd[2][i] = z[i][0] * v[1] - z[i][1] * v[0]; + for (j = 0; j < 3; j++) { jfwd[3 + j][i] = z[i][j]; } + } + if (kj_invert6(jfwd) != 0) { return -1; } + + /* E: the roll axis carried by pitch and yaw, the pitch axis carried + by yaw, the yaw axis fixed */ + sb = sin(world->b * PM_PI / 180); cb = cos(world->b * PM_PI / 180); + sc = sin(world->c * PM_PI / 180); cc = cos(world->c * PM_PI / 180); + E[0][0] = cb * cc; E[1][0] = cb * sc; E[2][0] = -sb; + E[0][1] = -sc; E[1][1] = cc; E[2][1] = 0; + E[0][2] = 0; E[1][2] = 0; E[2][2] = 1; + + for (i = 0; i < 6; i++) { + /* linear pose words: the joint comes out in radians per unit */ + for (j = 0; j < 3; j++) { jac[i][j] = jfwd[i][j] * (180 / PM_PI); } + /* angular pose words through E: degrees per degree */ + for (m = 0; m < 3; m++) { + double s = 0; + for (j = 0; j < 3; j++) { s += jfwd[i][3 + j] * E[j][m]; } + jac[i][3 + m] = s; + } + } + return 0; +} // kinsJacobianFromDhArm() + int identityKinematicsJacobian(const double *joint, const EmcPose *world, double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], diff --git a/src/emc/kinematics/pumakins.c b/src/emc/kinematics/pumakins.c index 049d384b424..aace42d7b37 100644 --- a/src/emc/kinematics/pumakins.c +++ b/src/emc/kinematics/pumakins.c @@ -211,6 +211,25 @@ static int pumaKinematicsForward(const double * joint, return 0; } +/* The Jacobian from the arm's Denavit-Hartenberg chain, the PUMA 560 + table of Craig's Introduction to Robotics in his modified convention, + which is the one the forward above encodes: the shoulder turns about + the base z, the upper arm and forearm + about axes at right angles to it, the wrist about three axes meeting at + its centre, and D6 carries the tool point out along the flange z. */ +static int pumaKinematicsJacobian(const double * joint, + const EmcPose * world, + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], + const KINEMATICS_INVERSE_FLAGS * iflags) +{ + (void)iflags; + const double alpha[6] = { 0, -90, 0, -90, 90, -90 }; + const double a[6] = { 0, 0, hal_get_real(haldata->a2), hal_get_real(haldata->a3), 0, 0 }; + const double d[6] = { 0, 0, hal_get_real(haldata->d3), hal_get_real(haldata->d4), 0, 0 }; + + return kinsJacobianFromDhArm(alpha, a, d, joint, hal_get_real(haldata->d6), world, jac); +} // pumaKinematicsJacobian() + static int pumaKinematicsToolFrame(const double * joint, PmRotationMatrix * rot, const KINEMATICS_FORWARD_FLAGS * fflags) @@ -439,6 +458,7 @@ int switchkinsSetup(kparms* kp, switchkinsRegisterFrames(1, pumaKinematicsWorkFrame, pumaKinematicsToolFrame, &TOOL_FRAME_FLANGE); + switchkinsRegisterJacobian(1, pumaKinematicsJacobian); switchkinsDeclare(0, KINSTYPE_IDENTITY); switchkinsDeclare(1, KINSTYPE_PRIMARY); } else { @@ -451,6 +471,7 @@ int switchkinsSetup(kparms* kp, switchkinsRegisterFrames(0, pumaKinematicsWorkFrame, pumaKinematicsToolFrame, &TOOL_FRAME_FLANGE); + switchkinsRegisterJacobian(0, pumaKinematicsJacobian); *kset1 = identityKinematicsSetup; *kfwd1 = identityKinematicsForward; diff --git a/src/emc/kinematics/three21kins.c b/src/emc/kinematics/three21kins.c index 2a3dfe08ee5..30c7f938e15 100644 --- a/src/emc/kinematics/three21kins.c +++ b/src/emc/kinematics/three21kins.c @@ -190,6 +190,25 @@ static int three21KinematicsForward(const double * joint, return 0; } +/* The Jacobian from the arm's Denavit-Hartenberg chain: the PUMA table + with the shoulder set A1 out along the first link and D1 up the base, + D2 and D3 both along the axis the upper arm turns about, and D6 carrying + the tool point out along the flange z. */ +static int three21KinematicsJacobian(const double * joint, + const EmcPose * world, + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS], + const KINEMATICS_INVERSE_FLAGS * iflags) +{ + (void)iflags; + const double alpha[6] = { 0, -90, 0, -90, 90, -90 }; + const double a[6] = { 0, hal_get_real(haldata->a1), hal_get_real(haldata->a2), + hal_get_real(haldata->a3), 0, 0 }; + const double d[6] = { hal_get_real(haldata->d1), hal_get_real(haldata->d2), + hal_get_real(haldata->d3), hal_get_real(haldata->d4), 0, 0 }; + + return kinsJacobianFromDhArm(alpha, a, d, joint, hal_get_real(haldata->d6), world, jac); +} // three21KinematicsJacobian() + static int three21KinematicsInverse(const EmcPose * world, double * joint, const KINEMATICS_INVERSE_FLAGS * iflags, @@ -396,6 +415,7 @@ int switchkinsSetup(kparms* kp, *kset1 = three21KinematicsSetup; *kfwd1 = three21KinematicsForward; *kinv1 = three21KinematicsInverse; + switchkinsRegisterJacobian(1, three21KinematicsJacobian); switchkinsDeclare(0, KINSTYPE_IDENTITY); switchkinsDeclare(1, KINSTYPE_PRIMARY); } else { @@ -403,6 +423,7 @@ int switchkinsSetup(kparms* kp, *kset0 = three21KinematicsSetup; *kfwd0 = three21KinematicsForward; *kinv0 = three21KinematicsInverse; + switchkinsRegisterJacobian(0, three21KinematicsJacobian); *kset1 = identityKinematicsSetup; *kfwd1 = identityKinematicsForward; From 30032d238f7673a090e2f5ee0dd8e9a4da6ad874 Mon Sep 17 00:00:00 2001 From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com> Date: Thu, 3 Sep 2026 18:21:25 +1000 Subject: [PATCH 9/9] tests: check the Jacobian of every module where it runs A realtime component loaded after the module under test, reaching it through the exported entry points; a failed check fails the load. Every module in the tree, every switchkins type, both direction settings on the tables. Two checks, neither reusing the module's own answer. Against the forward: perturb one joint, difference the forward, multiply by the Jacobian and expect that joint's unit vector, which catches a transposed matrix, a wrong sign, column or unit whichever way the module answered. Against the inverse: difference it here with a different step and compare entry by entry, the check for the gantry, whose forward is not one to one. Verified by mutation, one per module or shared routine, every one caught. The forward check is also a round trip of each module and found four whose forward and inverse disagreed, fixed in the commits before this one. maxkins, which disagrees away from c = 0 and u = 0, is checked against its inverse; the nutating heads read their angles from the inverse's joint argument, so they are checked against the forward. --- tests/kins-jacobian/checkresult | 4 + tests/kins-jacobian/jaccheck.c | 360 ++++++++++++++++++++++++++++++++ tests/kins-jacobian/skip | 4 + tests/kins-jacobian/test.sh | 173 +++++++++++++++ 4 files changed, 541 insertions(+) create mode 100755 tests/kins-jacobian/checkresult create mode 100644 tests/kins-jacobian/jaccheck.c create mode 100755 tests/kins-jacobian/skip create mode 100755 tests/kins-jacobian/test.sh diff --git a/tests/kins-jacobian/checkresult b/tests/kins-jacobian/checkresult new file mode 100755 index 00000000000..b49a90b17c6 --- /dev/null +++ b/tests/kins-jacobian/checkresult @@ -0,0 +1,4 @@ +#!/bin/sh +[ "$(grep -c 'jacobian agrees' "$1")" = "$(grep -c '^=== ' "$1")" ] \ + && [ "$(grep -c '^=== ' "$1")" -ge 20 ] \ + && ! grep -q "FAIL" "$1" diff --git a/tests/kins-jacobian/jaccheck.c b/tests/kins-jacobian/jaccheck.c new file mode 100644 index 00000000000..0e5501f6943 --- /dev/null +++ b/tests/kins-jacobian/jaccheck.c @@ -0,0 +1,360 @@ +/* Check a kinematics module's Jacobian where it runs in service. + * + * Loaded after the module under test, so kinematicsForward(), + * kinematicsInverse() and kinematicsJacobian() resolve to it. A + * failed check fails the load, and a failed load fails the test. + * + * Two checks, neither of which reuses the module's own answer. + * + * Against the forward: perturb one joint, difference the forward to + * get how the pose responds, and multiply by the reported Jacobian. + * The result has to be that joint's unit vector, since the Jacobian + * is the derivative of the inverse and the two are inverse maps. The + * forward is a separate piece of code from the inverse, so this + * catches a transposed matrix, a wrong sign, a wrong column and a + * wrong unit, whether the module answered in closed form or by + * differencing. + * + * Against the inverse: difference the inverse here, with a different + * step, and compare entry by entry. This is the check for a machine + * whose forward is not one to one, the gantry with two joints on one + * letter, where the product above is not the identity. + * + * 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("kinematics Jacobian checker"); + +static int joints = 3; +RTAPI_MP_INT(joints, "joint count the module under test was loaded for"); + +static int types = -1; +RTAPI_MP_INT(types, "how many switchkins types to check, from 0; -1 for all the module has"); + +static int r1 = -1, r2 = -1, r3 = -1; +RTAPI_MP_INT(r1, "joint number of the first joint to sweep"); +RTAPI_MP_INT(r2, "joint number of the second joint to sweep, -1 for none"); +RTAPI_MP_INT(r3, "joint number of the third joint to sweep, -1 for none"); + +#define MAX_ANGLES 8 +#define NO_ANGLE 9999 +static int angles[MAX_ANGLES] = { NO_ANGLE, NO_ANGLE, NO_ANGLE, NO_ANGLE, + NO_ANGLE, NO_ANGLE, NO_ANGLE, NO_ANGLE }; +RTAPI_MP_ARRAY_INT(angles, MAX_ANGLES, "values each swept joint takes; default 0,30,-25,90,180"); + +static int base[EMCMOT_MAX_JOINTS] = { 10, 20, 30 }; +RTAPI_MP_ARRAY_INT(base, EMCMOT_MAX_JOINTS, "joint values before the sweep, from joint 0"); + +static int frompose = 0; +RTAPI_MP_INT(frompose, "1 to read base and the sweep as pose coordinates and take the joints from the inverse"); + +static char *check = "both"; +RTAPI_MP_STRING(check, "fwd, inv or both: which checks to run"); + +static int tolexp = 6; +RTAPI_MP_INT(tolexp, "tolerance for the checks is 10 to the minus this"); + +/* switchkins.h is not an exported header, and a module rejects a type + it does not have, so the loop only needs an upper bound */ +#define MAX_TYPES 9 + +#define FWD_STEP 1e-5 /* joint units, for differencing the forward */ +#define INV_STEP 2e-3 /* pose units, for differencing the inverse; not + the step kins_util.c uses, on purpose */ + +static int comp_id = -1; +static int failures; +static int poses; +static double tolerance = 1e-6; +static int do_fwd = 1, do_inv = 1; + +static void expect(int ok, const char *what, const double *j, int m, int n) +{ + char pose[160]; + int i, k = 0; + + if (ok) { return; } + for (i = 0; i < joints && k < (int)sizeof(pose) - 12; i++) { + k += rtapi_snprintf(pose + k, sizeof(pose) - k, "%s%.4g", + i ? "," : "", j[i]); + } + rtapi_print_msg(RTAPI_MSG_ERR, "jaccheck: FAIL %s [%d][%d] at [%s]\n", + what, m, n, pose); + failures++; +} + +static double pose_coord(const EmcPose *p, int a) +{ + switch (a) { + case 0: return p->tran.x; + case 1: return p->tran.y; + case 2: return p->tran.z; + case 3: return p->a; + case 4: return p->b; + case 5: return p->c; + case 6: return p->u; + case 7: return p->v; + default: return p->w; + } +} + +static void pose_add(EmcPose *p, int a, double d) +{ + switch (a) { + case 0: p->tran.x += d; break; + case 1: p->tran.y += d; break; + case 2: p->tran.z += d; break; + case 3: p->a += d; break; + case 4: p->b += d; break; + case 5: p->c += d; break; + case 6: p->u += d; break; + case 7: p->v += d; break; + default: p->w += d; break; + } +} + +/* how the pose responds to joint m: column m of the forward's derivative. + A forward that iterates starts from the pose it is handed, so both + calls start from the pose the joints are known to reach. */ +static int fwd_column(const double *j, int m, KINEMATICS_FORWARD_FLAGS ff, + const EmcPose *near, double *col) +{ + double t[EMCMOT_MAX_JOINTS]; + EmcPose lo = *near, hi = *near; + KINEMATICS_INVERSE_FLAGS inf = 0; + int a; + + memcpy(t, j, sizeof(t)); + + t[m] = j[m] - FWD_STEP; + if (kinematicsForward(t, &lo, &ff, &inf)) { return -1; } + t[m] = j[m] + FWD_STEP; + if (kinematicsForward(t, &hi, &ff, &inf)) { return -1; } + + for (a = 0; a < EMCMOT_MAX_AXIS; a++) { + col[a] = (pose_coord(&hi, a) - pose_coord(&lo, a)) / (2 * FWD_STEP); + } + return 0; +} + +/* near is where the pose is expected to be, for a forward that iterates + from the pose it is handed; zero where nothing better is known */ +static void check_pose(const double *j, const EmcPose *near) +{ + double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS]; + double col[EMCMOT_MAX_AXIS]; + double qp[EMCMOT_MAX_JOINTS], qm[EMCMOT_MAX_JOINTS]; + EmcPose world = *near, p; + KINEMATICS_FORWARD_FLAGS ff = 0; + KINEMATICS_INVERSE_FLAGS inf = 0; + int m, n, a; + + m = kinematicsForward(j, &world, &ff, &inf); + if (m) { + rtapi_print_msg(RTAPI_MSG_ERR, + "jaccheck: forward started from [%.4g,%.4g,%.4g,%.4g,%.4g,%.4g]" + " and left [%.4g,%.4g,%.4g,%.4g,%.4g,%.4g]\n", + near->tran.x, near->tran.y, near->tran.z, near->a, near->b, near->c, + world.tran.x, world.tran.y, world.tran.z, world.a, world.b, world.c); + expect(0, "forward kinematics", j, m, -1); + return; + } + poses++; + + if (kinematicsJacobian(j, &world, jac, &inf)) { + /* say what the inverse makes of the same pose, since a module + that differences its inverse declines when that does not come + back to the joints it was given */ + memcpy(qp, j, sizeof(qp)); + if (kinematicsInverse(&world, qp, &inf, &ff)) { + rtapi_print_msg(RTAPI_MSG_ERR, "jaccheck: inverse fails at the pose\n"); + } else { + rtapi_print_msg(RTAPI_MSG_ERR, + "jaccheck: inverse gives [%.4g,%.4g,%.4g,%.4g,%.4g,%.4g] flags %lu\n", + qp[0], qp[1], qp[2], qp[3], qp[4], qp[5], inf); + } + expect(0, "jacobian declined", j, -1, -1); + return; + } + + /* rows the module has no joint for stay zero */ + for (m = joints; m < EMCMOT_MAX_JOINTS; m++) { + for (a = 0; a < EMCMOT_MAX_AXIS; a++) { + expect(jac[m][a] == 0, "row past the joint count", j, m, a); + } + } + + if (do_fwd) { + for (m = 0; m < joints; m++) { + if (fwd_column(j, m, ff, &world, col)) { + expect(0, "forward kinematics near the pose", j, m, -1); + return; + } + for (n = 0; n < joints; n++) { + double s = 0; + for (a = 0; a < EMCMOT_MAX_AXIS; a++) { s += jac[n][a] * col[a]; } + expect(fabs(s - (m == n ? 1.0 : 0.0)) < tolerance, + "jacobian times forward column", j, n, m); + } + } + } + + if (do_inv) { + for (a = 0; a < EMCMOT_MAX_AXIS; a++) { + p = world; + memcpy(qp, j, sizeof(qp)); + memcpy(qm, j, sizeof(qm)); + pose_add(&p, a, INV_STEP); + if (kinematicsInverse(&p, qp, &inf, &ff)) { + expect(0, "inverse kinematics near the pose", j, -1, a); + return; + } + pose_add(&p, a, -2 * INV_STEP); + if (kinematicsInverse(&p, qm, &inf, &ff)) { + expect(0, "inverse kinematics near the pose", j, -1, a); + return; + } + for (n = 0; n < joints; n++) { + double d = (qp[n] - qm[n]) / (2 * INV_STEP); + expect(fabs(d - jac[n][a]) < tolerance * (1 + fabs(d)), + "jacobian against the inverse", j, n, a); + } + } + } +} + +int rtapi_app_main(void) +{ + double j[EMCMOT_MAX_JOINTS]; + int angles_n; + int a, b, c, t, i; + int checked = 0; + + if (joints < 1 || joints > EMCMOT_MAX_JOINTS) { + rtapi_print_msg(RTAPI_MSG_ERR, "jaccheck: joints=%d\n", joints); + return -1; + } + /* the list given ends at the first untouched entry; none given means + the quarter and half turns where a sine changes sign or a cosine + vanishes, and the values in between */ + if (angles[0] == NO_ANGLE) { + static const int usual[] = { 0, 30, -25, 90, 180 }; + for (i = 0; i < (int)(sizeof(usual)/sizeof(usual[0])); i++) { angles[i] = usual[i]; } + } + for (angles_n = 0; angles_n < MAX_ANGLES; angles_n++) { + if (angles[angles_n] == NO_ANGLE) { break; } + } + for (tolerance = 1, i = 0; i < tolexp; i++) { tolerance *= 0.1; } + do_fwd = !strcmp(check, "fwd") || !strcmp(check, "both"); + do_inv = !strcmp(check, "inv") || !strcmp(check, "both"); + if (!do_fwd && !do_inv) { + rtapi_print_msg(RTAPI_MSG_ERR, "jaccheck: check=%s\n", check); + return -1; + } + + comp_id = hal_init("jaccheck"); + if (comp_id < 0) { return comp_id; } + + if (kinematicsType() == 0) { + rtapi_print_msg(RTAPI_MSG_ERR, "jaccheck: the module reports no type\n"); + hal_exit(comp_id); + return -1; + } + + for (i = 0; i < EMCMOT_MAX_JOINTS; i++) { j[i] = base[i]; } + + /* A switchable module's first forward after load restarts an + iterating forward from a stored pose that is still zero, which + for a hexapod is the singular pose it cannot leave; motion's first + cycle takes that failure and carries on. Take it here. */ + if (kinematicsSwitchable()) { + double q[EMCMOT_MAX_JOINTS]; + EmcPose seed; + KINEMATICS_FORWARD_FLAGS ff = 0; + KINEMATICS_INVERSE_FLAGS inf = 0; + ZERO_EMC_POSE(seed); + memcpy(q, j, sizeof(q)); + if (r1 >= 0) { q[r1] = angles[0]; } + if (r2 >= 0) { q[r2] = angles[0]; } + if (r3 >= 0) { q[r3] = angles[0]; } + if (frompose) { + for (i = 0; i < EMCMOT_MAX_AXIS; i++) { pose_add(&seed, i, q[i]); } + memset(q, 0, sizeof(q)); + kinematicsInverse(&seed, q, &inf, &ff); + } + kinematicsForward(q, &seed, &ff, &inf); + } + + /* every kinematics the module offers, since the answer is per type. + The module starts in type 0, and is not switched to it: a switch + restarts an iterating forward from a stored pose that is still + zero, which for a hexapod is the singular pose it cannot leave */ + for (t = 0; t < MAX_TYPES && (types < 0 || t < types); t++) { + if (kinematicsSwitchable() && t > 0 && kinematicsSwitch(t)) { break; } + checked++; + + for (a = 0; a < angles_n; a++) { + if (r1 >= 0) { j[r1] = angles[a]; } + for (b = 0; b < angles_n; b++) { + if (r2 >= 0) { j[r2] = angles[b]; } + for (c = 0; c < angles_n; c++) { + if (r3 >= 0) { j[r3] = angles[c]; } + if (frompose) { + /* base and sweep name a pose; the machine that + reaches it comes from the module's inverse */ + double q[EMCMOT_MAX_JOINTS]; + EmcPose want; + KINEMATICS_INVERSE_FLAGS inf = 0; + KINEMATICS_FORWARD_FLAGS ff = 0; + ZERO_EMC_POSE(want); + for (i = 0; i < EMCMOT_MAX_AXIS; i++) { pose_add(&want, i, j[i]); } + memset(q, 0, sizeof(q)); + if (kinematicsInverse(&want, q, &inf, &ff)) { + expect(0, "inverse kinematics at the base pose", j, -1, -1); + } else { + check_pose(q, &want); + } + } else { + EmcPose zero; + ZERO_EMC_POSE(zero); + check_pose(j, &zero); + } + if (r3 < 0) { break; } + } + if (r2 < 0) { break; } + } + if (r1 < 0) { break; } + } + + if (!kinematicsSwitchable()) { break; } + } + + if (failures) { + rtapi_print_msg(RTAPI_MSG_ERR, + "jaccheck: %d check(s) failed over %d pose(s)\n", + failures, poses); + hal_exit(comp_id); + return -1; + } + + rtapi_print("jaccheck: jacobian agrees for %d kinematics type(s), %d pose(s)\n", + checked, poses); + hal_ready(comp_id); + return 0; +} + +void rtapi_app_exit(void) { hal_exit(comp_id); } diff --git a/tests/kins-jacobian/skip b/tests/kins-jacobian/skip new file mode 100755 index 00000000000..a12f31a77c2 --- /dev/null +++ b/tests/kins-jacobian/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/kins-jacobian/test.sh b/tests/kins-jacobian/test.sh new file mode 100755 index 00000000000..57a3a2e45e3 --- /dev/null +++ b/tests/kins-jacobian/test.sh @@ -0,0 +1,173 @@ +#!/bin/bash +set -e + +${SUDO} halcompile --install jaccheck.c >/dev/null + +# One hal file per module: they all define the same entry points, so +# only one can be loaded at a time. A run that leaves the sweep at its +# default takes each rotary through the quarter and half turns where a +# sine changes sign or a cosine vanishes; the arms and the parallel +# machines name their own, away from the poses they cannot hold. +# ONLY= in the environment runs the entries for that module alone +run() { + local hal + case "$1" in "${ONLY:-}"*) ;; *) return 0 ;; esac + hal=$(mktemp --suffix=.hal) + { printf 'loadrt %s\n' "$1" + printf '%s\n' "$2" + printf 'loadrt jaccheck %s\n' "$3" + } > "$hal" + echo "=== $1" + halrun -f "$hal" + rm -f "$hal" +} + +# identity, including a gantry: two joints on one letter is the case where +# the forward is not one to one, so it is checked against the inverse +run "trivkins coordinates=XYZ" "" "joints=3" +run "trivkins coordinates=XYZY kinstype=BOTH" "" "joints=4 check=inv" +run "trivkins coordinates=XYZABCUVW" "" "joints=9 r1=3 r2=5" +run "userkins" "" "joints=3" +run "millturn" "" "joints=4" + +# linear maps and one rotation +run "corexykins" "" "joints=9" +run "rotatekins" "" "joints=9 r1=5" +run "matrixkins" \ + "setp matrixkins.C_xy 0.02 +setp matrixkins.C_xz -0.01 +setp matrixkins.C_yx 0.03 +setp matrixkins.C_yz 0.015 +setp matrixkins.C_zx -0.02 +setp matrixkins.C_zy 0.01 +setp matrixkins.C_zz 1.001" \ + "joints=9" + +# tables and heads; offsets set so no term drops out +run "maxkins" \ + "setp maxkins.pivot-length 100" \ + "joints=9 r1=4 r2=5 base=10,20,30,0,0,0,7,0,3" + +run "5axiskins coordinates=XYZBCW" "" "joints=6 r1=3 r2=4 base=10,20,30,0,0,5" +run "5axiskins coordinates=XYZBCW sparm=identityfirst" "" "joints=6 r1=3 r2=4 base=10,20,30,0,0,5" + +run "xyzac-trt-kins coordinates=XYZAC" \ + "setp xyzac-trt-kins.y-offset 3 +setp xyzac-trt-kins.z-offset 11 +setp xyzac-trt-kins.tool-offset 7 +setp xyzac-trt-kins.x-rot-point 1 +setp xyzac-trt-kins.y-rot-point 2 +setp xyzac-trt-kins.z-rot-point 5" \ + "joints=5 r1=3 r2=4" + +run "xyzbc-trt-kins coordinates=XYZBC" \ + "setp xyzbc-trt-kins.x-offset 3 +setp xyzbc-trt-kins.z-offset 11 +setp xyzbc-trt-kins.tool-offset 7 +setp xyzbc-trt-kins.x-rot-point 1 +setp xyzbc-trt-kins.y-rot-point 2 +setp xyzbc-trt-kins.z-rot-point 5" \ + "joints=5 r1=3 r2=4" + +# and both with the rotation sense the chapter asks for +run "xyzac-trt-kins coordinates=XYZAC" \ + "setp xyzac-trt-kins.conventional-directions 1 +setp xyzac-trt-kins.y-offset 3 +setp xyzac-trt-kins.z-offset 11 +setp xyzac-trt-kins.tool-offset 7 +setp xyzac-trt-kins.x-rot-point 1 +setp xyzac-trt-kins.y-rot-point 2 +setp xyzac-trt-kins.z-rot-point 5" \ + "joints=5 r1=3 r2=4" + +run "xyzbc-trt-kins coordinates=XYZBC" \ + "setp xyzbc-trt-kins.conventional-directions 1 +setp xyzbc-trt-kins.x-offset 3 +setp xyzbc-trt-kins.z-offset 11 +setp xyzbc-trt-kins.tool-offset 7 +setp xyzbc-trt-kins.x-rot-point 1 +setp xyzbc-trt-kins.y-rot-point 2 +setp xyzbc-trt-kins.z-rot-point 5" \ + "joints=5 r1=3 r2=4" + +run "xyzab_tdr_kins" \ + "setp xyzab_tdr_kins.x-offset 3 +setp xyzab_tdr_kins.z-offset 11 +setp xyzab_tdr_kins.tool-offset-z 7 +setp xyzab_tdr_kins.x-rot-point 1 +setp xyzab_tdr_kins.y-rot-point 2 +setp xyzab_tdr_kins.z-rot-point 5" \ + "joints=5 r1=3 r2=4" + +# The nutating heads read their rotary angles from the joint argument of +# the inverse rather than from the pose, so differencing the inverse +# about a pose cannot see the coupling; the forward is the check here. +run "xyzacb_trsrn" \ + "setp xyzacb_trsrn_kins.nut-angle 45 +setp xyzacb_trsrn_kins.y-pivot 100 +setp xyzacb_trsrn_kins.z-pivot 200 +setp xyzacb_trsrn_kins.x-offset 5 +setp xyzacb_trsrn_kins.y-offset 7 +setp xyzacb_trsrn_kins.y-rot-axis 300 +setp xyzacb_trsrn_kins.z-rot-axis 400 +setp xyzacb_trsrn_kins.tool-offset-z 50 +setp xyzacb_trsrn_kins.pre-rot 0.3 +setp xyzacb_trsrn_kins.primary-angle 20 +setp xyzacb_trsrn_kins.secondary-angle 35" \ + "joints=6 r1=3 r2=4 r3=5 check=fwd" + +run "xyzbca_trsrn" \ + "setp xyzbca_trsrn_kins.nut-angle 45 +setp xyzbca_trsrn_kins.x-pivot 100 +setp xyzbca_trsrn_kins.z-pivot 200 +setp xyzbca_trsrn_kins.x-offset 5 +setp xyzbca_trsrn_kins.y-offset 7 +setp xyzbca_trsrn_kins.x-rot-axis 300 +setp xyzbca_trsrn_kins.z-rot-axis 400 +setp xyzbca_trsrn_kins.tool-offset-z 50 +setp xyzbca_trsrn_kins.pre-rot 0.3 +setp xyzbca_trsrn_kins.primary-angle 20 +setp xyzbca_trsrn_kins.secondary-angle 35" \ + "joints=6 r1=3 r2=4 r3=5 check=fwd" + +# polar +run "rosekins" "" "joints=3 r1=2 base=10,5,0 angles=30,-25,90,120" + +# arms. Straight or folded they are singular, so the sweep keeps clear +# of 0 and 180 on the elbow. genserkins iterates its inverse to a +# tolerance the differences would not see through, so it is checked +# against its forward only; pumakins and three21kins answer from their +# Denavit-Hartenberg chain and both checks prove the answer. +run "scarakins" "" "joints=6 r1=1 r2=3 r3=0 base=0,0,20,0,0,0 angles=30,-25,90,120,-60" +# scorbot's inverse returns the elbow-up arm, shoulder above elbow, so the +# poses have to be ones it can return: j1 above j2, and j2 within a quarter +# turn of level +run "scorbot-kins" "" "joints=5 r1=1 base=0,70,-20,0,0 angles=40,55,70,85" +run "scorbot-kins" "" "joints=5 r1=2 base=0,80,0,0,0 angles=-60,-30,0,20" +run "pumakins" "setp pumakins.D6 50" "joints=6 r1=1 r2=2 r3=4 base=15,0,0,10,0,20 angles=20,45,-35,70" +run "three21kins" "" "joints=6 r1=1 r2=2 r3=4 base=15,0,0,10,0,20 angles=20,45,-35,70" +run "genserkins" "" "joints=9 r1=1 r2=2 r3=4 base=15,0,0,10,0,20 angles=20,45,-35,70 check=fwd" +# and with a joint counted relative to the one before it +run "genserkins" "setp genserkins.unrotate-3 1" "joints=9 r1=1 r2=2 r3=4 base=15,0,0,10,0,20 angles=20,45,-35,70 check=fwd" + +# parallel machines. The struts cannot tilt the platform far, and the +# forward of the hexapod and the pentapod iterates to a tolerance, so the +# product check on those two is held to what that tolerance allows. The +# hexapod module runs its own forward for its GUI pins in every type, with +# whatever joint values that type has, and identity joint values are not +# strut lengths it can converge from; its identity types are the shared +# ones trivkins covers, so only its own type is checked. +run "tripodkins" \ + "setp tripodkins.Bx 2 +setp tripodkins.Cx 1 +setp tripodkins.Cy 2" \ + "joints=3 frompose=1 base=1,1,2" +run "lineardeltakins" "" "joints=9 frompose=1 base=20,30,-200" +run "rotarydeltakins" "" "joints=9 r1=0 r2=1 frompose=1 base=0,0,-12 angles=0,2,-3" +run "genhexkins" \ + "setp genhexkins.screw-lead 0" \ + "joints=6 r1=3 r2=4 r3=5 frompose=1 base=2,3,20 angles=0,5,-7,10 tolexp=3 types=1" +run "genhexkins" \ + "setp genhexkins.screw-lead 5" \ + "joints=6 r1=3 r2=4 r3=5 frompose=1 base=2,3,20 angles=0,5,-7,10 tolexp=3 types=1" +run "pentakins" "" "joints=5 r1=3 r2=4 frompose=1 base=10,20,0 angles=0,5,-7,10 tolexp=3"