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 @@
- Kinematics
+ - Kinematics Conventions
- DH Parameters
- 5-Axis-Kinematics
- Switchable Kinematics
diff --git a/docs/src/motion/kinematics-conventions.adoc b/docs/src/motion/kinematics-conventions.adoc
new file mode 100644
index 00000000000..a0581be09e4
--- /dev/null
+++ b/docs/src/motion/kinematics-conventions.adoc
@@ -0,0 +1,388 @@
+:lang: en
+:toc:
+
+[[cha:kinematics-conventions]]
+= Kinematics Conventions
+
+== Introduction
+
+A kinematics module answers two questions for the rest of the system: where the
+controlled point is for a given set of joint positions, and which joint
+positions put the controlled point at a requested place. `kinematicsForward()`
+and `kinematicsInverse()` are those two answers.
+
+Anything that needs more than the position of the controlled point currently
+works it out for itself. A tilted work plane needs the direction the tool
+points in. A vismach model needs the whole chain of frames. A limit or
+singularity check needs the rate at which joints move per unit of world motion.
+None of that is available through the kinematics interface, so each consumer
+rebuilds the machine geometry from the same drawing, in its own notation, and
+the copies are kept in step by hand.
+
+For the `xyzacb-trsrn` machine in `configs/sim/axis/vismach/5axis` the geometry
+is written out three times:
+
+* in `src/hal/components/xyzacb_trsrn.comp`, as closed-form expressions for
+ `pos->tran.x`, `.y` and `.z`;
+* in the config's `remap_funcs_twp.py`, as the homogeneous matrices `Rp`, `Rs`
+ and `Rtc`, under a comment reading "these matrices must be the same as the
+ ones used to derive the kinematic model";
+* in `vismach/xyzacb-trsrn-gui.py`, as a chain of `HalRotate` calls, each
+ carrying a hand-chosen sign.
+
+The three copies have no shared vocabulary and no shared sign convention, so
+"the same as" cannot be checked by anything except a person reading all three.
+
+This chapter fixes the vocabulary. It says what frames exist, which way their
+rotations go, and what the tool frame is, so that a second module can be
+written from the same drawing and give the same answers, and so that a consumer
+can state what it needs without naming a particular machine.
+
+== Frames
+
+Four frames are involved. Each is right-handed.
+
+Joint space::
+ One coordinate per joint, in the units of that joint, linear or angular. This
+ is what the motion controller commands and what `kinematicsForward()` is
+ given.
+
+Machine frame::
+ Fixed to the machine bed. Its axes are the X, Y and Z of
+ <>. Nothing rotates it.
+
+World frame::
+ Fixed to the workpiece. This is what `kinematicsForward()` returns and what
+ `kinematicsInverse()` is given. On a machine with no rotary axes, or with all
+ rotaries at zero, the world frame and the machine frame coincide. On a
+ table-rotary machine they do not: the world frame turns with the table, and
+ the kinematics undoes that rotation so that a G-code position keeps pointing
+ at the same feature of the part however the table is set.
++
+This is the reason positions are reported in it. It is also the reason the
+identity between joints and axes breaks down as soon as a rotary carries the
+work.
+
+Tool frame::
+ Fixed to the tool. Its origin is the controlled point, as defined in
+ <>: the tool tip when a tool length
+ offset is in effect, the tip of the spindle otherwise. Its third axis is the
+ tool axis, defined in <> below.
+
+A pose is always a pair of frames: the moving one and the one it is measured
+in, and the two halves of this chapter use different pairs on purpose.
+
+Positions are reported in the world frame, because a G-code position should
+keep meaning the same feature of the part however the table is set. That is
+what `kinematicsForward()` and `kinematicsInverse()` work in.
+
+Orientations are reported against the machine frame, because there are two of
+them to report and the world frame is one of the two. A module gives the tool
+frame and the work frame separately, each in machine coordinates, and a
+consumer that wants the tool in workpiece coordinates composes them:
+
+ tool_in_work = transpose(work) * tool
+
+The reason for handing over the pair rather than that product is in
+<>.
+
+[[sec:rotation-sense]]
+== Rotation Sense
+
+LinuxCNC already states its rotation convention, in
+<>:
+
+[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 for kinematics. The rotation being
+described is the rotation of the tool relative to the workpiece. On a machine
+where the rotary carries the tool, that is also the direction the physical axis
+turns. On a machine where the rotary carries the work, the physical table turns
+the other way, and the kinematics module is what 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 marks the axes of a machine that moves the
+work instead with a prime: a table turning about Z is `+C'` when it produces
+the tool motion called `+C`.
+
+A kinematics module in this tree follows that convention. Given a world pose
+whose C value increases, the tool moves counterclockwise about world Z as seen
+from the workpiece, whichever member physically turns.
+
+=== conventional-directions
+
+`trtfuncs.c` and `maxkins.c` carry a `conventional-directions` HAL pin which
+selects the sign of the rotary terms, and both default it to false, that is, to
+the sense opposite the one above. Configurations relying on the historical
+default keep working, and new configurations should set the pin true.
+
+Two costs come with leaving it false, and they are worth naming because they
+are what a convention is for. The first is that the direction a program runs in
+depends on a HAL pin rather than on the G-code. The second is subtler: the
+rotary values a module reports are the raw joint values,
+
+[source,c]
+----
+pos->a = joints[JA];
+pos->c = joints[JC];
+----
+
+while the translations in the same call have been computed with the opposite
+sign. The returned pose therefore 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, which is the immediate reason
+the tool frame has to be an explicit 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. A plain vertical mill therefore has
+a tool axis of `[0, 0, 1]` in machine coordinates at all times, and a machine
+whose spindle is parallel to Z when its rotaries are at zero has one there
+too.
+
+That last part is a property of those machines, not a rule. Where the tool
+axis points at any particular joint set is whatever the machine's geometry
+makes it, and on a robot it is not even a fixed question: `genserkins` takes
+its Denavit-Hartenberg parameters from HAL pins, so the pose at all joints zero
+is a configuration choice. `pumakins` with its supplied parameters has the tool
+axis at `[0, 0, -1]` with every joint at zero, pointing straight down.
+
+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 axis is the
+direction that length is applied along.
+
+[[sec:approach-vector]]
+=== The opposite sense, and where the tree uses it
+
+Robot kinematics name the same line the other way round. ISO 9787 clause 5.3
+puts the mechanical interface coordinate system at the centre of the flange
+with its "+Zm axis points perpendicularly away from the mechanical interface",
+which is holder towards tip, the direction the tool advances in. The
+Denavit-Hartenberg approach vector is the same sense.
+
+`pumakins` builds exactly this frame already, as the rotation part of a
+`PmHomogeneous`, and it uses the approach sense: it reaches the tool tip by
+adding the tool length along the third column,
+
+[source,c]
+----
+hom.tran.x = hom.tran.x + hom.rot.z.x*PUMA_D6;
+----
+
+so its `hom.rot.z` runs holder towards tip.
+
+Both senses are defensible and the tree contains both, because they come from
+two standards for two classes of machine. ISO 9787 puts a robot's flange z
+pointing out of the mechanical interface towards the work, and machine tool
+practice puts z along the spindle, positive away from the work. Neither is
+going to give way.
+
+This chapter settles on tip towards holder for what a module reports, because
+that is the direction a tilted work plane commands and what the machine z of a
+mill already means to the operator.
+
+Note that turning one sense into the other is not a change of sign. Negating
+the third column of a rotation leaves a matrix of determinant -1, which is a
+reflection and not a frame any machine can hold. Reversing the tool axis and
+keeping a right-handed frame takes a half turn about one of the two transverse
+axes, and which of them is chosen decides where tool x ends up. So the relation
+between a module's native frame and the convention is a rotation in its own
+right, not a correction that can be left implicit.
+
+=== Declaring the native frame
+
+Because it is a rotation, a module states it rather than applying it by hand.
+It is declared where the module registers its tool frame, and the shared code
+applies it and checks once that it is a proper rotation, orthonormal with
+determinant +1. A module whose maths is already in the convention declares the
+identity and pays nothing.
+
+Doing it that way keeps the half turn in one place instead of one per module,
+makes it greppable, and stops the next Denavit-Hartenberg module quietly
+choosing the other half turn and being wrong about tool x while looking right
+about the tool axis.
+
+Nothing writes it at runtime. It is a property of how the module's maths is
+written, fixed when the module is written, and a machine that could change it
+while running would be a machine whose geometry moves underneath the program.
+
+=== What this is not
+
+A tool or holder mount orientation is a different quantity that this chapter
+does not cover. 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. They do not belong 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 are not free to reuse: they shift the
+rotary axis reading, and configurations depend on that. Reinterpreting them as
+an orientation would change what existing programs do, silently, on exactly
+the machines where both readings are plausible. A mount orientation is
+therefore a new field rather than a reinterpretation of that one, and it wants
+to be stated as a frame that can be checked for orthonormality and
+determinant, not as three angles carrying an ordering convention that is
+written down nowhere.
+
+Expressed as a matrix, a frame is the rotation whose columns are its axes
+written in the coordinates of whatever it is measured against, and the tool
+axis is the third column of the tool frame. A module reports the tool frame
+and the work frame against the machine, so the tool axis in workpiece
+coordinates, which is 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]`,
+`matrix[2,2]`, is the third column of `transpose(work) * tool`.
+
+=== Tool X, and why it needs a rule
+
+A tilted work plane commands only where the tool points. A five-axis machine
+has two rotary joints, reaching that direction uses both of them, and the
+rotation of the tool about its own axis is then whatever the kinematic chain
+leaves rather than anything the program chose. For cutting that does not
+matter, since the cutter is a solid of revolution. It matters as soon as the
+tool frame is used as a coordinate system for programming, which is exactly
+what `G68.2` does: the operator writes X and Y moves in the tilted plane and
+needs 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 appears as the `pre-rot` pin on the in-tree
+kinematics components and as `virtual_rot` in the TWP code, and it is the same
+quantity under both names. A machine with no such pin has no say in the matter:
+its tool X is whatever its chain produces, and a consumer that needs a defined
+one has to apply the rotation itself.
+
+The convention is:
+
+[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.
+
+Note that this fixes tool X only up to a half turn, since two opposite
+directions are both 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 point of stating the convention rather than a formula is that the formula
+differs from machine to machine and is derivable from the convention. This is
+worth working through once.
+
+Write the tool orientation as the product of the primary rotation, the
+secondary rotation and the virtual rotation:
+
+ 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 entry `M[2][0]` is zero. Solving that for `tc` gives the default virtual
+rotation.
+
+For a nutating head with 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 rotation matrices, 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. Applying either formula
+to the other machine yields a frame whose tool *Y* is horizontal, rotated by a
+quarter turn from what was wanted, and no test in the tree would notice,
+because 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. A module
+that documents only its `tc` formula cannot.
+
+[[sec:consumer-needs]]
+== What a Consumer Needs
+
+Two things are asked for repeatedly and are not currently available.
+
+The tool frame and the work frame at a given joint set::
+ Not just the controlled point, which `kinematicsForward()` already gives, but
+ the orientation with it. Consumers are tilted-work-plane handling, tool
+ length compensation along a tilted axis, previews and simulation models, and
+ any probing routine that has to say which way the stylus is facing.
++
+Both are reported against the machine frame, and separately, rather than as
+the single work-to-tool rotation. That is deliberate. The product is what a
+tilted work plane wants, and a caller that wants it composes the two:
++
+ tool_in_work = transpose(work) * tool
++
+but 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; a tracking
+display does the same. Give it the product and it cannot recover where either
+of them is.
++
+On a machine that turns only the tool the work frame is the identity, and on
+one that turns only the work the tool frame is. A table-rotary head-rotary
+mill returns a non-trivial pair, and is the reason for reporting them apart.
+
+The Jacobian::
+ The matrix relating world velocity to joint velocity at a given pose, so that
+ a world-space feed can be checked against the joint velocity, acceleration
+ and limit values that will actually be demanded of the machine, 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. Neither
+needs state carried between calls, and neither 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.
+
+== Checklist for a New Module
+
+* World coordinates are workpiece-attached, so a rotary carrying the work is
+ undone in `kinematicsForward()`.
+* Positive A, B and C are counterclockwise about world X, Y and Z, viewed from
+ the positive end, describing the motion of the tool relative to the
+ workpiece.
+* The tool axis points from the tool tip towards the holder, which is the
+ opposite of the Denavit-Hartenberg approach vector; a module deriving its
+ maths that way declares the half turn that relates the two rather than
+ applying it by hand.
+* The default virtual rotation about the tool axis puts tool X parallel to the
+ machine XY plane; the value is derived from the machine's own rotation matrices, and
+ those matrices are written down in the module.
+* The work frame and the tool frame are reported separately, each against the
+ machine, so a consumer that has to place both bodies can.
+* Geometry that a consumer needs is answered by the module, not restated in the
+ consumer.
+
+== References
+
+* <>, for the axis nomenclature and the
+ rotation convention this chapter builds on.
+* <>, for worked transformations of
+ the table-rotary and tilting-table configurations.
+* <>, 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.
diff --git a/src/emc/kinematics/kinematics.h b/src/emc/kinematics/kinematics.h
index 67e6565155d..8efeb26c640 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
@@ -201,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,
@@ -212,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/kins_util.c b/src/emc/kinematics/kins_util.c
index c82a4a2fc95..0076fa4aaf4 100644
--- a/src/emc/kinematics/kins_util.c
+++ b/src/emc/kinematics/kins_util.c
@@ -45,6 +45,7 @@
#include
#include
+#include
#include
#include
#include
@@ -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 the world.
+ 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/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;
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 // }
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;
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;
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,
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
+#include
+#include
+
+#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;
+}