diff --git a/src/Mod/Path/Generators/rotation_generator.py b/src/Mod/Path/Generators/rotation_generator.py new file mode 100644 index 0000000000..47749b49a5 --- /dev/null +++ b/src/Mod/Path/Generators/rotation_generator.py @@ -0,0 +1,205 @@ +# -*- coding: utf-8 -*- +# *************************************************************************** +# * Copyright (c) 2021 sliptonic * +# * * +# * 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 diff --git a/src/Mod/Path/PathTests/TestPathRotationGenerator.py b/src/Mod/Path/PathTests/TestPathRotationGenerator.py new file mode 100644 index 0000000000..c9a3065d58 --- /dev/null +++ b/src/Mod/Path/PathTests/TestPathRotationGenerator.py @@ -0,0 +1,169 @@ +# -*- coding: utf-8 -*- +# *************************************************************************** +# * Copyright (c) 2021 sliptonic * +# * * +# * 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)