{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# PATH WAYPOINTS AS PARAMETRIZED CURVE"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 10,
   "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/miniconda3/envs/jupyter/lib/python3.8/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\n",
    "\n",
    "def f(x,coeff):\n",
    "    y=0\n",
    "    j=len(coeff)\n",
    "    for k in range(j):\n",
    "        y += coeff[k]*x**(j-k-1)\n",
    "    return y\n",
    "\n",
    "# def df(x,coeff):\n",
    "#     return round(3*coeff[0]*x**2 + 2*coeff[1]*x**1 + coeff[2]*x**0,6)\n",
    "def df(x,coeff):\n",
    "    y=0\n",
    "    j=len(coeff)\n",
    "    for k in range(j-1):\n",
    "        y += (j-k-1)*coeff[k]*x**(j-k-2)\n",
    "    return y"
   ]
  },
  {
   "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": {
      "image/png": "[encoded data, 22796 characters]\n",
      "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(-1,2,0.001) #interp range of curve \n",
    "\n",
    "# VEHICLE REF FRAME\n",
    "plt.subplot(2,1,1)\n",
    "plt.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",
    "plt.subplot(2,1,2)\n",
    "plt.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')\n",
    "\n",
    "plt.tight_layout()\n",
    "plt.show()\n",
    "#plt.savefig(\"fitted_poly\")"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "def spline_planning(qs, qf, ts, tf, dqs=0.0, dqf=0.0, ddqs=0.0, ddqf=0.0):\n",
    "    \n",
    "    bc = np.array([ys, dys, ddys, yf, dyf, ddyf]).T \n",
    "    \n",
    "    C = np.array([[1,    xs,    xs**2,   xs**3,    xs**4,    xs**5],    #f(xs)=ys\n",
    "                  [0,     1,  2*xs**1, 3*xs**2,  4*xs**3,  5**xs^4],    #df(xs)=dys\n",
    "                  [0,     0,        1, 6*xs**1, 12*xs**2, 20**xs^3],    #ddf(xs)=ddys\n",
    "                  [1,    xf,    xf**2,   xf**3,    xf**4,    xf**5],    #f(xf)=yf\n",
    "                  [0,     1,  2*xf**1, 3*xf**2,  4*xf**3,  5**xf^4],    #df(xf)=dyf\n",
    "                  [0,     0,        1, 6*xf**1, 12*xf**2, 20**xf^3]])   #ddf(xf)=ddyf\n",
    "    \n",
    "    #To compute the polynomial coefficients we solve:\n",
    "    #Ax = B. \n",
    "    #Matrices A and B must have the same number of rows\n",
    "    a = np.linalg.lstsq(C,bc)[0]\n",
    "    return a"
   ]
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python [conda env:.conda-jupyter] *",
   "language": "python",
   "name": "conda-env-.conda-jupyter-py"
  },
  "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.5"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 4
}
