Meta - Paths Circle.ipynb

{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Контурное управление со сглаживанием на окружностях"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "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,
   "metadata": {},
   "outputs": [],
   "source": [
    "from kinematics import Vector, Quaternion, Transform\n",
    "import graphics"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Рассмотрим сглаживание траектории с помощью окружностей.\n",
    "При использовании окружности мы можем гарантировать сохранение постоянной скорости.\n",
    "\n",
    "Зададим начальную, поомежуточную и конечную точки, укажем радиус зоны сглаживания."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "s = Vector(2, -4, 0)\n",
    "i = Vector(2, 3, 5)\n",
    "e = Vector(-3, 3, 0)\n",
    "r = 2"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "d1 = i + -1 * s\n",
    "d2 = e + -1 * i\n",
    "n = d1.cross(d2).normalized()\n",
    "nd = i + n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "p1 = d1 + -1 * r * d1.normalized()\n",
    "nn = n.cross(d1).normalized()\n",
    "nnd = i + nn"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "a = d1.angle_to(-1 * d2, n)\n",
    "l = r / np.cos(a / 2)\n",
    "rr = r * np.tan(a / 2)\n",
    "c = i + Quaternion.from_angle_axis(a / 2, n) * (-l * d1.normalized())\n",
    "b = np.pi + a\n",
    "\n",
    "def fillet(t):\n",
    "    pos = c + Quaternion.from_angle_axis(-b / 2 + b * t, n) * (-1 * rr * (i + -1 * c).normalized())\n",
    "    return (\n",
    "        pos.x,\n",
    "        pos.y,\n",
    "        pos.z\n",
    "    )"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "l1 = d1.magnitude() - r\n",
    "l2 = np.abs(b * rr)\n",
    "l3 = d2.magnitude() - r\n",
    "\n",
    "def path(t):\n",
    "    total = l1 + l2 + l3\n",
    "    position = t * total\n",
    "    if position < l1:\n",
    "        progress = position / l1\n",
    "        return s + progress * l1 * d1.normalized()\n",
    "    if position < l1 + l2:\n",
    "        progress = (position - l1) / l2\n",
    "        f = fillet(progress)\n",
    "        return Vector(f[0], f[1], f[2])\n",
    "    if position < l1 + l2 + l3:\n",
    "        progress = (position - l1 - l2) / l3\n",
    "        return i + (r + (progress * l3)) * d2.normalized()\n",
    "    return Vector.zero()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "fig, ax = graphics.figure(6)\n",
    "ax.plot([s.x, i.x, e.x], [s.y, i.y, e.y], [s.z, i.z, e.z], label=\"path\")\n",
    "ax.plot([i.x, nd.x], [i.y, nd.y], [i.z, nd.z], label=\"path plane normal\")\n",
    "ax.plot([i.x, nnd.x], [i.y, nnd.y], [i.z, nnd.z], label=\"normal normal\")\n",
    "ax.plot([i.x, c.x], [i.y, c.y], [i.z, c.z], label=\"center connector\")\n",
    "\n",
    "l, = ax.plot([], [], [], label=\"motion\")\n",
    "\n",
    "t = np.arange(0, 1, 0.01)\n",
    "f = fillet(t)\n",
    "ax.plot(f[0], f[1], f[2], label=\"fillet\")\n",
    "\n",
    "total = 100\n",
    "\n",
    "def animate(frame):\n",
    "    t = frame / total\n",
    "    p = path(t)\n",
    "    l.set_data_3d([0, p.x], [0, p.y], [0, p.z])\n",
    "\n",
    "fig.legend()\n",
    "\n",
    "animate(50)\n",
    "fps = 25\n",
    "ani = animation.FuncAnimation(\n",
    "    fig,\n",
    "    animate,\n",
    "    frames=total,\n",
    "    interval=1000.0/fps\n",
    ")"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Как видно, скорость $V$ остается постоянной на всей траектории."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "v_path = np.vectorize(path)\n",
    "step = 0.001\n",
    "t = np.arange(0, 1, step)\n",
    "pos = v_path(t)\n",
    "x = []\n",
    "y = []\n",
    "z = []\n",
    "for p in pos:\n",
    "    x += [p.x]\n",
    "    y += [p.y]\n",
    "    z += [p.z]\n",
    "vx = np.diff(x) / step\n",
    "vy = np.diff(y) / step\n",
    "vz = np.diff(z) / step\n",
    "v = (vx ** 2 + vy ** 2 + vz ** 2) ** 0.5\n",
    "\n",
    "acx = np.diff(vx) / step\n",
    "acy = np.diff(vy) / step\n",
    "acz = np.diff(vz) / step\n",
    "\n",
    "fig = plt.figure(figsize=(8, 8))\n",
    "ax = fig.add_subplot(2, 2, 1)\n",
    "ax.plot(t, x, color=\"#c00000\", label=\"$x$\")\n",
    "ax.plot(t, y, color=\"#00c000\", label=\"$y$\")\n",
    "ax.plot(t, z, color=\"#0000c0\", label=\"$z$\")\n",
    "ax.legend()\n",
    "\n",
    "ava = fig.add_subplot(2, 2, 2)\n",
    "ava.plot(t[:-1], v, label=\"$v$\")\n",
    "ava.set_ylim((0, np.max(v) + 1))\n",
    "ava.legend()\n",
    "\n",
    "av = fig.add_subplot(2, 2, 3)\n",
    "av.plot(t[:-1], vx, color=\"#c00000\", label=\"$V_x$\")\n",
    "av.plot(t[:-1], vy, color=\"#00c000\", label=\"$V_y$\")\n",
    "av.plot(t[:-1], vz, color=\"#0000c0\", label=\"$V_z$\")\n",
    "av.legend()\n",
    "\n",
    "aa = fig.add_subplot(2, 2, 4)\n",
    "aa.plot(t[:-2], acx, color=\"#c00000\", label=\"$a_x$\")\n",
    "aa.plot(t[:-2], acy, color=\"#00c000\", label=\"$a_y$\")\n",
    "aa.plot(t[:-2], acz, color=\"#0000c0\", label=\"$a_z$\")\n",
    "aa.legend()\n",
    "\n",
    "fig.show()"
   ]
  }
 ],
 "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.2"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 4
}