mpc_python_learn/notebook/equations.ipynb

{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# STATE SPACE MODEL MATRICES"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 1,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/latex": [
       "$\\displaystyle \\left[\\begin{matrix}0 & 0 & - v \\sin{\\left(\\theta \\right)} & 0 & 0\\\\0 & 0 & v \\cos{\\left(\\theta \\right)} & 0 & 0\\\\0 & 0 & 0 & 0 & 0\\\\0 & 0 & 0 & 0 & 0\\\\0 & 0 & 0 & v \\cos{\\left(\\psi \\right)} & 0\\end{matrix}\\right]$"
      ],
      "text/plain": [
       "Matrix([\n",
       "[0, 0, -v*sin(theta),          0, 0],\n",
       "[0, 0,  v*cos(theta),          0, 0],\n",
       "[0, 0,             0,          0, 0],\n",
       "[0, 0,             0,          0, 0],\n",
       "[0, 0,             0, v*cos(psi), 0]])"
      ]
     },
     "execution_count": 1,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "import sympy as sp\n",
    "\n",
    "x,y,theta,psi,cte,v,w = sp.symbols(\"x y theta psi cte v w\")\n",
    "\n",
    "gs = sp.Matrix([[ sp.cos(theta)*v],\n",
    "               [ sp.sin(theta)*v],\n",
    "               [w],\n",
    "               [-w],\n",
    "               [ v*sp.sin(psi)]])\n",
    "\n",
    "state = sp.Matrix([x,y,theta,psi,cte])\n",
    "\n",
    "#A\n",
    "gs.jacobian(state)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/latex": [
       "$\\displaystyle \\left[\\begin{matrix}\\cos{\\left(\\theta \\right)} & 0\\\\\\sin{\\left(\\theta \\right)} & 0\\\\0 & 1\\\\0 & -1\\\\\\sin{\\left(\\psi \\right)} & 0\\end{matrix}\\right]$"
      ],
      "text/plain": [
       "Matrix([\n",
       "[cos(theta),  0],\n",
       "[sin(theta),  0],\n",
       "[         0,  1],\n",
       "[         0, -1],\n",
       "[  sin(psi),  0]])"
      ]
     },
     "execution_count": 2,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "state = sp.Matrix([v,w])\n",
    "\n",
    "#B\n",
    "gs.jacobian(state)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 1,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/latex": [
       "$\\displaystyle \\left[\\begin{matrix}0 & 0 & - v \\sin{\\left(\\theta \\right)}\\\\0 & 0 & v \\cos{\\left(\\theta \\right)}\\\\0 & 0 & 0\\end{matrix}\\right]$"
      ],
      "text/plain": [
       "Matrix([\n",
       "[0, 0, -v*sin(theta)],\n",
       "[0, 0,  v*cos(theta)],\n",
       "[0, 0,             0]])"
      ]
     },
     "execution_count": 1,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "import sympy as sp\n",
    "\n",
    "x,y,theta,psi,cte,v,w = sp.symbols(\"x y theta psi cte v w\")\n",
    "\n",
    "gs = sp.Matrix([[ sp.cos(theta)*v],\n",
    "               [ sp.sin(theta)*v],\n",
    "               [w]])\n",
    "\n",
    "state = sp.Matrix([x,y,theta])\n",
    "\n",
    "#A\n",
    "gs.jacobian(state)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/latex": [
       "$\\displaystyle \\left[\\begin{matrix}\\cos{\\left(\\theta \\right)} & 0\\\\\\sin{\\left(\\theta \\right)} & 0\\\\0 & 1\\end{matrix}\\right]$"
      ],
      "text/plain": [
       "Matrix([\n",
       "[cos(theta), 0],\n",
       "[sin(theta), 0],\n",
       "[         0, 1]])"
      ]
     },
     "execution_count": 2,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "state = sp.Matrix([v,w])\n",
    "\n",
    "#B\n",
    "gs.jacobian(state)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# PATH WAYPOINTS AS PARAMETRIZED CURVE"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "metadata": {},
   "outputs": [],
   "source": [
    "import numpy as np\n",
    "from scipy.interpolate import interp1d\n",
    "\n",
    "def compute_path_from_wp(start_xp, start_yp, step = 0.1):\n",
    "    final_xp=[]\n",
    "    final_yp=[]\n",
    "    delta = step #[m]\n",
    "\n",
    "    for idx in range(len(start_xp)-1):\n",
    "        section_len = np.sum(np.sqrt(np.power(np.diff(start_xp[idx:idx+2]),2)+np.power(np.diff(start_yp[idx:idx+2]),2)))\n",
    "\n",
    "        interp_range = np.linspace(0,1,np.floor(section_len/delta).astype(int))\n",
    "        \n",
    "        fx=interp1d(np.linspace(0,1,2),start_xp[idx:idx+2],kind=1)\n",
    "        fy=interp1d(np.linspace(0,1,2),start_yp[idx:idx+2],kind=1)\n",
    "        \n",
    "        final_xp=np.append(final_xp,fx(interp_range))\n",
    "        final_yp=np.append(final_yp,fy(interp_range))\n",
    "\n",
    "    return np.vstack((final_xp,final_yp))\n",
    "\n",
    "def get_nn_idx(state,path):\n",
    "\n",
    "    dx = state[0]-path[0,:]\n",
    "    dy = state[1]-path[1,:]\n",
    "    dist = np.sqrt(dx**2 + dy**2)\n",
    "    nn_idx = np.argmin(dist)\n",
    "\n",
    "    try:\n",
    "        v = [path[0,nn_idx+1] - path[0,nn_idx],\n",
    "             path[1,nn_idx+1] - path[1,nn_idx]]   \n",
    "        v /= np.linalg.norm(v)\n",
    "\n",
    "        d = [path[0,nn_idx] - state[0],\n",
    "             path[1,nn_idx] - state[1]]\n",
    "\n",
    "        if np.dot(d,v) > 0:\n",
    "            target_idx = nn_idx\n",
    "        else:\n",
    "            target_idx = nn_idx+1\n",
    "\n",
    "    except IndexError as e:\n",
    "        target_idx = nn_idx\n",
    "\n",
    "    return target_idx"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 11,
   "metadata": {},
   "outputs": [
    {
     "name": "stderr",
     "output_type": "stream",
     "text": [
      "/home/marcello/.local/lib/python3.6/site-packages/IPython/core/interactiveshell.py:3331: RankWarning: Polyfit may be poorly conditioned\n",
      "  exec(code_obj, self.user_global_ns, self.user_ns)\n"
     ]
    }
   ],
   "source": [
    "#define track\n",
    "wp=np.array([0,5,6,10,11,15, 0,0,2,2,0,4]).reshape(2,-1)\n",
    "track = compute_path_from_wp(wp[0,:],wp[1,:],step=0.5)\n",
    "\n",
    "#vehicle state\n",
    "state=[3.5,0.5,np.radians(30)]\n",
    "\n",
    "#given vehicle pos find lookahead waypoints\n",
    "nn_idx=get_nn_idx(state,track)-1 #index ox closest wp, take the previous to have a straighter line\n",
    "LOOKAHED=6\n",
    "lk_wp=track[:,nn_idx:nn_idx+LOOKAHED]\n",
    "\n",
    "#trasform lookahead waypoints to vehicle ref frame\n",
    "dx = lk_wp[0,:] - state[0]\n",
    "dy = lk_wp[1,:] - state[1]\n",
    "\n",
    "wp_vehicle_frame = np.vstack(( dx * np.cos(-state[2]) - dy * np.sin(-state[2]),\n",
    "                               dy * np.cos(-state[2]) + dx * np.sin(-state[2]) ))\n",
    "\n",
    "#fit poly\n",
    "coeff=np.polyfit(wp_vehicle_frame[0,:], wp_vehicle_frame[1,:], 5, rcond=None, full=False, w=None, cov=False)\n",
    "\n",
    "#def f(x,coeff):\n",
    "#    return coeff[0]*x**3+coeff[1]*x**2+coeff[2]*x**1+coeff[3]*x**0\n",
    "def f(x,coeff):\n",
    "    return  coeff[0]*x**5+coeff[1]*x**4+coeff[2]*x**3+coeff[3]*x**2+coeff[4]*x**1+coeff[5]*x**0"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 12,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "array([ 0.10275887,  0.03660033, -0.21750601,  0.03551043, -0.53861442,\n",
       "       -0.58083993])"
      ]
     },
     "execution_count": 12,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "coeff"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 13,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "(-0.7501943063424155,\n",
       " 15.750194306342419,\n",
       " -0.3404752506209626,\n",
       " 4.2088127135203335)"
      ]
     },
     "execution_count": 13,
     "metadata": {},
     "output_type": "execute_result"
    },
    {
     "data": {
      "image/png": "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
      "text/plain": [
       "<Figure size 432x288 with 2 Axes>"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    }
   ],
   "source": [
    "import matplotlib.pyplot as plt\n",
    "plt.style.use(\"ggplot\")\n",
    "\n",
    "x=np.arange(0,2,0.001) #interp range of curve \n",
    "\n",
    "# VEHICLE REF FRAME\n",
    "ax1=plt.subplot(2,1,1)\n",
    "ax1.set_title('parametrized curve, vehicle ref frame')\n",
    "plt.scatter(0,0)\n",
    "plt.scatter(wp_vehicle_frame[0,:],wp_vehicle_frame[1,:])\n",
    "plt.plot(x,[f(xs,coeff) for xs in x])\n",
    "plt.axis('equal')\n",
    "\n",
    "# MAP REF FRAME\n",
    "ax2=plt.subplot(2,1,2)\n",
    "ax2.set_title('waypoints, map ref frame')\n",
    "plt.scatter(state[0],state[1])\n",
    "plt.scatter(track[0,:],track[1,:])\n",
    "plt.scatter(track[0,nn_idx:nn_idx+LOOKAHED],track[1,nn_idx:nn_idx+LOOKAHED])\n",
    "plt.axis('equal')"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# COMPUTE ERRORS"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "* **crosstrack error** cte -> desired y-position - y-position of vehicle: this is the value of the fitted polynomial (road curve)\n",
    " \n",
    "$\n",
    "f = K_0 * x^3 + K_1 * x^2 + K_2 * x + K_3\n",
    "$\n",
    "\n",
    "Then for the origin cte = K_3\n",
    "  \n",
    "* **heading error** epsi ->  desired heading - heading of vehicle : is the inclination of  tangent to the  fitted polynomial (road curve)\n",
    "\n",
    "The derivative of the fitted poly has the form\n",
    "\n",
    "$\n",
    "f' = 3.0 * K_0 * x^2 + 2.0 * K_1 * x + K_2\n",
    "$\n",
    "\n",
    "Then for the origin the equation of the tangent in the origin is $y=k2$   \n",
    "\n",
    "epsi = -atan(K_2)\n",
    "\n",
    "in general:\n",
    "\n",
    "$\n",
    "y_{desired} = f(px) \\\\\n",
    "heading_{desired} = -atan(f`(px))\n",
    "$"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 7,
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "3.377314001132912\n",
      "22.461271020363764\n"
     ]
    }
   ],
   "source": [
    "#for 0\n",
    "\n",
    "cte=coeff[3]\n",
    "epsi=-np.arctan(coeff[2])\n",
    "print(cte)\n",
    "print(np.degrees(epsi))"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# ADD DELAY (for real time implementation)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "It is necessary to take *actuation latency* into account: so instead of using the actual state as estimated, the delay factored in using the kinematic model\n",
    "\n",
    "* $x_{delay} = 0.0 + v * dt$\n",
    "* $y_{delay} = 0.0$\n",
    "* $psi_{delay} = 0.0 + w * dt$\n",
    "* $cte_{delay} = cte + v * sin(epsi) * dt$\n",
    "* $epsi_{delay} = epsi - w * dt$\n",
    "\n",
    "Note that the starting position and heading is always 0; this is becouse the path is parametrized to **vehicle reference frame**"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": []
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3",
   "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.6.9"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 4
}