From 884ec29cd6dd13161d70abdfd5f8bd03be0faec3 Mon Sep 17 00:00:00 2001
From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com>
Date: Sun, 23 Aug 2026 14:32:38 +1000
Subject: [PATCH 01/10] docs: add a kinematics conventions chapter

A kinematics module reports the controlled point and nothing else, so
everything needing the rest of the machine geometry rebuilds it.  For
xyzacb-trsrn the same chain is written three times: closed-form in
xyzacb_trsrn.comp, as homogeneous matrices in the config's
remap_funcs_twp.py, and as signed HalRotate calls in the vismach model.  The
Python copy is kept in step by a comment saying its matrices "must be the
same as the ones used to derive the kinematic model".

Write down the vocabulary they would need to share: the four frames and which
one kinematicsForward() reports in, the rotation sense already stated under
Rotational Axes and its ISO 841 equivalent, what conventional-directions
costs at its default, and the definition of the tool frame.

Tool x is the part worth stating as a rule rather than a formula.  The
virtual rotation about tool z supplies what a five-axis machine cannot, and
the convention is that it leaves tool x parallel to the machine xy-plane; the
formula follows from the machine's own secondary rotation matrix, which is
why the two nutating configs in tree have different ones.

Also anchor the Rotational Axes section so it can be referenced.
---
 docs/po4a.cfg                               |   1 +
 docs/src/Master_Documentation.adoc          |   2 +
 docs/src/Submakefile                        |   1 +
 docs/src/gcode/machining-center.adoc        |   1 +
 docs/src/index.tmpl                         |   1 +
 docs/src/motion/kinematics-conventions.adoc | 333 ++++++++++++++++++++
 6 files changed, 339 insertions(+)
 create mode 100644 docs/src/motion/kinematics-conventions.adoc

diff --git a/docs/po4a.cfg b/docs/po4a.cfg
index 61308113169..c6a47ebddc2 100644
--- a/docs/po4a.cfg
+++ b/docs/po4a.cfg
@@ -360,6 +360,7 @@
 [type: AsciiDoc_def] src/motion/dh-parameters.adoc $lang:build/adoc/$lang/motion/dh-parameters.adoc
 [type: AsciiDoc_def] src/motion/dual-pid-example.adoc $lang:build/adoc/$lang/motion/dual-pid-example.adoc
 [type: AsciiDoc_def] src/motion/external-offsets.adoc $lang:build/adoc/$lang/motion/external-offsets.adoc
+[type: AsciiDoc_def] src/motion/kinematics-conventions.adoc $lang:build/adoc/$lang/motion/kinematics-conventions.adoc
 [type: AsciiDoc_def] src/motion/kinematics.adoc $lang:build/adoc/$lang/motion/kinematics.adoc
 [type: AsciiDoc_def] src/motion/pid-theory.adoc $lang:build/adoc/$lang/motion/pid-theory.adoc
 [type: AsciiDoc_def] src/motion/switchkins.adoc $lang:build/adoc/$lang/motion/switchkins.adoc
diff --git a/docs/src/Master_Documentation.adoc b/docs/src/Master_Documentation.adoc
index 993f6a318bc..12903195a5f 100644
--- a/docs/src/Master_Documentation.adoc
+++ b/docs/src/Master_Documentation.adoc
@@ -189,6 +189,8 @@ include::ladder/ladder-examples.adoc[]
 :leveloffset: 2
 include::motion/kinematics.adoc[]
 
+include::motion/kinematics-conventions.adoc[]
+
 include::motion/dh-parameters.adoc[]
 
 include::motion/5-axis-kinematics.adoc[]
diff --git a/docs/src/Submakefile b/docs/src/Submakefile
index b585ee4574b..b544081971d 100644
--- a/docs/src/Submakefile
+++ b/docs/src/Submakefile
@@ -251,6 +251,7 @@ DOC_SRCS_EN := \
 	ladder/ladder-intro.adoc \
 	lathe/lathe-user.adoc \
 	motion/kinematics.adoc \
+	motion/kinematics-conventions.adoc \
 	motion/dh-parameters.adoc \
 	motion/pid-theory.adoc \
 	motion/dual-pid-example.adoc \
diff --git a/docs/src/gcode/machining-center.adoc b/docs/src/gcode/machining-center.adoc
index e6760936099..27df8428626 100644
--- a/docs/src/gcode/machining-center.adoc
+++ b/docs/src/gcode/machining-center.adoc
@@ -108,6 +108,7 @@ The U, V and W axes also form a standard right-handed coordinate
 system. X and U are parallel, Y and V are parallel, and Z and W are
 parallel (when A, B, and C are rotated to zero).
 
+[[sec:rotational-axes]]
 === Rotational Axes
 
 The rotational axes are measured in degrees as wrapped linear axes in
diff --git a/docs/src/index.tmpl b/docs/src/index.tmpl
index d82c576d947..99cd603b891 100644
--- a/docs/src/index.tmpl
+++ b/docs/src/index.tmpl
@@ -158,6 +158,7 @@
 <div class="details-list">
 	<ul>
 		<li><a href="motion/kinematics.html">Kinematics</a></li>
+		<li><a href="motion/kinematics-conventions.html">Kinematics Conventions</a></li>
 		<li><a href="motion/dh-parameters.html">DH Parameters</a></li>
 		<li><a href="motion/5-axis-kinematics.html">5-Axis-Kinematics</a></li>
 		<li><a href="motion/switchkins.html">Switchable Kinematics</a></li>
diff --git a/docs/src/motion/kinematics-conventions.adoc b/docs/src/motion/kinematics-conventions.adoc
new file mode 100644
index 00000000000..0520eaeae28
--- /dev/null
+++ b/docs/src/motion/kinematics-conventions.adoc
@@ -0,0 +1,333 @@
+:lang: en
+:toc:
+
+[[cha:kinematics-conventions]]
+= Kinematics Conventions
+
+== Introduction
+
+A kinematics module answers two questions: where the controlled point is for a
+given set of joint positions, and which joint positions put it at a requested
+place. `kinematicsForward()` and `kinematicsInverse()` are those two answers.
+
+Everything else about a machine's geometry is currently rebuilt outside the
+module. A tilted work plane needs the direction the tool points in, a
+simulation model needs the whole chain of frames, a limit check needs the rate
+joints move per unit of commanded motion. For the `xyzacb-trsrn` machine that
+geometry is written three times: as closed-form expressions in
+`src/hal/components/xyzacb_trsrn.comp`, as homogeneous matrices in the config's
+`remap_funcs_twp.py`, and as a chain of `HalRotate` calls with hand-chosen
+signs in `vismach/xyzacb-trsrn-gui.py`. The three copies share no vocabulary
+and no sign convention, so "the same as" can be checked only by a person
+reading all three.
+
+This chapter fixes the vocabulary and states the rules, so that two modules
+written from the same drawing give the same answers and a consumer can say what
+it needs without naming a machine. <<sec:writing-a-module,Writing a Module>>
+collects the rules in one place.
+
+[[sec:frames]]
+== Frames
+
+Four frames, all right-handed.
+
+Joint space::
+  One coordinate per joint, in that joint's own units. This is what the motion
+  controller commands and what `kinematicsForward()` is given.
+
+Machine frame::
+  Fixed to the machine bed, with the X, Y and Z of
+  <<cha:cnc-machine-overview,CNC Machine Overview>>. Nothing rotates it.
+
+Work frame::
+  Fixed to the workpiece. `kinematicsForward()` reports in it, and
+  `kinematicsInverse()` is given it. Where no rotary carries the work it
+  coincides with the machine frame. Where one does, it turns with the table and
+  the module undoes that rotation, so that a G-code position keeps naming the
+  same feature of the part however the table is set.
+
+Tool frame::
+  Fixed to the tool. Its origin is the controlled point of
+  <<sec:controlled-point,Controlled Point>>, the tool tip when a tool length
+  offset is in effect and the tip of the spindle otherwise. Its third axis is
+  the tool axis, <<sec:tool-frame,The Tool Frame>>.
+
+The interface calls the work frame "world": the forward and inverse take a
+`struct EmcPose *world`, and the motion controller speaks of world mode. That
+name is historical and stays in the code. This chapter says work frame, because
+the frame is attached to the workpiece and not to the room.
+
+A pose is always a pair of frames, the moving one and the one it is measured
+against, and the two halves of this chapter use different pairs deliberately.
+
+Positions are measured in the work frame, which is what makes a program
+independent of how the table is set.
+
+Orientations are measured against the machine frame, and there are two of them.
+A module reports the tool frame and the work frame separately, each in machine
+coordinates. A consumer that wants the tool in workpiece coordinates composes
+them:
+
+  tool_in_work = transpose(work) * tool
+
+A frame written as a matrix is the rotation whose columns are its axes in the
+coordinates it is measured against, so the tool axis in workpiece coordinates,
+the vector a tilted work plane asks the machine to reach and the one existing
+TWP code reads out of `matrix[0,2]`, `matrix[1,2]` and `matrix[2,2]`, is the
+third column of that product.
+
+The pair is reported rather than the product because the product cannot be
+taken apart again, and a consumer that has to place both bodies needs each one
+against something that does not move. A simulation model draws the workpiece in
+one place and the tool in another; given only the product it can recover
+neither. Where only the tool turns, the work frame is the identity; where only
+the work turns, the tool frame is. A machine that turns both returns a real
+pair.
+
+[[sec:rotation-sense]]
+== Rotation Sense
+
+LinuxCNC states its rotation convention in
+<<sec:rotational-axes,Rotational Axes>>:
+
+[quote]
+The rotational axes are measured in degrees as wrapped linear axes in which the
+direction of positive rotation is counterclockwise when viewed from the
+positive end of the corresponding X, Y, or Z-axis. [...] Clockwise or
+counterclockwise is from the point of view of the workpiece.
+
+The second sentence is the one that matters here. The rotation described is
+that of the tool relative to the workpiece. Where the rotary carries the tool
+that is also the direction the physical axis turns; where it carries the work
+the table turns the other way, and the module converts between the two. This is
+the convention of ISO 841, which describes all motion as motion of the tool
+relative to the workpiece and primes the axes of a machine that moves the work:
+a table turning about Z is `+C'` when it produces the tool motion called `+C`.
+
+So: given a pose whose C value increases, the tool moves counterclockwise about
+work Z seen from the workpiece, whichever member physically turns.
+
+=== conventional-directions
+
+`trtfuncs.c` and `maxkins.c` carry a `conventional-directions` HAL pin that
+selects the sign of the rotary terms, and default it to false, which is the
+opposite sense. Existing configurations keep working; new configurations set it
+true.
+
+Leaving it false costs two things. The direction a program runs in depends on a
+HAL pin rather than on the G-code. And the rotary values the module reports are
+the raw joint values,
+
+[source,c]
+----
+pos->a = joints[JA];
+pos->c = joints[JC];
+----
+
+while the translations in the same call were computed with the opposite sign,
+so the returned pose does not describe its own orientation. A caller cannot
+rebuild the tool frame from `pos->a`, `pos->b` and `pos->c` without separately
+knowing how the pin is set. That is the immediate reason the tool frame has to
+be an answer from the module rather than something a caller derives from the
+pose.
+
+[[sec:tool-frame]]
+== The Tool Frame
+
+=== The tool axis
+
+The tool axis is the third axis of the tool frame. It points from the tip
+towards the holder, away from the material.
+
+It is a direction, not a distance, and is unrelated to the tool length: the
+length is the scalar the `tool-length` pin carries, and the tool axis is the
+direction that length is applied along.
+
+Where it points for a given joint set is whatever the machine's geometry makes
+it. A plain vertical mill has `[0, 0, 1]` in machine coordinates at all times,
+and a machine whose spindle is parallel to Z with its rotaries at zero has it
+there too, but that is a property of those machines and not a rule. `pumakins`
+with its supplied parameters has `[0, 0, -1]` with every joint at zero, and
+`genserkins` takes its Denavit-Hartenberg parameters from HAL pins, so for that
+module the question has no fixed answer at all.
+
+[[sec:approach-vector]]
+=== Native frames that point the other way
+
+Robot kinematics name the same line in the opposite sense. ISO 9787 clause 5.3
+places the mechanical interface coordinate system at the centre of the flange,
+where the "+Zm axis points perpendicularly away from the mechanical interface",
+which runs holder towards tip. The Denavit-Hartenberg approach vector is the
+same sense. `pumakins` builds that frame and uses it, reaching the tip by
+adding the tool length along the third column:
+
+[source,c]
+----
+hom.tran.x = hom.tran.x + hom.rot.z.x*PUMA_D6;
+----
+
+Both senses come from a standard for a class of machine, and the tree contains
+both. What a module reports is the machine tool sense, tip towards holder,
+because that is the direction a tilted work plane commands and what machine Z
+already means to a mill operator.
+
+Turning one sense into the other is not a change of sign. Negating the third
+column leaves a matrix of determinant -1, a reflection, which is not a frame
+any machine can hold. Reversing the tool axis and staying right-handed takes a
+half turn about one of the two transverse axes, and which one is chosen decides
+where tool X lands.
+
+Because it is a rotation in its own right, a module declares it rather than
+applying it by hand, as the last argument of `switchkinsRegisterFrames()`.
+Shared code applies it and checks once, at load, that it is orthonormal with
+determinant +1. `TOOL_FRAME_SPINDLE` is the identity, for a module whose maths
+is already in the convention; `TOOL_FRAME_FLANGE` is the half turn a
+Denavit-Hartenberg module needs. Keeping it in one place makes it greppable and
+stops the next such module quietly choosing the other half turn, which would be
+right about the tool axis and wrong about tool X.
+
+The declaration is fixed when the module is written and nothing changes it at
+runtime. A machine whose native frame moved while running would be a machine
+whose geometry moves underneath the program.
+
+=== Tool X
+
+A tilted work plane commands only where the tool points. Reaching that
+direction uses both rotaries of a five-axis machine, and the rotation of the
+tool about its own axis is then whatever the chain leaves rather than anything
+the program chose. For cutting that does not matter, the cutter being a solid
+of revolution. It matters as soon as the tool frame is used as a coordinate
+system for programming, which is what `G68.2` does: the operator writes X and Y
+moves in the tilted plane and has to know where its X points.
+
+So the software places it, through a virtual rotation about the tool axis
+applied after the physical joints. It is the `pre-rot` pin on the in-tree
+kinematics components and `virtual_rot` in the TWP code, one quantity under two
+names. A machine with no such pin has no say in the matter: its tool X is
+whatever the chain produces, and a consumer that needs a defined one applies
+the rotation itself.
+
+[IMPORTANT]
+By default, tool X lies parallel to the machine XY plane. Where the tool axis
+is vertical and that leaves tool X free, tool X is machine X. `G68.3 R` rotates
+the frame from there.
+
+This fixes tool X only up to a half turn, two opposite directions both being
+horizontal. Where a module has to choose, it takes the one that keeps the frame
+continuous with the previous pose.
+
+=== Deriving the default rotation
+
+The convention is stated rather than a formula because the formula differs from
+machine to machine and follows from the convention. Write the tool orientation
+as the product of the primary, secondary and virtual rotations:
+
+  M = Rp(theta_1) * Rs(theta_2) * Rz(tc)
+
+Tool X is the first column of `M`, so "tool X is horizontal" is the statement
+that `M[2][0]` is zero. Solving that for `tc` gives the default.
+
+For a nutating head of nutation angle `v`, writing `Sv = sin(v)`,
+`Cv = cos(v)`, `Ss = sin(theta_2)`, `Cs = cos(theta_2)`,
+`s = Cs + Cv*Cv*(1 - Cs)` and `t = Sv*Cv*(1 - Cs)`, the two nutating machines
+in the tree have different secondary rotations, and so different bottom rows:
+
+[cols="1,2,2",options="header"]
+|===
+| machine | bottom row of `Rs` | resulting default
+
+| `xyzacb-trsrn`
+| `[-Sv*Ss, t, s]`
+| `tc = atan2(Sv*Ss, t)`
+
+| `xyzbca-trsrn`
+| `[t, Sv*Ss, s]`
+| `tc = atan2(-t, Sv*Ss)`
+|===
+
+The two formulas look unrelated and are the same rule. Apply either to the
+other machine and the result is a frame whose tool *Y* is horizontal, a quarter
+turn from what was wanted, and no test in the tree notices: the tool still
+points where it was told to point, and only the meaning of X and Y in the
+tilted plane has changed. A module that documents its `Rs` and cites this rule
+can be checked. One that documents only its `tc` formula cannot.
+
+[[sec:consumer-needs]]
+== What a Module Reports
+
+Position, through `kinematicsForward()` and `kinematicsInverse()`, in the work
+frame.
+
+Orientation, through `kinematicsWorkFrame()` and `kinematicsToolFrame()`, each
+against the machine frame, as <<sec:frames,Frames>> describes. The consumers
+are tilted work plane handling, tool length compensation along a tilted axis,
+previews and simulation models, and probing routines that have to say which way
+the stylus faces.
+
+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.
+
+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
+in a realtime thread to be useful: the interesting callers, a limit check
+before a move and a preview before a program runs, are not in the servo loop.
+
+[[sec:writing-a-module]]
+== Writing a Module
+
+Frames::
+  Report positions in the work frame, so a rotary that carries the work is
+  undone in `kinematicsForward()`. Report the work frame and the tool frame
+  separately, each against the machine frame, so that a consumer placing both
+  bodies can.
+
+Signs::
+  Positive A, B and C are counterclockwise about work X, Y and Z viewed from
+  the positive end, describing the motion of the tool relative to the
+  workpiece. A new module does not offer a pin that reverses this.
+
+Tool axis::
+  Tip towards holder. A module whose maths is written with the
+  Denavit-Hartenberg approach vector declares the half turn that relates the
+  two, rather than applying it by hand.
+
+Tool X::
+  The default virtual rotation puts tool X parallel to the machine XY plane.
+  Derive the value from the module's own rotation matrices, and write those
+  matrices down in the module.
+
+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
+  in step, which is the situation this chapter exists to end.
+
+Mount orientation is not this::
+  A tool or holder mount orientation is a different quantity: a right-angle
+  head, a tool held at an angle, an end effector clocked on its flange. Those
+  vary from setup to setup and belong with the rest of the tool data, addressed
+  from the program where the interpreter can see them, and not in HAL alone
+  where lookahead and preview cannot see them and where they can move
+  underneath a running program. The tool table and `G43.1` already carry
+  per-tool A, B and C words, which is the right addressing route, but they
+  shift the rotary axis reading and configurations depend on that, so a mount
+  orientation is a new field rather than a reinterpretation of that one. It
+  wants to be stated as a frame, which can be checked for orthonormality and
+  determinant, rather than as three angles whose ordering convention is written
+  down nowhere.
+
+== References
+
+* <<cha:cnc-machine-overview,CNC Machine Overview>>, for the axis nomenclature
+  and the rotation convention this chapter builds on.
+* <<cha:5-axis-kinematics,5-Axis Kinematics>>, for worked transformations of
+  the table-rotary and tilting-table configurations.
+* <<cha:switchable-kinematics,Switchable Kinematics>>, for how a machine
+  presents more than one of these models at once.
+* ISO 841, Industrial automation systems and integration, Numerical control of
+  machines, Coordinate system and motion nomenclature.
+* ISO 9787, Robots and robotic devices, Coordinate systems and motion
+  nomenclatures, clause 5.3, for the flange frame the robot modules follow.

From 47a9f95326e30a197f565e7f0a0918e70c5d243a Mon Sep 17 00:00:00 2001
From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com>
Date: Sun, 23 Aug 2026 15:24:15 +1000
Subject: [PATCH 02/10] kinematics: add optional work frame and tool frame
 entry points

kinematicsForward() reports where the controlled point is and nothing about
which way anything faces, so a consumer that needs the geometry rebuilds it
for itself.  Add two entry points a module can answer with instead:

  int kinematicsToolFrame(const double *joint, PmRotationMatrix *rot,
                          const KINEMATICS_FORWARD_FLAGS *fflags);
  int kinematicsWorkFrame(const double *joint, PmRotationMatrix *rot,
                          const KINEMATICS_FORWARD_FLAGS *fflags);

Each returns the columns of that frame's axes in machine coordinates.
Conventions are in the Kinematics Conventions chapter.

They are reported separately rather than as the single work-to-tool rotation
because the product cannot be taken apart again.  A consumer that has to place
both bodies, a simulation model or a tracking display, needs each against
something that does not move; one that wants the tool in workpiece
coordinates, which is what a tilted work plane asks for, composes them with
toolFrameInWork().  Composing is a multiply, decomposing is impossible, so the
halves are what the module owes the caller.

Modules built on switchkins.c export both and dispatch on the current type,
returning -1 for a type that has not supplied them.  A type registers with
switchkinsRegisterFrames() from its switchkinsSetup(); leaving it out costs
nothing.  Adding it that way rather than as arguments to switchkinsSetup() and
switchkinsRegister() keeps both signatures as they are, so no module has to
change to build.

The tool frame has two live conventions, so a type also declares the rotation
relating its own to the one in use, and the dispatch applies it.  That is a
rotation and not a sign: negating the third column alone leaves determinant
-1, a reflection.  It is checked once at registration rather than on each
call.  The work frame needs none of this, having no tool axis to point the
wrong way.

Identity types answer both the same way whichever module asked for them, so
switchkins.c attaches the identity pair to any type whose forward is the
identity one, and every switchkins module gains correct frames for its
identity type without being touched.

Nothing in motion calls either, so no module is obliged to define them and a
module outside the tree need not know they exist.
---
 src/emc/kinematics/kinematics.h |  82 +++++++++++++++++++
 src/emc/kinematics/kins_util.c  | 136 ++++++++++++++++++++++++++++++++
 src/emc/kinematics/switchkins.c |  75 ++++++++++++++++++
 src/emc/kinematics/switchkins.h |  15 ++++
 4 files changed, 308 insertions(+)

diff --git a/src/emc/kinematics/kinematics.h b/src/emc/kinematics/kinematics.h
index 67e6565155d..30da3cb7af7 100644
--- a/src/emc/kinematics/kinematics.h
+++ b/src/emc/kinematics/kinematics.h
@@ -102,6 +102,54 @@ extern int kinematicsHome(struct EmcPose * world,
 
 extern KINEMATICS_TYPE kinematicsType(void);
 
+/* These two give the orientation of the tool and of the workpiece for a set
+   of joint values.  Each returns a rotation whose columns are that frame's
+   axes expressed in MACHINE coordinates, the frame fixed to the bed that
+   nothing rotates.  Note that this is not the frame kinematicsForward()
+   reports positions in, which is attached to the workpiece; see the
+   Kinematics Conventions chapter.
+
+   They are reported separately, and not as the single work-to-tool rotation,
+   because the product cannot be taken apart again.  A consumer that has to
+   place both bodies, a simulation model or a preview, needs each one against
+   the machine.  A consumer that wants the tool in workpiece coordinates,
+   which is what a tilted work plane asks for, composes them itself:
+
+       tool_in_work = transpose(work) * tool
+
+   The third column of the tool frame is the tool axis: a direction, not to be
+   confused with the tool length, which is the distance applied along it.  It
+   runs from the tool tip towards the holder.  The origin of the tool frame is
+   the controlled point that kinematicsForward() reports for the same joints.
+   Where a module applies a virtual rotation about the tool axis, the frame
+   returned includes it.
+
+   A module whose own maths is in the other sense, which is every module built
+   on the ISO 9787 flange frame or on Denavit-Hartenberg parameters, does not
+   fix that up by hand: it declares the rotation relating its frame to the
+   convention and the shared code applies it.  Reversing the tool axis is a
+   rotation, not a sign.  Negating the third column alone gives determinant -1,
+   a reflection, and which half turn is used decides where tool x ends up.
+
+   A machine that turns only the tool returns the identity for the work frame,
+   and one that turns only the work returns the identity for the tool frame.
+   Machines that do both, which is every table-rotary head-rotary mill, return
+   a non-trivial pair and are the reason for reporting them apart.
+
+   Both are optional.  Modules built on switchkins.c export them always and
+   return -1 for a switchkins type that has not supplied one; other modules
+   need not export them at all, so a caller resolving them dynamically has to
+   cope with their absence.
+
+   Return 0 on success, -1 if the frame is not available. */
+extern int kinematicsToolFrame(const double *joint,
+                               PmRotationMatrix *rot,
+                               const KINEMATICS_FORWARD_FLAGS *fflags);
+
+extern int kinematicsWorkFrame(const double *joint,
+                               PmRotationMatrix *rot,
+                               const KINEMATICS_FORWARD_FLAGS *fflags);
+
 /* parameters for use with switchkins.c */
 typedef struct kinematics_parms {
   char* sparm;     // module string parameter passed to kins
@@ -157,6 +205,40 @@ extern int identityKinematicsInverse(const struct EmcPose * world,
                                      const KINEMATICS_INVERSE_FLAGS * iflags,
                                      KINEMATICS_FORWARD_FLAGS * fflags);
 
+/* joints are axes, so neither frame ever turns */
+extern int identityKinematicsToolFrame(const double *joint,
+                                       PmRotationMatrix *rot,
+                                       const KINEMATICS_FORWARD_FLAGS *fflags);
+
+extern int identityKinematicsWorkFrame(const double *joint,
+                                       PmRotationMatrix *rot,
+                                       const KINEMATICS_FORWARD_FLAGS *fflags);
+
+/* Rotations relating a module's own frame to the tool frame convention.
+   TOOL_FRAME_SPINDLE is the identity, for maths already in the convention.
+   TOOL_FRAME_FLANGE is the half turn about tool x that turns an ISO 9787
+   flange frame, whose z points out of the mechanical interface towards the
+   work, into the convention. */
+extern const PmRotationMatrix TOOL_FRAME_SPINDLE;
+extern const PmRotationMatrix TOOL_FRAME_FLANGE;
+
+/* Post-multiply a module's native frame by the rotation it declared, in
+   place.  Modules built on switchkins.c never call this, the dispatch does it
+   for them; a standalone module calls it before returning.
+   Returns 0, or -1 if native is not a proper rotation. */
+extern int toolFrameApplyNative(PmRotationMatrix *rot,
+                                const PmRotationMatrix *native);
+
+/* out = transpose(work) * tool, the tool frame in workpiece coordinates.
+   out may alias neither input. */
+extern int toolFrameInWork(const PmRotationMatrix *work,
+                           const PmRotationMatrix *tool,
+                           PmRotationMatrix *out);
+
+/* True if m is orthonormal with determinant +1, so a frame a machine can
+   actually hold.  Used to check a declared rotation once, at load. */
+extern int toolFrameIsProper(const PmRotationMatrix *m);
+
 extern int kinematicsSwitchable(void);
 extern int kinematicsSwitch(int switchkins_type);
 //NOTE: switchable kinematics may require Interp::Synch
diff --git a/src/emc/kinematics/kins_util.c b/src/emc/kinematics/kins_util.c
index c82a4a2fc95..1576f7fb505 100644
--- a/src/emc/kinematics/kins_util.c
+++ b/src/emc/kinematics/kins_util.c
@@ -45,6 +45,7 @@
 
 #include <rtapi.h>
 #include <rtapi_string.h>
+#include <rtapi_math.h>
 #include <emcmotcfg.h>
 #include <emcpos.h>
 #include <kinematics.h>
@@ -364,3 +365,138 @@ int identityKinematicsInverse(const EmcPose * pos,
 
     return 0;
 } // identityKinematicsInverse()
+
+const PmRotationMatrix TOOL_FRAME_SPINDLE = {
+    { 1, 0, 0},   // tool x
+    { 0, 1, 0},   // tool y
+    { 0, 0, 1}    // tool axis
+};
+
+// half turn about tool x: reverses the tool axis and tool y, keeps tool x,
+// and keeps the frame right-handed.  Negating the tool axis on its own would
+// leave a reflection, which is not a frame any machine can hold.
+const PmRotationMatrix TOOL_FRAME_FLANGE = {
+    { 1,  0,  0},
+    { 0, -1,  0},
+    { 0,  0, -1}
+};
+
+int toolFrameIsProper(const PmRotationMatrix *m)
+{
+    const double c[3][3] = {
+        { m->x.x, m->y.x, m->z.x },
+        { m->x.y, m->y.y, m->z.y },
+        { m->x.z, m->y.z, m->z.z }
+    };
+    double det;
+    int a, b, k;
+
+    for (a = 0; a < 3; a++) {
+        for (b = a; b < 3; b++) {
+            double dot = 0;
+            for (k = 0; k < 3; k++) { dot += c[k][a] * c[k][b]; }
+            if (fabs(dot - (a == b ? 1.0 : 0.0)) > 1e-9) { return 0; }
+        }
+    }
+
+    det = c[0][0] * (c[1][1]*c[2][2] - c[1][2]*c[2][1])
+        - c[0][1] * (c[1][0]*c[2][2] - c[1][2]*c[2][0])
+        + c[0][2] * (c[1][0]*c[2][1] - c[1][1]*c[2][0]);
+
+    return fabs(det - 1.0) <= 1e-9;
+} // toolFrameIsProper()
+
+int toolFrameApplyNative(PmRotationMatrix *rot,
+                         const PmRotationMatrix *native)
+{
+    // rot holds the module's own frame, native the rotation relating it to
+    // the convention, so the answer is rot * native: the declared rotation is
+    // expressed in the module's frame, not in machine coordinates.
+    const double r[3][3] = {
+        { rot->x.x, rot->y.x, rot->z.x },
+        { rot->x.y, rot->y.y, rot->z.y },
+        { rot->x.z, rot->y.z, rot->z.z }
+    };
+    const double n[3][3] = {
+        { native->x.x, native->y.x, native->z.x },
+        { native->x.y, native->y.y, native->z.y },
+        { native->x.z, native->y.z, native->z.z }
+    };
+    double m[3][3];
+    int a, b, k;
+
+    if (!toolFrameIsProper(native)) {
+        rtapi_print_msg(RTAPI_MSG_ERR,
+            "toolFrameApplyNative: declared rotation is not a proper rotation\n");
+        return -1;
+    }
+
+    for (a = 0; a < 3; a++) {
+        for (b = 0; b < 3; b++) {
+            m[a][b] = 0;
+            for (k = 0; k < 3; k++) { m[a][b] += r[a][k] * n[k][b]; }
+        }
+    }
+
+    rot->x.x = m[0][0]; rot->y.x = m[0][1]; rot->z.x = m[0][2];
+    rot->x.y = m[1][0]; rot->y.y = m[1][1]; rot->z.y = m[1][2];
+    rot->x.z = m[2][0]; rot->y.z = m[2][1]; rot->z.z = m[2][2];
+
+    return 0;
+} // toolFrameApplyNative()
+
+int toolFrameInWork(const PmRotationMatrix *work,
+                    const PmRotationMatrix *tool,
+                    PmRotationMatrix *out)
+{
+    // transpose(work) * tool: both are given against the machine, and
+    // transposing the work frame turns "machine to work" out of "work to
+    // machine" without a general inverse, because a rotation is orthonormal
+    const double w[3][3] = {
+        { work->x.x, work->y.x, work->z.x },
+        { work->x.y, work->y.y, work->z.y },
+        { work->x.z, work->y.z, work->z.z }
+    };
+    const double t[3][3] = {
+        { tool->x.x, tool->y.x, tool->z.x },
+        { tool->x.y, tool->y.y, tool->z.y },
+        { tool->x.z, tool->y.z, tool->z.z }
+    };
+    double m[3][3];
+    int a, b, k;
+
+    for (a = 0; a < 3; a++) {
+        for (b = 0; b < 3; b++) {
+            m[a][b] = 0;
+            for (k = 0; k < 3; k++) { m[a][b] += w[k][a] * t[k][b]; }
+        }
+    }
+
+    out->x.x = m[0][0]; out->y.x = m[0][1]; out->z.x = m[0][2];
+    out->x.y = m[1][0]; out->y.y = m[1][1]; out->z.y = m[1][2];
+    out->x.z = m[2][0]; out->y.z = m[2][1]; out->z.z = m[2][2];
+
+    return 0;
+} // toolFrameInWork()
+
+int identityKinematicsWorkFrame(const double *joints,
+                                PmRotationMatrix *rot,
+                                const KINEMATICS_FORWARD_FLAGS *fflags)
+{
+    (void)joints;
+    (void)fflags;
+    // nothing carries the work, so it stays square with the machine
+    *rot = TOOL_FRAME_SPINDLE;
+    return 0;
+} // identityKinematicsWorkFrame()
+
+int identityKinematicsToolFrame(const double *joints,
+                                PmRotationMatrix *rot,
+                                const KINEMATICS_FORWARD_FLAGS *fflags)
+{
+    (void)joints;
+    (void)fflags;
+    // joints are axes, so the tool stays square with the machine
+    *rot = TOOL_FRAME_SPINDLE;
+    return 0;
+} // identityKinematicsToolFrame()
diff --git a/src/emc/kinematics/switchkins.c b/src/emc/kinematics/switchkins.c
index f1393867e35..4f62277a287 100644
--- a/src/emc/kinematics/switchkins.c
+++ b/src/emc/kinematics/switchkins.c
@@ -42,6 +42,9 @@ static kparms kp; // kinematics parms (common all types)
 static KS ksetups[SWITCHKINS_MAX_TYPES] = {NULL};
 static KF kfwds[SWITCHKINS_MAX_TYPES]   = {NULL};
 static KI kinvs[SWITCHKINS_MAX_TYPES]   = {NULL};
+static KT ktools[SWITCHKINS_MAX_TYPES]  = {NULL};
+static KT kworks[SWITCHKINS_MAX_TYPES]  = {NULL};
+static PmRotationMatrix knative[SWITCHKINS_MAX_TYPES];
 
 // types provided, counted in rtapi_app_main() once they are all in
 static int kins_count;
@@ -212,6 +215,39 @@ int kinematicsInverse(const EmcPose * pos,
     return r;
 } // kinematicsInverse()
 
+int kinematicsToolFrame(const double *joint,
+                        PmRotationMatrix *rot,
+                        const KINEMATICS_FORWARD_FLAGS *fflags)
+{
+    int r;
+
+    if (   switchkins_type < 0
+        || switchkins_type >= kins_count
+        || !ktools[switchkins_type]) {
+        return -1; // this type does not supply one; not an error
+    }
+    r = ktools[switchkins_type](joint, rot, fflags);
+    if (r) { return r; }
+
+    // the type answers in its own frame; put it in the convention here so
+    // no module has to get the half turn right for itself
+    return toolFrameApplyNative(rot, &knative[switchkins_type]);
+} // kinematicsToolFrame()
+
+int kinematicsWorkFrame(const double *joint,
+                        PmRotationMatrix *rot,
+                        const KINEMATICS_FORWARD_FLAGS *fflags)
+{
+    if (   switchkins_type < 0
+        || switchkins_type >= kins_count
+        || !kworks[switchkins_type]) {
+        return -1; // this type does not supply one; not an error
+    }
+    // no native rotation here: the work frame has no tool axis to point the
+    // wrong way, so there are not two conventions for it to be caught between
+    return kworks[switchkins_type](joint, rot, fflags);
+} // kinematicsWorkFrame()
+
 KINEMATICS_TYPE kinematicsType()
 {
     return KINEMATICS_BOTH;
@@ -240,6 +276,32 @@ int switchkinsRegister(int ktype, KS kset, KF kfwd, KI kinv)
     return 0;
 } // switchkinsRegister()
 
+int switchkinsRegisterFrames(int ktype, KT kwork, KT ktool,
+                             const PmRotationMatrix *native)
+{
+    if (ktype < 0 || ktype >= SWITCHKINS_MAX_TYPES) {
+        rtapi_print_msg(RTAPI_MSG_ERR,
+                        "switchkinsRegisterFrames: BAD switchkins_type <%d>"
+                        " (must be 0..%d)\n",
+                        ktype, SWITCHKINS_MAX_TYPES - 1);
+        register_error = 1;
+        return -1;
+    }
+    // check the declared rotation once here rather than on every call
+    if (!native || !toolFrameIsProper(native)) {
+        rtapi_print_msg(RTAPI_MSG_ERR,
+                        "switchkinsRegisterFrames: switchkins-type %d"
+                        " declared a rotation that is not orthonormal with"
+                        " determinant +1\n", ktype);
+        register_error = 1;
+        return -1;
+    }
+    kworks[ktype]  = kwork;
+    ktools[ktype]  = ktool;
+    knative[ktype] = *native;
+    return 0;
+} // switchkinsRegisterFrames()
+
 //*********************************************************************
 static char *coordinates;
 RTAPI_MP_STRING(coordinates, "Axes-to-joints-ordering");
@@ -251,7 +313,10 @@ EXPORT_SYMBOL(kinematicsSwitch);
 EXPORT_SYMBOL(kinematicsType);
 EXPORT_SYMBOL(kinematicsForward);
 EXPORT_SYMBOL(kinematicsInverse);
+EXPORT_SYMBOL(kinematicsToolFrame);
+EXPORT_SYMBOL(kinematicsWorkFrame);
 EXPORT_SYMBOL(switchkinsRegister);
+EXPORT_SYMBOL(switchkinsRegisterFrames);
 MODULE_LICENSE("GPL");
 
 static int    comp_id;
@@ -280,6 +345,16 @@ int rtapi_app_main(void)
     if (res) {emsg="switchkinsSetp FAIL"; goto error;}
     if (register_error) {emsg="switchkinsRegister FAIL"; goto error;}
 
+    // an identity type answers the tool frame the same way whichever module
+    // asked for it, so supply it here rather than in every switchkinsSetup()
+    for (i=0; i < SWITCHKINS_MAX_TYPES; i++) {
+        if (!ktools[i] && kfwds[i] == identityKinematicsForward) {
+            kworks[i]  = identityKinematicsWorkFrame;
+            ktools[i]  = identityKinematicsToolFrame;
+            knative[i] = TOOL_FRAME_SPINDLE;
+        }
+    }
+
     // the highest type provided by either route sets the count
     for (i=0; i < SWITCHKINS_MAX_TYPES; i++) {
         if (ksetups[i] || kfwds[i] || kinvs[i]) { kins_count = i + 1; }
diff --git a/src/emc/kinematics/switchkins.h b/src/emc/kinematics/switchkins.h
index 2f9ee530a7c..7815fe2ab39 100644
--- a/src/emc/kinematics/switchkins.h
+++ b/src/emc/kinematics/switchkins.h
@@ -21,6 +21,12 @@ typedef int (*KI)(const struct EmcPose * world,
                   const KINEMATICS_INVERSE_FLAGS * iflags,
                   KINEMATICS_FORWARD_FLAGS * fflags);
 
+// KinematicsWORKFRAME and KinematicsTOOLFRAME functions
+// (optional, see kinematics.h)
+typedef int (*KT)(const double *joint,
+                  PmRotationMatrix *rot,
+                  const KINEMATICS_FORWARD_FLAGS *fflags);
+
 // KinematicsSETUP functions
 typedef int (*KS)(const int   comp_id,     // halpins
                   const char* coordinates, // module parameter
@@ -37,4 +43,13 @@ extern int switchkinsSetup(kparms* ksetup_parms,
 
 // called from switchkinsSetup(), once per type it does not provide itself
 extern int switchkinsRegister(int ktype, KS kset, KF kfwd, KI kinv);
+
+// called from switchkinsSetup() for each type that reports its frames; a type
+// that does not simply omits the call.  Both are given, since a machine has a
+// work frame whether or not anything turns it.  native is the rotation
+// relating the type's own tool frame to the convention, TOOL_FRAME_SPINDLE
+// for maths already in it; it is checked once at load and applied by the
+// dispatch.
+extern int switchkinsRegisterFrames(int ktype, KT kwork, KT ktool,
+                                    const PmRotationMatrix *native);
 #endif // }

From d779531705be5cd591b512a02cecfceed238cea9 Mon Sep 17 00:00:00 2001
From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com>
Date: Sun, 23 Aug 2026 15:24:40 +1000
Subject: [PATCH 03/10] trtfuncs: supply the work and tool frames for xyzac and
 xyzbc

Both rotaries carry the work on these machines, so the tool never turns in the
machine frame and its frame is the identity.  All the rotation is the work's.

The forward transform already contains it: the coefficients it applies to a
displacement of the X, Y and Z joints are the rotation from machine into work,
so the work frame in machine coordinates is their transpose.

Checked against the forward transform by central difference over the linear
joints at four primary and four secondary angles with both settings of
conventional-directions: the composition transpose(work) * tool reproduces the
frame to 3e-9, the result is orthonormal with determinant one, and the tool
axis is machine z with the rotaries at zero.

The check also shows the sign question plainly.  With conventional-directions
true, A at 90 degrees on an xyzac machine puts the tool axis along -Y in work
coordinates, which is +Z turned counterclockwise about +X as the documentation
says it should be.  With the pin at its default of false the same move puts it
along +Y.
---
 src/emc/kinematics/kinematics.h     | 16 ++++++
 src/emc/kinematics/trtfuncs.c       | 76 +++++++++++++++++++++++++++++
 src/emc/kinematics/xyzac-trt-kins.c |  6 +++
 src/emc/kinematics/xyzbc-trt-kins.c |  6 +++
 4 files changed, 104 insertions(+)

diff --git a/src/emc/kinematics/kinematics.h b/src/emc/kinematics/kinematics.h
index 30da3cb7af7..8efeb26c640 100644
--- a/src/emc/kinematics/kinematics.h
+++ b/src/emc/kinematics/kinematics.h
@@ -283,6 +283,14 @@ extern int xyzacKinematicsInverse(const EmcPose * pos,
                                   const KINEMATICS_INVERSE_FLAGS * iflags,
                                   KINEMATICS_FORWARD_FLAGS * fflags);
 
+extern int xyzacKinematicsToolFrame(const double *joints,
+                                   PmRotationMatrix *rot,
+                                   const KINEMATICS_FORWARD_FLAGS *fflags);
+
+extern int xyzacKinematicsWorkFrame(const double *joints,
+                                   PmRotationMatrix *rot,
+                                   const KINEMATICS_FORWARD_FLAGS *fflags);
+
 
 extern int xyzbcKinematicsForward(const double *joints,
                                   EmcPose * pos,
@@ -294,4 +302,12 @@ extern int xyzbcKinematicsInverse(const EmcPose * pos,
                                   const KINEMATICS_INVERSE_FLAGS * iflags,
                                   KINEMATICS_FORWARD_FLAGS * fflags);
 
+extern int xyzbcKinematicsToolFrame(const double *joints,
+                                   PmRotationMatrix *rot,
+                                   const KINEMATICS_FORWARD_FLAGS *fflags);
+
+extern int xyzbcKinematicsWorkFrame(const double *joints,
+                                   PmRotationMatrix *rot,
+                                   const KINEMATICS_FORWARD_FLAGS *fflags);
+
 //*********************************************************************
diff --git a/src/emc/kinematics/trtfuncs.c b/src/emc/kinematics/trtfuncs.c
index 1a991b0068f..31c9ff1a6de 100644
--- a/src/emc/kinematics/trtfuncs.c
+++ b/src/emc/kinematics/trtfuncs.c
@@ -260,6 +260,45 @@ int xyzacKinematicsInverse(const EmcPose * pos,
     return 0;
 } // xyzacKinematicsInverse()
 
+int xyzacKinematicsWorkFrame(const double *joints,
+                             PmRotationMatrix *rot,
+                             const KINEMATICS_FORWARD_FLAGS *fflags)
+{
+    (void)fflags;
+    // the forward transform's coefficients for a displacement of the X, Y and
+    // Z joints are the rotation from machine into work, so the work frame in
+    // machine coordinates is their transpose, written out directly here
+    const double a_rad = joints[JA]*TO_RAD;
+    const double c_rad = joints[JC]*TO_RAD;
+
+    rtapi_real con = hal_get_bool(haldata->conventional_directions) ? 1.0 : -1.0;
+
+    rot->x.x =       cos(c_rad);
+    rot->y.x = con * sin(c_rad);
+    rot->z.x = 0;
+
+    rot->x.y = - con * sin(c_rad) * cos(a_rad);
+    rot->y.y =         cos(c_rad) * cos(a_rad);
+    rot->z.y =   con *              sin(a_rad);
+
+    rot->x.z =         sin(c_rad) * sin(a_rad);
+    rot->y.z = - con * cos(c_rad) * sin(a_rad);
+    rot->z.z =                      cos(a_rad);
+
+    return 0;
+} // xyzacKinematicsWorkFrame()
+
+int xyzacKinematicsToolFrame(const double *joints,
+                             PmRotationMatrix *rot,
+                             const KINEMATICS_FORWARD_FLAGS *fflags)
+{
+    (void)joints;
+    (void)fflags;
+    // both rotaries carry the work, so the tool never turns in the machine
+    *rot = TOOL_FRAME_SPINDLE;
+    return 0;
+} // xyzacKinematicsToolFrame()
+
 int xyzbcKinematicsForward(const double *joints,
                            EmcPose * pos,
                            const KINEMATICS_FORWARD_FLAGS * fflags,
@@ -364,3 +403,40 @@ int xyzbcKinematicsInverse(const EmcPose * pos,
 
     return 0;
 } // xyzbcKinematicsInverse()
+
+int xyzbcKinematicsWorkFrame(const double *joints,
+                             PmRotationMatrix *rot,
+                             const KINEMATICS_FORWARD_FLAGS *fflags)
+{
+    (void)fflags;
+    // see the comment in xyzacKinematicsWorkFrame()
+    const double b_rad = joints[JB]*TO_RAD;
+    const double c_rad = joints[JC]*TO_RAD;
+
+    rtapi_real con = hal_get_bool(haldata->conventional_directions) ? 1.0 : -1.0;
+
+    rot->x.x =         cos(c_rad) * cos(b_rad);
+    rot->y.x =   con * sin(c_rad) * cos(b_rad);
+    rot->z.x = - con *              sin(b_rad);
+
+    rot->x.y = - con * sin(c_rad);
+    rot->y.y =         cos(c_rad);
+    rot->z.y = 0;
+
+    rot->x.z =   con * cos(c_rad) * sin(b_rad);
+    rot->y.z =         sin(c_rad) * sin(b_rad);
+    rot->z.z =                      cos(b_rad);
+
+    return 0;
+} // xyzbcKinematicsWorkFrame()
+
+int xyzbcKinematicsToolFrame(const double *joints,
+                             PmRotationMatrix *rot,
+                             const KINEMATICS_FORWARD_FLAGS *fflags)
+{
+    (void)joints;
+    (void)fflags;
+    // both rotaries carry the work, so the tool never turns in the machine
+    *rot = TOOL_FRAME_SPINDLE;
+    return 0;
+} // xyzbcKinematicsToolFrame()
diff --git a/src/emc/kinematics/xyzac-trt-kins.c b/src/emc/kinematics/xyzac-trt-kins.c
index 47655ec0f14..504f177e9ee 100644
--- a/src/emc/kinematics/xyzac-trt-kins.c
+++ b/src/emc/kinematics/xyzac-trt-kins.c
@@ -38,11 +38,17 @@ int switchkinsSetup(kparms* kp,
         *kset1 = trtKinematicsSetup; // trt: xyzac,xyzbc
         *kfwd1 = xyzacKinematicsForward;
         *kinv1 = xyzacKinematicsInverse;
+        switchkinsRegisterFrames(1, xyzacKinematicsWorkFrame,
+                                 xyzacKinematicsToolFrame,
+                                 &TOOL_FRAME_SPINDLE);
     } else {
         rtapi_print("\n!!! switchkins-type 0 is %s\n",kp->kinsname);
         *kset0 = trtKinematicsSetup; // trt: xyzac,xyzbc
         *kfwd0 = xyzacKinematicsForward;
         *kinv0 = xyzacKinematicsInverse;
+        switchkinsRegisterFrames(0, xyzacKinematicsWorkFrame,
+                                 xyzacKinematicsToolFrame,
+                                 &TOOL_FRAME_SPINDLE);
 
         *kset1 = identityKinematicsSetup;
         *kfwd1 = identityKinematicsForward;
diff --git a/src/emc/kinematics/xyzbc-trt-kins.c b/src/emc/kinematics/xyzbc-trt-kins.c
index aa1289baf28..6915099c832 100644
--- a/src/emc/kinematics/xyzbc-trt-kins.c
+++ b/src/emc/kinematics/xyzbc-trt-kins.c
@@ -38,11 +38,17 @@ int switchkinsSetup(kparms* kp,
         *kset1 = trtKinematicsSetup; // trt: xyzac,xyzbc
         *kfwd1 = xyzbcKinematicsForward;
         *kinv1 = xyzbcKinematicsInverse;
+        switchkinsRegisterFrames(1, xyzbcKinematicsWorkFrame,
+                                 xyzbcKinematicsToolFrame,
+                                 &TOOL_FRAME_SPINDLE);
     } else {
         rtapi_print("\n!!! switchkins-type 0 is %s\n",kp->kinsname);
         *kset0 = trtKinematicsSetup; // trt: xyzac,xyzbc
         *kfwd0 = xyzbcKinematicsForward;
         *kinv0 = xyzbcKinematicsInverse;
+        switchkinsRegisterFrames(0, xyzbcKinematicsWorkFrame,
+                                 xyzbcKinematicsToolFrame,
+                                 &TOOL_FRAME_SPINDLE);
 
         *kset1 = identityKinematicsSetup;
         *kfwd1 = identityKinematicsForward;

From 33342c1502eb3763db7421d367ce31dfc87c140b Mon Sep 17 00:00:00 2001
From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com>
Date: Sun, 23 Aug 2026 15:29:33 +1000
Subject: [PATCH 04/10] trivkins: supply the work and tool frames

trivkins does not build on switchkins.c, so it does not pick up the identity
frames the way a switchkins identity type does.  Hand them through, since it
is the kinematics most machines run and a caller that has to special-case the
commonest module has not gained much.
---
 src/emc/kinematics/trivkins.c | 16 ++++++++++++++++
 1 file changed, 16 insertions(+)

diff --git a/src/emc/kinematics/trivkins.c b/src/emc/kinematics/trivkins.c
index 3ea56b49aa8..4b3685dc6d6 100644
--- a/src/emc/kinematics/trivkins.c
+++ b/src/emc/kinematics/trivkins.c
@@ -38,6 +38,20 @@ int kinematicsInverse(const EmcPose * pos,
     return identityKinematicsInverse(pos, joints, iflags, fflags);
 }
 
+int kinematicsToolFrame(const double *joints,
+                        PmRotationMatrix *rot,
+                        const KINEMATICS_FORWARD_FLAGS *fflags)
+{
+    return identityKinematicsToolFrame(joints, rot, fflags);
+}
+
+int kinematicsWorkFrame(const double *joints,
+                        PmRotationMatrix *rot,
+                        const KINEMATICS_FORWARD_FLAGS *fflags)
+{
+    return identityKinematicsWorkFrame(joints, rot, fflags);
+}
+
 static KINEMATICS_TYPE ktype = -1;
 
 KINEMATICS_TYPE kinematicsType()
@@ -56,6 +70,8 @@ KINS_NOT_SWITCHABLE
 EXPORT_SYMBOL(kinematicsType);
 EXPORT_SYMBOL(kinematicsForward);
 EXPORT_SYMBOL(kinematicsInverse);
+EXPORT_SYMBOL(kinematicsToolFrame);
+EXPORT_SYMBOL(kinematicsWorkFrame);
 MODULE_LICENSE("GPL");
 
 static int comp_id;

From 418c91a4b1b48064d98cd4dacb2ce97fa69c485a Mon Sep 17 00:00:00 2001
From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com>
Date: Sun, 23 Aug 2026 15:29:34 +1000
Subject: [PATCH 05/10] xyzacb_trsrn, xyzbca_trsrn: supply the work and tool
 frames

These are the machines the split is for: a rotary carries the work and two
more carry the tool, so neither frame is the identity and neither can be
recovered from their product.

The work frame is the table rotation, written in machine coordinates.  The
tool frame is the primary rotation about z times the nutating secondary,
written as two matrices and multiplied rather than expanded, so it can be read
against the matrices in the config's remap_funcs_twp.py.

Identity kinematics leaves both square with the machine, and so does tool
kinematics: there the world axes are the tool axes by construction, which is
what makes a G1 Z move run along the tool, so there is no machine-relative
frame to report.

Checked against the forward transform of each module at 27 poses.  The
coefficients the forward applies to a displacement of the linear joints are
the transpose of the work frame alone, as they should be, since turning the
head does not move the tool tip when a linear joint moves.  The tool axis,
recovered separately as the direction the tip retreats along when the tool
gets longer, matches the third column of transpose(work) * tool.  For tool
kinematics the same coefficients come out as the transpose of the whole chain
including the virtual rotation, which is the statement that the world frame is
the tool frame.  Agreement is to 3e-9 throughout.
---
 src/hal/components/xyzacb_trsrn.comp | 84 ++++++++++++++++++++++++++++
 src/hal/components/xyzbca_trsrn.comp | 84 ++++++++++++++++++++++++++++
 2 files changed, 168 insertions(+)

diff --git a/src/hal/components/xyzacb_trsrn.comp b/src/hal/components/xyzacb_trsrn.comp
index dfbe4466ace..098af8a7aa2 100644
--- a/src/hal/components/xyzacb_trsrn.comp
+++ b/src/hal/components/xyzacb_trsrn.comp
@@ -90,6 +90,8 @@ EXPORT_SYMBOL(kinematicsSwitchable);
 EXPORT_SYMBOL(kinematicsSwitch);
 EXPORT_SYMBOL(kinematicsInverse);
 EXPORT_SYMBOL(kinematicsForward);
+EXPORT_SYMBOL(kinematicsToolFrame);
+EXPORT_SYMBOL(kinematicsWorkFrame);
 
 static rtapi_u32 switchkins_type;
 
@@ -304,6 +306,88 @@ int kinematicsForward(const double *j,
     return 0;
 } // kinematicsForward()
 
+// These modules do not link kins_util.c, so they cannot reach the shared
+// TOOL_FRAME_SPINDLE: a kernel module has to resolve its own symbols.
+static void frame_square_with_machine(PmRotationMatrix *rot)
+{
+    rot->x.x = 1; rot->y.x = 0; rot->z.x = 0;
+    rot->x.y = 0; rot->y.y = 1; rot->z.y = 0;
+    rot->x.z = 0; rot->y.z = 0; rot->z.z = 1;
+}
+
+int kinematicsToolFrame(const double *j,
+                        PmRotationMatrix *rot,
+                        const KINEMATICS_FORWARD_FLAGS *fflags)
+{
+    (void)fflags;
+    double       nu = hal_get_real(haldata->nut_angle); // degrees
+    double       Sv = sin(nu*TO_RAD);
+    double       Cv = cos(nu*TO_RAD);
+    double       Ss = sin(j[4]*TO_RAD);
+    double       Cs = cos(j[4]*TO_RAD);
+    double       Sp = sin(j[5]*TO_RAD);
+    double       Cp = cos(j[5]*TO_RAD);
+    double        r = Cs + Sv*Sv*(1-Cs);
+    double        s = Cs + Cv*Cv*(1-Cs);
+    double        t = Sv*Cv*(1-Cs);
+    int a, b, k;
+
+    // identity kinematics, and tool kinematics where the world axes are the
+    // tool axes by construction, both leave the tool square with the machine
+    if (switchkins_type != 1) {
+        frame_square_with_machine(rot);
+        return 0;
+    }
+
+    // the primary joint turns the head about z
+    const double Rp[3][3] = {{Cp, -Sp, 0}, {Sp, Cp, 0}, {0, 0, 1}};
+
+    // the nutating secondary joint
+    const double Rs[3][3] = {{Cs, -Cv*Ss, Sv*Ss},
+                             {Cv*Ss, r, t},
+                             {-Sv*Ss, t, s}};
+
+    double M[3][3];
+    for (a = 0; a < 3; a++) {
+        for (b = 0; b < 3; b++) {
+            M[a][b] = 0;
+            for (k = 0; k < 3; k++) { M[a][b] += Rp[a][k] * Rs[k][b]; }
+        }
+    }
+
+    rot->x.x = M[0][0]; rot->y.x = M[0][1]; rot->z.x = M[0][2];
+    rot->x.y = M[1][0]; rot->y.y = M[1][1]; rot->z.y = M[1][2];
+    rot->x.z = M[2][0]; rot->y.z = M[2][1]; rot->z.z = M[2][2];
+
+    return 0;
+} // kinematicsToolFrame()
+
+int kinematicsWorkFrame(const double *j,
+                        PmRotationMatrix *rot,
+                        const KINEMATICS_FORWARD_FLAGS *fflags)
+{
+    (void)fflags;
+    double       Sw = sin(j[3]*TO_RAD);
+    double       Cw = cos(j[3]*TO_RAD);
+
+    // in tool kinematics the world axes are the tool axes, so the work is not
+    // being reported against the machine and there is nothing to turn
+    if (switchkins_type != 1) {
+        frame_square_with_machine(rot);
+        return 0;
+    }
+
+    // the A joint carries the work: its frame in machine coordinates
+    // is a rotation about x by the joint value
+    const double W[3][3] = {{1, 0, 0}, {0, Cw, Sw}, {0, -Sw, Cw}};
+
+    rot->x.x = W[0][0]; rot->y.x = W[0][1]; rot->z.x = W[0][2];
+    rot->x.y = W[1][0]; rot->y.y = W[1][1]; rot->z.y = W[1][2];
+    rot->x.z = W[2][0]; rot->y.z = W[2][1]; rot->z.z = W[2][2];
+
+    return 0;
+} // kinematicsWorkFrame()
+
 int kinematicsInverse(const EmcPose * pos,
                       double *j,
                       const KINEMATICS_INVERSE_FLAGS * iflags,
diff --git a/src/hal/components/xyzbca_trsrn.comp b/src/hal/components/xyzbca_trsrn.comp
index b8f451c17f9..12e40344125 100644
--- a/src/hal/components/xyzbca_trsrn.comp
+++ b/src/hal/components/xyzbca_trsrn.comp
@@ -90,6 +90,8 @@ EXPORT_SYMBOL(kinematicsSwitchable);
 EXPORT_SYMBOL(kinematicsSwitch);
 EXPORT_SYMBOL(kinematicsInverse);
 EXPORT_SYMBOL(kinematicsForward);
+EXPORT_SYMBOL(kinematicsToolFrame);
+EXPORT_SYMBOL(kinematicsWorkFrame);
 
 static rtapi_u32 switchkins_type;
 
@@ -309,6 +311,88 @@ int kinematicsForward(const double *j,
     return 0;
 } // kinematicsForward()
 
+// These modules do not link kins_util.c, so they cannot reach the shared
+// TOOL_FRAME_SPINDLE: a kernel module has to resolve its own symbols.
+static void frame_square_with_machine(PmRotationMatrix *rot)
+{
+    rot->x.x = 1; rot->y.x = 0; rot->z.x = 0;
+    rot->x.y = 0; rot->y.y = 1; rot->z.y = 0;
+    rot->x.z = 0; rot->y.z = 0; rot->z.z = 1;
+}
+
+int kinematicsToolFrame(const double *j,
+                        PmRotationMatrix *rot,
+                        const KINEMATICS_FORWARD_FLAGS *fflags)
+{
+    (void)fflags;
+    double       nu = hal_get_real(haldata->nut_angle); // degrees
+    double       Sv = sin(nu*TO_RAD);
+    double       Cv = cos(nu*TO_RAD);
+    double       Ss = sin(j[3]*TO_RAD);
+    double       Cs = cos(j[3]*TO_RAD);
+    double       Sp = sin(j[5]*TO_RAD);
+    double       Cp = cos(j[5]*TO_RAD);
+    double        r = Cs + Sv*Sv*(1-Cs);
+    double        s = Cs + Cv*Cv*(1-Cs);
+    double        t = Sv*Cv*(1-Cs);
+    int a, b, k;
+
+    // identity kinematics, and tool kinematics where the world axes are the
+    // tool axes by construction, both leave the tool square with the machine
+    if (switchkins_type != 1) {
+        frame_square_with_machine(rot);
+        return 0;
+    }
+
+    // the primary joint turns the head about z
+    const double Rp[3][3] = {{Cp, -Sp, 0}, {Sp, Cp, 0}, {0, 0, 1}};
+
+    // the nutating secondary joint
+    const double Rs[3][3] = {{r, -Cv*Ss, t},
+                             {Cv*Ss, Cs, -Sv*Ss},
+                             {t, Sv*Ss, s}};
+
+    double M[3][3];
+    for (a = 0; a < 3; a++) {
+        for (b = 0; b < 3; b++) {
+            M[a][b] = 0;
+            for (k = 0; k < 3; k++) { M[a][b] += Rp[a][k] * Rs[k][b]; }
+        }
+    }
+
+    rot->x.x = M[0][0]; rot->y.x = M[0][1]; rot->z.x = M[0][2];
+    rot->x.y = M[1][0]; rot->y.y = M[1][1]; rot->z.y = M[1][2];
+    rot->x.z = M[2][0]; rot->y.z = M[2][1]; rot->z.z = M[2][2];
+
+    return 0;
+} // kinematicsToolFrame()
+
+int kinematicsWorkFrame(const double *j,
+                        PmRotationMatrix *rot,
+                        const KINEMATICS_FORWARD_FLAGS *fflags)
+{
+    (void)fflags;
+    double       Sw = sin(j[4]*TO_RAD);
+    double       Cw = cos(j[4]*TO_RAD);
+
+    // in tool kinematics the world axes are the tool axes, so the work is not
+    // being reported against the machine and there is nothing to turn
+    if (switchkins_type != 1) {
+        frame_square_with_machine(rot);
+        return 0;
+    }
+
+    // the B joint carries the work: its frame in machine coordinates
+    // is a rotation about y by the joint value
+    const double W[3][3] = {{Cw, 0, -Sw}, {0, 1, 0}, {Sw, 0, Cw}};
+
+    rot->x.x = W[0][0]; rot->y.x = W[0][1]; rot->z.x = W[0][2];
+    rot->x.y = W[1][0]; rot->y.y = W[1][1]; rot->z.y = W[1][2];
+    rot->x.z = W[2][0]; rot->y.z = W[2][1]; rot->z.z = W[2][2];
+
+    return 0;
+} // kinematicsWorkFrame()
+
 int kinematicsInverse(const EmcPose * pos,
                       double *j,
                       const KINEMATICS_INVERSE_FLAGS * iflags,

From 362ea45e5e031beeb18e55e2556e49ace9be1ef4 Mon Sep 17 00:00:00 2001
From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com>
Date: Sun, 23 Aug 2026 17:09:31 +1000
Subject: [PATCH 06/10] pumakins: supply the work and tool frames

The arm carries the tool and nothing carries the work, so the work frame is
the identity and this is the first module whose own tool maths is not in the
convention.

pumakins builds the ISO 9787 mechanical interface frame, whose z points
perpendicularly away from the flange, and it relies on that: it reaches the
tool tip by adding PUMA_D6 along the third column.  So it answers in its own
frame and declares TOOL_FRAME_FLANGE, and switchkins turns it into the
convention.  Nothing in the module itself flips a sign.

Lift the rotation out of the forward kinematics into pumaFlangeRotation()
rather than writing it twice, which is the whole point: a second copy of a
machine's geometry that has to be kept in step by hand is the thing this work
exists to remove.  The block moves verbatim and the forward kinematics loses
the locals that went with it.

At every joint zero the module's own frame is diag(1, -1, -1), a half turn
about x, so after the declared half turn it reports the identity: tool axis
[0, 0, 1], tool x [1, 0, 0].  A puma at zero and a vertical mill at zero give
the same answer, which is right, because both have the tool pointing down at
the work.
---
 src/emc/kinematics/pumakins.c | 69 +++++++++++++++++++++++++++++++----
 1 file changed, 61 insertions(+), 8 deletions(-)

diff --git a/src/emc/kinematics/pumakins.c b/src/emc/kinematics/pumakins.c
index f055e73a502..5aaa4066193 100644
--- a/src/emc/kinematics/pumakins.c
+++ b/src/emc/kinematics/pumakins.c
@@ -29,21 +29,17 @@ struct haldata {
     hal_real_t a2, a3, d3, d4, d6;
 } *haldata = NULL;
 
-static int pumaKinematicsForward(const double * joint,
-                                 EmcPose * world,
-                                 const KINEMATICS_FORWARD_FLAGS * fflags,
-                                 KINEMATICS_INVERSE_FLAGS * iflags)
+/* The flange orientation for a joint set: the ISO 9787 mechanical interface
+   frame, whose z points out of the interface towards the work.  Shared by the
+   forward kinematics and the tool frame so the two cannot drift apart. */
+static void pumaFlangeRotation(const double * joint, PmRotationMatrix * rot)
 {
-   (void)fflags;
    double s1, s2, s3, s4, s5, s6;
    double c1, c2, c3, c4, c5, c6;
    double s23;
    double c23;
    double t1, t2, t3, t4, t5;
-   double sumSq, k;
    PmHomogeneous hom;
-   PmPose worldPose;
-   PmRpy rpy;
 
    /* Calculate sin of joints for future use */
    s1 = sin(joint[0]*PM_PI/180);
@@ -99,6 +95,37 @@ static int pumaKinematicsForward(const double * joint,
    hom.rot.z.y = -s1 * t1 + c1 * s4 * s5;
    hom.rot.z.z = s23 * c4 * s5 - c23 * c5;
 
+   *rot = hom.rot;
+} // pumaFlangeRotation()
+
+static int pumaKinematicsForward(const double * joint,
+                                 EmcPose * world,
+                                 const KINEMATICS_FORWARD_FLAGS * fflags,
+                                 KINEMATICS_INVERSE_FLAGS * iflags)
+{
+   (void)fflags;
+   double s1, s2, s3;
+   double c1, c2, c3;
+   double s23;
+   double c23;
+   double t1, t2;
+   double sumSq, k;
+   PmHomogeneous hom;
+   PmPose worldPose;
+   PmRpy rpy;
+
+   pumaFlangeRotation(joint, &hom.rot);
+
+   /* Calculate sin and cos of joints for the position vector */
+   s1 = sin(joint[0]*PM_PI/180);
+   s2 = sin(joint[1]*PM_PI/180);
+   s3 = sin(joint[2]*PM_PI/180);
+   c1 = cos(joint[0]*PM_PI/180);
+   c2 = cos(joint[1]*PM_PI/180);
+   c3 = cos(joint[2]*PM_PI/180);
+   s23 = c2 * s3 + s2 * c3;
+   c23 = c2 * c3 - s2 * s3;
+
    rtapi_real PUMA_A2 = hal_get_real(haldata->a2);
    rtapi_real PUMA_A3 = hal_get_real(haldata->a3);
    rtapi_real PUMA_D3 = hal_get_real(haldata->d3);
@@ -174,6 +201,27 @@ static int pumaKinematicsForward(const double * joint,
    return 0;
 }
 
+static int pumaKinematicsToolFrame(const double * joint,
+                                   PmRotationMatrix * rot,
+                                   const KINEMATICS_FORWARD_FLAGS * fflags)
+{
+   (void)fflags;
+   // answers in the flange frame; switchkins applies the declared half turn
+   pumaFlangeRotation(joint, rot);
+   return 0;
+} // pumaKinematicsToolFrame()
+
+static int pumaKinematicsWorkFrame(const double * joint,
+                                   PmRotationMatrix * rot,
+                                   const KINEMATICS_FORWARD_FLAGS * fflags)
+{
+   (void)joint;
+   (void)fflags;
+   // the arm carries the tool and nothing carries the work
+   *rot = TOOL_FRAME_SPINDLE;
+   return 0;
+} // pumaKinematicsWorkFrame()
+
 static int pumaKinematicsInverse(const EmcPose * world,
                                  double * joint,
                                  const KINEMATICS_INVERSE_FLAGS * iflags,
@@ -371,6 +419,11 @@ int switchkinsSetup(kparms* kp,
     *kset0 = pumaKinematicsSetup;
     *kfwd0 = pumaKinematicsForward;
     *kinv0 = pumaKinematicsInverse;
+    // the maths is the ISO 9787 flange frame, so the tool axis it produces
+    // runs holder towards tip, the opposite of the convention
+    switchkinsRegisterFrames(0, pumaKinematicsWorkFrame,
+                             pumaKinematicsToolFrame,
+                             &TOOL_FRAME_FLANGE);
 
     *kset1 = identityKinematicsSetup;
     *kfwd1 = identityKinematicsForward;

From 46ef1dda5bba196b10ce260020d60ab5f3ccfa1e Mon Sep 17 00:00:00 2001
From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com>
Date: Sun, 23 Aug 2026 17:35:00 +1000
Subject: [PATCH 07/10] tests: cover the frame helpers

Pins down the two properties the chapter is about.

Relating one tool axis convention to the other is a rotation, not a change of
sign: a negated third column is refused because it is a reflection, the
declared rotation post-multiplies so it is read in the module's own frame, the
half turn keeps tool x and reverses the other two, and applying it twice is
the identity.  Also checks the pumakins zero pose, whose own frame is a half
turn about x, ends up as the identity after the declaration it makes.

And composing the two reported frames means transposing the work one:
toolFrameInWork() leaves the tool alone when nothing turns the work, gives a
proper rotation, and composes a work frame with its own inverse back to the
identity.

Verified by mutation rather than by passing: making TOOL_FRAME_FLANGE negate
only the tool axis fails nine checks, reversing the multiplication order fails
four, and dropping the transpose in toolFrameInWork() fails two.

Built the way tests/blendmath builds, compiling the source under test directly
with the rest garbage-collected by the linker.
---
 tests/tool-frame/checkresult       |   2 +
 tests/tool-frame/skip              |   4 +
 tests/tool-frame/test.sh           |  17 ++++
 tests/tool-frame/test_tool_frame.c | 158 +++++++++++++++++++++++++++++
 4 files changed, 181 insertions(+)
 create mode 100755 tests/tool-frame/checkresult
 create mode 100755 tests/tool-frame/skip
 create mode 100755 tests/tool-frame/test.sh
 create mode 100644 tests/tool-frame/test_tool_frame.c

diff --git a/tests/tool-frame/checkresult b/tests/tool-frame/checkresult
new file mode 100755
index 00000000000..722c4557b62
--- /dev/null
+++ b/tests/tool-frame/checkresult
@@ -0,0 +1,2 @@
+#!/bin/sh
+grep -q "all tool frame checks passed" "$1" && ! grep -q "FAIL" "$1"
diff --git a/tests/tool-frame/skip b/tests/tool-frame/skip
new file mode 100755
index 00000000000..ee99160224a
--- /dev/null
+++ b/tests/tool-frame/skip
@@ -0,0 +1,4 @@
+#!/bin/sh
+# This test compiles kins_util.c from the source tree, which is only
+# available in run-in-place builds. Skip when testing installed packages.
+[ -z "$SYSTEM_BUILD" ]
diff --git a/tests/tool-frame/test.sh b/tests/tool-frame/test.sh
new file mode 100755
index 00000000000..2dae535797b
--- /dev/null
+++ b/tests/tool-frame/test.sh
@@ -0,0 +1,17 @@
+#!/bin/bash
+set -e
+
+# RIP layout: $HEADERS is $TOPDIR/include
+TOPDIR=$(dirname "$HEADERS")
+
+# kins_util.c holds the shared kinematics helpers.  Only the tool frame ones
+# are exercised here, so build with function sections and let the linker drop
+# the rest rather than dragging in the HAL pin setup the others need.
+gcc -O2 -Wall -ffunction-sections -fdata-sections -DULAPI \
+    -I"$HEADERS" -I"$TOPDIR/src" -I"$TOPDIR/src/emc" \
+    -o test_tool_frame test_tool_frame.c "$TOPDIR/src/emc/kinematics/kins_util.c" \
+    -L"$LIBDIR" -Wl,-rpath,"$LIBDIR" -Wl,--gc-sections \
+    -lposemath -llinuxcnchal -lm
+
+./test_tool_frame
+rm -f test_tool_frame
diff --git a/tests/tool-frame/test_tool_frame.c b/tests/tool-frame/test_tool_frame.c
new file mode 100644
index 00000000000..dde817ae554
--- /dev/null
+++ b/tests/tool-frame/test_tool_frame.c
@@ -0,0 +1,158 @@
+/* Unit tests for the tool frame helpers in kins_util.c.
+ *
+ * The property worth pinning down is that relating one tool axis convention
+ * to the other is a rotation and not a change of sign: negating the third
+ * column on its own leaves a reflection, which is not a frame any machine can
+ * hold, and it silently loses tool x as well.
+ */
+#include <stdio.h>
+#include <math.h>
+#include <string.h>
+
+#include "emcpos.h"
+#include "kinematics.h"
+
+static int failures;
+
+static void check(int ok, const char *what)
+{
+    if (!ok) { printf("FAIL: %s\n", what); failures++; }
+}
+
+static PmRotationMatrix mat(double xx, double yx, double zx,
+                            double xy, double yy, double zy,
+                            double xz, double yz, double zz)
+{
+    /* written out in the layout it prints in, so the literal below reads as
+       the matrix it is: columns are tool x, tool y, tool axis */
+    PmRotationMatrix m;
+    m.x.x = xx; m.y.x = yx; m.z.x = zx;
+    m.x.y = xy; m.y.y = yy; m.z.y = zy;
+    m.x.z = xz; m.y.z = yz; m.z.z = zz;
+    return m;
+}
+
+static int same(const PmCartesian *a, double x, double y, double z)
+{
+    return fabs(a->x - x) < 1e-12
+        && fabs(a->y - y) < 1e-12
+        && fabs(a->z - z) < 1e-12;
+}
+
+/* rotation by 40 degrees about z then 25 about y, an arbitrary proper
+   rotation with no zeros to hide a transposition */
+static PmRotationMatrix arbitrary(void)
+{
+    const double a = 40.0 * M_PI / 180.0, b = 25.0 * M_PI / 180.0;
+    const double ca = cos(a), sa = sin(a), cb = cos(b), sb = sin(b);
+    return mat( ca*cb, -sa,  ca*sb,
+                sa*cb,  ca,  sa*sb,
+                  -sb,   0,     cb);
+}
+
+int main(void)
+{
+    PmRotationMatrix m, r;
+
+    /* the supplied constants are usable as declarations */
+    check(toolFrameIsProper(&TOOL_FRAME_SPINDLE), "TOOL_FRAME_SPINDLE is proper");
+    check(toolFrameIsProper(&TOOL_FRAME_FLANGE),  "TOOL_FRAME_FLANGE is proper");
+
+    /* TOOL_FRAME_FLANGE is a half turn about tool x */
+    check(same(&TOOL_FRAME_FLANGE.x,  1,  0,  0), "flange keeps tool x");
+    check(same(&TOOL_FRAME_FLANGE.y,  0, -1,  0), "flange reverses tool y");
+    check(same(&TOOL_FRAME_FLANGE.z,  0,  0, -1), "flange reverses the tool axis");
+
+    /* the mistake this exists to prevent: negating the tool axis alone is a
+       reflection, and toolFrameIsProper has to reject it */
+    m = TOOL_FRAME_SPINDLE;
+    m.z.x = -m.z.x; m.z.y = -m.z.y; m.z.z = -m.z.z;
+    check(!toolFrameIsProper(&m), "a negated third column is rejected");
+
+    /* and so are the other ways of not being a rotation */
+    m = TOOL_FRAME_SPINDLE; m.x.x = 2.0;
+    check(!toolFrameIsProper(&m), "a scaled column is rejected");
+    m = TOOL_FRAME_SPINDLE; m.y.x = 0.5;
+    check(!toolFrameIsProper(&m), "non-orthogonal columns are rejected");
+
+    /* applying a declared rotation */
+    r = arbitrary();
+    m = r;
+    check(toolFrameApplyNative(&m, &TOOL_FRAME_SPINDLE) == 0, "identity applies");
+    check(memcmp(&m, &r, sizeof m) == 0, "identity changes nothing");
+
+    m = r;
+    check(toolFrameApplyNative(&m, &TOOL_FRAME_FLANGE) == 0, "flange applies");
+    check(toolFrameIsProper(&m), "the result is still a proper rotation");
+    check(same(&m.x, r.x.x, r.x.y, r.x.z), "tool x survives the half turn");
+    check(same(&m.z, -r.z.x, -r.z.y, -r.z.z), "the tool axis is reversed");
+    check(same(&m.y, -r.y.x, -r.y.y, -r.y.z), "tool y is reversed with it");
+
+    /* the half turn is its own inverse */
+    check(toolFrameApplyNative(&m, &TOOL_FRAME_FLANGE) == 0, "flange applies again");
+    check(memcmp(&m, &r, sizeof m) == 0, "twice is the identity");
+
+    /* the declared rotation is in the module's frame, so it post-multiplies.
+       pre-multiplying would give a different answer for a non-commuting pair,
+       which is what this catches. */
+    m = r;
+    toolFrameApplyNative(&m, &TOOL_FRAME_FLANGE);
+    check(fabs(m.y.x - (-r.y.x)) < 1e-12, "post-multiplied, not pre-multiplied");
+
+    /* an improper declaration is refused rather than applied */
+    m = r;
+    r.z.x = -r.z.x; r.z.y = -r.z.y; r.z.z = -r.z.z;   /* reuse r as a bad native */
+    check(toolFrameApplyNative(&m, &r) == -1, "an improper declaration is refused");
+
+    /* pumakins' own frame at every joint zero is a half turn about x, so the
+       declaration it makes turns it into the identity: the same answer a
+       vertical mill gives, which is right, because both point at the work */
+    m = mat(1, 0,  0,
+            0, -1, 0,
+            0, 0, -1);
+    check(toolFrameIsProper(&m), "the puma zero pose frame is proper");
+    check(toolFrameApplyNative(&m, &TOOL_FRAME_FLANGE) == 0, "puma declaration applies");
+    check(same(&m.x, 1, 0, 0) && same(&m.y, 0, 1, 0) && same(&m.z, 0, 0, 1),
+          "a puma at zero reports the same frame as a vertical mill");
+
+    /* the two frames are reported against the machine and composed by the
+       caller; the product is what a tilted work plane wants, and it is the
+       thing that cannot be taken apart again, which is why it is not what
+       the module returns */
+    {
+        PmRotationMatrix work, tool, in_work, back;
+
+        /* nothing turns the work: the tool in work coordinates is the tool */
+        work = TOOL_FRAME_SPINDLE;
+        tool = arbitrary();
+        toolFrameInWork(&work, &tool, &in_work);
+        check(memcmp(&in_work, &tool, sizeof in_work) == 0,
+              "identity work frame leaves the tool frame alone");
+
+        /* nothing turns the tool: the tool in work coordinates is the inverse
+           of the work rotation, so composing it back gives the identity */
+        work = arbitrary();
+        tool = TOOL_FRAME_SPINDLE;
+        toolFrameInWork(&work, &tool, &in_work);
+        check(toolFrameIsProper(&in_work), "the composition is a proper rotation");
+        toolFrameInWork(&in_work, &TOOL_FRAME_SPINDLE, &back);
+        toolFrameInWork(&work, &back, &in_work);
+        check(same(&in_work.x, 1, 0, 0) && same(&in_work.y, 0, 1, 0)
+              && same(&in_work.z, 0, 0, 1),
+              "work composed with its own inverse is the identity");
+
+        /* both turn, which is the case the split exists for: the work frame
+           must be transposed, not just multiplied in */
+        work = arbitrary();
+        tool = TOOL_FRAME_FLANGE;
+        toolFrameInWork(&work, &tool, &in_work);
+        check(toolFrameIsProper(&in_work), "a mixed rotation composes properly");
+        check(fabs(in_work.z.x - (work.x.x*tool.z.x + work.x.y*tool.z.y
+                                + work.x.z*tool.z.z)) < 1e-12,
+              "the work frame is transposed, not applied directly");
+    }
+
+    if (failures) { printf("%d failure(s)\n", failures); return 1; }
+    printf("all tool frame checks passed\n");
+    return 0;
+}

From cffa81d986cd1c480d95fc2979d68a59ca6eba88 Mon Sep 17 00:00:00 2001
From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com>
Date: Mon, 24 Aug 2026 14:18:03 +1000
Subject: [PATCH 08/10] kinematics: answer which joints point the tool
 somewhere

The frames say where the tool points for a set of joint values.  The question
a tilted work plane actually asks is the other way round: which joint values
point it at the orientation the program wants.  Today that is answered outside
the kinematics, in per-machine trigonometry in the TWP remap, once per machine
and once per pair of rotary letters, which is how the copy in the tree came to
check the primary angle against the secondary joint's limits.

kinematicsToolFrameInverse() asks the module instead.  It reports every set of
joint values that reaches the orientation, and nothing else: no joint limits
and no preference between the answers, because the caller knows the limits and
knows whether the operator asked for the shortest move or for one direction of
rotation only.  A module that reports its frames needs to supply nothing, the
generic search in kins_util.c drives them; a module with a closed form
registers it and that is used instead.

The request is a tool axis and optionally a tool x as well, and asking for tool
x does not require a joint that can reach it.  A five axis machine spends both
rotaries on the tool axis and the turn about that axis is not a joint, it is
the virtual rotation, so the answer comes back in two parts: the poses that
reach the axis, and the turn that places tool x, which is zero where a third
orientation joint did the job instead.  The caller writes one path and the kind
of machine becomes a number rather than a branch.  That is what the controls
do, a Heidenhain PLANE VECTOR block carrying the normal and the base vector
together and a Fanuc G68.2 defining where the plane's X points; neither refuses
a program for naming both.

Where a request still leaves the machine free, at a singular pose or on a
machine with a spare orientation joint, one point of the family is reported,
the one nearest the seed, with the number of free directions alongside.
Returning samples of a curve as though they were alternatives would be worse
than saying so.

Three details in the search are not incidental.  The damping is adaptive,
which is what keeps a rank deficient pose from turning finite difference noise
into a step of thousands of degrees.  The Jacobian is central differenced,
because a one sided error is first order in the step and shows up as a
spurious singular value, which is exactly what the rank test must not see.
And the joint unit is discovered by adding a whole turn and asking whether the
frame came back, because every module in the tree takes degrees but the
interface does not say so.

The unit test drives it through a table rotary machine, a nutating head and a
machine with both, and checks the nutating answers against the closed form the
remap uses.
---
 docs/src/motion/kinematics-conventions.adoc |  80 +++
 src/emc/kinematics/kinematics.h             |  76 +++
 src/emc/kinematics/kins_util.c              | 517 ++++++++++++++++++++
 src/emc/kinematics/switchkins.c             |  49 ++
 src/emc/kinematics/switchkins.h             |  14 +
 tests/tool-frame/test_tool_frame.c          | 411 ++++++++++++++++
 6 files changed, 1147 insertions(+)

diff --git a/docs/src/motion/kinematics-conventions.adoc b/docs/src/motion/kinematics-conventions.adoc
index 0520eaeae28..6d30f7ed630 100644
--- a/docs/src/motion/kinematics-conventions.adoc
+++ b/docs/src/motion/kinematics-conventions.adoc
@@ -264,6 +264,11 @@ are tilted work plane handling, tool length compensation along a tilted axis,
 previews and simulation models, and probing routines that have to say which way
 the stylus faces.
 
+The joint values that reach a requested orientation, through
+`kinematicsToolFrameInverse()`, which is the inverse of the pair above and the
+question a tilted work plane asks when it has to orient the machine.
+<<sec:orientation-inverse,The Orientation Inverse>> 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
@@ -276,6 +281,76 @@ None needs state carried between calls, and none needs the module to be running
 in a realtime thread to be useful: the interesting callers, a limit check
 before a move and a preview before a program runs, are not in the servo loop.
 
+[[sec:orientation-inverse]]
+== The Orientation Inverse
+
+Pointing the tool somewhere is the question a tilted work plane asks on every
+orienting move, and the question a program written as tool vectors asks on
+every block. It is the inverse of the tool frame: not where the tool points
+for these joints, but which joints point it there.
+
+It has more than one answer. A five-axis machine reaches a given tool axis two
+ways, one with the secondary rotary positive and one with it negative, and
+which of the two is wanted depends on the joint limits and on what the operator
+asked for, shortest move or one direction of rotation only. So the module
+reports every set of joint values that reaches the orientation and stops there.
+It does not apply the joint limits and it does not rank the answers, because
+neither is geometry: a module that picked for the caller would be picking with
+less to go on than the caller has.
+
+The request is a tool axis, and optionally a tool x as well. The two have to be
+at right angles, being two axes of one frame.
+
+Asking for tool x does not require a joint that can reach it. A five-axis
+machine spends both rotaries on the tool axis, and the turn about that axis is
+not a joint at all: it is the virtual rotation of <<sec:tool-frame,The Tool
+Frame>>. So the answer comes back in two parts. Where the joints can place tool
+x, on a machine with a third orientation joint, they do, and the reported turn
+is zero. Where they cannot, the joints reach the axis and the reported turn
+finishes the job. The caller writes one path either way, and which kind of
+machine it has is a number that happens to be zero rather than a branch.
+
+This is what the controls do. A Heidenhain `PLANE VECTOR` block carries the
+normal and the base vector together, the normal reached by the rotaries and the
+base vector applied as a rotation of the coordinate system; `SEQ` then picks
+between the rotary solutions. A Fanuc `G68.2` defines the plane including where
+its X points, and `G53.1` moves the rotaries to align the tool axis. Siemens
+draws the same line from the other side: the turn of the tool about itself is
+`THETA`, and it exists only where a third rotary axis does. None of them
+refuses a program for naming both directions on a five-axis machine, and
+neither should this.
+
+Some requests still do not pin the machine down. A five-axis machine asked to
+point the tool along the axis its primary rotary turns about can hold any
+primary angle; a machine with three orientation joints asked only for a tool
+axis has a whole curve of solutions. In both cases the answer is a continuum,
+so one point of it is reported, the one nearest where the machine already is,
+along with the number of directions left free. A caller that hands back a list
+of samples from a curve as though they were alternatives is telling the
+operator something false.
+
+=== What a module has to supply
+
+Nothing, if it already reports its frames. The shared code answers the question
+by searching: it finds which joints move `transpose(work) * tool`, and solves
+for them. That is the whole reason the frames are worth reporting. A module
+that supplies them gets the inverse without deriving anything.
+
+A module with a closed form registers it and that is used instead. It is faster
+than a search and it knows its own degenerate poses without having to discover
+them. The nutating heads are the case in point: for those,
+
+  cos(secondary) = (Kzz - Cv^2) / (1 - Cv^2)
+
+with `Cv = cos(v)` for a nutation angle `v` and `Kzz` the z component of the
+requested tool axis, and the primary follows from the other two components.
+Both roots of the arc cosine are solutions, which is where the pair of answers
+comes from.
+
+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:writing-a-module]]
 == Writing a Module
 
@@ -300,6 +375,11 @@ Tool X::
   Derive the value from the module's own rotation matrices, and write those
   matrices down in the module.
 
+Orientation inverse::
+  A module that reports its frames gets it from the shared search and needs to
+  do nothing. Register a closed form only where one exists, and where it does,
+  say which poses it treats as degenerate.
+
 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 8efeb26c640..e6c459c3179 100644
--- a/src/emc/kinematics/kinematics.h
+++ b/src/emc/kinematics/kinematics.h
@@ -239,6 +239,82 @@ extern int toolFrameInWork(const PmRotationMatrix *work,
    actually hold.  Used to check a declared rotation once, at load. */
 extern int toolFrameIsProper(const PmRotationMatrix *m);
 
+/* The inverse of kinematicsToolFrame(): which joint values point the tool
+   along a requested direction.  This is the question a tilted work plane asks
+   when it has to orient the machine, and the one vector format G-code asks
+   for every block.
+
+   axis_in_work is the wanted tool axis and x_in_work the wanted tool x, both
+   in workpiece coordinates, both in the sense of transpose(work) * tool.
+   x_in_work may be NULL, which leaves the spin about the tool free.  Where it
+   is given, the two have to be at right angles, being two axes of one frame.
+
+   Asking for tool x does not require a joint that can reach it.  A five axis
+   machine spends both rotaries on the tool axis, and the turn about that axis
+   is not a joint at all: it is the virtual rotation, the pre-rot pin on the
+   in-tree components.  So where the joints can place tool x, on a machine with
+   a third orientation joint, they do and tool_spin comes back zero; where they
+   cannot, the joints reach the axis and tool_spin carries the turn about it
+   that finishes the job, in radians, in the sense of the virtual rotation.
+   Either way the caller writes one path, and which kind of machine it has is a
+   number that happens to be zero rather than a branch.  tool_spin may be NULL,
+   but then a request for tool x that the joints cannot reach has nowhere to
+   put its answer and reports no solutions.
+
+   seed is a full set of joint values, normally where the machine is now.  The
+   joints that do not affect the tool orientation are copied from it, and it
+   breaks the tie where a machine has more orientation joints than the request
+   constrains.
+
+   solutions receives max_solutions complete sets of joint values, one after
+   another, each num_joints long.  free_directions, if not NULL, receives one
+   entry per solution: 0 where the joints are pinned down, and n where the
+   solution is one point of an n dimensional family, which happens at a
+   singular pose and on a machine with a spare orientation joint.  In that case
+   one representative is reported, the one nearest the seed, because the answer
+   is a continuum and a list of samples from it would be arbitrary.
+
+   Joint limits are not applied and no solution is preferred over another: the
+   module answers what the geometry permits, and the caller picks by whatever
+   rule it works to, shortest move or positive rotation only or whatever else.
+
+   Returns the number of solutions, 0 if the orientation cannot be reached, or
+   -1 if the module cannot answer.
+
+   This is not a realtime routine.  It searches, and how long it takes depends
+   on the machine and the request. */
+#define TOOL_FRAME_MAX_SOLUTIONS 8
+#define TOOL_FRAME_MAX_FREE      4
+
+extern int kinematicsToolFrameInverse(const PmCartesian *axis_in_work,
+                                      const PmCartesian *x_in_work,
+                                      const double *seed,
+                                      double *solutions,
+                                      int max_solutions,
+                                      int *free_directions,
+                                      double *tool_spin);
+
+/* The generic implementation of the above, driven by a module's own frame
+   functions, so that a module gets it for free once it supplies them.  A
+   module with a closed form registers that instead: it is faster, and it
+   knows its own degenerate poses without having to find them.
+
+   num_joints is the length of seed and of each row of solutions. */
+typedef int (*kinsFrameFunc)(const double *joint,
+                             PmRotationMatrix *rot,
+                             const KINEMATICS_FORWARD_FLAGS *fflags);
+
+extern int toolFrameSolve(kinsFrameFunc work,
+                          kinsFrameFunc tool,
+                          int num_joints,
+                          const PmCartesian *axis_in_work,
+                          const PmCartesian *x_in_work,
+                          const double *seed,
+                          double *solutions,
+                          int max_solutions,
+                          int *free_directions,
+                          double *tool_spin);
+
 extern int kinematicsSwitchable(void);
 extern int kinematicsSwitch(int switchkins_type);
 //NOTE: switchable kinematics may require Interp::Synch
diff --git a/src/emc/kinematics/kins_util.c b/src/emc/kinematics/kins_util.c
index 1576f7fb505..ee9d6593c1d 100644
--- a/src/emc/kinematics/kins_util.c
+++ b/src/emc/kinematics/kins_util.c
@@ -500,3 +500,520 @@ int identityKinematicsToolFrame(const double *joints,
     *rot = TOOL_FRAME_SPINDLE;
     return 0;
 } // identityKinematicsToolFrame()
+
+//----------------------------------------------------------------------
+// toolFrameSolve()
+//
+// The inverse of the tool orientation, built on nothing but a module's own
+// work and tool frame functions, so that supplying those is enough and no
+// module has to hand-derive a formula.
+//
+// The problem is small: the only joints that can turn the tool are rotary
+// ones, there are rarely more than three of them, and the orientation is a
+// function of those joints alone.  So the routine finds which joints move
+// transpose(work) * tool, and solves for them by damped least squares from a
+// spread of starting points, keeping the roots that are distinct.
+//
+// Three things are worth naming because they are what the naive version gets
+// wrong.
+//
+// The damping is adaptive.  At a singular pose the Jacobian loses rank, and a
+// fixed small damping turns the noise in the near-null direction into a step
+// of thousands of degrees.  Raising the damping when a step fails and lowering
+// it when one succeeds is what keeps those poses solvable at all.
+//
+// The Jacobian is taken with central differences.  A one sided difference has
+// an error of the same order as the step, and it appears as a spurious small
+// singular value, which is exactly what the rank test must not see.
+//
+// The joint unit is discovered rather than assumed.  Every module in the tree
+// takes rotary joints in degrees, but the interface does not say so, and the
+// search has to cover exactly one turn.  Adding a whole turn and asking
+// whether the frame came back settles it, and rescaling into a unit where one
+// turn is 2*pi makes the damping and the step limits the same on any module.
+//----------------------------------------------------------------------
+
+#define TFS_MAX_RES     6       // three for the tool axis, three for tool x
+#define TFS_ITERS      60
+#define TFS_FD_STEP     1e-6    // internal radians
+#define TFS_MOVED_TOL   1e-9    // frame difference that counts as movement
+#define TFS_RANK_TOL    1e-4    // a direction worth less than this is free
+#define TFS_SOLVED      1e-18   // sum of squared residuals
+#define TFS_STEP_LIMIT  0.4     // internal radians per iteration
+
+typedef struct {
+    kinsFrameFunc work;
+    kinsFrameFunc tool;
+    int    num_joints;
+    const  double *seed;
+    int    nfree;
+    int    free[TOOL_FRAME_MAX_FREE];
+    double scale[TOOL_FRAME_MAX_FREE];  // joint units per internal radian
+    int    nres;
+    double want[TFS_MAX_RES];
+    double joint[EMCMOT_MAX_JOINTS];    // scratch, rebuilt on every call
+} tfs_ctx;
+
+// transpose(work) * tool at a joint set, as the columns the request names
+static int tfs_frame(tfs_ctx *c, const double *joint, double *axis, double *xdir)
+{
+    KINEMATICS_FORWARD_FLAGS fflags = 0;
+    PmRotationMatrix w, t, m;
+
+    if (c->work(joint, &w, &fflags)) { return -1; }
+    if (c->tool(joint, &t, &fflags)) { return -1; }
+    toolFrameInWork(&w, &t, &m);
+
+    axis[0] = m.z.x; axis[1] = m.z.y; axis[2] = m.z.z;
+    xdir[0] = m.x.x; xdir[1] = m.x.y; xdir[2] = m.x.z;
+    return 0;
+}
+
+// joint values for a point of the internal search space
+static void tfs_joints(tfs_ctx *c, const double *u)
+{
+    int i;
+    for (i = 0; i < c->num_joints; i++) { c->joint[i] = c->seed[i]; }
+    for (i = 0; i < c->nfree; i++) {
+        c->joint[c->free[i]] = u[i] * c->scale[i];
+    }
+}
+
+static int tfs_res(tfs_ctx *c, const double *u, double *r)
+{
+    double axis[3], xdir[3];
+    int i;
+
+    tfs_joints(c, u);
+    if (tfs_frame(c, c->joint, axis, xdir)) { return -1; }
+
+    for (i = 0; i < 3; i++) { r[i] = axis[i] - c->want[i]; }
+    if (c->nres > 3) {
+        for (i = 0; i < 3; i++) { r[3+i] = xdir[i] - c->want[3+i]; }
+    }
+    return 0;
+}
+
+static double tfs_norm2(const double *r, int n)
+{
+    double s = 0;
+    int i;
+    for (i = 0; i < n; i++) { s += r[i]*r[i]; }
+    return s;
+}
+
+static int tfs_jac(tfs_ctx *c, const double *u, double J[TFS_MAX_RES][TOOL_FRAME_MAX_FREE])
+{
+    double up[TOOL_FRAME_MAX_FREE], rp[TFS_MAX_RES], rm[TFS_MAX_RES];
+    int i, k;
+
+    for (k = 0; k < c->nfree; k++) {
+        for (i = 0; i < c->nfree; i++) { up[i] = u[i]; }
+        up[k] = u[k] + TFS_FD_STEP;
+        if (tfs_res(c, up, rp)) { return -1; }
+        up[k] = u[k] - TFS_FD_STEP;
+        if (tfs_res(c, up, rm)) { return -1; }
+        for (i = 0; i < c->nres; i++) {
+            J[i][k] = (rp[i] - rm[i]) / (2*TFS_FD_STEP);
+        }
+    }
+    return 0;
+}
+
+// in place inverse of an n by n matrix by Gauss-Jordan with partial pivoting,
+// n being at most TOOL_FRAME_MAX_FREE
+static int tfs_inv(double A[TOOL_FRAME_MAX_FREE][TOOL_FRAME_MAX_FREE], int n)
+{
+    double aug[TOOL_FRAME_MAX_FREE][2*TOOL_FRAME_MAX_FREE];
+    int i, j, col, piv;
+
+    for (i = 0; i < n; i++) {
+        for (j = 0; j < n; j++) { aug[i][j] = A[i][j]; }
+        for (j = 0; j < n; j++) { aug[i][n+j] = (i == j) ? 1.0 : 0.0; }
+    }
+    for (col = 0; col < n; col++) {
+        piv = col;
+        for (i = col+1; i < n; i++) {
+            if (fabs(aug[i][col]) > fabs(aug[piv][col])) { piv = i; }
+        }
+        if (fabs(aug[piv][col]) < 1e-300) { return -1; }
+        if (piv != col) {
+            for (j = 0; j < 2*n; j++) {
+                double sw = aug[col][j]; aug[col][j] = aug[piv][j]; aug[piv][j] = sw;
+            }
+        }
+        {
+            double d = aug[col][col];
+            for (j = 0; j < 2*n; j++) { aug[col][j] /= d; }
+        }
+        for (i = 0; i < n; i++) {
+            double f = aug[i][col];
+            if (i == col || f == 0.0) { continue; }
+            for (j = 0; j < 2*n; j++) { aug[i][j] -= f*aug[col][j]; }
+        }
+    }
+    for (i = 0; i < n; i++) {
+        for (j = 0; j < n; j++) { A[i][j] = aug[i][n+j]; }
+    }
+    return 0;
+}
+
+// rank by counting pivots, which is all that is needed to say how many
+// directions the request leaves free
+static int tfs_rank(const double J[TFS_MAX_RES][TOOL_FRAME_MAX_FREE], int m, int n)
+{
+    double a[TFS_MAX_RES][TOOL_FRAME_MAX_FREE];
+    double big = 0;
+    int i, j, col, piv, rank = 0;
+
+    for (i = 0; i < m; i++) {
+        for (j = 0; j < n; j++) {
+            a[i][j] = J[i][j];
+            if (fabs(a[i][j]) > big) { big = fabs(a[i][j]); }
+        }
+    }
+    if (big <= 0) { return 0; }
+
+    for (col = 0; col < n && rank < m; col++) {
+        piv = rank;
+        for (i = rank+1; i < m; i++) {
+            if (fabs(a[i][col]) > fabs(a[piv][col])) { piv = i; }
+        }
+        if (fabs(a[piv][col]) < TFS_RANK_TOL*big) { continue; }
+        if (piv != rank) {
+            for (j = 0; j < n; j++) {
+                double sw = a[rank][j]; a[rank][j] = a[piv][j]; a[piv][j] = sw;
+            }
+        }
+        for (i = rank+1; i < m; i++) {
+            double f = a[i][col]/a[rank][col];
+            for (j = 0; j < n; j++) { a[i][j] -= f*a[rank][j]; }
+        }
+        rank++;
+    }
+    return rank;
+}
+
+// damped least squares with adaptive damping.  Returns 1 when the residual is
+// down to the solved threshold, 0 otherwise, and leaves u where it stopped.
+static int tfs_levmar(tfs_ctx *c, double *u)
+{
+    double r[TFS_MAX_RES], r2[TFS_MAX_RES];
+    double J[TFS_MAX_RES][TOOL_FRAME_MAX_FREE];
+    double A[TOOL_FRAME_MAX_FREE][TOOL_FRAME_MAX_FREE];
+    double g[TOOL_FRAME_MAX_FREE], step[TOOL_FRAME_MAX_FREE];
+    double u2[TOOL_FRAME_MAX_FREE];
+    double f, f2, lambda = 1e-3;
+    int i, j, k, it;
+
+    if (tfs_res(c, u, r)) { return 0; }
+    f = tfs_norm2(r, c->nres);
+
+    for (it = 0; it < TFS_ITERS && f > TFS_SOLVED; it++) {
+        double trace = 0, big = 0;
+
+        if (tfs_jac(c, u, J)) { return 0; }
+
+        for (i = 0; i < c->nfree; i++) {
+            for (j = 0; j < c->nfree; j++) {
+                double s = 0;
+                for (k = 0; k < c->nres; k++) { s += J[k][i]*J[k][j]; }
+                A[i][j] = s;
+            }
+            trace += A[i][i];
+            g[i] = 0;
+            for (k = 0; k < c->nres; k++) { g[i] += J[k][i]*r[k]; }
+        }
+        trace = trace/c->nfree + 1e-30;
+
+        for (i = 0; i < c->nfree; i++) { A[i][i] += lambda*trace; }
+        if (tfs_inv(A, c->nfree)) { return 0; }
+
+        for (i = 0; i < c->nfree; i++) {
+            step[i] = 0;
+            for (j = 0; j < c->nfree; j++) { step[i] -= A[i][j]*g[j]; }
+            if (fabs(step[i]) > big) { big = fabs(step[i]); }
+        }
+        if (big > TFS_STEP_LIMIT) {
+            for (i = 0; i < c->nfree; i++) { step[i] *= TFS_STEP_LIMIT/big; }
+        }
+        for (i = 0; i < c->nfree; i++) { u2[i] = u[i] + step[i]; }
+
+        if (tfs_res(c, u2, r2)) { return 0; }
+        f2 = tfs_norm2(r2, c->nres);
+
+        if (f2 < f) {
+            for (i = 0; i < c->nfree; i++) { u[i] = u2[i]; }
+            for (i = 0; i < c->nres; i++) { r[i] = r2[i]; }
+            f = f2;
+            lambda *= 0.3;
+            if (lambda < 1e-12) { lambda = 1e-12; }
+        } else {
+            lambda *= 4.0;
+            if (lambda > 1e12) { break; }
+        }
+    }
+    return f <= TFS_SOLVED;
+}
+
+static double tfs_wrap(double a)
+{
+    while (a >  PM_PI) { a -= 2*PM_PI; }
+    while (a < -PM_PI) { a += 2*PM_PI; }
+    return a;
+}
+
+// which joints turn the tool, and what one turn of each is worth in its own
+// units.  Returns the count, or -1 if a joint moves the tool without having a
+// period, which the search has no way to bound.
+static int tfs_survey(tfs_ctx *c)
+{
+    double base_axis[3], base_x[3], axis[3], xdir[3];
+    static const double candidate[2] = { 360.0, 2*PM_PI };
+    int i, k, n = 0;
+
+    for (i = 0; i < c->num_joints; i++) { c->joint[i] = c->seed[i]; }
+    if (tfs_frame(c, c->joint, base_axis, base_x)) { return -1; }
+
+    for (i = 0; i < c->num_joints; i++) {
+        double moved = 0;
+        int p;
+
+        for (k = 0; k < c->num_joints; k++) { c->joint[k] = c->seed[k]; }
+        c->joint[i] = c->seed[i] + 1e-4;
+        if (tfs_frame(c, c->joint, axis, xdir)) { return -1; }
+        for (k = 0; k < 3; k++) {
+            if (fabs(axis[k] - base_axis[k]) > moved) { moved = fabs(axis[k] - base_axis[k]); }
+            if (fabs(xdir[k] - base_x[k])   > moved) { moved = fabs(xdir[k] - base_x[k]); }
+        }
+        if (moved <= TFS_MOVED_TOL) { continue; }
+
+        if (n >= TOOL_FRAME_MAX_FREE) { return -1; }
+
+        c->scale[n] = 0;
+        for (p = 0; p < 2; p++) {
+            double back = 0;
+            c->joint[i] = c->seed[i] + candidate[p];
+            if (tfs_frame(c, c->joint, axis, xdir)) { return -1; }
+            for (k = 0; k < 3; k++) {
+                if (fabs(axis[k] - base_axis[k]) > back) { back = fabs(axis[k] - base_axis[k]); }
+                if (fabs(xdir[k] - base_x[k])    > back) { back = fabs(xdir[k] - base_x[k]); }
+            }
+            if (back <= TFS_MOVED_TOL) {
+                c->scale[n] = candidate[p]/(2*PM_PI);
+                break;
+            }
+        }
+        if (c->scale[n] == 0) { return -1; }
+
+        c->free[n] = i;
+        n++;
+    }
+    c->nfree = n;
+    return n;
+}
+
+// enumerate the roots for whatever the context currently constrains
+static int tfs_search(tfs_ctx *c,
+                      double *solutions,
+                      int max_solutions,
+                      int *free_directions)
+{
+    double kept[TOOL_FRAME_MAX_SOLUTIONS][TOOL_FRAME_MAX_FREE];
+    double u[TOOL_FRAME_MAX_FREE], useed[TOOL_FRAME_MAX_FREE];
+    double r[TFS_MAX_RES], J[TFS_MAX_RES][TOOL_FRAME_MAX_FREE];
+    int index[TOOL_FRAME_MAX_FREE];
+    int found = 0, per_axis, first = 1, i, k;
+
+    for (i = 0; i < TOOL_FRAME_MAX_FREE; i++) { u[i] = 0; useed[i] = 0; }
+
+    // nothing on this machine turns the tool, so the only candidate is where
+    // the machine already is
+    if (c->nfree == 0) {
+        if (tfs_res(c, u, r)) { return -1; }
+        if (tfs_norm2(r, c->nres) > TFS_SOLVED) { return 0; }
+        for (i = 0; i < c->num_joints; i++) { solutions[i] = c->seed[i]; }
+        if (free_directions) { free_directions[0] = 0; }
+        return 1;
+    }
+
+    for (i = 0; i < c->nfree; i++) {
+        useed[i] = c->seed[c->free[i]] / c->scale[i];
+        index[i] = 0;
+    }
+
+    // Quarter turns of each free joint, starting from where the machine is so
+    // that a machine with a free direction reports the answer nearest its
+    // present pose.  Two per turn already enters every basin on the machines
+    // in the tree, and four is the margin for one that is not: the roots are
+    // few and widely separated, because they come from the two branches of an
+    // arc cosine and not from anything finely structured.
+    per_axis = 4;
+
+    for (;;) {
+        int solved, rank, dup = 0;
+
+        if (first) {
+            for (i = 0; i < c->nfree; i++) { u[i] = useed[i]; }
+        } else {
+            for (i = 0; i < c->nfree; i++) {
+                u[i] = -PM_PI + (2*PM_PI*index[i])/per_axis;
+            }
+        }
+
+        solved = tfs_levmar(c, u);
+        if (solved) {
+            for (i = 0; i < c->nfree; i++) { u[i] = tfs_wrap(u[i]); }
+            if (tfs_res(c, u, r) || tfs_jac(c, u, J)) { return -1; }
+
+            rank = tfs_rank((const double (*)[TOOL_FRAME_MAX_FREE])J,
+                            c->nres, c->nfree);
+            tfs_joints(c, u);
+
+            // a rank deficient root means the request does not pin the machine
+            // down and the answer is a continuum.  Report this one point of it
+            // and say so, rather than returning samples of a curve alongside
+            // roots that mean something else.
+            if (c->nfree - rank > 0) {
+                for (i = 0; i < c->num_joints; i++) { solutions[i] = c->joint[i]; }
+                if (free_directions) { free_directions[0] = c->nfree - rank; }
+                return 1;
+            }
+
+            // Two roots are the same pose if going from one to the other
+            // does not move the tool.  That covers landing on a root already
+            // found, and it also covers the case a distance test would get
+            // wrong: near a singularity the search reaches points a long way
+            // apart in joint values whose frames differ by less than it can
+            // resolve, and those are one answer and not several.
+            for (k = 0; k < found; k++) {
+                double mid[TOOL_FRAME_MAX_FREE] = {0};
+
+                for (i = 0; i < c->nfree; i++) {
+                    mid[i] = kept[k][i] + tfs_wrap(u[i] - kept[k][i])/2;
+                }
+                if (tfs_res(c, mid, r)) { return -1; }
+                if (tfs_norm2(r, c->nres) <= TFS_SOLVED) { dup = 1; break; }
+            }
+
+            if (!dup) {
+                // the dedupe evaluated other points, so rebuild this one
+                tfs_joints(c, u);
+                for (i = 0; i < c->num_joints; i++) {
+                    solutions[found*c->num_joints + i] = c->joint[i];
+                }
+                if (free_directions) { free_directions[found] = 0; }
+                for (i = 0; i < c->nfree; i++) { kept[found][i] = u[i]; }
+                found++;
+                if (found >= max_solutions) { return found; }
+            }
+        }
+
+        if (first) { first = 0; continue; }
+
+        for (i = 0; i < c->nfree; i++) {
+            if (++index[i] < per_axis) { break; }
+            index[i] = 0;
+        }
+        if (i == c->nfree) { break; }
+    }
+
+    return found;
+}
+
+// the turn about the tool axis that carries the tool x this pose achieves onto
+// the one the caller asked for
+static int tfs_spin(tfs_ctx *c, const double *joint,
+                    const PmCartesian *x_in_work, double *spin)
+{
+    KINEMATICS_FORWARD_FLAGS fflags = 0;
+    PmRotationMatrix w, t, m;
+    double along_x, along_y;
+
+    if (c->work(joint, &w, &fflags)) { return -1; }
+    if (c->tool(joint, &t, &fflags)) { return -1; }
+    toolFrameInWork(&w, &t, &m);
+
+    along_x = m.x.x*x_in_work->x + m.x.y*x_in_work->y + m.x.z*x_in_work->z;
+    along_y = m.y.x*x_in_work->x + m.y.y*x_in_work->y + m.y.z*x_in_work->z;
+
+    *spin = atan2(along_y, along_x);
+    return 0;
+}
+
+int toolFrameSolve(kinsFrameFunc work,
+                   kinsFrameFunc tool,
+                   int num_joints,
+                   const PmCartesian *axis_in_work,
+                   const PmCartesian *x_in_work,
+                   const double *seed,
+                   double *solutions,
+                   int max_solutions,
+                   int *free_directions,
+                   double *tool_spin)
+{
+    tfs_ctx c;
+    int found, i;
+
+    if (!work || !tool || !seed || !solutions || !axis_in_work
+        || num_joints <= 0 || num_joints > EMCMOT_MAX_JOINTS
+        || max_solutions <= 0) {
+        return -1;
+    }
+    if (max_solutions > TOOL_FRAME_MAX_SOLUTIONS) {
+        max_solutions = TOOL_FRAME_MAX_SOLUTIONS;
+    }
+
+    c.work = work;
+    c.tool = tool;
+    c.num_joints = num_joints;
+    c.seed = seed;
+    c.nres = x_in_work ? 6 : 3;
+    c.want[0] = axis_in_work->x;
+    c.want[1] = axis_in_work->y;
+    c.want[2] = axis_in_work->z;
+    if (x_in_work) {
+        double square = axis_in_work->x * x_in_work->x
+                      + axis_in_work->y * x_in_work->y
+                      + axis_in_work->z * x_in_work->z;
+
+        // the two vectors are two axes of one frame, so a request where they
+        // are not at right angles is not a frame and cannot be reached by
+        // anything
+        if (fabs(square) > 1e-6) { return -1; }
+
+        c.want[3] = x_in_work->x;
+        c.want[4] = x_in_work->y;
+        c.want[5] = x_in_work->z;
+    }
+
+    if (tfs_survey(&c) < 0) { return -1; }
+
+    found = tfs_search(&c, solutions, max_solutions, free_directions);
+    if (found != 0 || !x_in_work) {
+        if (tool_spin) {
+            for (i = 0; i < (found > 0 ? found : 0); i++) { tool_spin[i] = 0; }
+        }
+        return found;
+    }
+
+    // The joints cannot place tool x, which is the ordinary case: a five axis
+    // machine spends both rotaries reaching the tool axis and the turn about
+    // that axis is not a joint at all.  It is still reachable, as a rotation
+    // of the frame rather than a motion of the machine, so answer with the
+    // poses that reach the axis and the turn that finishes the job.  That is
+    // what a control does with a Heidenhain base vector or a Fanuc G68.2
+    // block, neither of which refuses the program for asking.
+    if (!tool_spin) { return 0; }
+
+    c.nres = 3;
+    found = tfs_search(&c, solutions, max_solutions, free_directions);
+    if (found <= 0) { return found; }
+
+    for (i = 0; i < found; i++) {
+        if (tfs_spin(&c, solutions + i*num_joints, x_in_work, &tool_spin[i])) {
+            return -1;
+        }
+    }
+    return found;
+}
diff --git a/src/emc/kinematics/switchkins.c b/src/emc/kinematics/switchkins.c
index 4f62277a287..7e9838c2527 100644
--- a/src/emc/kinematics/switchkins.c
+++ b/src/emc/kinematics/switchkins.c
@@ -44,6 +44,7 @@ static KF kfwds[SWITCHKINS_MAX_TYPES]   = {NULL};
 static KI kinvs[SWITCHKINS_MAX_TYPES]   = {NULL};
 static KT ktools[SWITCHKINS_MAX_TYPES]  = {NULL};
 static KT kworks[SWITCHKINS_MAX_TYPES]  = {NULL};
+static KTI ktinvs[SWITCHKINS_MAX_TYPES] = {NULL};
 static PmRotationMatrix knative[SWITCHKINS_MAX_TYPES];
 
 // types provided, counted in rtapi_app_main() once they are all in
@@ -248,6 +249,38 @@ int kinematicsWorkFrame(const double *joint,
     return kworks[switchkins_type](joint, rot, fflags);
 } // kinematicsWorkFrame()
 
+int kinematicsToolFrameInverse(const PmCartesian *axis_in_work,
+                               const PmCartesian *x_in_work,
+                               const double *seed,
+                               double *solutions,
+                               int max_solutions,
+                               int *free_directions,
+                               double *tool_spin)
+{
+    if (   switchkins_type < 0
+        || switchkins_type >= kins_count
+        || !ktools[switchkins_type]
+        || !kworks[switchkins_type]) {
+        return -1; // this type does not report its frames, so it cannot answer
+    }
+
+    // a type that derived the answer by hand knows its own degenerate poses
+    // and is faster than a search, so it wins where it exists
+    if (ktinvs[switchkins_type]) {
+        return ktinvs[switchkins_type](axis_in_work, x_in_work, seed,
+                                       solutions, max_solutions,
+                                       free_directions, tool_spin);
+    }
+
+    // the dispatch itself is what the search calls, so the native rotation
+    // and the per-type lookup are already accounted for
+    return toolFrameSolve(kinematicsWorkFrame, kinematicsToolFrame,
+                          kp.max_joints,
+                          axis_in_work, x_in_work, seed,
+                          solutions, max_solutions, free_directions,
+                          tool_spin);
+} // kinematicsToolFrameInverse()
+
 KINEMATICS_TYPE kinematicsType()
 {
     return KINEMATICS_BOTH;
@@ -302,6 +335,20 @@ int switchkinsRegisterFrames(int ktype, KT kwork, KT ktool,
     return 0;
 } // switchkinsRegisterFrames()
 
+int switchkinsRegisterToolFrameInverse(int ktype, KTI kinv)
+{
+    if (ktype < 0 || ktype >= SWITCHKINS_MAX_TYPES) {
+        rtapi_print_msg(RTAPI_MSG_ERR,
+                        "switchkinsRegisterToolFrameInverse: BAD"
+                        " switchkins_type <%d> (must be 0..%d)\n",
+                        ktype, SWITCHKINS_MAX_TYPES - 1);
+        register_error = 1;
+        return -1;
+    }
+    ktinvs[ktype] = kinv;
+    return 0;
+} // switchkinsRegisterToolFrameInverse()
+
 //*********************************************************************
 static char *coordinates;
 RTAPI_MP_STRING(coordinates, "Axes-to-joints-ordering");
@@ -315,8 +362,10 @@ EXPORT_SYMBOL(kinematicsForward);
 EXPORT_SYMBOL(kinematicsInverse);
 EXPORT_SYMBOL(kinematicsToolFrame);
 EXPORT_SYMBOL(kinematicsWorkFrame);
+EXPORT_SYMBOL(kinematicsToolFrameInverse);
 EXPORT_SYMBOL(switchkinsRegister);
 EXPORT_SYMBOL(switchkinsRegisterFrames);
+EXPORT_SYMBOL(switchkinsRegisterToolFrameInverse);
 MODULE_LICENSE("GPL");
 
 static int    comp_id;
diff --git a/src/emc/kinematics/switchkins.h b/src/emc/kinematics/switchkins.h
index 7815fe2ab39..e5cffd94853 100644
--- a/src/emc/kinematics/switchkins.h
+++ b/src/emc/kinematics/switchkins.h
@@ -52,4 +52,18 @@ extern int switchkinsRegister(int ktype, KS kset, KF kfwd, KI kinv);
 // dispatch.
 extern int switchkinsRegisterFrames(int ktype, KT kwork, KT ktool,
                                     const PmRotationMatrix *native);
+
+// KinematicsTOOLFRAMEINVERSE function (optional, see kinematics.h)
+typedef int (*KTI)(const PmCartesian *axis_in_work,
+                   const PmCartesian *x_in_work,
+                   const double *seed,
+                   double *solutions,
+                   int max_solutions,
+                   int *free_directions,
+                   double *tool_spin);
+
+// called from switchkinsSetup() only by a type that has a closed form for the
+// tool orientation inverse.  A type that does not gets the generic search,
+// which needs nothing beyond the frames it already registered.
+extern int switchkinsRegisterToolFrameInverse(int ktype, KTI kinv);
 #endif // }
diff --git a/tests/tool-frame/test_tool_frame.c b/tests/tool-frame/test_tool_frame.c
index dde817ae554..c23cbac8f1e 100644
--- a/tests/tool-frame/test_tool_frame.c
+++ b/tests/tool-frame/test_tool_frame.c
@@ -12,6 +12,9 @@
 #include "emcpos.h"
 #include "kinematics.h"
 
+#define DEG (M_PI/180.0)
+#define NUTATION 45.0
+
 static int failures;
 
 static void check(int ok, const char *what)
@@ -50,6 +53,170 @@ static PmRotationMatrix arbitrary(void)
                   -sb,   0,     cb);
 }
 
+
+/* ------------------------------------------------------------------
+ * Machine models for the tool orientation inverse.
+ *
+ * These are the frame functions a module supplies, written out here so the
+ * solver can be exercised without loading one.  Rotary joints are in degrees,
+ * as every module in the tree takes them, except radMachine, which is in
+ * radians to prove the solver does not assume.
+ * ------------------------------------------------------------------ */
+
+static PmRotationMatrix rows(const double m[3][3])
+{
+    return mat(m[0][0], m[0][1], m[0][2],
+               m[1][0], m[1][1], m[1][2],
+               m[2][0], m[2][1], m[2][2]);
+}
+
+static PmRotationMatrix rot_z(double rad)
+{
+    const double c = cos(rad), s = sin(rad);
+    const double m[3][3] = {{c, -s, 0}, {s, c, 0}, {0, 0, 1}};
+    return rows(m);
+}
+
+/* the nutating secondary joint of the trsrn heads */
+static PmRotationMatrix rot_nutate(double rad)
+{
+    const double v = NUTATION*DEG, sv = sin(v), cv = cos(v);
+    const double ss = sin(rad), cs = cos(rad);
+    const double r = cs + sv*sv*(1 - cs);
+    const double q = cs + cv*cv*(1 - cs);
+    const double t = sv*cv*(1 - cs);
+    const double m[3][3] = {{   cs, -cv*ss, sv*ss},
+                            {cv*ss,      r,     t},
+                            {-sv*ss,     t,     q}};
+    return rows(m);
+}
+
+static PmRotationMatrix product(const PmRotationMatrix *a,
+                                const PmRotationMatrix *b)
+{
+    const double x[3][3] = {{a->x.x, a->y.x, a->z.x},
+                            {a->x.y, a->y.y, a->z.y},
+                            {a->x.z, a->y.z, a->z.z}};
+    const double y[3][3] = {{b->x.x, b->y.x, b->z.x},
+                            {b->x.y, b->y.y, b->z.y},
+                            {b->x.z, b->y.z, b->z.z}};
+    double m[3][3];
+    int i, j, k;
+
+    for (i = 0; i < 3; i++) {
+        for (j = 0; j < 3; j++) {
+            m[i][j] = 0;
+            for (k = 0; k < 3; k++) { m[i][j] += x[i][k]*y[k][j]; }
+        }
+    }
+    return rows(m);
+}
+
+static int identityFrame(const double *j, PmRotationMatrix *rot,
+                         const KINEMATICS_FORWARD_FLAGS *fflags)
+{
+    (void)j; (void)fflags;
+    *rot = TOOL_FRAME_SPINDLE;
+    return 0;
+}
+
+/* xyzac: both rotaries carry the table, the tool stays square with the
+   machine.  j[3] is A, j[4] is C. */
+static int xyzacWork(const double *j, PmRotationMatrix *rot,
+                     const KINEMATICS_FORWARD_FLAGS *fflags)
+{
+    const double a = j[3]*DEG, c = j[4]*DEG;
+    const double m[3][3] = {{      cos(c),        sin(c),      0},
+                            {-sin(c)*cos(a), cos(c)*cos(a), sin(a)},
+                            { sin(c)*sin(a),-cos(c)*sin(a), cos(a)}};
+    (void)fflags;
+    *rot = rows(m);
+    return 0;
+}
+
+/* a nutating spindle head with nothing turning the work.  j[3] is the
+   nutating secondary joint, j[4] the primary about z. */
+static int headTool(const double *j, PmRotationMatrix *rot,
+                    const KINEMATICS_FORWARD_FLAGS *fflags)
+{
+    PmRotationMatrix p = rot_z(j[4]*DEG), s = rot_nutate(j[3]*DEG);
+    (void)fflags;
+    *rot = product(&p, &s);
+    return 0;
+}
+
+/* the same head, in radians, to exercise the period discovery */
+static int radTool(const double *j, PmRotationMatrix *rot,
+                   const KINEMATICS_FORWARD_FLAGS *fflags)
+{
+    PmRotationMatrix p = rot_z(j[4]), s = rot_nutate(j[3]);
+    (void)fflags;
+    *rot = product(&p, &s);
+    return 0;
+}
+
+/* a table rotary and a nutating head at once, so three joints turn the tool
+   and a bare tool axis leaves one of them free.  j[3] is the table A, j[4]
+   the nutating joint, j[5] the head primary. */
+static int mixedWork(const double *j, PmRotationMatrix *rot,
+                     const KINEMATICS_FORWARD_FLAGS *fflags)
+{
+    const double a = j[3]*DEG;
+    const double m[3][3] = {{1,       0,      0},
+                            {0,  cos(a), sin(a)},
+                            {0, -sin(a), cos(a)}};
+    (void)fflags;
+    *rot = rows(m);
+    return 0;
+}
+
+static int mixedTool(const double *j, PmRotationMatrix *rot,
+                     const KINEMATICS_FORWARD_FLAGS *fflags)
+{
+    PmRotationMatrix p = rot_z(j[5]*DEG), s = rot_nutate(j[4]*DEG);
+    (void)fflags;
+    *rot = product(&p, &s);
+    return 0;
+}
+
+/* what the module would report: the tool frame in workpiece coordinates */
+static PmRotationMatrix in_work(kinsFrameFunc work, kinsFrameFunc tool,
+                                const double *j)
+{
+    KINEMATICS_FORWARD_FLAGS f = 0;
+    PmRotationMatrix w, t, out;
+
+    work(j, &w, &f);
+    tool(j, &t, &f);
+    toolFrameInWork(&w, &t, &out);
+    return out;
+}
+
+static int axis_matches(kinsFrameFunc work, kinsFrameFunc tool,
+                        const double *j, const PmCartesian *want)
+{
+    PmRotationMatrix m = in_work(work, tool, j);
+    return fabs(m.z.x - want->x) < 1e-9
+        && fabs(m.z.y - want->y) < 1e-9
+        && fabs(m.z.z - want->z) < 1e-9;
+}
+
+/* does the list hold a solution whose free joints are these, to a degree */
+static int holds(const double *sols, int count, int njoints,
+                 const int *which, const double *value, int n)
+{
+    int s, i, ok;
+
+    for (s = 0; s < count; s++) {
+        ok = 1;
+        for (i = 0; i < n; i++) {
+            if (fabs(sols[s*njoints + which[i]] - value[i]) > 1e-6) { ok = 0; }
+        }
+        if (ok) { return 1; }
+    }
+    return 0;
+}
+
 int main(void)
 {
     PmRotationMatrix m, r;
@@ -152,6 +319,250 @@ int main(void)
               "the work frame is transposed, not applied directly");
     }
 
+
+    /* ------------------------------------------------------------------
+     * The tool orientation inverse.
+     * ------------------------------------------------------------------ */
+    {
+        double seed[6]  = {10, 20, 30, 10, 5, 0};
+        double truth[6] = {10, 20, 30, 34.4, 68.8, 0};
+        double sols[TOOL_FRAME_MAX_SOLUTIONS*6];
+        int free_dirs[TOOL_FRAME_MAX_SOLUTIONS];
+        double spin[TOOL_FRAME_MAX_SOLUTIONS];
+        PmRotationMatrix want;
+        PmCartesian axis, xdir;
+        int n, i;
+
+        /* a table rotary machine, tool axis only: the two rotaries pin it
+           down, and there are two ways to get there */
+        want = in_work(xyzacWork, identityFrame, truth);
+        axis = want.z;
+        n = toolFrameSolve(xyzacWork, identityFrame, 5, &axis, NULL, seed,
+                           sols, TOOL_FRAME_MAX_SOLUTIONS, free_dirs, spin);
+        check(n == 2, "xyzac reports both ways to reach a tool axis");
+        for (i = 0; i < n; i++) {
+            check(axis_matches(xyzacWork, identityFrame, sols + i*5, &axis),
+                  "every xyzac solution reaches the requested axis");
+            check(free_dirs[i] == 0, "an xyzac solution is pinned down");
+            check(sols[i*5 + 0] == seed[0] && sols[i*5 + 1] == seed[1]
+                  && sols[i*5 + 2] == seed[2],
+                  "the joints that do not turn the tool are copied from the seed");
+        }
+        {
+            const int which[2] = {3, 4};
+            const double value[2] = {34.4, 68.8};
+            check(holds(sols, n, 5, which, value, 2),
+                  "the pose the request was built from is one of them");
+        }
+
+        /* the singular pose: the tool axis is the axis the primary turns
+           about, so the primary is free and the answer is a family */
+        axis.x = 0; axis.y = 0; axis.z = 1;
+        n = toolFrameSolve(xyzacWork, identityFrame, 5, &axis, NULL, seed,
+                           sols, TOOL_FRAME_MAX_SOLUTIONS, free_dirs, spin);
+        check(n == 1, "a singular pose reports one representative, not a sample");
+        check(free_dirs[0] == 1, "and says one direction is free");
+        check(fabs(sols[3]) < 1e-6, "the joint the request does pin down is set");
+        check(fabs(sols[4] - seed[4]) < 1e-6,
+              "the free joint is left where the machine already is");
+
+        /* approaching the singularity: the two solutions stay two until the
+           spin about the tool stops being worth anything, and then the answer
+           becomes the family rather than a scatter of points that differ by
+           more than the tool can tell apart */
+        axis.x = sin(0.01*DEG); axis.y = 0; axis.z = cos(0.01*DEG);
+        n = toolFrameSolve(xyzacWork, identityFrame, 5, &axis, NULL, seed,
+                           sols, TOOL_FRAME_MAX_SOLUTIONS, free_dirs, spin);
+        check(n == 2 && free_dirs[0] == 0 && free_dirs[1] == 0,
+              "a hundredth of a degree off the pole still has two solutions");
+
+        axis.x = sin(0.001*DEG); axis.y = 0; axis.z = cos(0.001*DEG);
+        n = toolFrameSolve(xyzacWork, identityFrame, 5, &axis, NULL, seed,
+                           sols, TOOL_FRAME_MAX_SOLUTIONS, free_dirs, spin);
+        check(n == 1 && free_dirs[0] == 1,
+              "a thousandth of a degree off it, the spin is free in practice");
+
+        /* xyzac turns the work through a full sphere, so straight down is a
+           pose and not a refusal: A at half a turn */
+        axis.x = 0; axis.y = 0; axis.z = -1;
+        n = toolFrameSolve(xyzacWork, identityFrame, 5, &axis, NULL, seed,
+                           sols, TOOL_FRAME_MAX_SOLUTIONS, free_dirs, spin);
+        check(n == 1 && free_dirs[0] == 1,
+              "the other pole is reachable, and free about the tool as well");
+        check(fabs(fabs(sols[3]) - 180.0) < 1e-6, "reached with A at half a turn");
+
+        /* a machine where nothing turns the tool answers for the one pose it
+           has, and refuses anything else */
+        axis.x = 0; axis.y = 0; axis.z = 1;
+        n = toolFrameSolve(identityFrame, identityFrame, 5, &axis, NULL, seed,
+                           sols, TOOL_FRAME_MAX_SOLUTIONS, free_dirs, spin);
+        check(n == 1 && free_dirs[0] == 0,
+              "a machine with no orientation joints reports its one pose");
+        axis.x = 0; axis.y = 1; axis.z = 0;
+        n = toolFrameSolve(identityFrame, identityFrame, 5, &axis, NULL, seed,
+                           sols, TOOL_FRAME_MAX_SOLUTIONS, free_dirs, spin);
+        check(n == 0, "and cannot reach any other");
+
+        /* out of reach: a nutating head sweeps a cone, and with a nutation
+           of 45 degrees it cannot get the tool below the horizontal */
+        {
+            double head_seed[5] = {0, 0, 0, 10, 5};
+
+            axis.x = 0; axis.y = 0; axis.z = -1;
+            n = toolFrameSolve(identityFrame, headTool, 5, &axis, NULL,
+                               head_seed, sols, TOOL_FRAME_MAX_SOLUTIONS,
+                               free_dirs, spin);
+            check(n == 0, "an unreachable axis reports no solutions");
+        }
+
+        /* the nutating head, checked against the closed form the TWP remap
+           uses: cos(secondary) = (Kzz - Cv^2)/(1 - Cv^2), which has the two
+           roots +theta and -theta */
+        {
+            double head_seed[5]  = {0, 0, 0, 10, 5};
+            double head_truth[5] = {0, 0, 0, 40.0, 25.0};
+            const double cv = cos(NUTATION*DEG);
+            double closed, s;
+
+            want = in_work(identityFrame, headTool, head_truth);
+            axis = want.z;
+            closed = acos((axis.z - cv*cv)/(1 - cv*cv))/DEG;
+
+            n = toolFrameSolve(identityFrame, headTool, 5, &axis, NULL,
+                               head_seed, sols, TOOL_FRAME_MAX_SOLUTIONS,
+                               free_dirs, spin);
+            check(n == 2, "the nutating head reports both secondary roots");
+            for (i = 0; i < n; i++) {
+                s = fabs(sols[i*5 + 3]);
+                check(fabs(s - closed) < 1e-6,
+                      "the search agrees with the closed form of the remap");
+                check(axis_matches(identityFrame, headTool, sols + i*5, &axis),
+                      "every nutating solution reaches the requested axis");
+            }
+            check(fabs(sols[0*5 + 3] + sols[1*5 + 3]) < 1e-6,
+                  "the two roots are opposite, as acos gives them");
+
+            /* the same machine written in radians: the joint unit is
+               discovered, so the answer is the same shape */
+            head_seed[3] = 10*DEG; head_seed[4] = 5*DEG;
+            n = toolFrameSolve(identityFrame, radTool, 5, &axis, NULL,
+                               head_seed, sols, TOOL_FRAME_MAX_SOLUTIONS,
+                               free_dirs, spin);
+            check(n == 2, "a module taking radians is solved too");
+            for (i = 0; i < n; i++) {
+                check(fabs(fabs(sols[i*5 + 3])/DEG - closed) < 1e-6,
+                      "and gives the same angles once the unit is accounted for");
+            }
+        }
+
+        /* three joints turn the tool.  A bare tool axis leaves the spin about
+           it free, and asking for tool x as well pins the machine down. */
+        {
+            double mix_seed[6]  = {0, 0, 0, 5, 10, 15};
+            double mix_truth[6] = {0, 0, 0, 20, 45.8, 57.3};
+
+            want = in_work(mixedWork, mixedTool, mix_truth);
+            axis = want.z;
+            xdir = want.x;
+
+            n = toolFrameSolve(mixedWork, mixedTool, 6, &axis, NULL, mix_seed,
+                               sols, TOOL_FRAME_MAX_SOLUTIONS, free_dirs, spin);
+            check(n == 1, "a spare orientation joint gives a family, not a list");
+            check(free_dirs[0] == 1, "and one free direction is reported");
+            check(axis_matches(mixedWork, mixedTool, sols, &axis),
+                  "the representative reaches the requested axis");
+
+            for (i = 0; i < TOOL_FRAME_MAX_SOLUTIONS; i++) { spin[i] = 99; }
+            n = toolFrameSolve(mixedWork, mixedTool, 6, &axis, &xdir, mix_seed,
+                               sols, TOOL_FRAME_MAX_SOLUTIONS, free_dirs, spin);
+            check(n == 2, "asking for tool x as well pins it down");
+            for (i = 0; i < n; i++) {
+                PmRotationMatrix got = in_work(mixedWork, mixedTool, sols + i*6);
+                check(free_dirs[i] == 0, "with nothing left free");
+                check(spin[i] == 0.0,
+                      "and no turn about the tool left over, the joints did it");
+                check(fabs(got.x.x - xdir.x) < 1e-9
+                      && fabs(got.x.y - xdir.y) < 1e-9
+                      && fabs(got.x.z - xdir.z) < 1e-9,
+                      "and tool x where it was asked for");
+            }
+            {
+                const int which[3] = {3, 4, 5};
+                const double value[3] = {20, 45.8, 57.3};
+                check(holds(sols, n, 6, which, value, 3),
+                      "the pose the request was built from is one of them");
+            }
+        }
+
+
+        /* Asking a five axis machine for tool x as well.  Its two rotaries are
+           spent on the tool axis and the turn about that axis is not a joint,
+           so the answer is the poses that reach the axis plus the turn that
+           places tool x, which is what the virtual rotation applies. */
+        {
+            double head_seed[5]  = {0, 0, 0, 10, 5};
+            double head_truth[5] = {0, 0, 0, 40.0, 25.0};
+            PmRotationMatrix got, want_frame;
+            PmCartesian want_x;
+            double c_s, s_s, dot;
+
+            want_frame = in_work(identityFrame, headTool, head_truth);
+            axis = want_frame.z;
+
+            /* a tool x at right angles to that axis, but not the one this
+               machine happens to produce: turn the achieved one by 30 degrees
+               about the axis */
+            c_s = cos(30*DEG); s_s = sin(30*DEG);
+            want_x.x = c_s*want_frame.x.x + s_s*want_frame.y.x;
+            want_x.y = c_s*want_frame.x.y + s_s*want_frame.y.y;
+            want_x.z = c_s*want_frame.x.z + s_s*want_frame.y.z;
+
+            n = toolFrameSolve(identityFrame, headTool, 5, &axis, &want_x,
+                               head_seed, sols, TOOL_FRAME_MAX_SOLUTIONS,
+                               free_dirs, spin);
+            check(n == 2, "the axis is still reached both ways");
+            for (i = 0; i < n; i++) {
+                check(axis_matches(identityFrame, headTool, sols + i*5, &axis),
+                      "every solution reaches the requested axis");
+                got = in_work(identityFrame, headTool, sols + i*5);
+                /* turning the achieved frame by the reported spin has to land
+                   tool x where it was asked for */
+                c_s = cos(spin[i]); s_s = sin(spin[i]);
+                check(fabs(c_s*got.x.x + s_s*got.y.x - want_x.x) < 1e-9
+                      && fabs(c_s*got.x.y + s_s*got.y.y - want_x.y) < 1e-9
+                      && fabs(c_s*got.x.z + s_s*got.y.z - want_x.z) < 1e-9,
+                      "and the reported turn places tool x");
+            }
+
+            /* with nowhere to report the turn, the request cannot be answered
+               rather than being answered wrongly */
+            n = toolFrameSolve(identityFrame, headTool, 5, &axis, &want_x,
+                               head_seed, sols, TOOL_FRAME_MAX_SOLUTIONS,
+                               free_dirs, NULL);
+            check(n == 0, "and without somewhere to put it, no solutions");
+
+            /* the two vectors are two axes of one frame */
+            dot = 0.5;
+            want_x.x = axis.x + dot; want_x.y = axis.y; want_x.z = axis.z;
+            check(toolFrameSolve(identityFrame, headTool, 5, &axis, &want_x,
+                                 head_seed, sols, TOOL_FRAME_MAX_SOLUTIONS,
+                                 free_dirs, spin) == -1,
+                  "a tool x not at right angles to the axis is refused");
+        }
+
+        /* the arguments are checked rather than trusted */
+        axis.x = 0; axis.y = 0; axis.z = 1;
+        check(toolFrameSolve(NULL, identityFrame, 5, &axis, NULL, seed, sols,
+                             TOOL_FRAME_MAX_SOLUTIONS, free_dirs, spin) == -1,
+              "a missing frame function is refused");
+        check(toolFrameSolve(xyzacWork, identityFrame, 0, &axis, NULL, seed,
+                             sols, TOOL_FRAME_MAX_SOLUTIONS, free_dirs, spin) == -1,
+              "a bogus joint count is refused");
+        check(toolFrameSolve(xyzacWork, identityFrame, 5, NULL, NULL, seed,
+                             sols, TOOL_FRAME_MAX_SOLUTIONS, free_dirs, spin) == -1,
+              "a missing target is refused");
+    }
+
     if (failures) { printf("%d failure(s)\n", failures); return 1; }
     printf("all tool frame checks passed\n");
     return 0;

From 002fedd3a851ec52ccac1b60ac81064a9784ff0f Mon Sep 17 00:00:00 2001
From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com>
Date: Wed, 26 Aug 2026 21:49:16 +1000
Subject: [PATCH 09/10] trsrn: set the kinematics pins up once, at load

Both modules build their pins on the first kinematicsType() call and
test an is_setup flag that nothing ever sets, so every later call runs
the setup again.  The second run reassigns haldata to a fresh block and
then fails to create the pins that block points at, all of them already
taken, so the module is left reading through null references and the
next forward or inverse call takes realtime with it.

Motion asks twice when num_extrajoints is greater than zero, once in
rtapi_app_main and once in init_comm_buffers, so that combination
cannot be running today.

Latching the flag would stop the repeat, but the setup does not belong
in a function whose job is to answer a question.  halcompile has a hook
for this, used by homecomp for the same reason: EXTRA_SETUP() runs once
from the generated setup, before the component is made ready.  So the
pins exist from the moment the module is loaded, kinematicsType() only
answers, and the hal_set_unready() and hal_ready() calls the old
placement needed are gone with it.
---
 src/hal/components/xyzacb_trsrn.comp | 11 +++++------
 src/hal/components/xyzbca_trsrn.comp |  9 +++++----
 2 files changed, 10 insertions(+), 10 deletions(-)

diff --git a/src/hal/components/xyzacb_trsrn.comp b/src/hal/components/xyzacb_trsrn.comp
index 098af8a7aa2..4efedcae6ce 100644
--- a/src/hal/components/xyzacb_trsrn.comp
+++ b/src/hal/components/xyzacb_trsrn.comp
@@ -7,6 +7,7 @@ FIXME
 """;
 pin out si32 dummy=0 "dummy pin to satisfy halcompile";
 option period no;
+option extra_setup;
 
 license "GPL";
 author "David Mueller";
@@ -43,13 +44,14 @@ static struct haldata {
 } *haldata;
 
 
-static int xyzacb_trsrn_setup(void) {
+EXTRA_SETUP() {
+    (void)__comp_inst;
+    (void)prefix;
+    (void)extra_arg;
 #define HAL_PREFIX "xyzacb_trsrn_kins"
     int res=0;
     // inherit comp_id from rtapi_main()
     if (comp_id < 0) goto error;
-    // set unready to allow creation of pins
-    if (hal_set_unready(comp_id)) goto error;
 
     haldata = hal_malloc(sizeof(struct haldata));
     if (!haldata) goto error;
@@ -76,7 +78,6 @@ static int xyzacb_trsrn_setup(void) {
     res += hal_pin_new_bool(comp_id, HAL_OUT, &haldata->kinstype_is_2, 0, "kinstype.is-2");
 
     if (res) goto error;
-    hal_ready(comp_id);
     rtapi_print("*** %s setup ok\n",__FILE__);
     return 0;
 error:
@@ -136,8 +137,6 @@ int kinematicsSwitch(int new_switchkins_type)
 
 KINEMATICS_TYPE kinematicsType()
 {
-static bool is_setup=0;
-    if (!is_setup)  xyzacb_trsrn_setup();
     return KINEMATICS_BOTH; // set as required
            // Note: If kinematics are identity, using KINEMATICS_BOTH
            //       may be used in order to allow a gui to display
diff --git a/src/hal/components/xyzbca_trsrn.comp b/src/hal/components/xyzbca_trsrn.comp
index 12e40344125..763b2801c33 100644
--- a/src/hal/components/xyzbca_trsrn.comp
+++ b/src/hal/components/xyzbca_trsrn.comp
@@ -7,6 +7,7 @@ FIXME
 """;
 pin out si32 dummy=0 "dummy pin to satisfy halcompile";
 option period no;
+option extra_setup;
 
 license "GPL";
 author "David Mueller";
@@ -43,7 +44,10 @@ static struct haldata {
 } *haldata;
 
 
-static int xyzbca_trsrn_setup(void) {
+EXTRA_SETUP() {
+    (void)__comp_inst;
+    (void)prefix;
+    (void)extra_arg;
 #define HAL_PREFIX "xyzbca_trsrn_kins"
     int res=0;
     // inbherit comp_id from rtapi_main()
@@ -76,7 +80,6 @@ static int xyzbca_trsrn_setup(void) {
     res += hal_pin_new_bool(comp_id, HAL_OUT, &haldata->kinstype_is_2, 0, "kinstype.is-2");
 
     if (res) goto error;
-    hal_ready(comp_id);
     rtapi_print("*** %s setup ok\n",__FILE__);
     return 0;
 error:
@@ -136,8 +139,6 @@ int kinematicsSwitch(int new_switchkins_type)
 
 KINEMATICS_TYPE kinematicsType()
 {
-static bool is_setup=0;
-    if (!is_setup)  xyzbca_trsrn_setup();
     return KINEMATICS_BOTH; // set as required
            // Note: If kinematics are identity, using KINEMATICS_BOTH
            //       may be used in order to allow a gui to display

From 1e5da0de497b4d176ed275910a920baed5f3fc07 Mon Sep 17 00:00:00 2001
From: Luca Toniolo <10792599+grandixximo@users.noreply.github.com>
Date: Wed, 26 Aug 2026 19:07:39 +1000
Subject: [PATCH 10/10] tests: check the shipped frame functions where they run

The frame cases so far run machines written for the test.  The frame
functions a real machine uses were compiled and nothing more, which
leaves the hand-written matrix entries per module unexercised, and
those are where a sign or a transposed pair hides.

So run them in service: a realtime component loaded after the module
under test, reaching it through the same exported entry points motion
uses.  A failed check fails the load, and a failed load fails the test.

The work frame has a tie to the forward kinematics and is checked
against it rather than against a matrix copied out of the module: a row
of the work frame is how the reported position responds to one machine
axis, measured by central difference.

The tool frame has no such tie on a machine that carries the work.  Its
forward reports the rotary joint values, which say how the work is
turned and nothing about where the tool points, and the tool length
offset moves the reported origin as well as the tip, so it does not
isolate the tool axis either.  What is checked is that the frame is a
rotation, that a spindle the module calls fixed never moves, and that a
joint carrying the whole head turns the reported frame about the
machine's z and does nothing else.  The last one is what catches a
frame built for the wrong joint or composed in the wrong order.

Every kinematics type a module offers, since the frames are per type,
and the geometry checks are asked only of the type where the module
models its own machine, which is not always the first.  A type that
reports no frames is skipped, which the interface allows, so the test
also fails a module that reports frames for no type at all.

The hal file sets the geometry, which matters more than it sounds: a
nutating head defaults to a nutation angle of zero, which leaves the
spindle square with the machine and the interesting geometry untested.

The helpers in kins_util.c are not exported to a loadable module, and
working the answers out in the test is better anyway: nothing the
module under test uses is reused to judge it.

Checked by mutation, in failed checks: reversing the head composition
order in a trsrn module 300, a flipped sign in its work frame 150, one
in the xyzac work frame 18, declaring the wrong spindle convention
there 25, a flipped sign in the puma flange 72.  The tree as it stands
fails none.
---
 tests/kins-frames/checkresult  |   3 +
 tests/kins-frames/control      |   1 +
 tests/kins-frames/framecheck.c | 293 +++++++++++++++++++++++++++++++++
 tests/kins-frames/skip         |   4 +
 tests/kins-frames/test.sh      |  53 ++++++
 5 files changed, 354 insertions(+)
 create mode 100755 tests/kins-frames/checkresult
 create mode 100644 tests/kins-frames/control
 create mode 100644 tests/kins-frames/framecheck.c
 create mode 100755 tests/kins-frames/skip
 create mode 100755 tests/kins-frames/test.sh

diff --git a/tests/kins-frames/checkresult b/tests/kins-frames/checkresult
new file mode 100755
index 00000000000..5fefd687bac
--- /dev/null
+++ b/tests/kins-frames/checkresult
@@ -0,0 +1,3 @@
+#!/bin/sh
+[ "$(grep -c 'frames agree' "$1")" = 5 ] \
+    && ! grep -q "FAIL" "$1"
diff --git a/tests/kins-frames/control b/tests/kins-frames/control
new file mode 100644
index 00000000000..5cc4825bdc1
--- /dev/null
+++ b/tests/kins-frames/control
@@ -0,0 +1 @@
+Restrictions: sudo
diff --git a/tests/kins-frames/framecheck.c b/tests/kins-frames/framecheck.c
new file mode 100644
index 00000000000..a2be114cc68
--- /dev/null
+++ b/tests/kins-frames/framecheck.c
@@ -0,0 +1,293 @@
+/* Check a kinematics module's reported frames where they run in service.
+ *
+ * Loaded after the module under test, so kinematicsForward(),
+ * kinematicsWorkFrame() and kinematicsToolFrame() resolve to it.  A
+ * failed check fails the load, and a failed load fails the test.
+ *
+ * The work frame has a tie to the forward kinematics and is checked
+ * against it: a row of it is how the reported position responds to one
+ * machine axis, measured here by central difference.
+ *
+ * The tool frame has no such tie on a machine that carries the work.
+ * Its forward reports the rotary joint values, which describe how the
+ * work is turned, and say nothing about where the tool points.  So the
+ * tool frame is checked for what can be checked: that it is a rotation,
+ * that a spindle the module calls fixed never moves, and that a joint
+ * which turns the whole head about the machine's z turns the reported
+ * frame with it and does nothing else.  That last one catches a frame
+ * built for the wrong joint or composed in the wrong order.
+ *
+ * Author: LinuxCNC
+ * License: GPL Version 2
+ * System: Linux
+ *
+ * Copyright (c) 2026 All rights reserved.
+ */
+
+#include <rtapi.h>
+#include <rtapi_app.h>
+#include <rtapi_math.h>
+#include <rtapi_string.h>
+#include <hal.h>
+#include <emcpos.h>
+#include <emcmotcfg.h>
+#include <kinematics.h>
+
+MODULE_LICENSE("GPL");
+MODULE_DESCRIPTION("kinematics frame checker");
+
+static int joints = 5;
+RTAPI_MP_INT(joints, "joint count the module under test was loaded for");
+
+static int carries_tool = 0;
+RTAPI_MP_INT(carries_tool, "1 when the machine carries the tool rather than the work");
+
+static int fixed_spindle = 0;
+RTAPI_MP_INT(fixed_spindle, "1 when the module reports a spindle square with the machine");
+
+static int ktype = 0;
+RTAPI_MP_INT(ktype, "switchkins type where the module models its own machine");
+
+static int spin = -1;
+RTAPI_MP_INT(spin, "joint that turns the whole head about the machine's z, -1 for none");
+
+static int r1 = -1, r2 = -1, r3 = -1;
+RTAPI_MP_INT(r1, "joint number of the first rotary to sweep");
+RTAPI_MP_INT(r2, "joint number of the second rotary, -1 for none");
+RTAPI_MP_INT(r3, "joint number of the third rotary, -1 for none");
+
+/* 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 TO_RAD (M_PI / 180.0)
+#define STEP   1e-6
+#define TURN   15.0
+#define TOL    1e-6
+
+static int comp_id = -1;
+static int failures;
+
+static void expect(int ok, const char *what, const double *j)
+{
+    char pose[128];
+    int i, n = 0;
+
+    if (ok) { return; }
+    for (i = 0; i < joints && n < (int)sizeof(pose) - 12; i++) {
+        n += rtapi_snprintf(pose + n, sizeof(pose) - n, "%s%.4g",
+                            i ? "," : "", j[i]);
+    }
+    rtapi_print_msg(RTAPI_MSG_ERR, "framecheck: FAIL %s at [%s]\n", what, pose);
+    failures++;
+}
+
+static int close3(const PmCartesian *a, double x, double y, double z)
+{
+    return fabs(a->x - x) < TOL && fabs(a->y - y) < TOL && fabs(a->z - z) < TOL;
+}
+
+/* The helpers in kins_util.c are not exported to a loadable module, and
+   working the answers out here is the better test anyway: nothing the
+   module under test uses is reused to judge it. */
+static double dot(const PmCartesian *a, const PmCartesian *b)
+{
+    return a->x * b->x + a->y * b->y + a->z * b->z;
+}
+
+static int is_rotation(const PmRotationMatrix *m)
+{
+    PmCartesian cross;
+
+    if (fabs(dot(&m->x, &m->x) - 1) > TOL) { return 0; }
+    if (fabs(dot(&m->y, &m->y) - 1) > TOL) { return 0; }
+    if (fabs(dot(&m->z, &m->z) - 1) > TOL) { return 0; }
+    if (fabs(dot(&m->x, &m->y)) > TOL) { return 0; }
+    if (fabs(dot(&m->x, &m->z)) > TOL) { return 0; }
+    if (fabs(dot(&m->y, &m->z)) > TOL) { return 0; }
+
+    /* right handed, so the third column is the cross product of the
+       other two rather than its negative */
+    cross.x = m->x.y * m->y.z - m->x.z * m->y.y;
+    cross.y = m->x.z * m->y.x - m->x.x * m->y.z;
+    cross.z = m->x.x * m->y.y - m->x.y * m->y.x;
+    return close3(&cross, m->z.x, m->z.y, m->z.z);
+}
+
+/* how the reported position responds to a displacement of machine axis
+   jno: column jno of the forward transform's linear part */
+static void response(const double *j, int jno, PmCartesian *out)
+{
+    double t[EMCMOT_MAX_JOINTS];
+    EmcPose lo, hi;
+    KINEMATICS_FORWARD_FLAGS ff = 0;
+    KINEMATICS_INVERSE_FLAGS inf = 0;
+
+    memcpy(t, j, sizeof(t));
+
+    t[jno] = j[jno] - STEP;
+    kinematicsForward(t, &lo, &ff, &inf);
+    t[jno] = j[jno] + STEP;
+    kinematicsForward(t, &hi, &ff, &inf);
+
+    out->x = (hi.tran.x - lo.tran.x) / (2 * STEP);
+    out->y = (hi.tran.y - lo.tran.y) / (2 * STEP);
+    out->z = (hi.tran.z - lo.tran.z) / (2 * STEP);
+}
+
+/* turn a frame about the machine's z, which is what a joint carrying
+   the whole head does to everything above it */
+static void turn_about_z(double deg, const PmRotationMatrix *m,
+                         PmRotationMatrix *out)
+{
+    const double c = cos(deg * TO_RAD);
+    const double s = sin(deg * TO_RAD);
+
+    out->x.x = c * m->x.x - s * m->x.y;
+    out->x.y = s * m->x.x + c * m->x.y;
+    out->x.z = m->x.z;
+    out->y.x = c * m->y.x - s * m->y.y;
+    out->y.y = s * m->y.x + c * m->y.y;
+    out->y.z = m->y.z;
+    out->z.x = c * m->z.x - s * m->z.y;
+    out->z.y = s * m->z.x + c * m->z.y;
+    out->z.z = m->z.z;
+}
+
+/* Reporting the frames is optional, and a switchable module usually
+   supplies them for some of its types and not others, so a type that
+   declines is skipped rather than failed. */
+static int supplies_frames(const double *j)
+{
+    KINEMATICS_FORWARD_FLAGS ff = 0;
+    PmRotationMatrix m;
+
+    if (kinematicsWorkFrame(j, &m, &ff)) { return 0; }
+    if (kinematicsToolFrame(j, &m, &ff)) { return 0; }
+    return 1;
+}
+
+static void check(const double *j, int own_kinematics)
+{
+    KINEMATICS_FORWARD_FLAGS ff = 0;
+    PmRotationMatrix work, tool, turned, want;
+    PmCartesian d;
+    double t[EMCMOT_MAX_JOINTS];
+
+    kinematicsWorkFrame(j, &work, &ff);
+    kinematicsToolFrame(j, &tool, &ff);
+
+    expect(is_rotation(&work), "work frame is a rotation", j);
+    expect(is_rotation(&tool), "tool frame is a rotation", j);
+
+    if (carries_tool) {
+        /* nothing turns the work, at any pose */
+        expect(close3(&work.x, 1, 0, 0) && close3(&work.y, 0, 1, 0)
+            && close3(&work.z, 0, 0, 1), "work frame is the machine frame", j);
+    } else {
+        /* the forward transform maps a machine displacement to a work
+           one, so a row of the work frame is one of its columns */
+        response(j, 0, &d);
+        expect(close3(&d, work.x.x, work.y.x, work.z.x), "work frame against X", j);
+        response(j, 1, &d);
+        expect(close3(&d, work.x.y, work.y.y, work.z.y), "work frame against Y", j);
+        response(j, 2, &d);
+        expect(close3(&d, work.x.z, work.y.z, work.z.z), "work frame against Z", j);
+    }
+
+    /* the rest describes the module's own machine, so its other
+       kinematics types, identity and the tool frame's own, are not
+       asked: they leave everything square with the machine */
+    if (!own_kinematics) { return; }
+
+    if (fixed_spindle) {
+        expect(close3(&tool.x, 1, 0, 0) && close3(&tool.y, 0, 1, 0)
+            && close3(&tool.z, 0, 0, 1), "the spindle stays square", j);
+    }
+
+    if (spin >= 0) {
+        memcpy(t, j, sizeof(t));
+        t[spin] = j[spin] + TURN;
+        kinematicsToolFrame(t, &turned, &ff);
+        turn_about_z(TURN, &tool, &want);
+        expect(close3(&turned.x, want.x.x, want.x.y, want.x.z),
+               "tool x turns with the head", j);
+        expect(close3(&turned.y, want.y.x, want.y.y, want.y.z),
+               "tool y turns with the head", j);
+        expect(close3(&turned.z, want.z.x, want.z.y, want.z.z),
+               "tool axis turns with the head", j);
+    }
+}
+
+int rtapi_app_main(void)
+{
+    /* rotary values away from the identity, including the quarter and
+       half turns where a sine changes sign or a cosine vanishes */
+    static const double angle[] = { 0, 30, -25, 90, 180 };
+    const int angles = sizeof(angle) / sizeof(angle[0]);
+    double j[EMCMOT_MAX_JOINTS];
+    int a, b, c, t;
+    int checked = 0;
+
+    if (joints < 1 || joints > EMCMOT_MAX_JOINTS) {
+        rtapi_print_msg(RTAPI_MSG_ERR, "framecheck: joints=%d\n", joints);
+        return -1;
+    }
+
+    comp_id = hal_init("framecheck");
+    if (comp_id < 0) { return comp_id; }
+
+    if (kinematicsType() == 0) {
+        rtapi_print_msg(RTAPI_MSG_ERR, "framecheck: the module reports no type\n");
+        hal_exit(comp_id);
+        return -1;
+    }
+
+    memset(j, 0, sizeof(j));
+    if (!carries_tool) { j[0] = 10; j[1] = 20; j[2] = 30; }
+
+    /* every kinematics the module offers, not just the one it starts
+       in: the frames a switchable module reports are per type, and the
+       type that turns the work is rarely the default */
+    for (t = 0; t < MAX_TYPES; t++) {
+        if (kinematicsSwitchable() && kinematicsSwitch(t)) { break; }
+        if (!supplies_frames(j)) { continue; }
+        checked++;
+
+        for (a = 0; a < angles; a++) {
+            if (r1 >= 0) { j[r1] = angle[a]; }
+            for (b = 0; b < angles; b++) {
+                if (r2 >= 0) { j[r2] = angle[b]; }
+                for (c = 0; c < angles; c++) {
+                    if (r3 >= 0) { j[r3] = angle[c]; }
+                    check(j, t == ktype);
+                    if (r3 < 0) { break; }
+                }
+                if (r2 < 0) { break; }
+            }
+            if (r1 < 0) { break; }
+        }
+
+        if (!kinematicsSwitchable()) { break; }
+    }
+
+    if (!checked) {
+        rtapi_print_msg(RTAPI_MSG_ERR,
+                        "framecheck: the module reports frames for no type\n");
+        hal_exit(comp_id);
+        return -1;
+    }
+
+    if (failures) {
+        rtapi_print_msg(RTAPI_MSG_ERR,
+                        "framecheck: %d check(s) failed\n", failures);
+        hal_exit(comp_id);
+        return -1;
+    }
+
+    rtapi_print("framecheck: frames agree for %d kinematics type(s)\n", checked);
+    hal_ready(comp_id);
+    return 0;
+}
+
+void rtapi_app_exit(void) { hal_exit(comp_id); }
diff --git a/tests/kins-frames/skip b/tests/kins-frames/skip
new file mode 100755
index 00000000000..a12f31a77c2
--- /dev/null
+++ b/tests/kins-frames/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-frames/test.sh b/tests/kins-frames/test.sh
new file mode 100755
index 00000000000..4bb8e108765
--- /dev/null
+++ b/tests/kins-frames/test.sh
@@ -0,0 +1,53 @@
+#!/bin/bash
+set -e
+
+${SUDO} halcompile --install framecheck.c >/dev/null
+
+# One hal file per module: they all define the same entry points, so
+# only one can be loaded at a time.
+run() {
+    local hal
+    hal=$(mktemp --suffix=.hal)
+    { printf 'loadrt %s\n' "$1"
+      printf '%s\n' "$2"
+      printf 'loadrt framecheck %s\n' "$3"
+    } > "$hal"
+    echo "=== $1"
+    halrun -f "$hal"
+    rm -f "$hal"
+}
+
+run "xyzac-trt-kins coordinates=XYZAC" \
+    "setp xyzac-trt-kins.y-offset 3
+setp xyzac-trt-kins.z-offset 11
+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 fixed_spindle=1"
+
+run "xyzbc-trt-kins coordinates=XYZBC" \
+    "setp xyzbc-trt-kins.y-offset 3
+setp xyzbc-trt-kins.z-offset 11
+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 fixed_spindle=1"
+
+# a nutation angle of zero leaves the head square with the machine and
+# the interesting geometry untested, so give both a real one
+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.tool-offset-z 50" \
+    "joints=6 r1=3 r2=4 r3=5 spin=5 ktype=1"
+
+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.tool-offset-z 50" \
+    "joints=6 r1=3 r2=4 r3=5 spin=5 ktype=1"
+
+run "pumakins" "setp pumakins.A2 300" \
+    "joints=6 carries_tool=1 r1=0 r2=3 r3=4 spin=0"