normalize code coming along

README.md

@@ -8,6 +8,8 @@ I'll be learning along the way, but the plan is to follow the lead of [GRBL](htt
 
 `pip install pygcode`
 
+FIXME: well, that's the plan... give me some time to get it going though.
+
 ## Usage
 
 Just brainstorming here...

pygcode/gcodes.py

@@ -1,6 +1,8 @@
+import sys
 from collections import defaultdict
 from copy import copy
 
+from .utils import Vector3, Quaternion, quat2coord_system
 from .words import Word, text2words
 
 from .exceptions import GCodeParameterError, GCodeWordStrError
@@ -249,15 +251,19 @@ class GCode(object):
             if l in self.modal_param_letters
         ])
 
-    def get_param_dict(self, letters=None):
+    def get_param_dict(self, letters=None, lc=False):
         """
         Get gcode parameters as a dict
         gcode parameter like "X3.1, Y-2" would return {'X': 3.1, 'Y': -2}
         :param letters: iterable whitelist of letters to include as dict keys
+        :param lc: lower case parameter letters
         :return: dict of gcode parameters' (letter, value) pairs
         """
+        letter_mod = lambda x: x
+        if lc:
+            letter_mod = lambda x: x.lower()
         return dict(
-            (w.letter, w.value) for w in self.params.values()
+            (letter_mod(w.letter), w.value) for w in self.params.values()
             if (letters is None) or (w.letter in letters)
         )
 
@@ -470,6 +476,7 @@ class GCodeAbsoluteDistanceMode(GCodeDistanceMode):
     word_key = Word('G', 90)
     modal_group = MODAL_GROUP_MAP['distance']
 
+
 class GCodeIncrementalDistanceMode(GCodeDistanceMode):
     """G91: Incremental Distance Mode"""
     word_key = Word('G', 91)
@@ -701,20 +708,48 @@ class GCodePlaneSelect(GCode):
     modal_group = MODAL_GROUP_MAP['plane_selection']
     exec_order = 150
 
+    # -- Plane Orientation Quaternion
+    # Such that...
+    #   vectorXY = Vector3(<your coords in X/Y plane>)
+    #   vectorZX = GCodeSelectZXPlane.quat * vectorXY
+    #   vectorZX += some_offset_vector
+    #   vectorXY = GCodeSelectZXPlane.quat.conjugate() * vectorZX
+    # note: all quaternions use the XY plane as a basis
+    # To transform from ZX to YZ planes via these quaternions, you must
+    # first translate it to XY, like so:
+    #   vectorYZ = GCodeSelectYZPlane.quat * (GCodeSelectZXPlane.quat.conjugate() * vectorZX)
+    quat = None  # Quaternion
+
+    # -- Plane Normal
+    # Vector normal to plane (such that XYZ axes follow the right-hand rule)
+    normal = None  # Vector3
+
 
 class GCodeSelectXYPlane(GCodePlaneSelect):
     """G17: select XY plane (default)"""
     word_key = Word('G', 17)
+    quat = Quaternion()  # no effect
+    normal = Vector3(0, 0, 1)
 
 
 class GCodeSelectZXPlane(GCodePlaneSelect):
     """G18: select ZX plane"""
     word_key = Word('G', 18)
+    quat = quat2coord_system(
+        Vector3(1, 0, 0), Vector3(0, 1, 0),
+        Vector3(0, 0, 1), Vector3(1, 0, 0)
+    )
+    normal = Vector3(0, 1, 0)
 
 
 class GCodeSelectYZPlane(GCodePlaneSelect):
     """G19: select YZ plane"""
     word_key = Word('G', 19)
+    quat = quat2coord_system(
+        Vector3(1, 0, 0), Vector3(0, 1, 0),
+        Vector3(0, 1, 0), Vector3(0, 0, 1)
+    )
+    normal = Vector3(1, 0, 0)
 
 
 class GCodeSelectUVPlane(GCodePlaneSelect):

pygcode/machine.py

@@ -6,10 +6,13 @@ from .gcodes import (
     # Modal GCodes
     GCodeIncrementalDistanceMode,
     GCodeUseInches, GCodeUseMillimeters,
+    # Utilities
+    words2gcodes,
 )
 from .block import Block
 from .line import Line
 from .words import Word
+from .utils import Vector3, Quaternion
 
 from .exceptions import MachineInvalidAxis, MachineInvalidState
 
@@ -134,6 +137,10 @@ class Position(object):
     def values(self):
         return dict(self._value)
 
+    @property
+    def vector(self):
+        return Vector3(self._value['X'], self._value['Y'], self._value['Z'])
+
     def __repr__(self):
         return "<{class_name}: {coordinates}>".format(
             class_name=self.__class__.__name__,
@@ -356,12 +363,26 @@ class Machine(object):
             return modal_gcodes[0]
         return None
 
+    def block_modal_gcodes(self, block):
+        """
+        Block's GCode list in current machine mode
+        :param block: Block instance
+        :return: list of gcodes, block.gcodes + <modal gcode, if there is one>
+        """
+        assert isinstance(block, Block), "invalid parameter"
+        gcodes = copy(block.gcodes)
+        modal_gcode = self.modal_gcode(block.modal_params)
+        if modal_gcode:
+            gcodes.append(modal_gcode)
+        return sorted(gcodes)
+
     def process_gcodes(self, *gcode_list, **kwargs):
         """
         Process gcodes
         :param gcode_list: list of GCode instances
         :param modal_params: list of Word instances to be applied to current movement mode
         """
+        gcode_list = list(gcode_list) # make appendable
         # Add modal gcode to list of given gcodes
         modal_params = kwargs.get('modal_params', [])
         if modal_params:

pygcode/transform.py

@@ -0,0 +1,134 @@
+from math import acos
+
+from .gcodes import GCodeArcMove, GCodeArcMoveCW, GCodeArcMoveCCW
+from .gcodes import GCodeSelectXYPlane, GCodeSelectYZPlane, GCodeSelectZXPlane
+from .gcodes import GCodeAbsoluteDistanceMode, GCodeIncrementalDistanceMode
+from .gcodes import GCodeAbsoluteArcDistanceMode, GCodeIncrementalArcDistanceMode
+
+from .machine import Position
+from .utils import Vector3, Quaternion, plane_projection
+
+# ==================== Arcs (G2,G3) --> Linear Motion (G1) ====================
+
+
+class ArcLinearizeMethod(object):
+    pass
+
+    def __init__(self, max_error, radius)
+        self.max_error = max_error
+        self.radius = radius
+
+    def get_max_wedge_angle(self):
+        """Calculate angular coverage of a single line reaching maximum allowable error"""
+        raise NotImplementedError("not overridden")
+
+
+class ArcLinearizeInside(ArcLinearizeMethod):
+    """Start and end points of each line are on the original arc"""
+    # Attributes / Trade-offs:
+    #   - Errors cause arc to be slightly smaller
+    #       - pocket milling action will remove less material
+    #       - perimeter milling action will remove more material
+    #   - Each line is the same length
+    #   - Simplest maths, easiest to explain & visually verify
+
+    def get_max_wedge_angle(self):
+        return 2 * acos((self.radius - self.max_error) / self.radius)
+
+
+
+
+
+class ArcLinearizeOutside(ArcLinearizeMethod):
+    """Mid-points of each line are on the original arc, first and last lines are 1/2 length"""
+    # Attributes / Trade-offs:
+    #   - Errors cause arc to be slightly larger
+    #       - pocket milling action will remove more material
+    #       - perimeter milling action will remove less material
+    #   - 1st and last lines are 1/2 length of the others
+
+
+class ArcLinearizeMid(ArcLinearizeMethod):
+    """Lines cross original arc from tangent of arc radius - precision/2, until it reaches arc radius + precision/2"""
+    # Attributes / Trade-offs:
+    #   - Closest to original arc (error distances are equal inside and outside the arc)
+    #   - Most complex to calculate (but who cares, that's only done once)
+    #   - default linearizing method as it's probably the best
+
+
+DEFAULT_LA_METHOD = ArcLinearizeMid
+DEFAULT_LA_PLANE = GCodeSelectXYPlane
+DEFAULT_LA_DISTMODE = GCodeAbsoluteDistanceMode
+DEFAULT_LA_ARCDISTMODE = GCodeIncrementalArcDistanceMode
+
+def linearize_arc(arc_gcode, start_pos, plane=None, method_class=None,
+                  dist_mode=None, arc_dist_mode=None,
+                  max_error=0.01, precision_fmt="{0:.3f}"):
+    # set defaults
+    if method_class is None:
+        method_class = DEFAULT_LA_method_class
+    if plane_selection is None:
+        plane_selection = DEFAULT_LA_PLANE
+    if dist_mode is None:
+        dist_mode = DEFAULT_LA_DISTMODE
+    if arc_dist_mode is None:
+        arc_dist_mode = DEFAULT_LA_ARCDISTMODE
+
+    # Parameter Type Assertions
+    assert isinstance(arc_gcode, GCodeArcMove), "bad arc_gcode type: %r" % arc_gcode
+    assert isinstance(start_pos, Position), "bad start_pos type: %r" % start_pos
+    assert isinstance(plane, GCodePlaneSelect), "bad plane type: %r" % plane
+    assert issubclass(method_class, ArcLinearizeMethod), "bad method_class type: %r" % method_class
+    assert isinstance(dist_mode, (GCodeAbsoluteDistanceMode, GCodeIncrementalDistanceMode)), "bad dist_mode type: %r" % dist_mode
+    assert isinstance(arc_dist_mode, (GCodeAbsoluteArcDistanceMode, GCodeIncrementalArcDistanceMode)), "bad arc_dist_mode type: %r" % arc_dist_mode
+    assert max_error > 0, "max_error must be > 0"
+
+    # Arc Start
+    arc_start = start_pos.vector
+    # Arc End
+    arc_end_coords = dict((l, 0.0) for l in 'xyz')
+    arc_end_coords.update(g.arc_gcode('XYZ', lc=True))
+    arc_end = Vector3(**arc_end_coords)
+    if isinstance(dist_mode, GCodeIncrementalDistanceMode):
+        arc_end += start_pos.vector
+    # Arc Center
+    arc_center_ijk = dict((l, 0.0) for l in 'IJK')
+    arc_center_ijk.update(g.arc_gcode('IJK'))
+    arc_center_coords = dict(({'I':'x','J':'y','K':'z'}[k], v) for (k, v) in arc_center_ijk.items())
+    arc_center = Vector3(**arc_center_coords)
+    if isinstance(arc_dist_mode, GCodeIncrementalArcDistanceMode):
+        arc_center += start_pos.vector
+
+    # Planar Projections
+    arc_p_start = plane_projection(arc_start, plane.normal)
+    arc_p_end = plane_projection(arc_p_end, plane.normal)
+    arc_p_center = plane_projection(arc_center, plane.normal)
+
+    # Radii, center-point adjustment
+    r1 = arc_p_start - arc_p_center
+    r2 = arc_p_end  - arc_p_center
+    radius = (abs(r1) + abs(r2)) / 2.0
+
+    arc_p_center = ( # average radii along the same vectors
+        (arc_p_start - (r1.normalized() * radius)) +
+        (arc_p_end - (r2.normalized() * radius))
+    ) / 2.0
+    # FIXME: nice idea, but I don't think it's correct...
+    #        ie: re-calculation of r1 & r2 will not yield r1 == r2
+    #        I think I have to think more pythagoreanly... yeah, that's a word now
+
+    method = method_class(
+        max_error=max_error,
+        radius=radius,
+    )
+
+    #plane_projection(vect, normal)
+
+    pass
+    # Steps:
+    #   - calculate:
+    #       -
+    #   - calculate number of linear segments
+
+
+# ==================== Arc Precision Adjustment ====================

pygcode/utils.py

@@ -0,0 +1,65 @@
+import sys
+
+if sys.version_info < (3, 0):
+    from euclid import Vector3, Quaternion
+else:
+    from euclid3 import Vector3, Quaternion
+
+
+# ==================== Geometric Utilities ====================
+def quat2align(to_align, with_this, normalize=True):
+    """
+    Calculate Quaternion that will rotate a given vector to align with another
+    can accumulate with perpendicular alignemnt vectors like so:
+        (x_axis, z_axis) = (Vector3(1, 0, 0), Vector3(0, 0, 1))
+        q1 = quat2align(v1, z_axis)
+        q2 = quat2align(q1 * v2, x_axis)
+        # assuming v1 is perpendicular to v2
+        q3 = q2 * q1  # application of q3 to any vector will re-orient it to the
+                      # coordinate system defined by (v1,v2), as (z,x) respectively.
+    :param to_align: Vector3 instance to be rotated
+    :param with_this: Vector3 instance as target for alignment
+    :result: Quaternion such that: q * to_align == with_this
+    """
+    # Normalize Vectors
+    if normalize:
+        to_align = to_align.normalized()
+        with_this = with_this.normalized()
+    # Calculate Quaternion
+    return Quaternion.new_rotate_axis(
+        angle=to_align.angle(with_this),
+        axis=to_align.cross(with_this),
+    )
+
+
+def quat2coord_system(origin1, origin2, align1, align2):
+    """
+    Calculate Quaternion to apply to any vector to re-orientate it to another
+    (target) coordinate system.
+    (note: both origin and align coordinate systems must use right-hand-rule)
+    :param origin1: origin(1|2) are perpendicular vectors in the original coordinate system
+    :param origin2: see origin1
+    :param align1: align(1|2) are 2 perpendicular vectors in the target coordinate system
+    :param align2: see align1
+    :return: Quaternion such that q * origin1 = align1, and q * origin2 = align2
+    """
+    # Normalize Vectors
+    origin1 = origin1.normalized()
+    origin2 = origin2.normalized()
+    align1 = align1.normalized()
+    align2 = align2.normalized()
+    # Calculate Quaternion
+    q1 = quat2align(origin1, align1, normalize=False)
+    q2 = quat2align(q1 * origin2, align2, normalize=False)
+    return q2 * q1
+
+
+def plane_projection(vect, normal):
+    """
+    Project vect to a plane represented by normal
+    :param vect: vector to be projected (Vector3)
+    :param normal: normal of plane to project on to (Vector3)
+    :return: vect projected onto plane represented by normal
+    """
+    n = normal.normalized()
+    return v - (n * v.dot(n))

scripts/pygcode-normalize.py

@@ -0,0 +1,98 @@
+#!/usr/bin/env python
+import argparse
+
+for pygcode_lib_type in ('installed_lib', 'relative_lib'):
+    try:
+        # pygcode
+        from pygcode import Machine, Mode, Line
+        from pygcode import GCodeArcMove, GCodeArcMoveCW, GCodeArcMoveCCW
+        from pygcode import split_gcodes
+        from pygcode.transform import linearize_arc
+
+    except ImportError:
+        import sys, os, inspect
+        # Add pygcode (relative to this test-path) to the system path
+        _this_path = os.path.dirname(os.path.abspath(inspect.getfile(inspect.currentframe())))
+        sys.path.insert(0, os.path.join(_this_path, '..'))
+        if pygcode_lib_type == 'installed_lib':
+            continue # import was attempted before sys.path addition. retry import
+        raise # otherwise the raised ImportError is a genuine problem
+    break
+
+
+# =================== Command Line Arguments ===================
+# --- Defaults
+DEFAULT_PRECISION = 0.005  # mm
+DEFAULT_MACHINE_MODE = 'G0 G54 G17 G21 G90 G94 M5 M9 T0 F0 S0'
+
+# --- Create Parser
+parser = argparse.ArgumentParser(description='Normalize gcode for machine consistency using different CAM software')
+parser.add_argument(
+    'infile', type=argparse.FileType('r'), nargs=1,
+    help="gcode file to normalize",
+)
+
+parser.add_argument(
+    '--precision', '-p', dest='precision', type=float, default=DEFAULT_PRECISION,
+    help="maximum positional error when generating gcodes (eg: arcs to lines)",
+)
+
+# Machine
+parser.add_argument(
+    '--machine_mode', '-mm', dest='machine_mode', default=DEFAULT_MACHINE_MODE,
+    help="Machine's startup mode as gcode (default: '%s')" % DEFAULT_MACHINE_MODE,
+)
+
+# Arcs
+parser.add_argument(
+    '--arcs_linearize', '-al', dest='arcs_linearize',
+    action='store_const', const=True, default=False,
+    help="convert G2/3 commands to a series of linear G1 linear interpolations",
+)
+parser.add_argument(
+    '--arc_alignment', '-aa', dest='arc_alignment', type=str, choices=('XYZ','IJK','R'),
+    default=None,
+    help="enforce precision on arcs, if XYZ the destination is altered to match the radius"
+         "if IJK or R then the arc'c centre point is moved to assure precision",
+)
+
+# --- Parse Arguments
+args = parser.parse_args()
+
+
+# =================== Create Virtual CNC Machine ===================
+class MyMode(Mode):
+    default_mode = args.machine_mode
+
+class MyMachine(Machine):
+    MODE_CLASS = MyMode
+
+machine = MyMachine()
+
+# =================== Utility Functions ===================
+def gcodes2str(gcodes):
+    return ' '.join("%s" % g for g in gcodes)
+
+
+# =================== Process File ===================
+print(args)
+
+for line_str in args.infile[0].readlines():
+    line = Line(line_str)
+
+    effective_gcodes = machine.block_modal_gcodes(line.block)
+
+    if any(isinstance(g, GCodeArcMove) for g in effective_gcodes):
+        print("---------> Found an Arc <----------")
+        (before, (arc,), after) = split_gcodes(effective_gcodes, GCodeArcMove)
+        if before:
+            print(gcodes2str(before))
+        print(str(arc))
+        if after:
+            print(gcodes2str(after))
+
+
+
+    print("%r, %s" % (sorted(line.block.gcodes), line.block.modal_params))
+
+    machine.process_block(line.block)