improvements to arc linearization

This commit is contained in:
Peter Boin 2017-07-15 23:20:15 +10:00
parent 2c0de02b6d
commit efeb76592d
6 changed files with 283 additions and 85 deletions

View File

@ -167,6 +167,10 @@ class GCode(object):
self.word = gcode_word
self.params = {}
# Whitespace as prefix
# if True, str(self) will repalce self.word code with whitespace
self._whitespace_prefix = False
# Add Given Parameters
for param_word in param_words:
self.add_parameter(param_word)
@ -194,8 +198,11 @@ class GCode(object):
"{}".format(self.params[k])
for k in sorted(self.params.keys())
])
return "{gcode}{parameters}".format(
gcode=self.word,
word_str = str(self.word)
if self._whitespace_prefix:
word_str = ' ' * len(word_str)
return "{word_str}{parameters}".format(
word_str=word_str,
parameters=param_str,
)
@ -468,36 +475,42 @@ class GCodeDrillingCycle(GCodeCannedCycle):
"""G81: Drilling Cycle"""
param_letters = GCodeCannedCycle.param_letters | set('RLP')
word_key = Word('G', 81)
modal_param_letters = GCodeCannedCycle.param_letters | set('RLP')
class GCodeDrillingCycleDwell(GCodeCannedCycle):
"""G82: Drilling Cycle, Dwell"""
param_letters = GCodeCannedCycle.param_letters | set('RLP')
word_key = Word('G', 82)
modal_param_letters = GCodeCannedCycle.param_letters | set('RLP')
class GCodeDrillingCyclePeck(GCodeCannedCycle):
"""G83: Drilling Cycle, Peck"""
param_letters = GCodeCannedCycle.param_letters | set('RLQ')
word_key = Word('G', 83)
modal_param_letters = GCodeCannedCycle.param_letters | set('RLQ')
class GCodeDrillingCycleChipBreaking(GCodeCannedCycle):
"""G73: Drilling Cycle, ChipBreaking"""
param_letters = GCodeCannedCycle.param_letters | set('RLQ')
word_key = Word('G', 73)
modal_param_letters = GCodeCannedCycle.param_letters | set('RLQ')
class GCodeBoringCycleFeedOut(GCodeCannedCycle):
"""G85: Boring Cycle, Feed Out"""
param_letters = GCodeCannedCycle.param_letters | set('RLP')
word_key = Word('G', 85)
modal_param_letters = GCodeCannedCycle.param_letters | set('RLP')
class GCodeBoringCycleDwellFeedOut(GCodeCannedCycle):
"""G89: Boring Cycle, Dwell, Feed Out"""
param_letters = GCodeCannedCycle.param_letters | set('RLP')
word_key = Word('G', 89)
modal_param_letters = GCodeCannedCycle.param_letters | set('RLP')
class GCodeThreadingCycle(GCodeCannedCycle):
@ -561,6 +574,7 @@ class GCodeFeedRateMode(GCode):
modal_group = MODAL_GROUP_MAP['feed_rate_mode']
exec_order = 30
class GCodeInverseTimeMode(GCodeFeedRateMode):
"""G93: Inverse Time Mode"""
word_key = Word('G', 93)
@ -883,16 +897,24 @@ class GCodePathBlendingMode(GCodePathControlMode):
# CODE PARAMETERS DESCRIPTION
# G98 Canned Cycle Return Level
class GCodeCannedReturnMode(GCode):
modal_group = MODAL_GROUP_MAP['canned_cycles_return']
exec_order = 220
class GCodeCannedCycleReturnLevel(GCodeCannedReturnMode):
"""G98: Canned Cycle Return Level"""
"""G98: Canned Cycle Return to the level set prior to cycle start"""
# "retract to the position that axis was in just before this series of one or more contiguous canned cycles was started"
word_key = Word('G', 98)
class GCodeCannedCycleReturnToR(GCodeCannedReturnMode):
"""G99: Canned Cycle Return to the level set by R"""
# "retract to the position specified by the R word of the canned cycle"
word_key = Word('G', 99)
# ======================= Other Modal Codes =======================
# CODE PARAMETERS DESCRIPTION
# F Set Feed Rate

View File

@ -357,7 +357,9 @@ class Machine(object):
return None
if self.mode.motion is None:
raise MachineInvalidState("unable to assign modal parameters when no motion mode is set")
(modal_gcodes, unasigned_words) = words2gcodes([self.mode.motion.word] + modal_params)
params = copy(self.mode.motion.params) # dict
params.update(dict((w.letter, w) for w in modal_params)) # override retained modal parameters
(modal_gcodes, unasigned_words) = words2gcodes([self.mode.motion.word] + params.values())
if unasigned_words:
raise MachineInvalidState("modal parameters '%s' cannot be assigned when in mode: %r" % (
' '.join(str(x) for x in unasigned_words), self.mode

View File

@ -1,4 +1,4 @@
from math import cos, acos, atan2, pi, sqrt, ceil
from math import sin, cos, tan, asin, acos, atan2, pi, sqrt, ceil
from .gcodes import GCodeLinearMove
from .gcodes import GCodeArcMove, GCodeArcMoveCW, GCodeArcMoveCCW
@ -14,6 +14,11 @@ from .utils import Vector3, Quaternion, plane_projection
# ==================== Arcs (G2,G3) --> Linear Motion (G1) ====================
class ArcLinearizeMethod(object):
# Chord Phase Offest:
# False : each line will span an equal portion of the arc
# True : the first & last chord will span 1/2 the angular distance of all other chords
chord_phase_offset = False
def __init__(self, max_error, plane_normal,
arc_p_start, arc_p_end, arc_p_center,
arc_radius, arc_angle, helical_start, helical_end):
@ -30,13 +35,103 @@ class ArcLinearizeMethod(object):
if self.max_error > self.arc_radius:
self.max_error = self.arc_radius
# Initializing
self._max_wedge_angle = None
self._wedge_count = None
self._wedge_angle = None
self._inner_radius = None
self._outer_radius = None
# Overridden Functions
def get_max_wedge_angle(self):
"""Calculate angular coverage of a single line reaching maximum allowable error"""
raise NotImplementedError("not overridden")
def get_inner_radius(self):
"""Radius each line is tangential to"""
# IMPORTANT: when overriding, calculate this using self.wedge_angle,
# (self.wedge_angle will almost always be < self.max_wedge_angle)
raise NotImplementedError("not overridden")
def get_outer_radius(self):
"""Radius from which each line forms a chord"""
# IMPORTANT: when overriding, calculate this using self.wedge_angle,
# (self.wedge_angle will almost always be < self.max_wedge_angle)
raise NotImplementedError("not overridden")
# Properties
@property
def max_wedge_angle(self):
if self._max_wedge_angle is None:
self._max_wedge_angle = self.get_max_wedge_angle()
return self._max_wedge_angle
@property
def wedge_count(self):
"""
Number of full wedges covered across the arc.
NB: if there is phase offset, then the actual number of linearized lines
is this + 1, because the first and last are considered to be the
same 'wedge'.
"""
if self._wedge_count is None:
self._wedge_count = int(ceil(abs(self.arc_angle) / self.max_wedge_angle))
return self._wedge_count
@property
def wedge_angle(self):
"""Angle each major chord stretches across the original arc"""
if self._wedge_angle is None:
self._wedge_angle = self.arc_angle / self.wedge_count
return self._wedge_angle
@property
def inner_radius(self):
if self._inner_radius is None:
self._inner_radius = self.get_inner_radius()
return self._inner_radius
@property
def outer_radius(self):
if self._outer_radius is None:
self._outer_radius = self.get_outer_radius()
return self._outer_radius
# Iter
def iter_vertices(self):
"""Yield absolute (<start vertex>, <end vertex>) for each line for the arc"""
raise NotImplementedError("not overridden")
start_vertex = self.arc_p_start - self.arc_p_center
outer_vertex = start_vertex.normalized() * self.outer_radius
d_helical = self.helical_end - self.helical_start
l_p_start = self.arc_p_center + start_vertex
l_start = l_p_start + self.helical_start
for i in range(self.wedge_count):
wedge_number = i + 1
# Current angle
cur_angle = self.wedge_angle * wedge_number
if self.chord_phase_offset:
cur_angle -= self.wedge_angle / 2.
elif wedge_number >= self.wedge_count:
break # stop 1 iteration short
# alow last arc to simply span across:
# <the end of the last line> -> <circle's end point>
# Next end point as projected on selected plane
q_end = Quaternion.new_rotate_axis(angle=cur_angle, axis=-self.plane_normal)
l_p_end = (q_end * outer_vertex) + self.arc_p_center
# += helical displacement (difference along plane's normal)
helical_displacement = self.helical_start + (d_helical * (cur_angle / self.arc_angle))
l_end = l_p_end + helical_displacement
yield (l_start, l_end)
# start of next line is the end of this one
l_start = l_end
# Last line always ends at the circle's end
yield (l_start, self.arc_p_end + self.helical_end)
class ArcLinearizeInside(ArcLinearizeMethod):
@ -48,31 +143,19 @@ class ArcLinearizeInside(ArcLinearizeMethod):
# - Each line is the same length
# - Simplest maths, easiest to explain & visually verify
chord_phase_offset = False
def get_max_wedge_angle(self):
"""Calculate angular coverage of a single line reaching maximum allowable error"""
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
def get_inner_radius(self):
"""Radius each line is tangential to"""
return abs(cos(self.wedge_angle / 2.) * self.arc_radius)
l_p_start = self.arc_p_start
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 one
l_start = l_end
def get_outer_radius(self):
"""Radius from which each line forms a chord"""
return self.arc_radius
class ArcLinearizeOutside(ArcLinearizeMethod):
@ -83,37 +166,19 @@ class ArcLinearizeOutside(ArcLinearizeMethod):
# - perimeter milling action will remove less material
# - 1st and last lines are 1/2 length of the others
chord_phase_offset = True
def get_max_wedge_angle(self):
"""Calculate angular coverage of a single line reaching maximum allowable error"""
return abs(2 * acos(self.arc_radius / (self.arc_radius + self.max_error)))
def iter_vertices(self):
# n wedges distributed like:
# - 1/2 wedge : first line
# - n-1 wedges : outer perimeter
# - last 1/2 wedge : last line
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
# radius of outer circle (across which the linear lines will span)
error_radius = start_radius.normalized() * (self.arc_radius / cos(wedge_angle / 2))
def get_inner_radius(self):
"""Radius each line is tangential to"""
return self.arc_radius
l_p_start = start_radius + self.arc_p_center
l_start = l_p_start + self.helical_start
for i in range(wedge_count):
cur_angle = (wedge_angle * i) + (wedge_angle / 2)
q_end = Quaternion.new_rotate_axis(angle=cur_angle, axis=-self.plane_normal)
# Projected on selected plane
l_p_end = (q_end * error_radius) + self.arc_p_center
# Helical displacement
l_end = l_p_end + self.helical_start + ((self.helical_end - self.helical_start) * (cur_angle / self.arc_angle))
yield (l_start, l_end)
# start of next line is the end of this one
l_start = l_end
yield (l_start, self.arc_p_end + self.helical_end)
def get_outer_radius(self):
"""Radius from which each line forms a chord"""
return abs(self.arc_radius / cos(self.wedge_angle / 2.))
class ArcLinearizeMid(ArcLinearizeMethod):
@ -123,6 +188,23 @@ class ArcLinearizeMid(ArcLinearizeMethod):
# - Most complex to calculate (but who cares, that's only done once)
# - default linearizing method as it's probably the best
chord_phase_offset = True
def get_max_wedge_angle(self):
"""Calculate angular coverage of a single line reaching maximum allowable error"""
d_radius = self.max_error / 2.
return abs(2. * acos((self.arc_radius - d_radius) / (self.arc_radius + d_radius)))
def get_inner_radius(self):
"""Radius each line is tangential to"""
d_radius = self.arc_radius * (tan(self.wedge_angle / 4.) ** 2)
return self.arc_radius - d_radius
def get_outer_radius(self):
"""Radius from which each line forms a chord"""
d_radius = self.arc_radius * (tan(self.wedge_angle / 4.) ** 2)
return self.arc_radius + d_radius
DEFAULT_LA_METHOD = ArcLinearizeMid
DEFAULT_LA_PLANE = GCodeSelectXYPlane
@ -154,11 +236,14 @@ 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(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
if isinstance(dist_mode, GCodeAbsoluteDistanceMode):
# given coordinates override those already defined
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)
else:
# given coordinates are += to arc's start coords
arc_end = arc_start + Vector3(**arc_gcode.get_param_dict('XYZ', lc=True))
# Planar Projections
arc_p_start = plane_projection(arc_start, plane.normal)

View File

@ -1,4 +1,5 @@
import sys
from copy import copy, deepcopy
if sys.version_info < (3, 0):
from euclid import Vector3, Quaternion
@ -64,3 +65,32 @@ def plane_projection(vect, normal):
# ref: https://en.wikipedia.org/wiki/Vector_projection
n = normal.normalized()
return vect - (n * vect.dot(n))
# ==================== GCode Utilities ====================
def omit_redundant_modes(gcode_iter):
"""
Replace redundant machine motion modes with whitespace,
:param gcode_iter: iterable to return with modifications
"""
from .machine import Machine, Mode
from .gcodes import MODAL_GROUP_MAP
class NullModeMachine(Machine):
MODE_CLASS = type('NullMode', (Mode,), {'default_mode': ''})
m = NullModeMachine()
for g in gcode_iter:
if (g.modal_group is not None) and (m.mode.modal_groups[g.modal_group] is not None):
# g-code has a modal groups, and the machine's mode
# (of the same modal group) is not None
if m.mode.modal_groups[g.modal_group].word == g.word:
# machine's mode & g-code's mode match (no machine change)
if g.modal_group == MODAL_GROUP_MAP['motion']:
# finally: g-code sets a motion mode in the machine
g = copy(g) # duplicate gcode object
# stop redundant g-code word from being printed
g._whitespace_prefix = True
m.process_gcodes(g)
yield g

View File

@ -216,7 +216,7 @@ class Word(object):
self._value_clean = WORD_MAP[letter]['clean_value']
self.letter = letter
self._value = self._value_class(value)
self.value = value
def __str__(self):
return "{letter}{value}".format(
@ -230,6 +230,20 @@ class Word(object):
string=str(self),
)
# Sorting
def __lt__(self, other):
return (self.letter, self.value) < (other.letter, other.value)
def __gt__(self, other):
return (self.letter, self.value) > (other.letter, other.value)
def __le__(self, other):
return (self.letter, self.value) <= (other.letter, other.value)
def __ge__(self, other):
return (self.letter, self.value) >= (other.letter, other.value)
# Equality
def __eq__(self, other):
if isinstance(other, six.string_types):
other = str2word(other)
@ -238,6 +252,7 @@ class Word(object):
def __ne__(self, other):
return not self.__eq__(other)
# Hashing
def __hash__(self):
return hash((self.letter, self.value))
@ -255,10 +270,6 @@ class Word(object):
def value(self, new_value):
self._value = self._value_class(new_value)
# Order
def __lt__(self, other):
return self.letter < other.letter
@property
def description(self):
return "%s: %s" % (self.letter, WORD_MAP[self.letter]['description'])

View File

@ -13,11 +13,16 @@ from collections import defaultdict
for pygcode_lib_type in ('installed_lib', 'relative_lib'):
try:
# pygcode
from pygcode import Word
from pygcode import Machine, Mode, Line
from pygcode import GCodeArcMove, GCodeArcMoveCW, GCodeArcMoveCCW
from pygcode import GCodeCannedCycle
from pygcode import split_gcodes
from pygcode import Comment
from pygcode.transform import linearize_arc
from pygcode.transform import ArcLinearizeInside, ArcLinearizeOutside, ArcLinearizeMid
from pygcode.gcodes import _subclasses
from pygcode.utils import omit_redundant_modes
except ImportError:
import sys, os, inspect
@ -34,6 +39,8 @@ for pygcode_lib_type in ('installed_lib', 'relative_lib'):
# --- Defaults
DEFAULT_PRECISION = 0.005 # mm
DEFAULT_MACHINE_MODE = 'G0 G54 G17 G21 G90 G94 M5 M9 T0 F0 S0'
DEFAULT_ARC_LIN_METHOD = 'm'
DEFAULT_CANNED_CODES = ','.join(str(w) for w in sorted(c.word_key for c in _subclasses(GCodeCannedCycle) if c.word_key))
# --- Create Parser
parser = argparse.ArgumentParser(description='Normalize gcode for machine consistency using different CAM software')
@ -54,32 +61,55 @@ parser.add_argument(
help="Machine's startup mode as gcode (default: '%s')" % DEFAULT_MACHINE_MODE,
)
# Arcs
parser.add_argument(
# Arc Linearizing
group = parser.add_argument_group(
"Arc Linearizing",
"Converting arcs (G2/G3 codes) into linear interpolations (G1 codes) to "
"aproximate the original arc. Indistinguishable from an original arc when "
"--precision is set low enough."
)
group.add_argument(
'--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",
help="convert G2,G3 commands to a series of linear interpolations (G1 codes)",
)
group.add_argument(
'--arc_lin_method', '-alm', dest='arc_lin_method', default=DEFAULT_ARC_LIN_METHOD,
help="Method of linearizing arcs, i=inner, o=outer, m=mid. List 2 "
"for <ccw>,<cw>, eg 'i,o'. 'i' is equivalent to 'i,i'. "
"(default: '%s')" % DEFAULT_ARC_LIN_METHOD,
metavar='{i,o,m}[,{i,o,m}]',
)
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]',
# Canned Cycles
group = parser.add_argument_group(
"Canned Cycle Simplification",
"Convert canned cycles into basic linear or scalar codes, such as linear "
"interpolation (G1), and pauses (or 'dwells', G4)"
)
group.add_argument(
'--canned_simplify', '-cs', dest='canned_simplify',
action='store_const', const=True, default=False,
help="Convert canned cycles into basic linear movements",
)
group.add_argument(
'---canned_codes', '-cc', dest='canned_codes', default=DEFAULT_CANNED_CODES,
help="List of canned gcodes to simplify, (default is '%s')" % DEFAULT_CANNED_CODES,
)
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",
)
#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()
# arc linearizing method (manually parsing)
# --- Manually Parsing : Arc Linearizing Method
# args.arc_lin_method = {Word('G2'): <linearize method class>, ... }
ARC_LIN_CLASS_MAP = {
'i': ArcLinearizeInside,
'o': ArcLinearizeOutside,
@ -94,15 +124,24 @@ if 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')]
args.arc_lin_method[Word('g2')] = ARC_LIN_CLASS_MAP[match.group('g2')]
if match.group('g3'):
args.arc_lin_method['G3'] = ARC_LIN_CLASS_MAP[match.group('g3')]
args.arc_lin_method[Word('g3')] = ARC_LIN_CLASS_MAP[match.group('g3')]
else:
args.arc_lin_method['G3'] = args.arc_lin_method['G2']
args.arc_lin_method[Word('g3')] = args.arc_lin_method[Word('g2')]
else:
args.arc_lin_method = defaultdict(lambda: ArcLinearizeInside) # just to be sure
# FIXME: change default to ArcLinearizeMid (when it's working)
args.arc_lin_method = defaultdict(lambda: ArcLinearizeMid) # just to be sure
# --- Manually Parsing : Canned Codes
# args.canned_codes = [Word('G73'), Word('G89'), ... ]
canned_code_words = set()
for word_str in re.split(r'\s*,\s*', args.canned_codes):
canned_code_words.add(Word(word_str))
args.canned_codes = canned_code_words
# =================== Create Virtual CNC Machine ===================
class MyMode(Mode):
@ -135,17 +174,18 @@ for line_str in args.infile[0].readlines():
print(gcodes2str(befores))
machine.process_gcodes(*befores)
#print("arc: %s" % str(arc))
print(Comment("linearized: %r" % 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)],
'method_class': args.arc_lin_method[arc.word],
'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):
for linear_gcode in omit_redundant_modes(linearize_arc(**linearize_params)):
print(linear_gcode)
machine.process_gcodes(arc)
@ -154,6 +194,14 @@ for line_str in args.infile[0].readlines():
machine.process_gcodes(*afters)
if line.comment:
print(str(line.comment))
elif args.canned_simplify and any((g.word in args.canned_codes) for g in effective_gcodes):
(befores, (canned,), afters) = split_gcodes(effective_gcodes, GCodeCannedCycle)
print(Comment('canning simplified: %r' % canned))
# TODO: simplify canned things
print(str(line))
machine.process_block(line.block)
else:
print(str(line))
machine.process_block(line.block)