Multiaxis kinematics and limits

.gitignore

@@ -20,6 +20,10 @@ share/menus/CNC.menu
 share/desktop-directories/linuxcnc-cnc.directory
 share/desktop-directories/linuxcnc-ref.directory
 share/desktop-directories/linuxcnc-doc.directory
+share/linuxcnc/switchkins.c
+share/linuxcnc/switchkins_setup.c
+share/linuxcnc/kins_util.c
+share/linuxcnc/kins_single.c
 src/modules.order
 /configs/*/emc.nml
 !/configs/common/emc.nml

configs/sim/axis/vismach/5axis/bridgemill/5axis.ini

@@ -4,8 +4,13 @@ MACHINE = Sim-5Axis Bridge Mill (xyzbcw)
   DEBUG = 0
 
 [DISPLAY]
-         GEOMETRY = XYZCBW
-        OPEN_FILE = ./5axisgui.ngc
+# GEOMETRY tells the AXIS live plot how to guess the tool position from
+# the nine axis values: letters translate, ABC rotate the point.  W is
+# deliberately absent: the plot would add the W value as a Z offset after
+# the rotations, while the kinematics spends W along the tool axis, so
+# the guess never matches.
+         GEOMETRY = XYZCB
+        OPEN_FILE = ./g532-fused-orient-move.ngc
        INCREMENTS = 10 mm, 1 mm, .1 mm
          JOG_AXES = XYZC
           DISPLAY = axis

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

@@ -17,7 +17,6 @@ net :gui-pivot-len <= 5axisgui.pivot_len
 net :gui-pivot-len => 5axiskins.pivot-length
 
 net :tool-len <= motion.tooloffset.z
-net :tool-len => 5axiskins.tool-length
 net :tool-len => 5axisgui.tool_length
 
 net :tool-diam <= halui.tool.diameter

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

@@ -1,3 +1,17 @@
+; 5axisgui.ngc - the historical demo program, unchanged.
+; It drills a sphere pattern with W words.  W was never a physical
+; quill: no motor is connected to its joint, the kinematics folds
+; the word into the XYZ slides, and the head sliding along the tool
+; axis is what plunges the rigidly mounted tool.  Watch the slides
+; in vismach do the stroke.
+;
+; The quirk, then and now: the controller keeps W as a separate
+; world coordinate, so the programmed XYZ point does not move and
+; the preview shows nothing of the stroke; only the W DRO tracks it.
+; Drilling the controller can see, check and preview is what the
+; tilted work plane family is for (g532-fused-orient-move.ngc,
+; g536-orient-then-move.ngc).
+
         #<r>  =   60 ; sphere radius
   #<rdelta>   =    5 ; safe distance
 #<drilldepth> =   -5
@@ -13,6 +27,7 @@
 #<bdelta> = [90/#<bctmax>]
 
 g49
+g12.1 p0 ; the TCP kinematics, in case a previous run left the identity one active
 t#<toolno>m6g43
 
 g53 g0 x0y0z#<zstart>b0c0 w0

configs/sim/axis/vismach/5axis/bridgemill/README

@@ -1,19 +1,59 @@
-This is a simulation of an XYZBCWY 5 axis bridge mill.
+This is a simulation of an XYZBCWY 5 axis bridge mill with a
+tilting head (B, C) and a W axis.
 
-Example demo:
+W is not a physical axis of the machine and never was: no motor is
+connected to its joint.  The kinematics folds a W word into the
+XYZ joints, so the head slides along the tool axis and the
+rigidly mounted tool goes with it: that is the whole trick, and it
+is what vismach shows.  There is no quill to draw because there is
+no quill.
+
+The quirk: the controller keeps W as a separate world coordinate,
+so a W word leaves the programmed XYZ point untouched and the
+preview cannot show the stroke; the W DRO tracks it.  It also
+means the planner does not see the tool-axis motion, so for
+drilling the controller can check and preview, use the tilted work
+plane family.
+
+Because the W joint is virtual, a point-to-point move on it
+(g53.5/g53.7 j5=) moves no motor on a real machine while the
+controller believes the world moved, the opposite of a W word.
+Do not drill that way.
+
+Demo programs:
+
+ g532-fused-orient-move.ngc -- drill a sphere pattern with tilted work
+                             planes (g68.2), using g53.2 to solve each
+                             orientation without moving (STAY), then
+                             one fused g0 that turns the head and
+                             travels to the hole at the same time
+                             (TCP motion)
+ g536-orient-then-move.ngc -- the same pattern with g53.6 (Heidenhain
+                             MOVE style): reorient about the fixed
+                             tip, then travel in the tilted plane
+ 5axisgui.ngc --------------- the historical demo program, unchanged:
+                             it drills the same pattern with W words,
+                             the slides doing the stroke while the
+                             preview stays blind to it
+
+The tool table provides tool 100 (length 100).  Load it with
+t100m6g43 so the vismach tool and the kinematics pivot length
+(pivotsum: 250 + tool length) match.
+
+Example MDI:
 
  1) $ linuxcnc 5axis.ini
  2) F1 ---------- Estop off
     F2 ---------- Machine on
     CTRL-HOME --- home all
     F5 ---------- MDI tab
  3) orient vismach gui as required
- 4) g0w10   ; retract w
+ 4) g0w10   ; head slides up the tool axis, tip with it
  5) g43h100 ; tool offset (100)
  6) g0b45   ; tilt 45 deg wrt z
  7) g0c30   ; rotate 30 deg in xy
- 8) g0w-10  ; simulate drill
- 9) g0w10   ; retract drill
+ 8) g0w-10  ; head slides back, tip plunges 10 along the tool axis
+ 9) g0w10   ; and back
 10) etc
 
 Note: Motion for the W coordinate is incorporated

configs/sim/axis/vismach/5axis/bridgemill/g532-fused-orient-move.ngc

@@ -0,0 +1,50 @@
+; g532-fused-orient-move.ngc - drill a sphere pattern with tilted work planes:
+; g68.2 tilts the plane onto each hole normal, g53.2 solves the head
+; orientation without moving (STAY), and one fused g0 turns the head
+; and travels to the hole at the same time (TCP motion).  See README
+; for the other demos.
+
+        #<r>  =   60 ; sphere radius
+  #<rdelta>   =    5 ; safe distance
+#<drilldepth> =   -5
+   #<zsafe>   =   20 ; clearance above the ball for start and stop
+    #<bctmax> =    8
+    #<cctmax> =   16
+    #<toolno> =  100
+    #<frate>  = 1000
+
+#<bct>    = 0
+#<cct>    = 0
+#<cdelta> = [360/#<cctmax>]
+#<bdelta> = [90/#<bctmax>]
+
+g49
+g12.1 p0 ; the TCP kinematics, in case a previous run left the identity one active
+t#<toolno>m6g43
+
+g53 g0 x0 y0 z0 b0 c0
+g10 l20 p0 x0 y0 z[#<r>+#<rdelta>+#<zsafe>] b0 c0 ; ball center at program origin, we park above it
+f#<frate>
+o100 while [#<bct> lt #<bctmax>]
+       #<b> = [[#<bctmax>-1-#<bct>]*#<bdelta>] ; top ring first, the way in stays outside the ball
+       #<cct> = 0
+o200   while [#<cct> lt #<cctmax>]
+o210      if [[#<bct> mod 2] eq 0]
+              #<c> = [#<cct> * #<cdelta>]
+o210      else
+              #<c> = [360 - [1+ #<cct>] * #<cdelta>]
+o210      endif
+          g68.2 p1 j[90-#<b>] k#<c>  ; plane Z is the sphere radius at b, c
+          g53.2                      ; solve the orientation, stay put
+          g0 x0 y0 z[#<r>+#<rdelta>] b#<_orient_rot2> c#<_orient_rot1> ; one TCP turn and travel, C orients first
+          g1 z[#<r>+#<drilldepth>]
+          g0 z[#<r>+#<rdelta>]
+          #<cct> = [#<cct>+1]
+o200   endwhile
+       #<bct> = [#<bct>+1]
+o100 endwhile
+
+g69
+g53 g0 z0         ; up, clear of the ball
+g53 g0 x0 y0 b0 c0
+m2

configs/sim/axis/vismach/5axis/bridgemill/g536-orient-then-move.ngc

@@ -0,0 +1,49 @@
+; g536-orient-then-move.ngc - same sphere as g532-fused-orient-move.ngc, but with g53.6
+; (Heidenhain MOVE style): reorient about the fixed tip, a TCP move,
+; then travel to the next hole in the tilted plane with a separate g0.
+; g532-fused-orient-move.ngc fuses the turn and the travel into one move instead.
+
+        #<r>  =   60 ; sphere radius
+  #<rdelta>   =    5 ; safe distance
+#<drilldepth> =   -5
+   #<zsafe>   =   20 ; clearance above the ball for start and stop
+    #<bctmax> =    8
+    #<cctmax> =   16
+    #<toolno> =  100
+    #<frate>  = 1000
+
+#<bct>    = 0
+#<cct>    = 0
+#<cdelta> = [360/#<cctmax>]
+#<bdelta> = [90/#<bctmax>]
+
+g49
+g12.1 p0 ; the TCP kinematics, in case a previous run left the identity one active
+t#<toolno>m6g43
+
+g53 g0 x0 y0 z0 b0 c0
+g10 l20 p0 x0 y0 z[#<r>+#<rdelta>+#<zsafe>] b0 c0 ; ball center at program origin, we park above it
+f#<frate>
+o100 while [#<bct> lt #<bctmax>]
+       #<b> = [[#<bctmax>-1-#<bct>]*#<bdelta>] ; top ring first, the way in stays outside the ball
+       #<cct> = 0
+o200   while [#<cct> lt #<cctmax>]
+o210      if [[#<bct> mod 2] eq 0]
+              #<c> = [#<cct> * #<cdelta>]
+o210      else
+              #<c> = [360 - [1+ #<cct>] * #<cdelta>]
+o210      endif
+          g68.2 p1 j[90-#<b>] k#<c>  ; plane Z is the sphere radius at b, c
+          g53.6                      ; reorient about the tip, a TCP move
+          g0 x0 y0 z[#<r>+#<rdelta>]
+          g1 z[#<r>+#<drilldepth>]
+          g0 z[#<r>+#<rdelta>]
+          #<cct> = [#<cct>+1]
+o200   endwhile
+       #<bct> = [#<bct>+1]
+o100 endwhile
+
+g69
+g53 g0 z0         ; up, clear of the ball
+g53 g0 x0 y0 b0 c0
+m2

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

@@ -16,9 +16,7 @@ loadusr -W ./max5gui.py
 
 # set a visible tool
 setp max5gui.tool-radius 3
-# 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
+net tool-len motion.tooloffset.z max5gui.tool-length
 
 # add motion controller functions to servo thread
 addf motion-command-handler servo-thread

configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/README

@@ -1,27 +1,20 @@
 This is a simulation configuration for a 6 axis machine with one table rotary and two spindle rotary joints
 
-This simulation also includes a python remap of Gcodes for tilted workplane (TWP) functionality.
-Both the kinematic and the twp remap support nutation of the secondary rotary joints (ie A or B ) form 0 to 90°.
-Hence this also works for the 'usual' orthogonal spindle rotary-tilt type machines by setting the nutation angle to 90°.
+This simulation uses the interpreter's tilted work plane codes, documented in the G-code section of the manual:
 
-Implemented TWP functionality:
-G68.2 : defines twp using euler-angles, pitch-roll-yaw, 2-vectors, 3 points, optionally with offset in XYZ and rotation in XY
-G68.3 : defines twp from current tool orientation, optionally with offset in XYZ and rotation in XY
-G68.4 : same as G68.2 but as an incremental definition from an active TWP plane
-G69 : cancels the current twp (resets all parameters, moves to G54 and sets Identity kinematics)
-G53.1 (P) : spindle orientation without tcp, switches to G59 and activates tool kinematics
-G53.3 (P XYZ) : same as G53.1 but with simultaneous move the the XYZ coords on the twp plane
-G53.6 (P) : same as G53.1 but spindle orientation with tcp
+G68.2 : defines the plane by three angles, three points or two vectors, with an origin in XYZ and a turn R about the plane's Z
+G68.3 : defines the plane from the current tool direction, with an origin in XYZ and a turn R
+G68.4 : any G68.2 form, composed onto the active plane
+G69 : cancels the plane
+G53.1 (P Q) : orients the tool to the plane, rotaries only, the linear joints stay where they are
+G53.3 (P Q XYZ) : orients the tool and moves to XYZ in the plane, interpolated in joint space
+G53.6 (P Q) : orients the tool with the tool centre point held
 
-- Spindle is C primary, A secondary or B secondary as defined in the [TWP] section of the ini file
-- All G53.x commands will respect axis limits as set in the ini file for the respective primary and secondary spindle joints.
-- The P word sets the orientation strategy: 0(default)=shortest distance,
-                                            1=positive rotation only,
-                                            2=negative rotation only
-                                            (this applies to the primary rotary, the secondary moves the shortest distance)
+The orientation codes need the TCP kinematics (G12.1 P1). P picks the solution, nearest to the present rotary position first. Q0 holds the table and lets the head do it, Q1 lets the table take part as well.
+
+The kinematic supports nutation of the secondary rotary joint (A or B) from 0 to 90 degrees, so this also covers the usual orthogonal spindle rotary-tilt machines by setting the nutation angle to 90 degrees.
+
+The python maths this configuration used to carry as a remap lives on in tests/kins-twp, where the kinematics module is checked against it.
 
 For more:
 https://forum.linuxcnc.org/show-your-stuff/49103-kinematic-model-for-a-5axis-mill-with-universal-nutating-head?start=0#271334
-
-Full Documentation can be found at:
-https://github.com/Sigma1912/LinuxCNC_Demo_Configs/tree/main/table-rotary_spindle-rotary-nutating/Documentation

configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/demos/incremental_repetition.ngc

@@ -1,4 +1,5 @@
 G69
+g12.1 p1 (the TCP kinematics: the plane codes need it)
 g10 l2 p0 x1000 y-1000 z-1000
 m6 t3 g43 h3
 g68.2 q121 i25 j-10
@@ -13,5 +14,6 @@ o100 REPEAT[100]
  g0 y50
 o100 ENDREPEAT
 g69
+g13.1
 M2
 

configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/demos/incremental_repetition_back_and_forth.ngc

@@ -1,4 +1,5 @@
 g69
+g12.1 p1 (the TCP kinematics: the plane codes need it)
 g10 l2 p0 x1300 y-200 z-1400
 m6 t3 g43 h3
 g68.2 q121 i0 j5
@@ -20,4 +21,5 @@ o100 REPEAT[1000]
 
 o100 ENDREPEAT
 g69
+g13.1
 M2

configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/demos/incremental_repetition_g533.ngc

@@ -1,4 +1,5 @@
 G69
+g12.1 p1 (the TCP kinematics: the plane codes need it)
 g10 l2 p0 x1000 y-1000 z-1000
 m6 t3 g43 h3
 g68.2 q121 i25 j-10
@@ -7,7 +8,7 @@ o100 REPEAT[100]
  g68.4 q131 i-35 j-35 k0
  ;g53.3 p0 x50y50z150
  g53.6
- x50y50z150
+ g0 x50y50z150
  g0 z100
  g0 x-50
  g0 y-50
@@ -16,5 +17,6 @@ o100 REPEAT[100]
  g0 x0y0z120
 o100 ENDREPEAT
 g69
+g13.1
 M2
 

configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/demos/simple_example.ngc

@@ -1,4 +1,6 @@
 g69
+g13.1
+g12.1 p1 (the TCP kinematics: the plane codes need it)
 g10 l2 p0 x1300 y-200 z-1400
 m6 t3 g43 h3
 g0 x0y0z100

configs/sim/axis/vismach/5axis/table-rotary_spindle-rotary-nutating/demos/square.ngc

@@ -5,5 +5,4 @@ o<square>sub
  g0 x50
  g0 y50
  g0 x0y0z120
- g52 x0y0z0
 o<square>endsub