From a1b3fc41d2c2093efd47aea2edcc77f91bae1bd5 Mon Sep 17 00:00:00 2001 From: Peter Boin Date: Mon, 10 Jul 2017 00:32:17 +1000 Subject: [PATCH] normalize code coming along --- README.md | 2 + pygcode/gcodes.py | 39 +++++++++- pygcode/machine.py | 21 ++++++ pygcode/transform.py | 134 +++++++++++++++++++++++++++++++++++ pygcode/utils.py | 65 +++++++++++++++++ scripts/pygcode-normalize.py | 98 +++++++++++++++++++++++++ 6 files changed, 357 insertions(+), 2 deletions(-) create mode 100644 pygcode/transform.py create mode 100644 pygcode/utils.py create mode 100755 scripts/pygcode-normalize.py diff --git a/README.md b/README.md index cc50c33..ca8ab5b 100644 --- a/README.md +++ b/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... diff --git a/pygcode/gcodes.py b/pygcode/gcodes.py index 167aea2..cd23752 100644 --- a/pygcode/gcodes.py +++ b/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() + # 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): diff --git a/pygcode/machine.py b/pygcode/machine.py index 9ff7300..a0be733 100644 --- a/pygcode/machine.py +++ b/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 + + """ + 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: diff --git a/pygcode/transform.py b/pygcode/transform.py new file mode 100644 index 0000000..ea10821 --- /dev/null +++ b/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 ==================== diff --git a/pygcode/utils.py b/pygcode/utils.py new file mode 100644 index 0000000..3a8f8ed --- /dev/null +++ b/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)) diff --git a/scripts/pygcode-normalize.py b/scripts/pygcode-normalize.py new file mode 100755 index 0000000..f27c918 --- /dev/null +++ b/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)