diff --git a/examples/worm_cutting_tool/cutting_tool_worm_assembly.ipynb b/examples/worm_cutting_tool/cutting_tool_worm_assembly.ipynb index 1f8d118..64c291d 100644 --- a/examples/worm_cutting_tool/cutting_tool_worm_assembly.ipynb +++ b/examples/worm_cutting_tool/cutting_tool_worm_assembly.ipynb @@ -14,68 +14,24 @@ }, { "cell_type": "code", - "execution_count": 5, + "execution_count": 1, "id": "7eacf041-aa83-49e2-9cbe-066f177197f6", "metadata": {}, "outputs": [], "source": [ - "import sympy as sp\n", - "import numpy as np" + "import sympy as sym\n", + "import numpy as np\n", + "from pygears.transformation import symbolic_transformation, numeric_transformation" ] }, { "cell_type": "code", - "execution_count": 6, - "id": "980417d0-c79d-4501-a7cc-9725b3bbea83", - "metadata": {}, - "outputs": [], - "source": [ - "def symbolic_transformation(angle, axis, translation=np.array([0., 0., 0.])):\n", - " \"\"\"\n", - " see http://en.wikipedia.org/wiki/SO%284%29#The_Euler.E2.80.93Rodrigues_formula_for_3D_rotations\n", - " sympy enabled transformation\n", - " angle: angle of rotation\n", - " axis: the axis of the rotation\n", - " translation: translation of transformation\n", - " \"\"\"\n", - " assert len(axis) == 3\n", - " a = sp.cos(angle / 2)\n", - " axis_normalized = axis / sp.sqrt(axis.dot(axis))\n", - " (b, c, d) = -axis_normalized * sp.sin(angle / 2)\n", - " mat = sp.Matrix(\n", - " [\n", - " [\n", - " a**2 + b**2 - c**2 - d**2,\n", - " 2 * (b * c - a * d),\n", - " 2 * (b * d + a * c),\n", - " translation[0],\n", - " ],\n", - " [\n", - " 2 * (b * c + a * d),\n", - " a**2 + c**2 - b**2 - d**2,\n", - " 2 * (c * d - a * b),\n", - " translation[1],\n", - " ],\n", - " [\n", - " 2 * (b * d - a * c),\n", - " 2 * (c * d + a * b),\n", - " a**2 + d**2 - b**2 - c**2,\n", - " translation[2],\n", - " ],\n", - " [0.0, 0.0, 0.0, 1.0],\n", - " ]\n", - " )\n", - " return sp.simplify(mat)\n" - ] - }, - { - "cell_type": "code", - "execution_count": 7, + "execution_count": 5, "id": "5852aa56-e66b-4f3c-a50d-0cb4cb21abd2", "metadata": {}, "outputs": [], "source": [ - "t = sp.Symbol(\"t\")\n", + "t = sym.Symbol(\"t\")\n", "T1 = symbolic_transformation(np.pi / 2.,\n", " np.array([1., 0., 0.]),\n", " np.array([12.5,0., 1.15]))\n", @@ -86,12 +42,12 @@ " np.array([1., 0., 0.]),\n", " np.array([0., 0., t]))\n", "\n", - "T = sp.nsimplify(T2.inv() @ T1.inv() @ T3, tolerance=10e-16)" + "T = sym.nsimplify(T2.inv() @ T1.inv() @ T3, tolerance=10e-16)" ] }, { "cell_type": "code", - "execution_count": 8, + "execution_count": 7, "id": "7c9837b8-caf7-4447-bf0d-eba9085197a5", "metadata": {}, "outputs": [ @@ -104,14 +60,14 @@ " [ 0. , 0. , 0. , 0. ]])" ] }, - "execution_count": 8, + "execution_count": 7, "metadata": {}, "output_type": "execute_result" } ], "source": [ - "T_fn = sp.lambdify(t, T)\n", - "dT_fn = sp.lambdify(t, T.diff(t))\n", + "T_fn = sym.lambdify(t, T)\n", + "dT_fn = sym.lambdify(t, T.diff(t))\n", "dT_fn(0.)" ] }, @@ -196,19 +152,24 @@ }, { "cell_type": "code", - "execution_count": 15, + "execution_count": 9, "id": "cfd8026b-5a84-4882-a1de-63580776a579", "metadata": {}, "outputs": [], "source": [ - "import sympy as sp\n", - "t, x, z, m = sp.symbols([\"t\", \"x\", \"z\", \"m\"], real=True)\n", - "s, alpha, n_t, y = sp.symbols([\"s\", \"alpha\", \"n_t\", \"y\"], real=True, positiv=True)" + "import sympy as sym\n", + "import numpy as np\n", + "import scipy as sp\n", + "\n", + "from pygears.transformation import symbolic_transformation, numeric_transformation\n", + "\n", + "t, x, z, m = sym.symbols([\"t\", \"x\", \"z\", \"m\"], real=True)\n", + "s, alpha, n_t, y = sym.symbols([\"s\", \"alpha\", \"n_t\", \"y\"], real=True, positiv=True)" ] }, { "cell_type": "code", - "execution_count": 16, + "execution_count": 10, "id": "65bb90d7-0f5b-410e-9a3a-0f9953a4d846", "metadata": {}, "outputs": [ @@ -225,19 +186,19 @@ "[ 0, 0, 0, 1.0]])" ] }, - "execution_count": 16, + "execution_count": 10, "metadata": {}, "output_type": "execute_result" } ], "source": [ - "T_spiral = symbolic_transformation(t * 2 * sp.pi, np.array([0, 0, 1]), np.array([0, 0, m * sp.pi * n_t * t ]))\n", + "T_spiral = symbolic_transformation(t * 2 * sym.pi, np.array([0, 0, 1]), np.array([0, 0, m * sym.pi * n_t * t ]))\n", "T_spiral" ] }, { "cell_type": "code", - "execution_count": 17, + "execution_count": 11, "id": "0f305b8b-0fb5-4b71-80b6-9a2b43b59e26", "metadata": {}, "outputs": [ @@ -254,19 +215,19 @@ "[ 1]])" ] }, - "execution_count": 17, + "execution_count": 11, "metadata": {}, "output_type": "execute_result" } ], "source": [ - "l = sp.Matrix([0, s * sp.cos(alpha), s * sp.sin(alpha), 1])\n", + "l = sym.Matrix([0, s * sym.cos(alpha), s * sym.sin(alpha), 1])\n", "l" ] }, { "cell_type": "code", - "execution_count": 18, + "execution_count": 12, "id": "da3c8575-99ad-4258-8734-c165ea65b014", "metadata": {}, "outputs": [ @@ -283,7 +244,7 @@ "[ 1.0]])" ] }, - "execution_count": 18, + "execution_count": 12, "metadata": {}, "output_type": "execute_result" } @@ -295,7 +256,7 @@ }, { "cell_type": "code", - "execution_count": 19, + "execution_count": 13, "id": "e47eb83b-6e89-4246-a82a-bd5629aedc2a", "metadata": {}, "outputs": [ @@ -308,19 +269,19 @@ "asin(x/(s*cos(alpha)))/(2*pi)" ] }, - "execution_count": 19, + "execution_count": 13, "metadata": {}, "output_type": "execute_result" } ], "source": [ - "x_cross_section = sp.simplify(sp.solve(spiral[0] - x, t)[1])\n", + "x_cross_section = sym.simplify(sym.solve(spiral[0] - x, t)[1])\n", "x_cross_section # parameter s" ] }, { "cell_type": "code", - "execution_count": 20, + "execution_count": 14, "id": "c2954b39-eea0-4e27-987f-07a5d0dedaad", "metadata": {}, "outputs": [ @@ -337,19 +298,19 @@ "[ 1.0]])" ] }, - "execution_count": 20, + "execution_count": 14, "metadata": {}, "output_type": "execute_result" } ], "source": [ - "spiral_x = sp.simplify(spiral.subs({t: x_cross_section}))\n", + "spiral_x = sym.simplify(spiral.subs({t: x_cross_section}))\n", "spiral_x" ] }, { "cell_type": "code", - "execution_count": 21, + "execution_count": 15, "id": "268c6302-4e7d-45e0-be28-1b64cf8b766e", "metadata": {}, "outputs": [ @@ -362,65 +323,285 @@ "sqrt(x**2 + y**2)/cos(alpha)" ] }, - "execution_count": 21, + "execution_count": 15, "metadata": {}, "output_type": "execute_result" } ], "source": [ - "y_cross_section = sp.simplify(sp.solve(spiral_x[1]- y, s)[0])\n", + "y_cross_section = sym.simplify(sym.solve(spiral_x[1]- y, s)[0])\n", "y_cross_section" ] }, { "cell_type": "code", - "execution_count": 22, + "execution_count": 16, "id": "7284bd6a-b47b-483c-8e9f-79fcaecce5e3", "metadata": {}, "outputs": [ { "data": { "text/latex": [ - "$\\displaystyle \\left[\\begin{matrix}x\\\\\\left|{y}\\right|\\\\\\frac{m n_{t} \\operatorname{asin}{\\left(\\frac{x}{\\sqrt{x^{2} + y^{2}}} \\right)}}{2} + \\sqrt{x^{2} + y^{2}} \\tan{\\left(\\alpha \\right)}\\\\1.0\\end{matrix}\\right]$" + "$\\displaystyle \\left[\\begin{matrix}x\\\\y\\\\\\frac{m n_{t} \\operatorname{asin}{\\left(\\frac{x}{r{\\left(x,y \\right)}} \\right)}}{2} + r{\\left(x,y \\right)} \\tan{\\left(\\alpha \\right)}\\\\1.0\\end{matrix}\\right]$" ], "text/plain": [ "Matrix([\n", - "[ x],\n", - "[ Abs(y)],\n", - "[m*n_t*asin(x/sqrt(x**2 + y**2))/2 + sqrt(x**2 + y**2)*tan(alpha)],\n", - "[ 1.0]])" + "[ x],\n", + "[ y],\n", + "[m*n_t*asin(x/r(x, y))/2 + r(x, y)*tan(alpha)],\n", + "[ 1.0]])" ] }, - "execution_count": 22, + "execution_count": 16, "metadata": {}, "output_type": "execute_result" } ], "source": [ - "spiral_xy = sp.simplify(spiral_x.subs({s: y_cross_section}))\n", + "f_r = sym.Function(\"r\")(x, y)\n", + "spiral_xy = sym.simplify(spiral_x.subs({s: y_cross_section}))\n", + "spiral_xy = spiral_xy.subs({sym.Abs(y): y, sym.sqrt(x**2 + y**2): f_r})\n", "spiral_xy" ] }, + { + "cell_type": "code", + "execution_count": 17, + "id": "e7b9f7cc-59f0-4dc6-922c-c94a753c0fd5", + "metadata": {}, + "outputs": [ + { + "data": { + "text/latex": [ + "$\\displaystyle z{\\left(x,y \\right)}$" + ], + "text/plain": [ + "z(x, y)" + ] + }, + "execution_count": 17, + "metadata": {}, + "output_type": "execute_result" + } + ], + "source": [ + "z = sym.Function(\"z\")(x, y)\n", + "z" + ] + }, + { + "cell_type": "code", + "execution_count": 18, + "id": "080e1845-8ff4-493e-aa28-5b677bfa3cb4", + "metadata": {}, + "outputs": [ + { + "data": { + "text/latex": [ + "$\\displaystyle \\frac{y - y_{p} + z{\\left(x,y \\right)} \\frac{\\partial}{\\partial y} z{\\left(x,y \\right)}}{\\sqrt{\\left(y - y_{p}\\right)^{2} + z^{2}{\\left(x,y \\right)}}}$" + ], + "text/plain": [ + "(y - y_p + z(x, y)*Derivative(z(x, y), y))/sqrt((y - y_p)**2 + z(x, y)**2)" + ] + }, + "execution_count": 18, + "metadata": {}, + "output_type": "execute_result" + } + ], + "source": [ + "y_p =sym.Symbol(\"y_p\")\n", + "dist_p = sym.sqrt(z**2 + (y - y_p)**2)\n", + "dist_p.diff(y)" + ] + }, + { + "cell_type": "markdown", + "id": "d9559950-1520-439c-8e7a-35b4006f104a", + "metadata": {}, + "source": [ + "## for x = 0, for a first guess of t" + ] + }, + { + "cell_type": "code", + "execution_count": 6, + "id": "0811a6d6-4544-4a99-b4bd-ba5cf6b9027e", + "metadata": {}, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "-2.605082438917929\n", + "-2.5926567550137163\n", + "-2.576276171400715\n", + "-2.5572968584132614\n", + "-2.5371037205738425\n", + "-2.516988590529518\n", + "-2.497957291921094\n", + "-2.4810015545771567\n", + "-2.4665124345839313\n", + "-2.455011136367867\n", + "-2.4464950349176995\n", + "-2.4409613820509275\n", + "-2.438356798763718\n", + "-2.438428922923462\n", + "-2.4408671029182987\n", + "-2.445442245573777\n", + "-2.4521135558911062\n", + "-2.460220787391342\n", + "-2.469626739199975\n", + "-2.4799773695959573\n" + ] + }, + { + "data": { + "text/plain": [ + "" + ] + }, + "execution_count": 6, + "metadata": {}, + "output_type": "execute_result" + } + ], + "source": [ + "import scipy as sp\n", + "import numpy as np\n", + "from freecad import part\n", + "from freecad import app\n", + "from pygears.transformation import numeric_transformation\n", + "\n", + "debug = False\n", + "def compute():\n", + " module = 1.\n", + " num_threads = 3\n", + " alpha = np.deg2rad(20.)\n", + " x = 0.\n", + " r_w = 7.5\n", + " y_p = 5\n", + " r_thales = r_w / 2.\n", + " y_thales = y_p + r_thales\n", + " height = 5.\n", + " \n", + " def length(y):\n", + " return (x**2 + y**2)**(0.5)\n", + " \n", + " def dlength_dy(y):\n", + " return y / length(y)\n", + " \n", + " def z(y, t):\n", + " r = length(y)\n", + " return module * num_threads * np.arcsin(x / r) / 2 + r * np.tan(alpha) + t\n", + " # return r * np.tan(alpha) + t\n", + " \n", + " def dz_dy(y):\n", + " r = length(y)\n", + " return -module * num_threads * x * dlength_dy(y) / \\\n", + " (2 * np.sqrt(1 - x ** 2 / r ** 2 ) * r ** 2) + \\\n", + " np.tan(alpha) * dlength_dy(y)\n", + " # return np.tan(alpha) * dlength_dy(y)\n", + " \n", + " def distance_yp(y, t):\n", + " return np.sqrt((y_p - y) ** 2 + z(y, t) ** 2)\n", + " \n", + " def ddistance_yp_dy(y, t):\n", + " return (y - y_p + z(y, t) * dz_dy(y)) / distance_yp(y, t)\n", + " \n", + " def distance_ythales(y, t):\n", + " return np.sqrt((y_thales - y) ** 2 + z(y, t) ** 2)\n", + " \n", + " def min_ground(pars):\n", + " y, t = pars\n", + " # return the normal-condition + the intesection of the tooth-flank with the thales circle\n", + " return ddistance_yp_dy(y, t) ** 2 + (distance_ythales(y, t) - r_thales) ** 2\n", + "\n", + " def min_head(pars):\n", + " y, t = pars\n", + " r_0 = y_p - module # * (1 + clearence)\n", + " y_inner = r_0 * np.cos(np.arcsin(x / r_0))\n", + " return ddistance_yp_dy(y, t) ** 2 + (y - y_inner) ** 2\n", + "\n", + " def analytic_solution_for_x_0():\n", + " t = - (r_w + y_p) * np.tan(alpha)\n", + " y = y_p + r_w * np.sin(alpha) ** 2\n", + " return np.array([y, t])\n", + " \n", + " start = analytic_solution_for_x_0()\n", + " if debug:\n", + " print(f\"analytic solution: {analytic_solution_for_x_0()}\")\n", + " print(f\"min_ground analytic: {min_ground(start)}\")\n", + " print(f\"thales analytic: {distance_ythales(start[0], start[1]) - r_thales}\")\n", + " print(f\"normal analytic: {ddistance_yp_dy(start[0], start[1])}\")\n", + " print()\n", + "\n", + "\n", + " xyz = []\n", + " for x in np.linspace(-height / 2, height / 2, 20):\n", + " xyz_section = []\n", + " t_start = sp.optimize.minimize(min_ground, start).x[1]\n", + " # t_end = sp.optimize.minimize(min_head, [y_p, 0.]).x[1]\n", + " t_end = 3\n", + " for t in np.linspace(t_start, t_end, 20):\n", + " # compute the transformation\n", + " T0 = numeric_transformation((y_p * np.tan(alpha) + np.sign(alpha) * module * np.pi / 4) / r_w,\n", + " np.array([0., 0., 1.]), \n", + " np.array([0., 0.,0.]))\n", + " T1 = numeric_transformation(np.pi / 2.,\n", + " np.array([0., 1., 0.]),\n", + " np.array([0., y_p + r_w, 0.]))\n", + " T2 = numeric_transformation(-t / r_w,\n", + " np.array([0., 0., 1.]),\n", + " np.array([0., 0., 0.]))\n", + " T = T0 @ np.linalg.inv(T2) @ np.linalg.inv(T1)\n", + " y = sp.optimize.minimize(distance_yp, y_p, (t)).x[0]\n", + " z_i = z(y, t) # - y_p * np.tan(alpha) + np.sign(alpha) * module * np.pi / 4\n", + " point = np.array([x, y, z_i, 1.])\n", + " xyz_section.append((T @ point)[:3])\n", + " xyz.append(np.array(xyz_section))\n", + "\n", + " return np.array(xyz)\n", + "\n", + "\n", + "curves = []\n", + "for line in compute().transpose(1, 0, 2):\n", + " bs = part.BSplineCurve()\n", + " points = [app.Vector(*point) for point in line]\n", + " bs.interpolate(points)\n", + " curves.append(bs.toShape())\n", + "\n", + "part.show(part.makeLoft(curves))" + ] + }, + { + "cell_type": "code", + "execution_count": 5, + "id": "d804123e-3a1f-418a-b042-3619b1286c29", + "metadata": {}, + "outputs": [ + { + "data": { + "text/plain": [ + "[1, 2]" + ] + }, + "execution_count": 5, + "metadata": {}, + "output_type": "execute_result" + } + ], + "source": [ + "[1,2,3,4][:2]" + ] + }, { "cell_type": "code", "execution_count": null, - "id": "301cc8e3-b28d-4f36-8fc2-f4333d4f0bf1", + "id": "012a264b-12b2-4fa3-a734-60251262e914", "metadata": {}, "outputs": [], - "source": [ - "import scipy as scp\n", - "def contact_point(m, n_t, alpha, rw_worm):\n", - " def z(x, y, t):\n", - " r = np.sqrt(x ** 2 + y ** 2)\n", - " return m * n_t * np.asin(x / r) + r * np.tan(alpha) + t\n", - "\n", - " def contact_point(x, t):\n", - " def distance_to_pitch_point(y):\n", - " return (y - rw_worm) ** 2 + z(x, y, t) ** 2\n", - "\n", - " y = scp.optimize.minimize(distance_to_pitch_point, rw_worm).x\n", - " return np.array([x, y, z(x, y, t)]" - ] + "source": [] } ], "metadata": { diff --git a/setup.py b/setup.py index e91e7dc..a632fa4 100644 --- a/setup.py +++ b/setup.py @@ -9,6 +9,6 @@ setup( maintainer_email="sppedflyer@gmail.com", url="https://github.com/looooo/FCGear", description="gears for FreeCAD", - install_requires=["numpy", "scipy"], + install_requires=["numpy", "scipy", "sympy"], include_package_data=True, )