GitHub-Proxy/pygcode
1
0
Fork 0
mirror of https://git.mirrors.martin98.com/https://github.com/petaflot/pygcode synced 2025-08-13 06:19:03 +08:00
Code Issues Packages Projects Releases Wiki Activity

arc calcs for linearizing (wip)

Browse Source
This commit is contained in:
Peter Boin 2017-07-11 00:11:32 +10:00
parent a1b3fc41d2
commit c3f822f802
5 changed files with 190 additions and 50 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

48
pygcode/gcodes.py
View File

@ -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):

12
pygcode/machine.py
View File

@ -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

144
pygcode/transform.py
View File

@ -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)

3
pygcode/utils.py
View File

@ -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))

33
scripts/pygcode-normalize.py
View File

@ -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)