203 lines
6.9 KiB
Python
203 lines
6.9 KiB
Python
# -*- coding: utf-8 -*-
|
|
# ***************************************************************************
|
|
# * Copyright (c) 2021 sliptonic <shopinthewoods@gmail.com> *
|
|
# * *
|
|
# * This program is free software; you can redistribute it and/or modify *
|
|
# * it under the terms of the GNU Lesser General Public License (LGPL) *
|
|
# * as published by the Free Software Foundation; either version 2 of *
|
|
# * the License, or (at your option) any later version. *
|
|
# * for detail see the LICENCE text file. *
|
|
# * *
|
|
# * This program is distributed in the hope that it will be useful, *
|
|
# * but WITHOUT ANY WARRANTY; without even the implied warranty of *
|
|
# * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the *
|
|
# * GNU Library General Public License for more details. *
|
|
# * *
|
|
# * You should have received a copy of the GNU Library General Public *
|
|
# * License along with this program; if not, write to the Free Software *
|
|
# * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 *
|
|
# * USA *
|
|
# * *
|
|
# ***************************************************************************
|
|
|
|
|
|
# Technical Debt. This generator currently assumes 3+2 axis rotation of CA.
|
|
# The main generator function should be extended to include other flavors of 3+2
|
|
|
|
|
|
import math
|
|
import Path
|
|
import FreeCAD
|
|
from enum import Enum
|
|
|
|
__title__ = "Rotation toolpath Generator"
|
|
__author__ = "sliptonic (Brad Collette)"
|
|
__url__ = "https://www.freecad.org"
|
|
__doc__ = "Generates the rotation toolpath"
|
|
|
|
|
|
if False:
|
|
Path.Log.setLevel(Path.Log.Level.DEBUG, Path.Log.thisModule())
|
|
Path.Log.trackModule(Path.Log.thisModule())
|
|
else:
|
|
Path.Log.setLevel(Path.Log.Level.INFO, Path.Log.thisModule())
|
|
|
|
|
|
class refAxis(Enum):
|
|
x = FreeCAD.Vector(1, 0, 0)
|
|
y = FreeCAD.Vector(0, 1, 0)
|
|
z = FreeCAD.Vector(0, 0, 1)
|
|
|
|
|
|
def relAngle(vec, ref):
|
|
"""
|
|
Takes a vector and a reference axis (refAxis) vector. Calculates the
|
|
relative angle. The result is returned in degrees (plus or minus)
|
|
"""
|
|
|
|
Path.Log.debug("vec: {} ref: {}".format(vec, ref))
|
|
norm = vec * 1 # copy vec so we don't alter original
|
|
|
|
if ref == refAxis.x:
|
|
plane = refAxis.y.value
|
|
elif ref == refAxis.y:
|
|
plane = refAxis.z.value
|
|
else:
|
|
plane = refAxis.x.value
|
|
|
|
norm.projectToPlane(FreeCAD.Vector(0, 0, 0), plane)
|
|
|
|
ref = ref.value
|
|
rot = FreeCAD.Rotation(norm, ref)
|
|
ang = math.degrees(rot.Angle)
|
|
angle = ang * plane.dot(rot.Axis)
|
|
Path.Log.debug("relative ang: {}".format(angle))
|
|
|
|
return angle
|
|
|
|
|
|
def __getCRotation(normalVector, cMin=-360, cMax=360):
|
|
"""
|
|
Calculate the valid C axis rotations component to align the normalVector
|
|
with either the +y or -y axis.
|
|
multiple poses may be possible. Returns a list of all valid poses
|
|
"""
|
|
Path.Log.debug("normalVector: {} cMin: {} cMax: {}".format(normalVector, cMin, cMax))
|
|
|
|
angle = relAngle(normalVector, refAxis.y)
|
|
|
|
# Given an angle, there are four possibilities; rotating +- to each of the
|
|
# two axes +y and -y
|
|
candidates = [angle]
|
|
if angle == 0:
|
|
candidates.append(180)
|
|
elif angle == 180:
|
|
candidates.append(0)
|
|
elif angle >= 0:
|
|
candidates.append(angle - 180)
|
|
candidates.append(180 + angle)
|
|
candidates.append(angle - 360)
|
|
else:
|
|
candidates.append(angle + 180)
|
|
candidates.append(-180 + angle)
|
|
candidates.append(angle + 360)
|
|
|
|
# final results are candidates that don't violate rotation limits
|
|
results = [c for c in candidates if c >= cMin and c <= cMax]
|
|
|
|
return results
|
|
|
|
|
|
def __getARotation(normalVector, aMin=-360, aMax=360):
|
|
"""
|
|
Calculate the A axis rotation component.
|
|
Final rotation is always assumed to be around +X. The sign of the returned
|
|
value indicates direction of rotation.
|
|
|
|
Returns None if rotation violates min/max constraints
|
|
"""
|
|
|
|
angle = relAngle(normalVector, refAxis.z)
|
|
|
|
# only return a result if it doesn't violate rotation constraints
|
|
if angle > aMin and angle <= aMax:
|
|
return angle
|
|
else:
|
|
return None
|
|
|
|
|
|
def generate(normalVector, aMin=-360, aMax=360, cMin=-360, cMax=360, compound=False):
|
|
"""
|
|
Generates Gcode rotation to align a vector (alignVector) with the positive Z axis.
|
|
|
|
It first rotates around the Z axis (C rotation)
|
|
to align the vector the positive Y axis. Then around the X axis
|
|
(A rotation).
|
|
|
|
The min and max arguments dictate the range of motion allowed rotation in
|
|
the respective axis.
|
|
Default assumes continuous rotation.
|
|
|
|
Returns a list of path commands for the shortest valid solution
|
|
|
|
If compound is False, axis moves will be broken out to individual commands
|
|
|
|
The normalVector input from a typical face (f) can be obtained like this:
|
|
|
|
u, v = f.ParameterRange[:2]
|
|
n = f.normalAt(u,v)
|
|
plm = obj.getGlobalPlacement()
|
|
rot = plm.Rotation
|
|
normalVector = rot.multVec(n
|
|
"""
|
|
|
|
Path.Log.track(
|
|
"\n=============\n normalVector: {}\n aMin: {}\n aMax: {}\n cMin: {}\n cMax: {}".format(
|
|
normalVector, aMin, aMax, cMin, cMax
|
|
)
|
|
)
|
|
|
|
# Calculate C rotation
|
|
cResults = __getCRotation(normalVector, cMin, cMax)
|
|
Path.Log.debug("C Rotation results {}".format(cResults))
|
|
|
|
solutions = []
|
|
for result in cResults:
|
|
|
|
# calculate a new vector based on the result
|
|
rot = FreeCAD.Rotation(FreeCAD.Vector(0, 0, 1), result)
|
|
newvec = rot.multVec(normalVector)
|
|
|
|
# Get the candidate A rotation for the new vector
|
|
aResult = __getARotation(newvec, aMin, aMax)
|
|
|
|
Path.Log.debug("\n=====\nFor C Rotation: {}\n Calculated A {}\n".format(result, aResult))
|
|
|
|
if aResult is not None:
|
|
solutions.append({"A": aResult, "C": result})
|
|
|
|
if len(solutions) == 0: # No valid solution found
|
|
raise ValueError("No valid rotation solution found")
|
|
|
|
# find pose with the shortest transit length
|
|
best = solutions[0]
|
|
curlen = math.fabs(best["A"]) + math.fabs(best["C"])
|
|
for solution in solutions[1:]:
|
|
testlen = math.fabs(solution["A"]) + math.fabs(solution["C"])
|
|
if testlen < curlen:
|
|
best = solution
|
|
curlen = testlen
|
|
|
|
Path.Log.debug("best result: {}".format(best))
|
|
|
|
# format and return rotation commands
|
|
commands = []
|
|
if compound:
|
|
commands.append(Path.Command("G0", best))
|
|
else:
|
|
for key, val in best.items():
|
|
print(key, val)
|
|
commands.append(Path.Command("G0", {key: val}))
|
|
|
|
return commands
|