interp: the tilted work plane, G68.2, G68.4 and G69

docs/src/config/python-interface.adoc

@@ -179,6 +179,15 @@ see <<python:reading-ini-values,ReadingINI file values>> for an example.
 *g5x_offset*:: '(returns tuple of floats)' -
   offset of the currently active coordinate system.
 
+*g68_active*:: '(returns integer)' -
+  a tilted work plane (G68.2) is in effect.
+
+*g68_offset*:: '(returns tuple of floats)' -
+  origin of the tilted work plane, in the coordinate system it was defined in.
+
+*g68_rotation*:: '(returns tuple of floats)' -
+  rotation of the tilted work plane, nine values row by row; its columns are the plane's X, Y and Z.
+
 *g92_offset*:: '(returns tuple of floats)' -
   pose of the current g92 offset.
 

docs/src/gcode/g-code.adoc

@@ -93,6 +93,7 @@ as the 'L number', and so on for any other letter.
 |<<gcode:g61,G61>>                 |Exact Path Mode
 |<<gcode:g61.1,G61.1>>             |Exact Stop Mode
 |<<gcode:g64,G64>>                 |Path Control Mode with Optional Tolerance
+|<<gcode:g68.2,G68.2 G68.4 G69>>       |Tilted Work Plane
 |<<gcode:g70,G70>>                 |Lathe finishing cycle
 |<<gcode:g71-g72,G71-G72>>         |Lathe roughing cycle
 |<<gcode:g73,G73>>                 |Drilling Cycle with Chip Breaking
@@ -1990,6 +1991,150 @@ G64 P0.015 Q2
 .G64 Heart
 image::images/G64_Heart_Q2.png["G64 Heart",align="center"]
 
+[[gcode:g68.2]]
+== G68.2, G68.4, G69 Tilted Work Plane(((G68.2 Tilted Work Plane)))
+
+[source,ngc]
+----
+G68.2 <P0> <Q-> X- Y- Z- I- J- K- <R->   (three angles, Euler)
+G68.2 P1 <Q-> X- Y- Z- I- J- K- <R->     (three angles about fixed axes)
+G68.2 P2 Q0 X- Y- Z- <R->                (three points: a shift of the origin, then)
+G68.2 P2 Q1 X- Y- Z- <R->                (a first point,)
+G68.2 P2 Q2 X- Y- Z- <R->                (a second point on the plane's +X,)
+G68.2 P2 Q3 X- Y- Z- <R->                (a third point on its +Y side; R on one block)
+G68.2 P3 Q1 X- Y- Z- I- J- K- <R->       (two vectors: the origin and +X, then)
+G68.2 P3 Q2 I- J- K-                     (+Z, the normal)
+G68.4 ...                                (any G68.2 form, on the active plane)
+G69                                      (cancel)
+----
+
+A tilted work plane is a coordinate system composed on top of the active one:
+the blocks between the definition and 'G69' are programmed in the plane, with
+X and Y in it and Z along its normal, while the work offset underneath, 'G54'
+say, is untouched. Positions on the display, probe results and
+<<gcode:g53,G53>> all take the plane into account. It is the same offset
+chain as always with one more stage, applied first:
+
+----
+absolute = tool offset + G5x + XY rotation applied to (G92 + origin + rotation applied to program)
+----
+
+.'G68.2 X50 Y30 Z40 J30': a plane tilted 30 degrees about X, on G54, on the machine coordinates
+image::images/g68-2-plane.svg["G68.2 tilted work plane",align="center"]
+
+'X', 'Y' and 'Z' are the plane's origin ('P2' gives it by points, below) and
+the plane's rotation is built from the rest of the words, both in the
+coordinate system that is active when the plane is defined: the work offset
+with 'G92' and the XY rotation in place, which is what the position display
+shows at that moment. A rotary word does not pass through the plane, since
+on a TCP kinematics the rotary coordinates are the rotary joints and a plane
+does not change what a joint is. Words left out are zero.
+
+'P' selects how the rotation is given:
+
+* 'P0', or no 'P': three angles 'I', 'J', 'K' applied one after another,
+  each about an axis of the plane as rotated so far (Euler angles). 'Q'
+  names the axes with three digits, 1 for X, 2 for Y and 3 for Z, no two
+  adjacent alike; the default is 'Q313', Z then X then Z.
+* 'P1': three angles 'I', 'J', 'K', each about an axis of the coordinate
+  system the plane is defined in, in the order 'Q' gives; the default is
+  'Q123', X then Y then Z.
+* 'P2': three points, over up to four blocks with 'Q0' to 'Q3'. The
+  direction from the first point to the second is the plane's +X, the third
+  point lies on the +Y side. The origin is the first point. The 'Q0' block,
+  which may be left out, moves the origin by its 'X', 'Y', 'Z' along the
+  plane's own axes, after 'R': 'Q0 X10' puts the origin 10 along the plane's
+  X from the first point, the way 'X10' in a program under the plane would.
+  'R' may be given on any one of the blocks.
+* 'P3': two vectors, over two blocks. 'Q1' gives the origin and the +X
+  direction in 'I', 'J', 'K'; 'Q2' gives +Z, the normal, in 'I', 'J', 'K'.
+  The X direction is kept as given. The normal need not be exactly at right
+  angles to it, within 5 degrees; the part of the normal along X is dropped.
+
+'R' turns the plane about its own Z after everything else, in degrees.
+
+The forms follow the Fanuc tilted working plane commands, with a few
+extensions: 'R' on 'P0', 'P1' and 'P3' (Fanuc gives it with 'P2' only),
+'Q' on 'P0' (Fanuc's Euler angles are always Z, X, Z), 'P1' orders that
+repeat an axis, such as 'Q121', besides the six that name each axis once,
+and all three angles zero, which is the plane of the coordinate system
+underneath rather than an error, as on a Fanuc with parameter ATW set.
+
+The blocks of a 'P2' or 'P3' definition have to follow one another; any
+other block in between is an error. A definition left unfinished in MDI is
+dropped when a program is opened. A definition with a plane already active
+replaces it, with the words in the coordinate system underneath, not in the
+old plane.
+
+'G68.4' takes any 'G68.2' form and composes it onto the active plane: the
+words are in the plane, and the result is a new plane relative to the old
+one. It needs a plane to build on.
+
+The G-code preview draws every plane a program defines, so a program can
+be checked for where its planes sit before it runs: a rectangle lying in
+the plane, over the moves made under it with a margin around them, the
+plane's own X, Y and Z at the centre of the rectangle in the colours of
+the machine axes, and a cross at the plane's origin, which need not lie
+in the rectangle: a program drilling a sphere puts every plane's origin
+at the centre and works out on the surface. The rectangle is drawn at the plane's Z where the program comes
+down to it, and otherwise at the end of the Z range the program worked
+in that is nearest to it, so a drilling cycle that stays above the
+origin shows its holes where they are cut; a plane nothing moved under
+is drawn as a square sized from the program. A plane restated with the
+same words is one plane, not one per restatement. The plane in effect
+on the machine, the one the last executed 'G68.2' set, is drawn over
+the others in its own colour, so as the program runs the plane it is
+in stands out from the ones it has been in and will be in; a plane set
+from MDI shows the same way, as a square with nothing under it. The plane
+in effect is drawn no smaller than the coordinate system axes, with its
+own axes as long as those, so its orientation can be checked against them
+before a program runs, however small the program.
+
+'G69' cancels the plane. So does the end of the program, 'M2' or 'M30', and
+an abort: the plane is not persistent and nothing about it is written to the
+parameter file.
+
+Defining the plane does not move anything.
+
+While a plane is active the codes that define the coordinate system the
+plane sits on are refused: 'G92', 'G92.1', 'G92.2', 'G92.3', 'G52', 'G10 L2',
+'G10 L20', 'G10 L10', 'G10 L11', and a change of coordinate system
+('G54' to 'G59.3'). Cancel the plane first.
+
+The active plane is reported in the modal G-code display as the code that
+defined it. Status carries it as `g68_offset`, `g68_rotation` and
+`g68_active`, next to the other offsets, for displays that want to show
+plane coordinates or draw the plane.
+
+.G68.2 Example
+[source,ngc]
+----
+G54
+G68.2 X50 Y50 Z0 I30 J20 K0     (Euler: 30 about Z, 20 about the new X)
+G0 X0 Y0 Z5                      (5 above the plane's origin, along its normal)
+G1 Z-3 F150                      (a hole 3 deep, straight into the plane)
+G0 Z5
+G68.4 X20 P1 I0 J0 K90           (a plane 20 along X in the old one, turned 90 about Z)
+G0 X0 Y0 Z5
+G69                              (back to G54 as it was)
+----
+
+It is an error if:
+
+* 'P' is not 0, 1, 2 or 3, or 'Q' with 'P0' or 'P1' is not three axis digits
+  with no two adjacent alike.
+* A 'P2' or 'P3' definition is interrupted, or its 'Q' words come out of
+  order.
+* The points of a 'P2' definition coincide or lie on one line, or a vector of
+  a 'P3' definition is zero or the two are 5 degrees or more off square.
+* A word the form does not read is given: 'I', 'J' or 'K' with 'P2', 'X',
+  'Y', 'Z' or 'R' on the 'P3' 'Q2' block, or 'A', 'B', 'C', 'U', 'V' or 'W'
+  with any form.
+* 'R' is given on more than one block of a 'P2' definition.
+* 'G68.4' is used with no plane active.
+* Cutter compensation is on.
+* Polar coordinates or a motion code are used on the same line.
+
 [[gcode:g70]]
 == G70 Lathe finishing cycle(((G70 Lathe finishing cycle)))
 

docs/src/gcode/overview.adoc

@@ -974,6 +974,7 @@ The modal groups are shown in the following Table.
 |Units (Group 6)                        | G20, G21
 |Cutter Diameter Compensation (Group 7) | G40, G41, G42, G41.1, G42.1
 |Tool Length Offset (Group 8)           | G43, G43.1, G43.2, G43.4, G49
+|Tilted Work Plane (Group 9)            | G68.2, G68.4, G69
 |Canned Cycles Return Mode (Group 10)   | G98, G99
 |Coordinate System (Group 12)           | G54, G55, G56, G57, G58, G59, G59.1, G59.2, G59.3
 |Control Mode (Group 13)                | G61, G61.1, G64

lib/python/preview_helpers.py

@@ -1,4 +1,5 @@
 #!/usr/bin/env python3
+from math import atan2, degrees, hypot
 
 def create_unitcode_and_initcode(s, inifile):
     a_axis_wrapped = inifile.getbool("AXIS_A", "WRAPPED_ROTARY", fallback=False)
@@ -21,7 +22,36 @@ def create_unitcode_and_initcode(s, inifile):
             position = "%s%.8f " % (axis, pos)
             initcode += position
     active_gcodes = s.gcodes
-    for i in (3,6,14,7,5,4,9,12,10,16,8,11,13,15):
+    # 12 is the tilted work plane, restated with its words in unitcode
+    for i in (3,6,14,7,5,4,9,10,16,8,11,13,15):
         if active_gcodes[i] > -1:
             initcode = initcode + 'G' + str(active_gcodes[i]/10) + ' '
+    plane = workplane_code(s)
+    if plane:
+        unitcode = unitcode + ' ' + plane
     return unitcode, initcode
+
+def workplane_code(s):
+    """The tilted work plane in effect on the machine as one G68.2 block,
+    or None.  The bare code in s.gcodes cannot restate it: G68.2 alone is
+    a plane at the origin and G68.4 alone has nothing to build on.  The
+    words are in machine units, so the block goes where the machine's G20
+    or G21 is in force; a later unit change converts the plane.  The block
+    selects the machine's coordinate system first, since the plane sits on
+    it and a later block may reselect it but not change it."""
+    if not getattr(s, "g68_active", 0):
+        return None
+    r = s.g68_rotation
+    o = s.g68_offset
+    # G68.2 P1 turns about X by I, then Y by J, then Z by K: Rz Ry Rx
+    c = hypot(r[0], r[3])
+    j = atan2(-r[6], c)
+    if c > 1e-12:
+        i = atan2(r[7], r[8])
+        k = atan2(r[3], r[0])
+    else:
+        i = 0.0
+        k = atan2(-r[1], r[4])
+    system = next((g for g in s.gcodes[1:] if 540 <= g <= 593), 540)
+    return "G%s G68.2 P1 X%.12f Y%.12f Z%.12f I%.12f J%.12f K%.12f" % (
+        system // 10 if system % 10 == 0 else system / 10, o[0], o[1], o[2], degrees(i), degrees(j), degrees(k))

lib/python/rs274/glcanon.py

@@ -182,6 +182,9 @@ def __init__(self, colors, geometry, is_foam=0, foam_w=1.5, foam_z=0.0):
         # fixture may hold several pieces. A canon is built per file load, so
         # nothing else ever clears this.
         self.workpieces = []
+        # The tilted work planes the program defined, in order, from the
+        # renderer's records at the end of the parse.
+        self.workplanes = []
 
     def comment(self, arg):
         """``(WORKPIECE,...)``, ``stop``, ``notify`` and the foam Z levels.
@@ -256,6 +259,8 @@ def adopt_geometry(self, pg):
         # hands over, so a partial load yields the partial list it always did.
         self.tool_list = [tool for _lineno, tool, _points
                           in self.program_geometry.toolchanges]
+        self.workplanes = [glcanon_scene.WorkPlane(*record)
+                           for record in pg.workplanes()]
 
     # -- the program record ------------------------------------------------
 
@@ -543,6 +548,10 @@ class GlCanonDraw:
         'limits': (1.0, 0.0, 0.0),
         'workpiece': glcanon_scene.WORKPIECE_COLOR,
         'workpiece_alpha': glcanon_scene.WORKPIECE_ALPHA,
+        'workplane': glcanon_scene.WORKPLANE_COLOR,
+        'workplane_alpha': glcanon_scene.WORKPLANE_ALPHA,
+        'workplane_active': glcanon_scene.WORKPLANE_ACTIVE_COLOR,
+        'workplane_active_alpha': glcanon_scene.WORKPLANE_ACTIVE_ALPHA,
     }
     def __init__(self, s=None, lp=None, g=None):
         self.stat = s
@@ -1023,6 +1032,12 @@ def get_workpiece_opacity(self):
         blending no one can explain."""
         return min(1.0, max(0.0, float(getattr(self, 'workpiece_opacity', 0.0))))
 
+    def get_show_workplane(self):
+        """Whether the tilted work planes the program defined are drawn.
+        Defaulted like get_show_workpiece, and toggled the same way, by
+        setting self.show_workplane."""
+        return getattr(self, 'show_workplane', True)
+
     def get_workpieces(self):
         """The stock the loaded program declared, as rs274.glcanon_scene
         .Workpiece records - the declared params, the outline in machine
@@ -1163,6 +1178,7 @@ def frame_context(self) -> glcanon_scene.FrameContext:
             show_small_origin=self.show_small_origin,
             show_workpiece=self.get_show_workpiece(),
             workpiece_opacity=self.get_workpiece_opacity(),
+            show_workplane=self.get_show_workplane(),
             program_alpha=self.get_program_alpha(),
             grid_size=self.get_grid_size(),
             highlight_line=self.get_highlight_line(),
@@ -1247,6 +1263,15 @@ def posstrs(self):
                 positions[X] = _x * math.cos(t) - _y * math.sin(t)
                 positions[Y] = _x * math.sin(t) + _y * math.cos(t)
                 positions = [(i-j) for i, j in zip(positions, s.g92_offset)]
+                if s.g68_active:
+                    # the tilted work plane sits inside G92
+                    r = s.g68_rotation
+                    _x = positions[X] - s.g68_offset[X]
+                    _y = positions[Y] - s.g68_offset[Y]
+                    _z = positions[Z] - s.g68_offset[Z]
+                    positions[X] = r[0]*_x + r[3]*_y + r[6]*_z
+                    positions[Y] = r[1]*_x + r[4]*_y + r[7]*_z
+                    positions[Z] = r[2]*_x + r[5]*_y + r[8]*_z
             else:
                 positions = list(positions)