kinematics: let a module report where the tool points - #4455
Open
grandixximo wants to merge 7 commits into
Open
Conversation
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.
kinematicsForward() reports where the controlled point is and nothing about
which way the tool faces, so a consumer that needs the tool frame rebuilds the
machine geometry for itself. Add an entry point the module can answer with
instead:
int kinematicsToolFrame(const double *joint,
PmRotationMatrix *rot,
const KINEMATICS_FORWARD_FLAGS *fflags);
The columns of the rotation are the tool x, y and z direction vectors in world
coordinates; its origin is the controlled point kinematicsForward() gives for
the same joints, so the pair is the whole tool pose. Conventions are in the
Kinematics Conventions chapter.
Modules built on switchkins.c export it always and dispatch on the current
type, returning -1 for a type that has not supplied one, which is a report
rather than an error. A type registers with switchkinsRegisterToolFrame()
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.
Identity types answer the same way whichever module asked for them, so
switchkins.c attaches identityKinematicsToolFrame() to any type whose forward
is the identity one, and every switchkins module gains a correct tool frame
for its identity type without being touched.
Nothing in motion calls it, so no module is obliged to define it and a module
outside the tree need not know it exists. A caller resolving it dynamically
has to cope with its absence.
The forward transform already contains the rotation: the coefficients it applies to a displacement of the X, Y and Z joints are the columns of the rotation the table contributes, which is the orientation of the tool in the work. Write those coefficients out as the tool frame and register it for whichever switchkins type holds the trt kinematics. 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: columns agree to 3e-9, the result is orthonormal with determinant one, and tool z is world 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 tool z along -Y, 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 tool z along +Y.
trivkins does not build on switchkins.c, so it does not pick up the identity tool frame the way a switchkins identity type does. Hand it through, since it is the kinematics most machines run and a caller that has to special-case the commonest module has not gained much.
These modules carry their own switchkins dispatch, so they answer for all three of their types directly. For TCP kinematics the frame is the table rotation times the primary rotation about z times the nutating secondary rotation, written out as three 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 the frame unrotated, 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. 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 table rotation alone, as they should be, since turning the head does not move the tool tip when a linear joint moves. Tool z, recovered separately as the direction the tip retreats along when the tool gets longer, matches the third column of the product. For tool kinematics the same linear-joint 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.
The first module whose own maths is not in the convention, and so the first real exercise of the declared rotation. 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.
Pins down the property the chapter is about: relating one tool axis convention to the other is a rotation, not a change of sign. Checks that a negated third column is refused because it is a reflection, that the declared rotation post-multiplies so it is read in the module's own frame, that the half turn keeps tool x and reverses the other two, and that 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. Verified by mutation rather than by passing: making TOOL_FRAME_FLANGE negate only the tool axis fails nine checks, and reversing the multiplication order fails four including the one written for it. Built the way tests/blendmath builds, compiling the source under test directly with the rest garbage-collected by the linker.
grandixximo
force-pushed
the
kins-tool-frame
branch
from
August 23, 2026 08:17
2b21950 to
067ce10
Compare
This file contains hidden or bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment
1 participant
Add this suggestion to a batch that can be applied as a single commit.This suggestion is invalid because no changes were made to the code.Suggestions cannot be applied while the pull request is closed.Suggestions cannot be applied while viewing a subset of changes.Only one suggestion per line can be applied in a batch.Add this suggestion to a batch that can be applied as a single commit.Applying suggestions on deleted lines is not supported.You must change the existing code in this line in order to create a valid suggestion.Outdated suggestions cannot be applied.This suggestion has been applied or marked resolved.Suggestions cannot be applied from pending reviews.Suggestions cannot be applied on multi-line comments.Suggestions cannot be applied while the pull request is queued to merge.Suggestion cannot be applied right now. Please check back later.
A kinematics module reports the controlled point and nothing about which way the tool faces, so everything that needs the tool frame rebuilds the machine geometry for itself. For
xyzacb-trsrnthe same chain is written three times: closed-form inxyzacb_trsrn.comp, as homogeneous matrices in the config'sremap_funcs_twp.py, and as signedHalRotatecalls in the vismach model. The only thing keeping them in step is a comment in the Python saying its matrices "must be the same as the ones used to derive the kinematic model".This adds an optional entry point a module can answer with instead, and a chapter saying what it answers in.
The columns are the tool frame axes in world coordinates. Its origin is the controlled point
kinematicsForward()already gives, so the pair is the whole tool pose.Nothing is obliged to change.
switchkinsSetup()andswitchkinsRegister()keep their signatures; a type opts in through a separate call. Nothing in motion calls the entry point, so no module has to define it, and an out-of-tree module need not know it exists. Modules built onswitchkins.cexport it and return -1 for a type that has not supplied one.The two conventions. The tree already contains both senses, and they come from two standards rather than from carelessness. ISO 9787 clause 5.3 puts a robot's flange z pointing perpendicularly away from the mechanical interface, and
pumakinsfollows it, reaching the tool tip by addingPUMA_D6along its third column. Machine tool practice puts z along the spindle, positive away from the work, which is whatG68.2commands. The chapter settles on the machine tool sense for what a module reports, and a module whose own maths is in the other one declares the rotation relating them rather than fixing it up by hand.That matters because it is not a change of sign. Negating the third column gives determinant -1, a reflection, and loses tool x as well. The declared rotation is checked once at registration for orthonormality and determinant +1, so a bad declaration fails at load rather than producing a quietly wrong frame.
Supplied for
trivkins, the identity switchkins types,xyzac-trt-kins,xyzbc-trt-kins, bothtrsrnnutating modules, andpumakins.Verification. The trt and trsrn frames were derived and then checked two independent ways against each module's own forward transform: the coefficients the forward applies to a linear-joint displacement must be the table rotation alone, and the tool axis recovered separately as the direction the tip retreats along when the tool gets longer must equal the third column. Agreement is 3e-9 across 27 poses per trsrn machine and 32 per trt machine, orthonormal, determinant one. The new test covers the shared helpers and was verified by mutation, not by passing: making the flange constant a reflection fails nine checks, and reversing the multiplication order fails four.
The
pumakinsrefactor lifts its rotation out of the forward kinematics into one function rather than writing it twice. The moved block is byte-identical.What is not covered:
pumakinsis not exercised end to end. The claim that it reports the identity at every joint zero is verified by computing its closed form, not by running the module, because there is no caller for the entry point yet. A non-realtime caller is the next piece of this work and is what would close it.Part of the multiaxis kinematics work in #4374, item 3. Replaces #4454, which was the chapter on its own; that could not merge alone without documenting an API the tree did not have.