lab3_1.ipynb

{
 "cells": [
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "#### Лаба 3\n",
    "\n",
    "#### задачи траекторного управления\n",
    "\n",
    "Подробное описание вывода формул в [trajectory_control_LAB3.md](./specifications/trajectory_control_LAB3.md)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "metadata": {},
   "outputs": [],
   "source": [
    "import sympy as sm\n",
    "import numpy as np\n",
    "import scipy as sp\n",
    "import matplotlib.pyplot as plt\n",
    "from IPython.display import display, Math\n",
    "import inspect\n",
    "import bisect"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 25,
   "metadata": {},
   "outputs": [],
   "source": [
    "def display_expr(expr):\n",
    "    frame = inspect.currentframe()\n",
    "    frame = inspect.getouterframes(frame)[1]\n",
    "    string = inspect.getframeinfo(frame[0]).code_context[0].strip()\n",
    "    args = string[string.find('(') + 1:-1].split(',')\n",
    "    \n",
    "    names = []\n",
    "    for i in args:\n",
    "        if i.find('=') != -1:\n",
    "            names.append(i.split('=')[1].strip())\n",
    "        else:\n",
    "            names.append(i)\n",
    "    if names[0]=='phi_1':\n",
    "        names[0] = '\\\\varphi_1'\n",
    "    if names[0]=='phi_2':\n",
    "        names[0] = '\\\\varphi_2'\n",
    "    if names[0]=='phi_3':\n",
    "        names[0] = '\\\\varphi_3'\n",
    "    if names[0]=='vm':\n",
    "        names[0] = 'v_m'\n",
    "    if names[0]=='vb':\n",
    "        names[0] = 'v_b'\n",
    "    if names[0]=='dots':\n",
    "        names[0] = '\\\\dot s'\n",
    "    if names[0]=='dote':\n",
    "        names[0] = '\\\\dot e'\n",
    "    if names[0]=='xi':\n",
    "        names[0] = '\\\\xi'\n",
    "    if names[0]=='bRm':\n",
    "        names[0] = '{}^{b}R_m'\n",
    "    if names[0]=='gamma':\n",
    "        names[0] = '\\\\gamma'\n",
    "    if names[0]=='dphidq':\n",
    "        names[0] = '\\\\frac{\\\\partial \\\\varphi(q)}{\\\\partial q}'\n",
    "\n",
    "    expr = expr.xreplace({n : round(n, 3) for n in expr.atoms(sm.Number)})\n",
    "    str = sm.latex(expr)\n",
    "    # str = str.replace('\\\\theta','\\\\vartheta')\n",
    "    str = str.replace('vx','v_x')\n",
    "    str = str.replace('vy','v_y')\n",
    "    str = str.replace('Fx','F_x')\n",
    "    str = str.replace('Fy','F_y') \n",
    "    str = str.replace('\\\\alpha_{asterisk}','\\\\alpha^{*}') \n",
    "    str = str.replace('dotxi','\\\\dot \\\\xi')\n",
    "    str = str.replace('\\\\xi_{dxds}','\\\\frac{\\partial x_{\\\\xi}(s)}{\\partial s}') \n",
    "\n",
    "    display(Math(names[0] + '=' + str))    "
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Task 1"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "$m\\ddot q = F$\n",
    "\n",
    "$\\dot q=R_O^I(\\alpha)v$\n",
    "\n",
    "$R_O^I = \\begin{bmatrix} cos\\alpha & sin\\alpha \\cr -sin\\alpha & cos\\alpha \\end{bmatrix}$\n",
    "\n",
    "Control low:\n",
    "\n",
    "$F=m*(\\dot{\\overline{v}} - k_q(\\dot q - \\overline{v}))$\n",
    "\n",
    "$\\overline{v}=u_e+u_s$\n",
    "\n",
    "$u_s=R_O^I(\\alpha)\\gamma^{-1}(q)  \\begin{bmatrix} V^* \\cr 0 \\end{bmatrix}$\n",
    "\n",
    "$\\large u_e = -k_e \\varphi(q)\\frac{\\partial \\varphi(q)}{\\partial q}$"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 14,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/latex": [
       "$\\displaystyle {}^{b}R_m=\\left[\\begin{matrix}\\cos{\\left(\\alpha \\right)} & \\sin{\\left(\\alpha \\right)}\\\\- \\sin{\\left(\\alpha \\right)} & \\cos{\\left(\\alpha \\right)}\\end{matrix}\\right]$"
      ],
      "text/plain": [
       "<IPython.core.display.Math object>"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "data": {
      "text/latex": [
       "$\\displaystyle \\varphi_1=\\left(x + 3\\right)^{2} + \\left(y - 2\\right)^{2} - 9$"
      ],
      "text/plain": [
       "<IPython.core.display.Math object>"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "data": {
      "text/latex": [
       "$\\displaystyle \\varphi_2=y - 2 \\cos{\\left(0.5 x + 0.3 \\right)} - 2$"
      ],
      "text/plain": [
       "<IPython.core.display.Math object>"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "data": {
      "text/latex": [
       "$\\displaystyle \\varphi_3=\\left(x - 12\\right)^{2} + \\left(y - 2\\right)^{2} - 25$"
      ],
      "text/plain": [
       "<IPython.core.display.Math object>"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    }
   ],
   "source": [
    "m = 2.8 # in kg\n",
    "x, y, alpha = sm.symbols(\"x y alpha\")\n",
    "x0 = -7\n",
    "y0 = 4\n",
    "alpha0 = 3*np.pi/4\n",
    "vs_des1 = 1 \n",
    "vs_des2 = 3\n",
    "vs_des3 = 5\n",
    "\n",
    "R1 = 3\n",
    "xc1 = -3\n",
    "yc1 = 2\n",
    "R3 = 5\n",
    "xc3 = 12\n",
    "yc3 = 2\n",
    "phi_1 = (x-xc1)**2+(y-yc1)**2-R1**2\n",
    "phi_2 = -2*sm.cos(0.5*x+0.3)+y-2\n",
    "phi_3 = (x-xc3)**2 + (y-yc3)**2 - R3**2\n",
    "\n",
    "bRm = sm.Matrix( [[sm.cos(alpha),sm.sin(alpha)],[-sm.sin(alpha),sm.cos(alpha)]] ) # from mobile to base\n",
    "display_expr(bRm)\n",
    "display_expr(phi_1)\n",
    "display_expr(phi_2)\n",
    "display_expr(phi_3)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 12,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/latex": [
       "$\\displaystyle \\left[\\begin{matrix}-0.707106781186547 & 0.707106781186548\\\\-0.707106781186548 & -0.707106781186547\\end{matrix}\\right]$"
      ],
      "text/plain": [
       "Matrix([\n",
       "[-0.707106781186547,  0.707106781186548],\n",
       "[-0.707106781186548, -0.707106781186547]])"
      ]
     },
     "execution_count": 12,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "bRm.subs(alpha,alpha0)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 16,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "58.64969623920736\n",
      "19.54989874640245\n",
      "11.729939247841472\n"
     ]
    }
   ],
   "source": [
    "s_all = [0, 7.243989865398947, 28.737350135440845, 42.94173297125839, 58.64969623920736]\n",
    "Tdes1 = s_all[-1]/vs_des1\n",
    "print(Tdes1) # simulation time\n",
    "Tdes2 = s_all[-1]/vs_des2\n",
    "print(Tdes2) # simulation time\n",
    "Tdes3 = s_all[-1]/vs_des3\n",
    "print(Tdes3) # simulation time"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 18,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "[7.243989865398947, 28.737350135440845, 42.94173297125839]\n",
      "[2.4146632884663157, 9.579116711813615, 14.313910990419464]\n",
      "[1.4487979730797895, 5.747470027088169, 8.588346594251679]\n"
     ]
    }
   ],
   "source": [
    "# times for switching the control\n",
    "switch_times = []\n",
    "for i in range(1,4):\n",
    "    switch_times.append(s_all[i]/vs_des1)\n",
    "print(switch_times)\n",
    "switch_times = []\n",
    "for i in range(1,4):\n",
    "    switch_times.append(s_all[i]/vs_des2)\n",
    "print(switch_times)\n",
    "switch_times = []\n",
    "for i in range(1,4):\n",
    "    switch_times.append(s_all[i]/vs_des3)\n",
    "print(switch_times)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 26,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/latex": [
       "$\\displaystyle \\gamma=\\left[\\begin{matrix}2 y - 4 & - 2 x - 6\\\\2 x + 6 & 2 y - 4\\end{matrix}\\right]$"
      ],
      "text/plain": [
       "<IPython.core.display.Math object>"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "data": {
      "text/latex": [
       "$\\displaystyle \\frac{\\partial \\varphi(q)}{\\partial q}=\\left[\\begin{matrix}2 x + 6\\\\2 y - 4\\end{matrix}\\right]$"
      ],
      "text/plain": [
       "<IPython.core.display.Math object>"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    }
   ],
   "source": [
    "gamma = sm.Matrix([[sm.diff(phi_1,y),-sm.diff(phi_1,x)],[sm.diff(phi_1,x),sm.diff(phi_1,y)]])\n",
    "display_expr(gamma)\n",
    "dphidq = sm.Matrix([[sm.diff(phi_1,x)],[sm.diff(phi_1,y)]])\n",
    "display_expr(dphidq)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 28,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/latex": [
       "$\\displaystyle \\gamma=\\left[\\begin{matrix}1 & - 1.0 \\sin{\\left(0.5 x + 0.3 \\right)}\\\\1.0 \\sin{\\left(0.5 x + 0.3 \\right)} & 1\\end{matrix}\\right]$"
      ],
      "text/plain": [
       "<IPython.core.display.Math object>"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "data": {
      "text/latex": [
       "$\\displaystyle \\frac{\\partial \\varphi(q)}{\\partial q}=\\left[\\begin{matrix}1.0 \\sin{\\left(0.5 x + 0.3 \\right)}\\\\1\\end{matrix}\\right]$"
      ],
      "text/plain": [
       "<IPython.core.display.Math object>"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    }
   ],
   "source": [
    "gamma = sm.Matrix([[sm.diff(phi_2,y),-sm.diff(phi_2,x)],[sm.diff(phi_2,x),sm.diff(phi_2,y)]])\n",
    "display_expr(gamma)\n",
    "dphidq = sm.Matrix([[sm.diff(phi_2,x)],[sm.diff(phi_2,y)]])\n",
    "display_expr(dphidq)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 27,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/latex": [
       "$\\displaystyle \\gamma=\\left[\\begin{matrix}2 y - 4 & 24 - 2 x\\\\2 x - 24 & 2 y - 4\\end{matrix}\\right]$"
      ],
      "text/plain": [
       "<IPython.core.display.Math object>"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "data": {
      "text/latex": [
       "$\\displaystyle \\frac{\\partial \\varphi(q)}{\\partial q}=\\left[\\begin{matrix}2 x - 24\\\\2 y - 4\\end{matrix}\\right]$"
      ],
      "text/plain": [
       "<IPython.core.display.Math object>"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    }
   ],
   "source": [
    "gamma = sm.Matrix([[sm.diff(phi_3,y),-sm.diff(phi_3,x)],[sm.diff(phi_3,x),sm.diff(phi_3,y)]])\n",
    "display_expr(gamma)\n",
    "dphidq = sm.Matrix([[sm.diff(phi_3,x)],[sm.diff(phi_3,y)]])\n",
    "display_expr(dphidq)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 29,
   "metadata": {},
   "outputs": [],
   "source": [
    "kq = 3\n",
    "ke = 1"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Task 2"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 31,
   "metadata": {},
   "outputs": [],
   "source": [
    "x, y,z, alpha = sm.symbols(\"x y z alpha\")\n",
    "phi_1 = 0.4*x**2 + 0.8*y**2 -225\n",
    "phi_2 = z+0.25*y**2-3"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 32,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/latex": [
       "$\\displaystyle gradphi_1=\\left[\\begin{matrix}0.8 x\\\\1.6 y\\\\0\\end{matrix}\\right]$"
      ],
      "text/plain": [
       "<IPython.core.display.Math object>"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    },
    {
     "data": {
      "text/latex": [
       "$\\displaystyle gradphi_2=\\left[\\begin{matrix}0\\\\0.5 y\\\\1\\end{matrix}\\right]$"
      ],
      "text/plain": [
       "<IPython.core.display.Math object>"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    }
   ],
   "source": [
    "gradphi_1 = sm.Matrix([[sm.diff(phi_1,x)],[sm.diff(phi_1,y)],[sm.diff(phi_1,z)]])\n",
    "gradphi_2 = sm.Matrix([[sm.diff(phi_2,x)],[sm.diff(phi_2,y)],[sm.diff(phi_2,z)]])\n",
    "display_expr(gradphi_1)\n",
    "display_expr(gradphi_2)"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Task 3 Алгоритм пассификации"
   ]
  },
  {
   "attachments": {},
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Модель\n",
    "\n",
    "$\\dot x = v$\n",
    "\n",
    "$m\\dot v=F_c$\n",
    "\n",
    "$\\dot R(\\alpha) = S(w)R(\\alpha)$\n",
    "\n",
    "$J\\dot w + w\\times J w = M_c$\n",
    "\n",
    "Определяем функцию Запаса:\n",
    "\n",
    "$V=\\frac{1}{2}|\\dot x -v_d|^2+\\frac{1}{2}|\\dot w -w_d|^2$\n",
    "\n",
    "берем производные и затем выбираем закон управления, делающий систему пассивной:\n",
    "\n",
    "$F = u_{pF}+u_F$\n",
    "\n",
    "$M_c=u_{pM}+u_M$\n",
    "\n",
    "$u_{pF}=\\dot v_d$ and $u_{pM}=w\\times Jw+J\\dot w_d$\n",
    "\n",
    "система пассивна по входу:\n",
    "\n",
    "$u=[u_F^T\\,\\,\\,u_M^T]^T$\n",
    "\n",
    "к выходу:\n",
    "\n",
    "$y=[(\\dot x-v_d)^T\\,\\,\\,(w-w_d)^T]^T$\n",
    "\n",
    "закон управления:\n",
    "\n",
    "$u=-K*y$\n",
    "\n",
    "$v_d=u_{\\tau}+u_{\\varphi}$\n",
    "\n",
    "$u_{\\tau}=\\gamma^{-1}\\cdot \\begin{bmatrix} v^* \\\\0 \\\\0 \\end{bmatrix}$\n",
    "\n",
    "$u_{\\varphi}=-K_{\\varphi}(\\varphi_1 \\frac{\\partial \\varphi_1}{\\partial x} + \\varphi_2 \\frac{\\partial \\varphi_2}{\\partial x})$\n",
    "\n",
    "$w_d = w_c+u_w$\n",
    "\n",
    "$u_w = -S(n_c)nk_w(1-n^Tn_c)$\n",
    "\n",
    "$S(n_c) = \\frac{1}{\\sqrt{3}}\\begin{bmatrix} 0&1&-1 \\\\-1&0&1 \\\\1&-1&0 \\end{bmatrix}$"
   ]
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "learn-multibody-dynamics",
   "language": "python",
   "name": "python3"
  },
  "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.8.18"
  },
  "orig_nbformat": 4
 },
 "nbformat": 4,
 "nbformat_minor": 2
}