6 - Delta Example.ipynb

{
 "cells": [
  {
   "cell_type": "markdown",
   "id": "driven-chair",
   "metadata": {},
   "source": [
    "# Пример с $\\Delta$-кинематикой\n",
    "\n",
    "## Прямая задача"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "spread-algebra",
   "metadata": {},
   "outputs": [],
   "source": [
    "from sympy import *\n",
    "\n",
    "\n",
    "def rz(a):\n",
    "    return Matrix([\n",
    "        [cos(a), -sin(a), 0, 0],\n",
    "        [sin(a), cos(a), 0, 0],\n",
    "        [0, 0, 1, 0],\n",
    "        [0, 0, 0, 1]\n",
    "    ])\n",
    "\n",
    "\n",
    "def ry(a):\n",
    "    return Matrix([\n",
    "        [cos(a), 0, sin(a), 0],\n",
    "        [0, 1, 0, 0],\n",
    "        [-sin(a), 0, cos(a), 0],\n",
    "        [0, 0, 0, 1]\n",
    "    ])\n",
    "\n",
    "\n",
    "def rx(a):\n",
    "    return Matrix([\n",
    "        [1, 0, 0, 0],\n",
    "        [0, cos(a), -sin(a), 0],\n",
    "        [0, sin(a), cos(a), 0],\n",
    "        [0, 0, 0, 1]\n",
    "    ])\n",
    "\n",
    "\n",
    "def trs(x, y, z):\n",
    "    return Matrix([\n",
    "        [1, 0, 0, x],\n",
    "        [0, 1, 0, y],\n",
    "        [0, 0, 1, z],\n",
    "        [0, 0, 0, 1]\n",
    "    ])\n",
    "\n",
    "\n",
    "def vec(x, y, z):\n",
    "    return Matrix([\n",
    "        [x],\n",
    "        [y],\n",
    "        [z],\n",
    "        [1]\n",
    "    ])"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "mathematical-syracuse",
   "metadata": {},
   "source": [
    "Дельта-принтер:\n",
    "![delta printer](./fig/delta.png)\n",
    "[источник изображения](https://www.3dprintersonlinestore.com/delta-3d-printer-kit)\n",
    "\n",
    "Возьмем $0$ в центре, $X$ направлено вправо, $Y$ - вглубь, $Z$ - вверх.\n",
    "\n",
    "Для описания положения \"ползунов\" будем отчситывать их высоту относительно основания. Первый ползун - в глубине, второй - слева, третий - справа."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "actual-magazine",
   "metadata": {},
   "outputs": [],
   "source": [
    "q1, q2, q3 = symbols(\"q_1, q_2, q_3\")"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "continuing-startup",
   "metadata": {},
   "source": [
    "Обратим внимание, что плоский перенос тяг вдоль оси, проходящей через центр и ползун не будет влиять на решение кинематики.\n",
    "За счет этого мысленно сместим тяги в центр подвижной платформы, и определим проекцию расстояния от крепления тяги до центра на горизонтальную плоскость.\n",
    "Это расстояние, например, в прошивке Marlin называется `delta radius` и обозначается $R$:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "public-titanium",
   "metadata": {},
   "outputs": [],
   "source": [
    "delta_radius = symbols(\"R\")"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "corrected-station",
   "metadata": {},
   "source": [
    "Длину тяг (`diagonal rod`) обозначим как $L$:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "contrary-slovenia",
   "metadata": {},
   "outputs": [],
   "source": [
    "diagonal_rod = symbols(\"L\")"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "cutting-stamp",
   "metadata": {},
   "source": [
    "Тогда, положение ползунов будет следующим:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "endangered-content",
   "metadata": {},
   "outputs": [],
   "source": [
    "slider1 = vec(0, delta_radius, q1)\n",
    "slider2 = rz(2*pi/3) * vec(0, delta_radius, q2)\n",
    "slider3 = rz(4*pi/3) * vec(0, delta_radius, q3)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "whole-feature",
   "metadata": {},
   "source": [
    "Положение центра подвижной платформы зададим как вектор-столбец:\n",
    "$$\n",
    "p = \n",
    "\\begin{bmatrix}\n",
    "    x \\\\\n",
    "    y \\\\\n",
    "    z \\\\\n",
    "    1\n",
    "\\end{bmatrix}\n",
    "$$"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "mathematical-antique",
   "metadata": {},
   "outputs": [],
   "source": [
    "x, y, z = symbols(\"x, y, z\")\n",
    "p = vec(x, y, z)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "heavy-butter",
   "metadata": {},
   "source": [
    "Тогда, \"очевидно\" что расстояние от центра платформы до каждого ползуна должно равняться длине тяги, а квадрат расстояния - квадрату длины тяги:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "liquid-promotion",
   "metadata": {},
   "outputs": [],
   "source": [
    "def distance_sq(a, b):\n",
    "    return (a[0] - b[0]) ** 2 +\\\n",
    "        (a[1] - b[1]) ** 2 +\\\n",
    "        (a[2] - b[2]) ** 2"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "three-colon",
   "metadata": {},
   "outputs": [],
   "source": [
    "condition1 = Eq(distance_sq(p, slider1), diagonal_rod**2)\n",
    "condition2 = Eq(distance_sq(p, slider2), diagonal_rod**2)\n",
    "condition3 = Eq(distance_sq(p, slider3), diagonal_rod**2)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "twelve-monthly",
   "metadata": {},
   "outputs": [],
   "source": [
    "import time\n",
    "start = time.time()\n",
    "fk_solution = solve(\n",
    "    [\n",
    "        condition1,\n",
    "        condition2,\n",
    "        condition3\n",
    "    ],\n",
    "    [\n",
    "        x,\n",
    "        y,\n",
    "        z\n",
    "    ],\n",
    ")\n",
    "end = time.time()\n",
    "print(\"Прошло секунд:\", end - start)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "changed-retro",
   "metadata": {},
   "source": [
    "Должно быть два решения:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "expired-livestock",
   "metadata": {
    "scrolled": true
   },
   "outputs": [],
   "source": [
    "len(fk_solution)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "miniature-exercise",
   "metadata": {},
   "source": [
    "Рассмотрим первое:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "peaceful-jefferson",
   "metadata": {},
   "outputs": [],
   "source": [
    "fk_solution[0][0]"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "optimum-methodology",
   "metadata": {},
   "outputs": [],
   "source": [
    "fk_solution[0][1]"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "novel-sessions",
   "metadata": {},
   "outputs": [],
   "source": [
    "fk_solution[0][2]"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "exclusive-sodium",
   "metadata": {},
   "source": [
    "Теперь нарисуем какой-нибудь принтер и посмотрим на его движение:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "caring-elimination",
   "metadata": {},
   "outputs": [],
   "source": [
    "from matplotlib import pyplot as plt\n",
    "from matplotlib import animation\n",
    "import numpy as np\n",
    "from IPython.display import HTML\n",
    "%matplotlib notebook"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "structured-postage",
   "metadata": {},
   "outputs": [],
   "source": [
    "def fk(r, l, pos1, pos2, pos3):\n",
    "    x = fk_solution[0][0].evalf(subs={\n",
    "        delta_radius: r,\n",
    "        diagonal_rod: l,\n",
    "        q1: pos1,\n",
    "        q2: pos2,\n",
    "        q3: pos3\n",
    "    })\n",
    "    y = fk_solution[0][1].evalf(subs={\n",
    "        delta_radius: r,\n",
    "        diagonal_rod: l,\n",
    "        q1: pos1,\n",
    "        q2: pos2,\n",
    "        q3: pos3\n",
    "    })\n",
    "    z = fk_solution[0][2].evalf(subs={\n",
    "        delta_radius: r,\n",
    "        diagonal_rod: l,\n",
    "        q1: pos1,\n",
    "        q2: pos2,\n",
    "        q3: pos3\n",
    "    })\n",
    "    return (x, y, z)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "single-retirement",
   "metadata": {},
   "outputs": [],
   "source": [
    "def generalized(t, total):\n",
    "    omega = t / total * 2 * np.pi\n",
    "    q1 = 350 + 25 * np.sin(omega)\n",
    "    q2 = 350 + 25 * np.sin(omega + 2/3*np.pi)\n",
    "    q3 = 350 + 25 * np.sin(omega + 4/3*np.pi)\n",
    "    return (q1, q2, q3)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "framed-drama",
   "metadata": {},
   "outputs": [],
   "source": [
    "delta_radius_ex = 124\n",
    "diagonal_rod_ex = 337\n",
    "\n",
    "fig = plt.figure()\n",
    "ax = fig.add_subplot(projection=\"3d\")\n",
    "\n",
    "ax.set_xlim([-delta_radius_ex, delta_radius_ex])\n",
    "ax.set_ylim([-delta_radius_ex, delta_radius_ex])\n",
    "ax.set_zlim([0, 2 * delta_radius_ex])\n",
    "ax.plot([0]*2, [delta_radius_ex] * 2, [0, 400], color=\"#000000\")\n",
    "ax.plot([-np.sqrt(3)/2*delta_radius_ex]*2, [-delta_radius_ex/2]*2, [0, 400], color=\"#000000\")\n",
    "ax.plot([np.sqrt(3)/2*delta_radius_ex]*2, [-delta_radius_ex/2]*2, [0, 400], color=\"#000000\")\n",
    "rod1, = ax.plot([], [], [], color=\"#ff0000\")\n",
    "rod2, = ax.plot([], [], [], color=\"#00ff00\")\n",
    "rod3, = ax.plot([], [], [], color=\"#0000ff\")\n",
    "\n",
    "total = 36\n",
    "\n",
    "def animate(frame):\n",
    "    q1, q2, q3 = generalized(frame, total)\n",
    "    x, y, z = fk(delta_radius_ex, diagonal_rod_ex, q1, q2, q3)\n",
    "    rod1.set_data_3d([0, x], [delta_radius_ex, y], [q1, z])\n",
    "    rod2.set_data_3d([-np.sqrt(3)/2*delta_radius_ex, x], [-delta_radius_ex/2, y], [q2, z])\n",
    "    rod3.set_data_3d([np.sqrt(3)/2*delta_radius_ex, x], [-delta_radius_ex/2, y], [q3, z])\n",
    "\n",
    "ani = animation.FuncAnimation(\n",
    "    fig,\n",
    "    animate,\n",
    "    frames=total,\n",
    "    interval=1000.0/25\n",
    ")"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "applied-continuity",
   "metadata": {},
   "source": [
    "## Обратная задача"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "chinese-conclusion",
   "metadata": {
    "scrolled": true
   },
   "outputs": [],
   "source": [
    "start = time.time()\n",
    "ik_solution = solve(\n",
    "    [\n",
    "        condition1,\n",
    "        condition2,\n",
    "        condition3\n",
    "    ],\n",
    "    [\n",
    "        q1,\n",
    "        q2,\n",
    "        q3\n",
    "    ],\n",
    ")\n",
    "end = time.time()\n",
    "print(\"Прошло секунд:\", end - start)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "aggressive-venice",
   "metadata": {},
   "source": [
    "Решений должно быть $2^3$:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "separated-monaco",
   "metadata": {},
   "outputs": [],
   "source": [
    "len(ik_solution)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "honest-nebraska",
   "metadata": {},
   "source": [
    "Рассмотрим восьмое:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "talented-passenger",
   "metadata": {},
   "outputs": [],
   "source": [
    "ik_solution[7][0]"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "complex-stanley",
   "metadata": {},
   "outputs": [],
   "source": [
    "ik_solution[7][1]"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "alive-dispatch",
   "metadata": {},
   "outputs": [],
   "source": [
    "ik_solution[7][2]"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "decreased-terry",
   "metadata": {},
   "source": [
    "Помоделируем наше решение:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "banner-soviet",
   "metadata": {},
   "outputs": [],
   "source": [
    "def ik(r, l, pos1, pos2, pos3):\n",
    "    q1 = ik_solution[7][0].evalf(subs={\n",
    "        delta_radius: r,\n",
    "        diagonal_rod: l,\n",
    "        x: pos1,\n",
    "        y: pos2,\n",
    "        z: pos3\n",
    "    })\n",
    "    q2 = ik_solution[7][1].evalf(subs={\n",
    "        delta_radius: r,\n",
    "        diagonal_rod: l,\n",
    "        x: pos1,\n",
    "        y: pos2,\n",
    "        z: pos3\n",
    "    })\n",
    "    q3 = ik_solution[7][2].evalf(subs={\n",
    "        delta_radius: r,\n",
    "        diagonal_rod: l,\n",
    "        x: pos1,\n",
    "        y: pos2,\n",
    "        z: pos3\n",
    "    })\n",
    "    return (q1, q2, q3)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "aquatic-forestry",
   "metadata": {},
   "outputs": [],
   "source": [
    "def position(t, total):\n",
    "    omega = t / total\n",
    "    r = delta_radius_ex / 2\n",
    "    h = 50\n",
    "    if omega < 0.25:\n",
    "        tau = omega/0.25\n",
    "        return (\n",
    "            -r + r*tau*2,\n",
    "            -r,\n",
    "            h\n",
    "        )\n",
    "    elif omega < 0.5:\n",
    "        tau = (omega - 0.25)/0.25\n",
    "        return (\n",
    "            r,\n",
    "            -r + r*tau*2,\n",
    "            h\n",
    "        )\n",
    "    elif omega < 0.75:\n",
    "        tau = (omega - 0.5)/0.25\n",
    "        return (\n",
    "            r - r*tau*2,\n",
    "            r,\n",
    "            h\n",
    "        )\n",
    "    else:\n",
    "        tau = (omega - 0.75)/0.25\n",
    "        return (\n",
    "            -r,\n",
    "            r - r*tau*2,\n",
    "            h\n",
    "        )"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "decent-wrapping",
   "metadata": {},
   "outputs": [],
   "source": [
    "fig = plt.figure()\n",
    "bx = fig.add_subplot(projection=\"3d\")\n",
    "\n",
    "bx.set_xlim([-delta_radius_ex, delta_radius_ex])\n",
    "bx.set_ylim([-delta_radius_ex, delta_radius_ex])\n",
    "bx.set_zlim([0, 2 * delta_radius_ex])\n",
    "bx.plot([0]*2, [delta_radius_ex] * 2, [0, 400], color=\"#000000\")\n",
    "bx.plot([-np.sqrt(3)/2*delta_radius_ex]*2, [-delta_radius_ex/2]*2, [0, 400], color=\"#000000\")\n",
    "bx.plot([np.sqrt(3)/2*delta_radius_ex]*2, [-delta_radius_ex/2]*2, [0, 400], color=\"#000000\")\n",
    "rod1, = bx.plot([], [], [], color=\"#ff0000\")\n",
    "rod2, = bx.plot([], [], [], color=\"#00ff00\")\n",
    "rod3, = bx.plot([], [], [], color=\"#0000ff\")\n",
    "\n",
    "total = 100\n",
    "\n",
    "def animate2(frame):\n",
    "    x, y, z = position(frame, total)\n",
    "    q1, q2, q3 = ik(delta_radius_ex, diagonal_rod_ex, x, y, z)\n",
    "    rod1.set_data_3d([0, x], [delta_radius_ex, y], [q1, z])\n",
    "    rod2.set_data_3d([-np.sqrt(3)/2*delta_radius_ex, x], [-delta_radius_ex/2, y], [q2, z])\n",
    "    rod3.set_data_3d([np.sqrt(3)/2*delta_radius_ex, x], [-delta_radius_ex/2, y], [q3, z])\n",
    "\n",
    "animate2(0)\n",
    "ani2 = animation.FuncAnimation(\n",
    "    fig,\n",
    "    animate2,\n",
    "    frames=total,\n",
    "    interval=1000.0/25\n",
    ")"
   ]
  }
 ],
 "metadata": {
  "@webio": {
   "lastCommId": null,
   "lastKernelId": null
  },
  "kernelspec": {
   "display_name": "venv",
   "language": "python",
   "name": "venv"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.9.5"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 5
}