draft generator and tests
This commit is contained in:
sliptonic
205
src/Mod/Path/Generators/rotation_generator.py
Normal file
205
src/Mod/Path/Generators/rotation_generator.py
Normal file
@@ -0,0 +1,205 @@
|
||||
# -*- 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 PathScripts.PathLog as PathLog
|
||||
import math
|
||||
import Path
|
||||
import FreeCAD
|
||||
from enum import Enum
|
||||
|
||||
__title__ = "Rotation Path Generator"
|
||||
__author__ = "sliptonic (Brad Collette)"
|
||||
__url__ = "https://www.freecadweb.org"
|
||||
__doc__ = "Generates the rotation toolpath"
|
||||
|
||||
|
||||
if False:
|
||||
PathLog.setLevel(PathLog.Level.DEBUG, PathLog.thisModule())
|
||||
PathLog.trackModule(PathLog.thisModule())
|
||||
else:
|
||||
PathLog.setLevel(PathLog.Level.INFO, PathLog.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)
|
||||
"""
|
||||
|
||||
PathLog.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)
|
||||
PathLog.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
|
||||
"""
|
||||
PathLog.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 continous 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
|
||||
"""
|
||||
|
||||
PathLog.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)
|
||||
PathLog.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)
|
||||
|
||||
PathLog.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
|
||||
|
||||
PathLog.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
|
||||
169
src/Mod/Path/PathTests/TestPathRotationGenerator.py
Normal file
169
src/Mod/Path/PathTests/TestPathRotationGenerator.py
Normal file
@@ -0,0 +1,169 @@
|
||||
# -*- 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 *
|
||||
# * *
|
||||
# ***************************************************************************
|
||||
|
||||
import Path
|
||||
import FreeCAD
|
||||
import Generators.rotation_generator as generator
|
||||
import PathScripts.PathLog as PathLog
|
||||
import PathTests.PathTestUtils as PathTestUtils
|
||||
import numpy as np
|
||||
|
||||
PathLog.setLevel(PathLog.Level.INFO, PathLog.thisModule())
|
||||
PathLog.trackModule(PathLog.thisModule())
|
||||
|
||||
|
||||
class TestPathRotationGenerator(PathTestUtils.PathTestBase):
|
||||
def test00(self):
|
||||
"""Test relAngle function"""
|
||||
v = FreeCAD.Vector(0.5, 0.5, 0.5)
|
||||
self.assertTrue(np.isclose(generator.relAngle(v, generator.refAxis.x), 45))
|
||||
self.assertTrue(np.isclose(generator.relAngle(v, generator.refAxis.y), 45))
|
||||
self.assertTrue(np.isclose(generator.relAngle(v, generator.refAxis.z), 45))
|
||||
|
||||
v = FreeCAD.Vector(-0.5, 0.5, 0.5)
|
||||
self.assertTrue(np.isclose(generator.relAngle(v, generator.refAxis.x), 135))
|
||||
self.assertTrue(np.isclose(generator.relAngle(v, generator.refAxis.y), -45))
|
||||
self.assertTrue(np.isclose(generator.relAngle(v, generator.refAxis.z), 45))
|
||||
|
||||
v = FreeCAD.Vector(-0.5, -0.5, -0.5)
|
||||
self.assertTrue(np.isclose(generator.relAngle(v, generator.refAxis.x), -135))
|
||||
self.assertTrue(np.isclose(generator.relAngle(v, generator.refAxis.y), -135))
|
||||
self.assertTrue(np.isclose(generator.relAngle(v, generator.refAxis.z), -135))
|
||||
|
||||
def test10(self):
|
||||
"""Test Basic Rotation Generator Return"""
|
||||
v1 = FreeCAD.Vector(0.0, 0.0, 1.0)
|
||||
args = {
|
||||
"normalVector": v1,
|
||||
"aMin": -360,
|
||||
"aMax": 360,
|
||||
"cMin": -360,
|
||||
"cMax": 360,
|
||||
"compound": True,
|
||||
}
|
||||
|
||||
result = generator.generate(**args)
|
||||
|
||||
self.assertTrue(type(result) is list)
|
||||
self.assertTrue(len(result) == 1)
|
||||
self.assertTrue(type(result[0]) is Path.Command)
|
||||
|
||||
command = result[0]
|
||||
self.assertTrue(np.isclose(command.Parameters["A"], 0))
|
||||
self.assertTrue(np.isclose(command.Parameters["C"], 0))
|
||||
|
||||
args["compound"] = False
|
||||
result = generator.generate(**args)
|
||||
self.assertTrue(len(result) == 2)
|
||||
|
||||
PathLog.debug(result)
|
||||
|
||||
def test20(self):
|
||||
"""Test non-zero rotation"""
|
||||
v1 = FreeCAD.Vector(0.5, 0.5, 0.5)
|
||||
args = {
|
||||
"normalVector": v1,
|
||||
"aMin": -360,
|
||||
"aMax": 360,
|
||||
"cMin": -360,
|
||||
"cMax": 360,
|
||||
"compound": True,
|
||||
}
|
||||
|
||||
result = generator.generate(**args)
|
||||
|
||||
command = result[0]
|
||||
PathLog.debug(command.Parameters)
|
||||
self.assertTrue(np.isclose(command.Parameters["A"], 54.736))
|
||||
self.assertTrue(np.isclose(command.Parameters["C"], 45))
|
||||
|
||||
PathLog.track(result)
|
||||
|
||||
def test30(self):
|
||||
"""Test A limits"""
|
||||
v1 = FreeCAD.Vector(0.5, 0.5, 0.5)
|
||||
|
||||
args = {"normalVector": v1, "cMin": -360, "cMax": 360, "compound": True}
|
||||
|
||||
# Constrain a axis rotation negative
|
||||
args["aMin"] = -90
|
||||
args["aMax"] = 0
|
||||
|
||||
result = generator.generate(**args)
|
||||
PathLog.debug(result)
|
||||
|
||||
command = result[0]
|
||||
self.assertTrue(np.isclose(command.Parameters["A"], -54.736))
|
||||
self.assertTrue(np.isclose(command.Parameters["C"], -135))
|
||||
|
||||
# Constrain a axis rotation positive
|
||||
args["aMin"] = 0
|
||||
args["aMax"] = 90
|
||||
|
||||
result = generator.generate(**args)
|
||||
PathLog.debug(result)
|
||||
|
||||
command = result[0]
|
||||
self.assertTrue(np.isclose(command.Parameters["A"], 54.736))
|
||||
self.assertTrue(np.isclose(command.Parameters["C"], 45))
|
||||
|
||||
def test40(self):
|
||||
"""Test C limits"""
|
||||
v1 = FreeCAD.Vector(0.5, 0.5, 0.5)
|
||||
|
||||
args = {"normalVector": v1, "aMin": -360, "aMax": 360, "compound": True}
|
||||
|
||||
# Constrain a axis rotation negative
|
||||
args["cMin"] = -180
|
||||
args["cMax"] = 0
|
||||
|
||||
result = generator.generate(**args)
|
||||
PathLog.debug(result)
|
||||
|
||||
command = result[0]
|
||||
self.assertTrue(np.isclose(command.Parameters["A"], -54.736))
|
||||
self.assertTrue(np.isclose(command.Parameters["C"], -135))
|
||||
|
||||
# Constrain a axis rotation positive
|
||||
args["cMin"] = 0
|
||||
args["cMax"] = 180
|
||||
|
||||
result = generator.generate(**args)
|
||||
PathLog.debug(result)
|
||||
|
||||
command = result[0]
|
||||
self.assertTrue(np.isclose(command.Parameters["A"], 54.736))
|
||||
self.assertTrue(np.isclose(command.Parameters["C"], 45))
|
||||
|
||||
def test50(self):
|
||||
"""Test handling of no valid solution"""
|
||||
v1 = FreeCAD.Vector(0.5, 0.5, 0.5)
|
||||
args = {
|
||||
"normalVector": v1,
|
||||
"aMin": 0,
|
||||
"aMax": 10,
|
||||
"cMin": 0,
|
||||
"cMax": 10,
|
||||
"compound": True,
|
||||
}
|
||||
|
||||
self.assertRaises(ValueError, generator.generate, **args)
|
||||
Reference in New Issue
Block a user