kinematics: let a module report where the tool points

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

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[]

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 \

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

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>

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);
+
 //*********************************************************************

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 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()