From c3f822f8025b960cabb478b7da71b6704a7a2dc3 Mon Sep 17 00:00:00 2001 From: Peter Boin Date: Tue, 11 Jul 2017 00:11:32 +1000 Subject: [PATCH] arc calcs for linearizing (wip) --- pygcode/gcodes.py | 48 ++++++++++-- pygcode/machine.py | 12 ++- pygcode/transform.py | 144 +++++++++++++++++++++++++++-------- pygcode/utils.py | 3 +- scripts/pygcode-normalize.py | 33 ++++++-- 5 files changed, 190 insertions(+), 50 deletions(-) diff --git a/pygcode/gcodes.py b/pygcode/gcodes.py index cd23752..6c08bdd 100644 --- a/pygcode/gcodes.py +++ b/pygcode/gcodes.py @@ -227,6 +227,17 @@ class GCode(object): self.params[word.letter] = word + # Assert Parameters + def assert_params(self): + """ + Assert validity of gcode's parameters. + This verification is irrespective of machine, or machine's state; + verification is g-code language-based verification + :raises: GCodeParameterError + """ + # to be overridden in inheriting classes + pass + def __getattr__(self, key): # Return parameter values (if valid parameter for gcode) if key in self.param_letters: @@ -334,6 +345,29 @@ class GCodeArcMove(GCodeMotion): """Arc Move""" param_letters = GCodeMotion.param_letters | set('IJKRP') + def assert_params(self): + param_letters = set(self.params.keys()) + # Parameter groups + params_xyz = set('XYZ') & set(param_letters) + params_ijk = set('IJK') & set(param_letters) + params_r = set('R') & set(param_letters) + params_ijkr = params_ijk | params_r + + # --- Parameter Groups + # XYZ: at least 1 + if not params_xyz: + raise GCodeParameterError("no XYZ parameters set for destination: %r" % arc_gcode) + # IJK or R: only in 1 group + if params_ijk and params_r: + raise GCodeParameterError("both IJK and R parameters defined: %r" % arc_gcode) + # IJKR: at least 1 + if not params_ijkr: + raise GCodeParameterError("neither IJK or R parameters defined: %r" % arc_gcode) + + # --- Parameter Values + if params_r and (self.R == 0): + raise GCodeParameterError("cannot plot a circle with a radius of zero: %r" % arc_gcode) + class GCodeArcMoveCW(GCodeArcMove): """G2: Arc Move (clockwise)""" @@ -729,27 +763,27 @@ class GCodeSelectXYPlane(GCodePlaneSelect): """G17: select XY plane (default)""" word_key = Word('G', 17) quat = Quaternion() # no effect - normal = Vector3(0, 0, 1) + 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) + Vector3(1., 0., 0.), Vector3(0., 1., 0.), + Vector3(0., 0., 1.), Vector3(1., 0., 0.) ) - normal = Vector3(0, 1, 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) + Vector3(1., 0., 0.), Vector3(0., 1., 0.), + Vector3(0., 1., 0.), Vector3(0., 0., 1.) ) - normal = Vector3(1, 0, 0) + normal = Vector3(1., 0., 0.) class GCodeSelectUVPlane(GCodePlaneSelect): diff --git a/pygcode/machine.py b/pygcode/machine.py index a0be733..1c625bc 100644 --- a/pygcode/machine.py +++ b/pygcode/machine.py @@ -52,6 +52,10 @@ class Position(object): self._value = defaultdict(lambda: 0.0, dict((k, 0.0) for k in self.axes)) self._value.update(kwargs) + def update(self, **coords): + for (k, v) in coords.items(): + setattr(self, k, v) + # Attributes Get/Set def __getattr__(self, key): if key in self.axes: @@ -428,8 +432,10 @@ class Machine(object): # =================== Machine Actions =================== def move_to(self, rapid=False, **coords): """Move machine to given position""" - given_position = Position(axes=self.axes, **coords) if isinstance(self.mode.distance, GCodeIncrementalDistanceMode): - self.pos += given_position + pos_delta = Position(axes=self.axes, **coords) + self.pos += pos_delta else: # assumed: GCodeAbsoluteDistanceMode - self.pos = given_position + new_pos = self.pos + new_pos.update(**coords) # only change given coordinates + self.pos = new_pos diff --git a/pygcode/transform.py b/pygcode/transform.py index ea10821..60fb6af 100644 --- a/pygcode/transform.py +++ b/pygcode/transform.py @@ -1,20 +1,21 @@ -from math import acos +from math import acos, atan2, pi, sqrt from .gcodes import GCodeArcMove, GCodeArcMoveCW, GCodeArcMoveCCW -from .gcodes import GCodeSelectXYPlane, GCodeSelectYZPlane, GCodeSelectZXPlane +from .gcodes import GCodePlaneSelect, GCodeSelectXYPlane, GCodeSelectYZPlane, GCodeSelectZXPlane from .gcodes import GCodeAbsoluteDistanceMode, GCodeIncrementalDistanceMode from .gcodes import GCodeAbsoluteArcDistanceMode, GCodeIncrementalArcDistanceMode from .machine import Position +from .exceptions import GCodeParameterError from .utils import Vector3, Quaternion, plane_projection -# ==================== Arcs (G2,G3) --> Linear Motion (G1) ==================== +# ==================== Arcs (G2,G3) --> Linear Motion (G1) ==================== class ArcLinearizeMethod(object): pass - def __init__(self, max_error, radius) + def __init__(self, max_error, radius): self.max_error = max_error self.radius = radius @@ -67,12 +68,12 @@ def linearize_arc(arc_gcode, start_pos, plane=None, method_class=None, # set defaults if method_class is None: method_class = DEFAULT_LA_method_class - if plane_selection is None: - plane_selection = DEFAULT_LA_PLANE + if plane is None: + plane = DEFAULT_LA_PLANE() if dist_mode is None: - dist_mode = DEFAULT_LA_DISTMODE + dist_mode = DEFAULT_LA_DISTMODE() if arc_dist_mode is None: - arc_dist_mode = DEFAULT_LA_ARCDISTMODE + arc_dist_mode = DEFAULT_LA_ARCDISTMODE() # Parameter Type Assertions assert isinstance(arc_gcode, GCodeArcMove), "bad arc_gcode type: %r" % arc_gcode @@ -86,40 +87,121 @@ def linearize_arc(arc_gcode, start_pos, plane=None, method_class=None, # 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_coords = dict(zip('xyz', arc_start.xyz)) + arc_end_coords.update(arc_gcode.get_param_dict('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) + arc_p_end = plane_projection(arc_end, plane.normal) + + # Arc radius, calcualted one of 2 ways: + # - R: arc radius is provided + # - IJK: arc's center-point is given, errors mitigated + arc_gcode.assert_params() + if 'R' in arc_gcode.params: + # R: radius magnitude specified + if abs(arc_p_start - arc_p_end) < max_error: + raise GCodeParameterError( + "arc starts and finishes in the same spot; cannot " + "speculate where circle's center is: %r" % arc_gcode + ) + + arc_radius = abs(arc_gcode.R) # arc radius (magnitude) + + else: + # IJK: radius vertex specified + arc_center_ijk = dict((l, 0.) for l in 'IJK') + arc_center_ijk.update(arc_gcode.get_param_dict('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 projection + arc_p_center = plane_projection(arc_center, plane.normal) + + # Radii + r1 = arc_p_start - arc_p_center + r2 = arc_p_end - arc_p_center + + # average the 2 radii to get the most accurate radius + arc_radius = (abs(r1) + abs(r2)) / 2. + + # Find Circle's Center (given radius) + arc_span = arc_p_end - arc_p_start # vector spanning from start -> end + arc_span_mid = arc_span * 0.5 # arc_span's midpoint + if arc_radius < abs(arc_span_mid): + raise GCodeParameterError("circle cannot reach endpoint at this radius: %r" % arc_gcode) + # vector from arc_span midpoint -> circle's centre + radius_mid_vect = arc_span_mid.normalized().cross(plane.normal) * sqrt(arc_radius**2 - abs(arc_span_mid)**2) + + if 'R' in arc_gcode.params: + # R: radius magnitude specified + if isinstance(arc_gcode, GCodeArcMoveCW) == (arc_gcode.R < 0): + arc_p_center = arc_p_start + arc_span_mid - radius_mid_vect + else: + arc_p_center = arc_p_start + arc_span_mid + radius_mid_vect + else: + # IJK: radius vertex specified + # arc_p_center is defined as per IJK params, this is an adjustment + arc_p_center_options = [ + arc_p_start + arc_span_mid - radius_mid_vect, + arc_p_start + arc_span_mid + radius_mid_vect + ] + if abs(arc_p_center_options[0] - arc_p_center) < abs(arc_p_center_options[1] - arc_p_center): + arc_p_center = arc_p_center_options[0] + else: + arc_p_center = arc_p_center_options[1] + + # Arc's angle (first rotated back to xy plane) + xy_c2start = plane.quat * (arc_p_start - arc_p_center) + xy_c2end = plane.quat * (arc_p_end - arc_p_center) + (a1, a2) = (atan2(*xy_c2start.yx), atan2(*xy_c2end.yx)) + if isinstance(arc_gcode, GCodeArcMoveCW): + arc_angle = (a1 - a2) % (2 * pi) + else: + arc_angle = -((a2 - a1) % (2 * pi)) + + # Helical interpolation + helical_start = plane.normal * arc_start.dot(plane.normal) + helical_end = plane.normal * arc_end.dot(plane.normal) + + # Parameters determined above: + # - arc_p_start arc start point + # - arc_p_end arc end point + # - arc_p_center arc center + # - arc_angle angle between start & end (>0 is ccw, <0 is cw) (radians) + # - 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, + )) - # 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, + radius=arc_radius, ) #plane_projection(vect, normal) diff --git a/pygcode/utils.py b/pygcode/utils.py index 3a8f8ed..5a93b94 100644 --- a/pygcode/utils.py +++ b/pygcode/utils.py @@ -61,5 +61,6 @@ def plane_projection(vect, normal): :param normal: normal of plane to project on to (Vector3) :return: vect projected onto plane represented by normal """ + # ref: https://en.wikipedia.org/wiki/Vector_projection n = normal.normalized() - return v - (n * v.dot(n)) + return vect - (n * vect.dot(n)) diff --git a/scripts/pygcode-normalize.py b/scripts/pygcode-normalize.py index f27c918..d5e3eb3 100755 --- a/scripts/pygcode-normalize.py +++ b/scripts/pygcode-normalize.py @@ -8,6 +8,7 @@ for pygcode_lib_type in ('installed_lib', 'relative_lib'): from pygcode import GCodeArcMove, GCodeArcMoveCW, GCodeArcMoveCCW from pygcode import split_gcodes from pygcode.transform import linearize_arc + from pygcode.transform import ArcLinearizeInside, ArcLinearizeOutside, ArcLinearizeMid except ImportError: import sys, os, inspect @@ -79,20 +80,36 @@ print(args) for line_str in args.infile[0].readlines(): line = Line(line_str) + 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 <----------") - (before, (arc,), after) = split_gcodes(effective_gcodes, GCodeArcMove) - if before: - print(gcodes2str(before)) - print(str(arc)) - if after: - print(gcodes2str(after)) + (befores, (arc,), afters) = split_gcodes(effective_gcodes, GCodeArcMove) + # TODO: debug printing (for now) + if befores: + print("befores: %s" % 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, + ) + machine.process_gcodes(arc) + + if afters: + print("afters: %s" % gcodes2str(afters)) + machine.process_gcodes(*afters) + else: + machine.process_block(line.block) print("%r, %s" % (sorted(line.block.gcodes), line.block.modal_params)) - - machine.process_block(line.block)