kinematics: a Jacobian entry point, supplied by every module

docs/src/motion/kinematics-conventions.adoc

@@ -279,12 +279,11 @@ The joint values that reach a requested orientation, through
 question a tilted work plane asks when it has to orient the machine.
 <<sec:orientation-inverse,The Orientation Inverse>> says what it answers.
 
-The Jacobian, relating commanded velocity to joint velocity at a given pose, so
-that a feed can be checked against the joint velocity, acceleration and limit
-values it will actually demand, and so that proximity to a singularity is a
-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.
+How joint motion follows world motion, through `kinematicsJacobian()`, so
+that a feed can be checked against the joint velocity and acceleration it will
+actually demand, and so that proximity to a singularity is a number rather
+than a surprise. <<sec:jacobian,The Jacobian>> says what it answers and in
+which units.
 
 All of these are functions of the joint values and the module's own geometry.
 None needs state carried between calls, and none needs the module to be running
@@ -377,6 +376,106 @@ The search is not a realtime routine. How long it takes depends on the machine
 and on the request, and the callers that want it, orienting a tilted work plane
 and previewing a program, are not in the servo loop.
 
+[[sec:jacobian]]
+== The Jacobian
+
+A feed is a speed in the work frame. What the machine has to deliver is a
+speed at each joint, and on any kinematics that is not the identity the two
+are related by where the machine is. The Jacobian is that relation at one
+pose: how each joint responds to a unit rate of each pose coordinate.
+
+  jac[j][a] = d joint[j] / d pose[a]
+
+Rows are joints. Columns are the pose coordinates in `EmcPose` order, X Y Z A
+B C U V W. It is the derivative of `kinematicsInverse()`: multiplied by a pose
+velocity it gives the joint velocity motion will command, which is what a feed
+limit compares with the joint limits. Joint `j` binds when
+
+  |jac[j] . tangent| * F
+
+exceeds that joint's velocity limit, `tangent` being the direction of the move
+in pose coordinates and `F` the feed along it. The acceleration limit follows
+from a second Jacobian taken further along the path, with no more from the
+module. A row that grows without bound is a pose approaching a singularity,
+where no world speed is slow enough for the joints to follow.
+
+=== Units
+
+Each entry is in joint units per pose unit, whatever units the module's own
+forward and inverse already use. Nothing is converted: a caller that feeds
+pose rates in `EmcPose` units gets joint rates in the units motion already
+commands, and never has to know which unit a rotary joint is in. On every
+module in the tree both are degrees, so a table rotary's own row is a 1 in its
+own column, and a robot's rotary rows carry degrees per millimetre against the
+linear columns.
+
+This is why the Jacobian, unlike the orientation inverse, does not need the
+interface to name the rotary joint unit. Every number in it is a ratio of
+quantities that already pass through `kinematicsForward()` and
+`kinematicsInverse()`, and the caller never combines it with anything measured
+in another unit.
+
+=== Frame
+
+The columns are pose coordinates, so the answer lives in the work frame, where
+`kinematicsForward()` reports positions. The A, B and C columns are rates of
+the pose words, the wrapped linear axes the planner already treats as
+coordinates, and not an angular velocity vector: on a machine that carries the
+work the forward writes the rotary joint into the pose word, and that column
+says exactly that, a 1 for its own joint.
+
+That makes this a different object from the frames of
+<<sec:frames,Frames>>, and the two rules are kept apart deliberately. A frame
+is an orientation, and a renderer placing two bodies needs each against
+something fixed, so frames are reported against the machine. A Jacobian is a
+derivative of the pose, and everything that uses it multiplies it by a pose
+rate, so it is reported where the pose is. A module whose maths produces a
+twist in the machine frame, which is what the Denavit-Hartenberg modules
+produce, turns it into pose word rates through the matrix of the axes each
+pose word turns about, once, inside the module. `genserkins` does this, and
+having it written once there is worth more than the closed form itself, since
+every consumer would otherwise guess it.
+
+=== What a module has to supply
+
+Nothing. If a module registers no Jacobian, the shared code computes one by
+central differences: it calls the module's inverse eighteen times, stepping
+each pose coordinate a small amount to either side on the solution branch
+the inverse flags select, and differences the results. The answer is as
+precise as the inverse itself; behind an inverse that iterates, that is the
+iteration's convergence tolerance divided by the step. Modules built on `switchkins.c` answer this way for every type that
+registers nothing; an identity type answers exactly.
+
+This cost matters because `kinematicsJacobian()` is meant to run in the
+servo thread, where checking a feed limit calls it once per cycle: each call
+then costs eighteen inverses. On the closed-form inverses in the tree that
+is under two microseconds. `genserkins`, the one module whose inverse
+iterates, would cost near eighty microseconds per call this way, against
+under two for the geometric Jacobian it registers instead.
+
+So a module whose inverse iterates should register a closed-form Jacobian,
+as below. Nothing enforces this. A module that registers nothing still gets
+a correct Jacobian, just one that costs eighteen inverses and carries the
+iteration's precision, which behind an iterating inverse is too slow for the
+servo thread.
+
+A module with a closed form registers it with `switchkinsRegisterJacobian()`.
+It is exact, it costs what the inverse costs, and it knows its own singular
+poses rather than discovering them as an inverse that fails a step away from
+the pose. Every module in the tree supplies one. The two arms whose inverse is
+a chain of arc tangents, `pumakins` and `three21kins`, take theirs from
+their Denavit-Hartenberg chain through `kinsJacobianFromDhArm()`, which any
+six-joint serial arm can call with its own table, written in the modified
+convention of Craig's Introduction to Robotics: each joint's axis crossed
+with the vector from it to the tool point gives the point's rate, the axis
+itself the angular rate, and the six by six that makes is inverted and
+taken through the roll, pitch and yaw rates the pose words are.
+
+A module reading its rotary angles from the joint argument of the inverse
+rather than from the pose, which the nutating heads do, has an inverse whose
+derivative about the pose is not the coupling the machine has. Such a module
+supplies the closed form, taken against the pose.
+
 [[sec:writing-a-module]]
 == Writing a Module
 
@@ -406,6 +505,12 @@ Orientation inverse::
   do nothing. Register a closed form only where one exists, and where it does,
   say which poses it treats as degenerate.
 
+Jacobian::
+  Rows are joints, columns are pose coordinates, entries in the units the
+  forward and inverse already use, reported where the pose is. A module with
+  a closed form inverse differentiates it and registers the result; one
+  without lets the shared code difference the inverse.
+
 Geometry stays in the module::
   Whatever a consumer needs to know about the machine's shape is answered by
   the module. A consumer that restates it has taken a copy that nothing keeps

src/emc/kinematics/5axiskins.c

@@ -162,6 +162,46 @@ static int fiveaxis_KinematicsInverse(const EmcPose * pos,
     return 0;
 } // fiveaxis_kinematicsInverse()
 
+static int fiveaxis_KinematicsJacobian(const double *joints,
+                                       const EmcPose * pos,
+                                       double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS],
+                                       const KINEMATICS_INVERSE_FLAGS * iflags)
+{
+    (void)joints;
+    (void)iflags;
+    rtapi_real pivot_length = hal_get_real(haldata->pivot_length);
+    const double R  = pivot_length + pos->w;
+    const double sb = sin(TO_RAD*pos->b), cb = cos(TO_RAD*pos->b);
+    const double sc = sin(TO_RAD*pos->c), cc = cos(TO_RAD*pos->c);
+    double dP[EMCMOT_MAX_AXIS][EMCMOT_MAX_AXIS];
+    int a;
+
+    memset(dP, 0, sizeof(dP));
+
+    // the computed position of the inverse is the pose less the pivot
+    // vector r = s2r(R, c, 180 - b), which is (R sin b cos c, R sin b sin c,
+    // -R cos b); each row is that coordinate differentiated
+    dP[0][0] = 1;
+    dP[0][4] = -R * cb * cc * TO_RAD;
+    dP[0][5] =  R * sb * sc * TO_RAD;
+    dP[0][8] = -sb * cc;
+
+    dP[1][1] = 1;
+    dP[1][4] = -R * cb * sc * TO_RAD;
+    dP[1][5] = -R * sb * cc * TO_RAD;
+    dP[1][8] = -sb * sc;
+
+    dP[2][2] = 1;
+    dP[2][4] = -R * sb * TO_RAD;
+    dP[2][8] =  cb;
+
+    for (a = 3; a < EMCMOT_MAX_AXIS; a++) { dP[a][a] = 1; }
+
+    return kinsJacobianFromMappedAxes(fiveaxis_max_joints,
+                                      (const double (*)[EMCMOT_MAX_AXIS])dP,
+                                      jac);
+} // fiveaxis_KinematicsJacobian()
+
 int fiveaxis_KinematicsSetup(const  int   comp_id,
                              const  char* coordinates,
                              kparms*      kp)
@@ -264,11 +304,13 @@ int switchkinsSetup(kparms* kp,
         *kinv1 = fiveaxis_KinematicsInverse;
         switchkinsDeclare(0, KINSTYPE_IDENTITY);
         switchkinsDeclare(1, KINSTYPE_PRIMARY);
+        switchkinsRegisterJacobian(1, fiveaxis_KinematicsJacobian);
     } else {
         rtapi_print("\n!!! switchkins-type 0 is %s\n",kp->kinsname);
         *kset0 = fiveaxis_KinematicsSetup;
         *kfwd0 = fiveaxis_KinematicsForward;
         *kinv0 = fiveaxis_KinematicsInverse;
+        switchkinsRegisterJacobian(0, fiveaxis_KinematicsJacobian);
 
         *kset1 = identityKinematicsSetup;
         *kfwd1 = identityKinematicsForward;

src/emc/kinematics/corexykins.c

@@ -6,6 +6,7 @@
 
 #include <rtapi.h>
 #include <rtapi_app.h>
+#include <rtapi_string.h>
 #include <hal.h>
 #include <kinematics.h>
 
@@ -49,6 +50,23 @@ int kinematicsInverse(const EmcPose *pos
     return 0;
 }
 
+int kinematicsJacobian(const double *joints,
+                       const EmcPose *pos,
+                       double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS],
+                       const KINEMATICS_INVERSE_FLAGS *iflags)
+{
+    int j;
+    (void)joints;
+    (void)pos;
+    (void)iflags;
+    memset(jac, 0, EMCMOT_MAX_JOINTS * EMCMOT_MAX_AXIS * sizeof(jac[0][0]));
+    // the two belt motors each carry x and y, in opposite senses for y
+    jac[0][0] = 1; jac[0][1] =  1;
+    jac[1][0] = 1; jac[1][1] = -1;
+    for (j = 2; j < 9; j++) { jac[j][j] = 1; }
+    return 0;
+}
+
 int kinematicsHome(EmcPose *world
                   ,double *joint
                   ,KINEMATICS_FORWARD_FLAGS *fflags
@@ -65,6 +83,7 @@ KINS_NOT_SWITCHABLE
 EXPORT_SYMBOL(kinematicsType);
 EXPORT_SYMBOL(kinematicsForward);
 EXPORT_SYMBOL(kinematicsInverse);
+EXPORT_SYMBOL(kinematicsJacobian);
 MODULE_LICENSE("GPL");
 
 static int comp_id;

src/emc/kinematics/genhexkins.c

@@ -544,6 +544,73 @@ static int genhexKinematicsInverse(const EmcPose * pos,
   return 0;
 } //genhexKinematicsInverse()
 
+/************************ genhexKinematicsJacobian() ***********************/
+/* A strut length changes by the component of its platform end's motion
+   along the strut.  That end moves with the platform, dP + w x (R a), so
+   the row for strut i is [u_i, (R a_i x u_i) . E] with u_i the unit strut
+   vector and E the matrix taking the rates of the roll, pitch and yaw
+   words to the angular velocity w for R = Rz(c) Ry(b) Rx(a).  The forward
+   kinematics builds the same rows for its Newton step, in radians. */
+
+static int genhexKinematicsJacobian(const double * joints,
+                                    const EmcPose * pos,
+                                    double jac[EMCMOT_MAX_JOINTS][EMCMOT_MAX_AXIS],
+                                    const KINEMATICS_INVERSE_FLAGS * iflags)
+{
+  PmCartesian aw, RMatrix_a, strut, u, moment;
+  PmRotationMatrix RMatrix;
+  PmRpy rpy;
+  PmCartesian E[3];
+  double sb, cb, sc, cc;
+  int i, m;
+
+  genhex_read_hal_pins();
+
+  /* the screw lead correction is a function of the pose too, and this
+     does not differentiate it; difference the inverse instead */
+  if (hal_get_real(haldata->screw_lead) != 0.0) {
+    return kinsJacobianFromInverse(genhexKinematicsInverse, NUM_STRUTS,
+                                   joints, pos, iflags, jac);
+  }
+
+  memset(jac, 0, EMCMOT_MAX_JOINTS * EMCMOT_MAX_AXIS * sizeof(jac[0][0]));
+
+  rpy.r = pos->a * PM_PI / 180.0;
+  rpy.p = pos->b * PM_PI / 180.0;
+  rpy.y = pos->c * PM_PI / 180.0;
+  pmRpyMatConvert(&rpy, &RMatrix);
+
+  /* w = E [da db dc]: the roll axis carried by pitch and yaw, the pitch
+     axis carried by yaw, and the yaw axis fixed */
+  sb = sin(rpy.p); cb = cos(rpy.p);
+  sc = sin(rpy.y); cc = cos(rpy.y);
+  E[0].x = cb*cc; E[0].y = cb*sc; E[0].z = -sb;
+  E[1].x = -sc;   E[1].y = cc;    E[1].z = 0;
+  E[2].x = 0;     E[2].y = 0;     E[2].z = 1;
+
+  for (i = 0; i < NUM_STRUTS; i++) {
+    double len;
+
+    pmMatCartMult(&RMatrix, &a[i], &RMatrix_a);
+    pmCartCartAdd(&pos->tran, &RMatrix_a, &aw);
+    pmCartCartSub(&aw, &b[i], &strut);
+    pmCartMag(&strut, &len);
+    if (len <= 0) { return -1; }
+    pmCartScalMult(&strut, 1.0/len, &u);
+    pmCartCartCross(&RMatrix_a, &u, &moment);
+
+    jac[i][0] = u.x;
+    jac[i][1] = u.y;
+    jac[i][2] = u.z;
+    for (m = 0; m < 3; m++) {
+      double dot;
+      pmCartCartDot(&moment, &E[m], &dot);
+      jac[i][3+m] = dot * PM_PI / 180.0;
+    }
+  }
+  return 0;
+} // genhexKinematicsJacobian()
+
 // HAL pin initializaion values. In small arrays so we can easily
 // address them in the pin creation loop.
 static const rtapi_real init_basex[NUM_STRUTS] = {
@@ -707,6 +774,7 @@ int switchkinsSetup(kparms* kp,
         *kinv1 = genhexKinematicsInverse;
         switchkinsDeclare(0, KINSTYPE_IDENTITY);
         switchkinsDeclare(1, KINSTYPE_PRIMARY);
+        switchkinsRegisterJacobian(1, genhexKinematicsJacobian);
     } else {
         rtapi_print("\n!!! switchkins-type 0 is %s\n",kp->kinsname);
         kp->fwd_iterates_mask = 0x1; //genhexkins switchkins_type==0
@@ -715,6 +783,7 @@ int switchkinsSetup(kparms* kp,
         *kset0 = genhexKinematicsSetup;
         *kfwd0 = genhexKinematicsForward;
         *kinv0 = genhexKinematicsInverse;
+        switchkinsRegisterJacobian(0, genhexKinematicsJacobian);
 
         *kset1 = identityKinematicsSetup;
         *kfwd1 = identityKinematicsForward;