5axiskins, maxkins: take the tool length through a pin like every other rotary module

configs/sim/axis/vismach/5axis/bridgemill/5axisgui.hal

@@ -13,20 +13,12 @@ net :jy2 joint.6.pos-fb
 # Note: no joint or motor connections are used
 # for the w coordinate
 
-# compute pivot-length needed for 5axiskins
-# according to gui pivot_len and tool z offset
-loadrt sum2 names=pivotsum
-addf pivotsum servo-thread
-
 net :gui-pivot-len <= 5axisgui.pivot_len
-net :gui-pivot-len => pivotsum.in0
+net :gui-pivot-len => 5axiskins.pivot-length
 
 net :tool-len <= motion.tooloffset.z
-net :tool-len => pivotsum.in1
+net :tool-len => 5axiskins.tool-length
 net :tool-len => 5axisgui.tool_length
 
-net :pivot-len <= pivotsum.out
-net :pivot-len => 5axiskins.pivot-length
-
 net :tool-diam <= halui.tool.diameter
 net :tool-diam => 5axisgui.tool_diam

configs/sim/axis/vismach/5axis/max5/max5kins.hal

@@ -15,9 +15,10 @@ loadrt not count=5
 loadusr -W max5gui
 
 # set a visible tool
-#setp max5gui.tool-length 35
 setp max5gui.tool-radius 3
-net tool-len motion.tooloffset.w max5gui.tool-length
+# the tool length is applied along the tool, not along Z, so the tip stays
+# on the programmed point as B tilts
+net tool-len motion.tooloffset.z max5gui.tool-length maxkins.tool-length
 
 # add motion controller functions to servo thread
 addf motion-command-handler servo-thread

docs/src/man/man9/kins.9.adoc

@@ -240,6 +240,22 @@ tilting head (B axis) and horizontal rotary mounted to the table (C axis).
 Provides UVW motion in the rotated coordinate system.
 The source file, maxkins.c, may be a useful starting point for other 5-axis systems.
 
+The W coordinate is added to the pivot to tip distance, so a quill or
+tool slide programmed on W extends the tool and the B correction follows
+it. This is the same treatment 5axiskins gives W. A tool length in the W
+column of the tool table therefore reaches the kinematics too, once a
+block commands W, and it adds to *maxkins.tool-length* rather than
+replacing it. Put a given length in one column or the other, not both.
+
+*maxkins.pivot-length*::
+  Distance from the B rotation point to the tool holder gauge line.
+
+*maxkins.tool-length*::
+  Tool length, applied along the tool rather than along Z, so that the tip
+  stays on the programmed point as B tilts. Net it from *motion.tooloffset.z*.
+  Left unconnected, the tool length stays where canon put it, along machine
+  Z, which is only correct at B0.
+
 === pentakins - Pentapod Kinematics
 
 Gives five degrees of freedom in position and orientation (XYZAB).
@@ -392,6 +408,18 @@ Note: These kinematics may be used with the vismach 5axisgui providing
 that the joint-letter assignments agree with the default ordering
 expected by it (XYZBCW `->` joints 0..5)
 
+*5axiskins.pivot-length*::
+  Distance from the B rotation point to the tool holder gauge line.
+
+*5axiskins.tool-length*::
+  Tool length, applied along the tool rather than along Z, so that the tip
+  stays on the programmed point as B and C move. Net it from
+  *motion.tooloffset.z*. Left unconnected, the tool length stays where canon
+  put it, along machine Z, which is only correct at B0. A tool length in the
+  W column of the tool table reaches the same place, once a block commands W,
+  and adds to this pin rather than replacing it. Put a given length in one
+  column or the other, not both.
+
 == SEE ALSO
 
 For additional information, see following subsections of the section 'Advanced Topics' of the LinuxCNC documentation:

src/emc/kinematics/5axiskins.c

@@ -65,6 +65,7 @@
 
 static struct haldata {
     hal_real_t pivot_length;
+    hal_real_t tool_length;
 } *haldata;
 static int fiveaxis_max_joints;
 
@@ -104,14 +105,15 @@ static int fiveaxis_KinematicsForward(const double *joints,
     (void)fflags;
     (void)iflags;
     rtapi_real pivot_length = hal_get_real(haldata->pivot_length);
-    PmCartesian r = s2r(pivot_length + joints[JW],
+    rtapi_real tool_length  = hal_get_real(haldata->tool_length);
+    PmCartesian r = s2r(pivot_length + joints[JW] + tool_length,
                         joints[JC],
                         180.0 - joints[JB]);
 
     // Note: 'principal' joints are used
     pos->tran.x = joints[JX] + r.x;
     pos->tran.y = joints[JY] + r.y;
-    pos->tran.z = joints[JZ] + pivot_length + r.z;
+    pos->tran.z = joints[JZ] + pivot_length + tool_length + r.z;
     pos->b      = joints[JB];
     pos->c      = joints[JC];
     pos->w      = joints[JW];
@@ -132,14 +134,15 @@ static int fiveaxis_KinematicsInverse(const EmcPose * pos,
     (void)iflags;
     (void)fflags;
     rtapi_real pivot_length = hal_get_real(haldata->pivot_length);
-    PmCartesian r = s2r(pivot_length + pos->w,
+    rtapi_real tool_length  = hal_get_real(haldata->tool_length);
+    PmCartesian r = s2r(pivot_length + pos->w + tool_length,
                         pos->c,
                         180.0 - pos->b);
 
     EmcPose P;  // computed position
     P.tran.x = pos->tran.x - r.x;
     P.tran.y = pos->tran.y - r.y;
-    P.tran.z = pos->tran.z - pivot_length - r.z;
+    P.tran.z = pos->tran.z - pivot_length - tool_length - r.z;
 
     P.b = pos->b;
     P.c = pos->c;
@@ -219,6 +222,10 @@ int fiveaxis_KinematicsSetup(const  int   comp_id,
                               DEFAULT_PIVOT_LENGTH, "%s.pivot-length", kp->halprefix);
     if(result < 0) goto error;
 
+    result = hal_pin_new_real(comp_id, HAL_IN, &(haldata->tool_length),
+                              0.0, "%s.tool-length", kp->halprefix);
+    if(result < 0) goto error;
+
     rtapi_print("Kinematics Module %s\n",__FILE__);
     rtapi_print("  module name = %s\n"
                 "  coordinates = %s  Requires: [KINS]JOINTS>=%d\n"

src/emc/kinematics/maxkins.c

@@ -31,6 +31,7 @@
 
 static struct haldata {
     hal_real_t pivot_length;
+    hal_real_t tool_length;
     hal_bool_t conventional_directions; //default is false
 } *haldata;
 
@@ -44,10 +45,11 @@ int kinematicsForward(const double *joints,
 
     rtapi_real con = hal_get_bool(haldata->conventional_directions) ? 1.0 : -1.0;
     rtapi_real pivot_length = hal_get_real(haldata->pivot_length);
+    rtapi_real tool_length = hal_get_real(haldata->tool_length);
 
     // B correction
-    const double zb = (pivot_length + joints[8]) * cos(d2r(joints[4]));
-    const double xb = (pivot_length + joints[8]) * sin(d2r(joints[4]));
+    const double zb = (pivot_length + joints[8] + tool_length) * cos(d2r(joints[4]));
+    const double xb = (pivot_length + joints[8] + tool_length) * sin(d2r(joints[4]));
 
     // C correction
     const double xyr = hypot(joints[0], joints[1]);
@@ -61,7 +63,7 @@ int kinematicsForward(const double *joints,
 
     pos->tran.x = xyr * cos(xytheta) - (con * xb) - xv;
     pos->tran.y = xyr * sin(xytheta) - joints[7];
-    pos->tran.z = joints[2] - zb - (con * zv) + pivot_length;
+    pos->tran.z = joints[2] - zb - (con * zv) + pivot_length + tool_length;
 
     pos->a = joints[3];
     pos->b = joints[4];
@@ -83,10 +85,11 @@ int kinematicsInverse(const EmcPose * pos,
 
     rtapi_real con = hal_get_bool(haldata->conventional_directions) ? 1.0 : -1.0;
     rtapi_real pivot_length = hal_get_real(haldata->pivot_length);
+    rtapi_real tool_length = hal_get_real(haldata->tool_length);
 
     // B correction
-    const double zb = (pivot_length + pos->w) * cos(d2r(pos->b));
-    const double xb = (pivot_length + pos->w) * sin(d2r(pos->b));
+    const double zb = (pivot_length + pos->w + tool_length) * cos(d2r(pos->b));
+    const double xb = (pivot_length + pos->w + tool_length) * sin(d2r(pos->b));
 
     // C correction
     const double xyr = hypot(pos->tran.x, pos->tran.y);
@@ -100,7 +103,7 @@ int kinematicsInverse(const EmcPose * pos,
 
     joints[0] = xyr * cos(xytheta) + (con * xb) + xv;
     joints[1] = xyr * sin(xytheta) + pos->v;
-    joints[2] = pos->tran.z + zb - (con * zv) - pivot_length;
+    joints[2] = pos->tran.z + zb - (con * zv) - pivot_length - tool_length;
 
     joints[3] = pos->a;
     joints[4] = pos->b;
@@ -133,6 +136,7 @@ int rtapi_app_main(void) {
     if(!haldata) { result = -ENOMEM; goto error; }
 
     result  = hal_pin_new_real(comp_id, HAL_IO, &(haldata->pivot_length), 0.666, "maxkins.pivot-length");
+    result += hal_pin_new_real(comp_id, HAL_IN, &(haldata->tool_length), 0.0, "maxkins.tool-length");
     // default is unconventional
     result += hal_pin_new_bool(comp_id, HAL_IN, &(haldata->conventional_directions), 0, "maxkins.conventional-directions");