first working arc linearizing

pygcode/gcodes.py

@@ -152,7 +152,7 @@ class GCode(object):
     # Execution Order
     exec_order = 999  # if not otherwise specified, run last
 
-    def __init__(self, *words):
+    def __init__(self, *words, **params):
         """
         :param word: Word instance defining gcode (eg: Word('G0') for rapid movement)
         :param params: list of Word instances (eg: Word('X-1.2') as x-coordinate)
@@ -170,6 +170,8 @@ class GCode(object):
         # Add Given Parameters
         for param_word in param_words:
             self.add_parameter(param_word)
+        for (k, v) in params.items():
+            self.add_parameter(Word(k, v))
 
     def __repr__(self):
         param_str = ''

pygcode/transform.py

@@ -1,5 +1,6 @@
-from math import acos, atan2, pi, sqrt
+from math import acos, atan2, pi, sqrt, ceil
 
+from .gcodes import GCodeLinearMove
 from .gcodes import GCodeArcMove, GCodeArcMoveCW, GCodeArcMoveCCW
 from .gcodes import GCodePlaneSelect, GCodeSelectXYPlane, GCodeSelectYZPlane, GCodeSelectZXPlane
 from .gcodes import GCodeAbsoluteDistanceMode, GCodeIncrementalDistanceMode
@@ -15,14 +16,27 @@ from .utils import Vector3, Quaternion, plane_projection
 class ArcLinearizeMethod(object):
     pass
 
-    def __init__(self, max_error, radius):
+    def __init__(self, max_error, plane_normal,
+                 arc_p_start, arc_p_end, arc_p_center,
+                 arc_radius, arc_angle, helical_start, helical_end):
         self.max_error = max_error
-        self.radius = radius
+        self.plane_normal = plane_normal
+        self.arc_p_start = arc_p_start
+        self.arc_p_end = arc_p_end
+        self.arc_p_center = arc_p_center
+        self.arc_radius = arc_radius
+        self.arc_angle = arc_angle
+        self.helical_start = helical_start
+        self.helical_end = helical_end
 
     def get_max_wedge_angle(self):
         """Calculate angular coverage of a single line reaching maximum allowable error"""
         raise NotImplementedError("not overridden")
 
+    def iter_vertices(self):
+        """Yield absolute (<start vertex>, <end vertex>) for each line for the arc"""
+        raise NotImplementedError("not overridden")
+
 
 class ArcLinearizeInside(ArcLinearizeMethod):
     """Start and end points of each line are on the original arc"""
@@ -34,9 +48,30 @@ class ArcLinearizeInside(ArcLinearizeMethod):
     #   - Simplest maths, easiest to explain & visually verify
 
     def get_max_wedge_angle(self):
-        return 2 * acos((self.radius - self.max_error) / self.radius)
+        return abs(2 * acos((self.arc_radius - self.max_error) / self.arc_radius))
 
+    def iter_vertices(self):
+        wedge_count = int(ceil(abs(self.arc_angle) / self.get_max_wedge_angle()))
+        wedge_angle = self.arc_angle / wedge_count
+        start_radius = self.arc_p_start - self.arc_p_center
+        helical_delta = (self.helical_end - self.helical_start) / wedge_count
 
+        l_p_start = start_radius + self.arc_p_center
+        l_start = l_p_start + self.helical_start
+        for i in range(wedge_count):
+            q_end = Quaternion.new_rotate_axis(
+                angle=wedge_angle * (i+1),
+                axis=-self.plane_normal,
+            )
+            # Projected on selected plane
+            l_p_end = (q_end * start_radius)  + self.arc_p_center
+            # Helical displacement
+            l_end = l_p_end + (self.helical_start + (helical_delta * (i+1)))
+
+            yield (l_start, l_end)
+
+            # start of next line is the end of this line
+            (l_p_start, l_start) = (l_p_end, l_end)
 
 
 
@@ -64,7 +99,7 @@ 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}"):
+                  max_error=0.01, decimal_places=3):
     # set defaults
     if method_class is None:
         method_class = DEFAULT_LA_method_class
@@ -177,40 +212,56 @@ def linearize_arc(arc_gcode, start_pos, plane=None, method_class=None,
     #   - helical_start distance along plane.normal of arc start
     #   - helical_disp  distance along plane.normal of arc end
 
-    # TODO: debug printing
-    print((
-        "linearize_arc params\n"
-        "   - arc_p_start   {arc_p_start}\n"
-        "   - arc_p_end     {arc_p_end}\n"
-        "   - arc_p_center  {arc_p_center}\n"
-        "   - arc_radius    {arc_radius}\n"
-        "   - arc_angle     {arc_angle:.4f} ({arc_angle_deg:.3f} deg)\n"
-        "   - helical_start {helical_start}\n"
-        "   - helical_end   {helical_end}\n"
-    ).format(
-        arc_p_start=arc_p_start,
-        arc_p_end=arc_p_end,
-        arc_p_center=arc_p_center,
-        arc_radius=arc_radius,
-        arc_angle=arc_angle, arc_angle_deg=arc_angle * (180/pi),
-        helical_start=helical_start,
-        helical_end=helical_end,
-    ))
+    #print((
+    #    "linearize_arc params\n"
+    #    "   - arc_p_start   {arc_p_start}\n"
+    #    "   - arc_p_end     {arc_p_end}\n"
+    #    "   - arc_p_center  {arc_p_center}\n"
+    #    "   - arc_radius    {arc_radius}\n"
+    #    "   - arc_angle     {arc_angle:.4f} ({arc_angle_deg:.3f} deg)\n"
+    #    "   - helical_start {helical_start}\n"
+    #    "   - helical_end   {helical_end}\n"
+    #).format(
+    #    arc_p_start=arc_p_start,
+    #    arc_p_end=arc_p_end,
+    #    arc_p_center=arc_p_center,
+    #    arc_radius=arc_radius,
+    #    arc_angle=arc_angle, arc_angle_deg=arc_angle * (180/pi),
+    #    helical_start=helical_start,
+    #    helical_end=helical_end,
+    #))
 
+    method_class_params = {
+        'max_error': max_error,
+        'plane_normal': plane.normal,
+        'arc_p_start': arc_p_start,
+        'arc_p_end': arc_p_end,
+        'arc_p_center': arc_p_center,
+        'arc_radius': arc_radius,
+        'arc_angle': arc_angle,
+        'helical_start': helical_start,
+        'helical_end': helical_end,
+    }
+    method = method_class(**method_class_params)
 
+    #import ipdb; ipdb.set_trace()
+    if isinstance(dist_mode, GCodeAbsoluteDistanceMode):
+        # Absolute coordinates
+        for line_vertices in method.iter_vertices():
+            (l_start, l_end) = line_vertices
+            yield GCodeLinearMove(**dict(zip('XYZ', l_end.xyz)))
+    else:
+        # Incremental coordinates (beware cumulative errors)
+        cur_pos = arc_start
+        for line_vertices in method.iter_vertices():
+            (l_start, l_end) = line_vertices
+            l_delta = l_end - cur_pos
             
-    method = method_class(
-        max_error=max_error,
-        radius=arc_radius,
-    )
-
-    #plane_projection(vect, normal)
-
-    pass
-    # Steps:
-    #   - calculate:
-    #       -
-    #   - calculate number of linear segments
+            # round delta coordinates (introduces errors)
+            for axis in 'xyz':
+                setattr(l_delta, axis, round(getattr(l_delta, axis), decimal_places))
+            yield GCodeLinearMove(**dict(zip('XYZ', l_delta.xyz)))
+            cur_pos += l_delta # mitigate errors by also adding them the accumulated cur_pos
 
 
 # ==================== Arc Precision Adjustment ====================

pygcode/words.py

@@ -16,7 +16,7 @@ def _clean_codestr(value):
     return "%g" % value
 
 CLEAN_NONE = lambda v: v
-CLEAN_FLOAT = lambda v: "%g" % v
+CLEAN_FLOAT = lambda v: "{0:g}".format(round(v, 3))
 CLEAN_CODE = _clean_codestr
 CLEAN_INT = lambda v: "%g" % v
 

scripts/pygcode-normalize.py

@@ -1,5 +1,7 @@
 #!/usr/bin/env python
 import argparse
+import re
+from collections import defaultdict
 
 for pygcode_lib_type in ('installed_lib', 'relative_lib'):
     try:
@@ -46,10 +48,18 @@ parser.add_argument(
 
 # Arcs
 parser.add_argument(
-    '--arcs_linearize', '-al', dest='arcs_linearize',
+    '--arc_linearize', '-al', dest='arc_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_lin_method', '-alm', dest='arc_lin_method', default='i',
+    help="Method of linearizing arcs, i=inner, o=outer, m=middle. List 2 "
+         "for <ccw>,<cw>, eg 'i,o'. also: 'm' is equivalent to 'm,m' ",
+    metavar='{i,o,m}[,{i,o,m]',
+)
+
 parser.add_argument(
     '--arc_alignment', '-aa', dest='arc_alignment', type=str, choices=('XYZ','IJK','R'),
     default=None,
@@ -61,6 +71,31 @@ parser.add_argument(
 args = parser.parse_args()
 
 
+# arc linearizing method (manually parsing)
+ARC_LIN_CLASS_MAP = {
+    'i': ArcLinearizeInside,
+    'o': ArcLinearizeOutside,
+    'm': ArcLinearizeMid,
+}
+
+arc_lin_method_regex = re.compile(r'^(?P<g2>[iom])(,(?P<g3>[iom]))?$', re.I)
+if args.arc_lin_method:
+    match = arc_lin_method_regex.search(args.arc_lin_method)
+    if not match:
+        raise RuntimeError("parameter for --arc_lin_method is invalid: '%s'" % args.arc_lin_method)
+
+    # changing args.arc_lin_method (because I'm a fiend)
+    args.arc_lin_method = {}
+    args.arc_lin_method['G2'] = ARC_LIN_CLASS_MAP[match.group('g2')]
+    if match.group('g3'):
+        args.arc_lin_method['G3'] = ARC_LIN_CLASS_MAP[match.group('g3')]
+    else:
+        args.arc_lin_method['G3'] = args.arc_lin_method['G2']
+else:
+    args.arc_lin_method = defaultdict(lambda: ArcLinearizeInside) # just to be sure
+
+
+
 # =================== Create Virtual CNC Machine ===================
 class MyMode(Mode):
     default_mode = args.machine_mode
@@ -76,40 +111,45 @@ def gcodes2str(gcodes):
 
 
 # =================== Process File ===================
-print(args)
 
 for line_str in args.infile[0].readlines():
     line = Line(line_str)
-    if line.comment:
-        print("===== %s" % line.comment.text)
+    #if line.comment:
+    #    print("===== %s" % line.comment.text)
 
     effective_gcodes = machine.block_modal_gcodes(line.block)
 
-    if any(isinstance(g, GCodeArcMove) for g in effective_gcodes):
-        print("---------> Found an Arc <----------")
+    if args.arc_linearize and any(isinstance(g, GCodeArcMove) for g in effective_gcodes):
+        #print("---------> Found an Arc <----------")
         (befores, (arc,), afters) = split_gcodes(effective_gcodes, GCodeArcMove)
         # TODO: debug printing (for now)
         if befores:
-            print("befores: %s" % gcodes2str(befores))
+            print(gcodes2str(befores))
             machine.process_gcodes(*befores)
-        print("arc: %s" % str(arc))
-        linearize_arc(
-            arc_gcode=arc,
-            start_pos=machine.pos,
-            plane=machine.mode.plane_selection,
-            method_class=ArcLinearizeInside,  # FIXME: selectable from args
-            dist_mode=machine.mode.distance,
-            arc_dist_mode=machine.mode.arc_ijk_distance,
-            max_error=args.precision,
-        )
+        #print("arc: %s" % str(arc))
+        linearize_params = {
+            'arc_gcode': arc,
+            'start_pos': machine.pos,
+            'plane': machine.mode.plane_selection,
+            'method_class': args.arc_lin_method["%s%i" % (arc.word.letter, arc.word.value)],
+            'dist_mode': machine.mode.distance,
+            'arc_dist_mode': machine.mode.arc_ijk_distance,
+            'max_error': args.precision,
+            'decimal_places': 3,
+        }
+        for linear_gcode in linearize_arc(**linearize_params):
+            print(linear_gcode)
         machine.process_gcodes(arc)
 
         if afters:
-            print("afters: %s" % gcodes2str(afters))
+            print(gcodes2str(afters))
             machine.process_gcodes(*afters)
+        if line.comment:
+            print(str(line.comment))
     else:
+        print(str(line))
         machine.process_block(line.block)
 
 
 
-    print("%r, %s" % (sorted(line.block.gcodes), line.block.modal_params))
+    #print("%r, %s" % (sorted(line.block.gcodes), line.block.modal_params))