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,36 @@ extern int kinematicsHome(struct EmcPose * world,
 
 extern KINEMATICS_TYPE kinematicsType(void);
 
+/* the tool frame function gives the orientation of the tool for a set of joint
+   values, as the rotation whose columns are the three tool frame axes expressed
+   in world coordinates.  The third of them is the tool axis, a direction and
+   not to be confused with the tool length, which is the distance applied along
+   it.  The origin of the frame is the controlled point that kinematicsForward()
+   reports for the same joints, so the two calls together are the whole tool
+   pose.
+
+   The conventions it answers in are in the Kinematics Conventions chapter of
+   the documentation: world coordinates are attached to the workpiece, and the
+   tool axis runs from the tool tip towards the holder.  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.
+
+   The entry point is optional.  Modules built on switchkins.c export it always
+   and return -1 for a switchkins type that has not supplied one; other modules
+   need not export it at all, so a caller resolving it dynamically has to cope
+   with its absence.
+
+   Returns 0 on success, -1 if no tool frame is available. */
+extern int kinematicsToolFrame(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 +187,30 @@ extern int identityKinematicsInverse(const struct EmcPose * world,
                                      const KINEMATICS_INVERSE_FLAGS * iflags,
                                      KINEMATICS_FORWARD_FLAGS * fflags);
 
+/* joints are axes, so the tool frame never turns */
+extern int identityKinematicsToolFrame(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);
+
+/* 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 +255,10 @@ 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 xyzbcKinematicsForward(const double *joints,
                                   EmcPose * pos,
@@ -212,4 +270,8 @@ 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);
+
 //*********************************************************************

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,96 @@ 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 identityKinematicsToolFrame(const double *joints,
+                                PmRotationMatrix *rot,
+                                const KINEMATICS_FORWARD_FLAGS *fflags)
+{
+    (void)joints;
+    (void)fflags;
+    // joints are axes, so the tool frame is the world frame at every pose
+    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;
+
+    return 0;
+} // identityKinematicsToolFrame()

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,16 @@ 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 pumaKinematicsInverse(const EmcPose * world,
                                  double * joint,
                                  const KINEMATICS_INVERSE_FLAGS * iflags,
@@ -371,6 +408,10 @@ 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
+    switchkinsRegisterToolFrame(0, pumaKinematicsToolFrame,
+                                &TOOL_FRAME_FLANGE);
 
     *kset1 = identityKinematicsSetup;
     *kfwd1 = identityKinematicsForward;