617 lines
90 KiB
Plaintext
617 lines
90 KiB
Plaintext
|
{
|
||
|
|
"cells": [
|
||
|
|
{
|
||
|
|
"cell_type": "code",
|
||
|
|
"execution_count": 4,
|
||
|
|
"metadata": {},
|
||
|
|
"outputs": [],
|
||
|
|
"source": [
|
||
|
|
"import numpy as np\n",
|
||
|
|
"import matplotlib.pyplot as plt\n",
|
||
|
|
"plt.style.use(\"ggplot\")\n",
|
||
|
|
"import time\n",
|
||
|
|
"from scipy.interpolate import interp1d\n",
|
||
|
|
"from scipy.integrate import odeint\n",
|
||
|
|
"from scipy.optimize import minimize"
|
||
|
|
]
|
||
|
|
},
|
||
|
|
{
|
||
|
|
"cell_type": "markdown",
|
||
|
|
"metadata": {},
|
||
|
|
"source": [
|
||
|
|
"differential drive kinematics model equations"
|
||
|
|
]
|
||
|
|
},
|
||
|
|
{
|
||
|
|
"cell_type": "code",
|
||
|
|
"execution_count": 5,
|
||
|
|
"metadata": {},
|
||
|
|
"outputs": [],
|
||
|
|
"source": [
|
||
|
|
"#Preliminaries\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\n",
|
||
|
|
"\n",
|
||
|
|
"def road_curve(state,track):\n",
|
||
|
|
" \n",
|
||
|
|
" #given vehicle pos find lookahead waypoints\n",
|
||
|
|
" nn_idx=get_nn_idx(state,track)\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",
|
||
|
|
" return np.polyfit(wp_vehicle_frame[0,:], wp_vehicle_frame[1,:], 3, rcond=None, full=False, w=None, cov=False)\n",
|
||
|
|
"\n",
|
||
|
|
"def f(x,coeff):\n",
|
||
|
|
" return round(coeff[0]*x**3 + coeff[1]*x**2 + coeff[2]*x**1 + coeff[3]*x**0,6)\n",
|
||
|
|
"\n",
|
||
|
|
"def df(x,coeff):\n",
|
||
|
|
" return round(3*coeff[0]*x**2 + 2*coeff[1]*x**1 + coeff[2]*x**0,6)"
|
||
|
|
]
|
||
|
|
},
|
||
|
|
{
|
||
|
|
"cell_type": "code",
|
||
|
|
"execution_count": 58,
|
||
|
|
"metadata": {},
|
||
|
|
"outputs": [],
|
||
|
|
"source": [
|
||
|
|
"from math import cos\n",
|
||
|
|
"# Define process model\n",
|
||
|
|
"def kinematics_model(xt,ut,K,dt=0.1):\n",
|
||
|
|
"\n",
|
||
|
|
" #current state\n",
|
||
|
|
" xp = xt[0]\n",
|
||
|
|
" yp = xt[1]\n",
|
||
|
|
" theta = xt[2]\n",
|
||
|
|
" etheta = xt[4]\n",
|
||
|
|
"\n",
|
||
|
|
" vt = ut[0]\n",
|
||
|
|
" wt = ut[1]\n",
|
||
|
|
"\n",
|
||
|
|
" #next state\n",
|
||
|
|
" xtp = xp + vt*cos(theta)*dt\n",
|
||
|
|
" ytp = yp + vt*np.sin(theta)*dt\n",
|
||
|
|
" thetatp = theta + wt*dt\n",
|
||
|
|
" ctetp = f(xp,K) - yp + vt*np.sin(etheta)*dt\n",
|
||
|
|
" ethetatp = theta - np.arctan(df(xp,K)) + wt*dt\n",
|
||
|
|
" \n",
|
||
|
|
" dqdt = [xtp,ytp,thetatp,ctetp,ethetatp]\n",
|
||
|
|
" return dqdt"
|
||
|
|
]
|
||
|
|
},
|
||
|
|
{
|
||
|
|
"cell_type": "markdown",
|
||
|
|
"metadata": {},
|
||
|
|
"source": [
|
||
|
|
"simulate"
|
||
|
|
]
|
||
|
|
},
|
||
|
|
{
|
||
|
|
"cell_type": "code",
|
||
|
|
"execution_count": 63,
|
||
|
|
"metadata": {},
|
||
|
|
"outputs": [],
|
||
|
|
"source": [
|
||
|
|
"# time points\n",
|
||
|
|
"t = np.arange(0, 40, 0.1)\n",
|
||
|
|
"\n",
|
||
|
|
"#fake inputs\n",
|
||
|
|
"# u = [v(t),\n",
|
||
|
|
"# w(t)]\n",
|
||
|
|
"u = np.zeros((2,len(t)))\n",
|
||
|
|
"u[0,100:]=0.4\n",
|
||
|
|
"u[1,200:]=0.1\n",
|
||
|
|
"u[1,300:]=-0.0"
|
||
|
|
]
|
||
|
|
},
|
||
|
|
{
|
||
|
|
"cell_type": "code",
|
||
|
|
"execution_count": 64,
|
||
|
|
"metadata": {},
|
||
|
|
"outputs": [],
|
||
|
|
"source": [
|
||
|
|
"track = compute_path_from_wp([0,5,10],[0,0,2])\n",
|
||
|
|
"\n",
|
||
|
|
"# initial conditions\n",
|
||
|
|
"q0 = np.array([0,0,0,0,0])\n",
|
||
|
|
"K = road_curve(q0,track)\n",
|
||
|
|
"q0[3] = f(q0[0],K)\n",
|
||
|
|
"q0[4] = - np.arctan(df(q0[0],K))\n",
|
||
|
|
"\n",
|
||
|
|
"# store solution\n",
|
||
|
|
"x = np.empty_like(t)\n",
|
||
|
|
"y = np.empty_like(t)\n",
|
||
|
|
"theta = np.empty_like(t)\n",
|
||
|
|
"cte = np.empty_like(t)\n",
|
||
|
|
"psi = np.empty_like(t)\n",
|
||
|
|
"\n",
|
||
|
|
"# record initial conditions\n",
|
||
|
|
"x[0] = q0[0]\n",
|
||
|
|
"y[0] = q0[1]\n",
|
||
|
|
"theta[0] = q0[2]\n",
|
||
|
|
"cte[0] = q0[3]\n",
|
||
|
|
"psi[0] = q0[4]\n",
|
||
|
|
"\n",
|
||
|
|
"# solve ODE\n",
|
||
|
|
"for i in range(1,len(t)):\n",
|
||
|
|
" # span for next time step\n",
|
||
|
|
" q_t = kinematics_model(q0,u[:,i-1],K,dt=0.1)\n",
|
||
|
|
"\n",
|
||
|
|
" # store solution for plotting\n",
|
||
|
|
" x[i] = q_t[0]\n",
|
||
|
|
" y[i] = q_t[1]\n",
|
||
|
|
" theta[i] = q_t[2]\n",
|
||
|
|
" cte[i] = q_t[3]\n",
|
||
|
|
" psi[i] = q_t[4]\n",
|
||
|
|
" # next initial condition\n",
|
||
|
|
" q0 = q_t"
|
||
|
|
]
|
||
|
|
},
|
||
|
|
{
|
||
|
|
"cell_type": "code",
|
||
|
|
"execution_count": 65,
|
||
|
|
"metadata": {},
|
||
|
|
"outputs": [
|
||
|
|
{
|
||
|
|
"data": {
|
||
|
|
"image/png": "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
|
||
|
|
"text/plain": [
|
||
|
|
"<Figure size 432x288 with 4 Axes>"
|
||
|
|
]
|
||
|
|
},
|
||
|
|
"metadata": {},
|
||
|
|
"output_type": "display_data"
|
||
|
|
}
|
||
|
|
],
|
||
|
|
"source": [
|
||
|
|
"#plot trajectory\n",
|
||
|
|
"plt.subplot(2, 2, 1)\n",
|
||
|
|
"plt.plot(x,y)\n",
|
||
|
|
"plt.scatter(track[0,:],track[1,:])\n",
|
||
|
|
"plt.ylabel('y')\n",
|
||
|
|
"plt.xlabel('x')\n",
|
||
|
|
"\n",
|
||
|
|
"#plot x(t)\n",
|
||
|
|
"plt.subplot(2, 2, 2)\n",
|
||
|
|
"plt.plot(t,theta)\n",
|
||
|
|
"plt.ylabel('theta(t)')\n",
|
||
|
|
"#plt.xlabel('time')\n",
|
||
|
|
"\n",
|
||
|
|
"\n",
|
||
|
|
"#plot y(t)\n",
|
||
|
|
"plt.subplot(2, 2, 3)\n",
|
||
|
|
"plt.plot(t,cte)\n",
|
||
|
|
"plt.ylabel('cte(t)')\n",
|
||
|
|
"#plt.xlabel('time')\n",
|
||
|
|
"\n",
|
||
|
|
"#plot theta(t)\n",
|
||
|
|
"plt.subplot(2, 2, 4)\n",
|
||
|
|
"plt.plot(t,psi)\n",
|
||
|
|
"plt.ylabel('psi(t)')\n",
|
||
|
|
"#plt.xlabel('time')\n",
|
||
|
|
"#plt.legend(loc='best')\n",
|
||
|
|
"\n",
|
||
|
|
"plt.tight_layout()\n",
|
||
|
|
"plt.show()"
|
||
|
|
]
|
||
|
|
},
|
||
|
|
{
|
||
|
|
"cell_type": "markdown",
|
||
|
|
"metadata": {},
|
||
|
|
"source": [
|
||
|
|
"mpc"
|
||
|
|
]
|
||
|
|
},
|
||
|
|
{
|
||
|
|
"cell_type": "code",
|
||
|
|
"execution_count": 75,
|
||
|
|
"metadata": {},
|
||
|
|
"outputs": [],
|
||
|
|
"source": [
|
||
|
|
"# Define Objective function\n",
|
||
|
|
"def objective(u_hat,*args):\n",
|
||
|
|
" \"\"\"\n",
|
||
|
|
" Computes objective function\n",
|
||
|
|
" \n",
|
||
|
|
" :param u_hat: array_like, input [v,w\n",
|
||
|
|
" v,w\n",
|
||
|
|
" ...]\n",
|
||
|
|
" \"\"\"\n",
|
||
|
|
" \n",
|
||
|
|
" #undo input flattening\n",
|
||
|
|
" u_hat = u_hat.reshape(2, -1).T\n",
|
||
|
|
" se = np.zeros(PRED_HZN) #squared_errors\n",
|
||
|
|
" \n",
|
||
|
|
" # Prediction\n",
|
||
|
|
" for k in range(PRED_HZN):\n",
|
||
|
|
" \n",
|
||
|
|
" # Initialize state for prediction\n",
|
||
|
|
" if k==0:\n",
|
||
|
|
" q_hat0 = args[0]\n",
|
||
|
|
" K=args[1] #road curve\n",
|
||
|
|
" \n",
|
||
|
|
" # Clamp control horizon\n",
|
||
|
|
" elif k>CTRL_HZN:\n",
|
||
|
|
" u_hat[k,:] = u_hat[CTRL_HZN,:]\n",
|
||
|
|
" \n",
|
||
|
|
" #DEBUG\n",
|
||
|
|
"# print(\"k : {}\".format(k))\n",
|
||
|
|
"# print(\"q_hat0 : {}\".format(q_hat0))\n",
|
||
|
|
"# print(\"ts : {}\".format(ts_hat))\n",
|
||
|
|
"# print(\"u_hat : {}\".format(u_hat[k,:]))\n",
|
||
|
|
" \n",
|
||
|
|
" q_hat = kinematics_model(q_hat0,u_hat[k,:],K,dt=0.1)\n",
|
||
|
|
" cte=q_hat[3]\n",
|
||
|
|
" psi=q_hat[4]\n",
|
||
|
|
" \n",
|
||
|
|
" q_hat0 = q_hat[:]\n",
|
||
|
|
"\n",
|
||
|
|
" if k >0:\n",
|
||
|
|
" delta_u_hat = np.sum(u_hat[k,:]-u_hat[k-1,:])\n",
|
||
|
|
" else:\n",
|
||
|
|
" delta_u_hat = 0\n",
|
||
|
|
"\n",
|
||
|
|
" se[k] = 100 * (cte)**2 + 15 *(psi)**2 + 15 * (delta_u_hat)**2\n",
|
||
|
|
"\n",
|
||
|
|
" # Sum of Squared Error calculation\n",
|
||
|
|
" obj = np.sum(se[1:])\n",
|
||
|
|
" return obj"
|
||
|
|
]
|
||
|
|
},
|
||
|
|
{
|
||
|
|
"cell_type": "markdown",
|
||
|
|
"metadata": {},
|
||
|
|
"source": [
|
||
|
|
"TODO: add heading error"
|
||
|
|
]
|
||
|
|
},
|
||
|
|
{
|
||
|
|
"cell_type": "markdown",
|
||
|
|
"metadata": {},
|
||
|
|
"source": [
|
||
|
|
"test optimization"
|
||
|
|
]
|
||
|
|
},
|
||
|
|
{
|
||
|
|
"cell_type": "code",
|
||
|
|
"execution_count": 76,
|
||
|
|
"metadata": {},
|
||
|
|
"outputs": [],
|
||
|
|
"source": [
|
||
|
|
"#PARAMS\n",
|
||
|
|
"v=1.5\n",
|
||
|
|
"vb=0.5\n",
|
||
|
|
"wb=0.5\n",
|
||
|
|
"\n",
|
||
|
|
"# Define horizons\n",
|
||
|
|
"PRED_HZN = 20 # Prediction Horizon\n",
|
||
|
|
"CTRL_HZN = 15 # Control Horizon\n",
|
||
|
|
"\n",
|
||
|
|
"delta_t_hat = 0.1 #time step"
|
||
|
|
]
|
||
|
|
},
|
||
|
|
{
|
||
|
|
"cell_type": "code",
|
||
|
|
"execution_count": 77,
|
||
|
|
"metadata": {},
|
||
|
|
"outputs": [
|
||
|
|
{
|
||
|
|
"name": "stdout",
|
||
|
|
"output_type": "stream",
|
||
|
|
"text": [
|
||
|
|
" fun: 84.46152413856359\n",
|
||
|
|
" jac: array([-1.65424156, -5.46248531, -4.66883278, -3.61091423, -7.25656319,\n",
|
||
|
|
" -9.97307396, -7.68648052, -5.46970558, -3.82968616, 0.67185783,\n",
|
||
|
|
" -1.15515804, -1.96808815, 1.28362656, -2.83778381, -0.42825508,\n",
|
||
|
|
" -0.31342316, 0. , 0. , 0. , 0. ,\n",
|
||
|
|
" 38.85329056, 29.67042637, 24.17370987, 19.24147987, 10.30806732,\n",
|
||
|
|
" 3.07283115, 1.61050224, 0.68769073, -0.09304619, 2.58471298,\n",
|
||
|
|
" -0.30147457, -1.62229347, 1.39114475, -2.71041679, -0.22286892,\n",
|
||
|
|
" 1.22416592, 0. , 0. , 0. , 0. ])\n",
|
||
|
|
" message: 'Iteration limit exceeded'\n",
|
||
|
|
" nfev: 5103\n",
|
||
|
|
" nit: 101\n",
|
||
|
|
" njev: 101\n",
|
||
|
|
" status: 9\n",
|
||
|
|
" success: False\n",
|
||
|
|
" x: array([ 1.64242978, 1.69757121, 1.81361202, 1.90182763, 1.90182763,\n",
|
||
|
|
" 1.90182763, 1.90182763, 1.90182763, 1.83511965, 1.84792843,\n",
|
||
|
|
" 1.68088567, 1.43951262, 1.44726228, 1.25067572, 1.25506942,\n",
|
||
|
|
" 1.37310177, 1.37310177, 1.37310177, 1.37310177, 1.37310177,\n",
|
||
|
|
" -0.5 , -0.5 , -0.49058099, -0.47680101, -0.47680101,\n",
|
||
|
|
" -0.47680101, -0.38569852, -0.26192364, -0.07803742, 0.00765328,\n",
|
||
|
|
" 0.14826226, 0.33421738, 0.29494089, 0.39434771, 0.37513876,\n",
|
||
|
|
" 0.25069961, 0.25069961, 0.25069961, 0.25069961, 0.25069961])\n",
|
||
|
|
"time elapsed: 3.1107125282287598\n"
|
||
|
|
]
|
||
|
|
}
|
||
|
|
],
|
||
|
|
"source": [
|
||
|
|
"starting_pos=np.array([0,0.25,0.15])\n",
|
||
|
|
"track = compute_path_from_wp([0,5,10],[0,0,2])\n",
|
||
|
|
"K = road_curve(starting_pos,track)\n",
|
||
|
|
"\n",
|
||
|
|
"# initial conditions\n",
|
||
|
|
"q0 = np.array([0,0,0,0,0])\n",
|
||
|
|
"q0[3] = f(q0[0],K)\n",
|
||
|
|
"q0[4] = - np.arctan(df(q0[0],K))\n",
|
||
|
|
"\n",
|
||
|
|
"#starting guess\n",
|
||
|
|
"u_hat0 = np.zeros((PRED_HZN,2))\n",
|
||
|
|
"u_hat0[:,0]=v\n",
|
||
|
|
"u_hat0[:,1]=0.1\n",
|
||
|
|
"u_hat0=u_hat0.flatten(\"F\")\n",
|
||
|
|
"\n",
|
||
|
|
"bnds=((v-2*vb,v+vb),)\n",
|
||
|
|
"for i in range (PRED_HZN-1):\n",
|
||
|
|
" bnds=bnds+((v-2*vb,v+vb),)\n",
|
||
|
|
" \n",
|
||
|
|
"bnds=bnds+((-wb,wb),)\n",
|
||
|
|
"for i in range (PRED_HZN-1):\n",
|
||
|
|
" bnds=bnds+((-wb,wb),)\n",
|
||
|
|
"\n",
|
||
|
|
"# print(u_hat0)\n",
|
||
|
|
"# print(u_hat0.reshape(2, -1).T)\n",
|
||
|
|
"\n",
|
||
|
|
"start = time.time()\n",
|
||
|
|
"\n",
|
||
|
|
"solution = minimize(objective,u_hat0,args=(q0[:],K,),method='SLSQP',bounds=bnds,options={'maxiter':100})\n",
|
||
|
|
"\n",
|
||
|
|
"end = time.time()\n",
|
||
|
|
"\n",
|
||
|
|
"print(solution)\n",
|
||
|
|
"print(\"time elapsed: {}\".format(end-start))"
|
||
|
|
]
|
||
|
|
},
|
||
|
|
{
|
||
|
|
"cell_type": "markdown",
|
||
|
|
"metadata": {},
|
||
|
|
"source": [
|
||
|
|
"check what the optimizer returns"
|
||
|
|
]
|
||
|
|
},
|
||
|
|
{
|
||
|
|
"cell_type": "code",
|
||
|
|
"execution_count": 78,
|
||
|
|
"metadata": {},
|
||
|
|
"outputs": [
|
||
|
|
{
|
||
|
|
"data": {
|
||
|
|
"image/png": "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
|
||
|
|
"text/plain": [
|
||
|
|
"<Figure size 432x288 with 1 Axes>"
|
||
|
|
]
|
||
|
|
},
|
||
|
|
"metadata": {},
|
||
|
|
"output_type": "display_data"
|
||
|
|
}
|
||
|
|
],
|
||
|
|
"source": [
|
||
|
|
"u=solution.x.reshape(2,-1).T\n",
|
||
|
|
"x=[]\n",
|
||
|
|
"y=[]\n",
|
||
|
|
"theta=[]\n",
|
||
|
|
"x.append(starting_pos[0])\n",
|
||
|
|
"y.append(starting_pos[1])\n",
|
||
|
|
"theta.append(starting_pos[2])\n",
|
||
|
|
"\n",
|
||
|
|
"for i in range(1,np.shape(u)[0]):\n",
|
||
|
|
" \n",
|
||
|
|
" # store solution for plotting\n",
|
||
|
|
" x.append(x[-1]+u[i,0]*np.cos(theta[-1])*0.1)\n",
|
||
|
|
" y.append(y[-1]+u[i,0]*np.sin(theta[-1])*0.1)\n",
|
||
|
|
" theta.append(theta[-1]+u[i,1]*0.1)\n",
|
||
|
|
" \n",
|
||
|
|
"plt.plot(x,y)\n",
|
||
|
|
"plt.plot(track[0,:],track[1,:])\n",
|
||
|
|
"plt.ylabel('y')\n",
|
||
|
|
"plt.xlabel('x')\n",
|
||
|
|
"\n",
|
||
|
|
"plt.tight_layout()\n",
|
||
|
|
"plt.show()"
|
||
|
|
]
|
||
|
|
},
|
||
|
|
{
|
||
|
|
"cell_type": "markdown",
|
||
|
|
"metadata": {},
|
||
|
|
"source": [
|
||
|
|
"compute MPC on the whole path"
|
||
|
|
]
|
||
|
|
},
|
||
|
|
{
|
||
|
|
"cell_type": "code",
|
||
|
|
"execution_count": 83,
|
||
|
|
"metadata": {},
|
||
|
|
"outputs": [
|
||
|
|
{
|
||
|
|
"name": "stdout",
|
||
|
|
"output_type": "stream",
|
||
|
|
"text": [
|
||
|
|
"SCIPY Optimization Time: Avrg: 0.2300s Max: 3.1533s Min: 0.0260s\n"
|
||
|
|
]
|
||
|
|
}
|
||
|
|
],
|
||
|
|
"source": [
|
||
|
|
"curr_pos=np.array([0,0.25,0.15])\n",
|
||
|
|
"start_wpx=[0,5,7.5,8,10,15]\n",
|
||
|
|
"start_wpy=[0,0,5,10,10,12]\n",
|
||
|
|
"track = compute_path_from_wp(start_wpx,start_wpy)\n",
|
||
|
|
"x=[]\n",
|
||
|
|
"y=[]\n",
|
||
|
|
"theta=[]\n",
|
||
|
|
"x.append(curr_pos[0])\n",
|
||
|
|
"y.append(curr_pos[1])\n",
|
||
|
|
"theta.append(curr_pos[2])\n",
|
||
|
|
"\n",
|
||
|
|
"opt_time=[]\n",
|
||
|
|
"\n",
|
||
|
|
"# Initialize input for prediction horizon\n",
|
||
|
|
"# u=[v1,v2,...,vp\n",
|
||
|
|
"# w1,w2,...,wp]\n",
|
||
|
|
"\n",
|
||
|
|
"# Initial guess \n",
|
||
|
|
"# u_hat0=[v1,v2,..vp,w1,w2,...,wp]\n",
|
||
|
|
"# uhat0 -> u_hat=optimize(obj,u_hat0)\n",
|
||
|
|
"\n",
|
||
|
|
"u_hat0 = np.zeros((PRED_HZN,2))\n",
|
||
|
|
"u_hat0[:,0]=v\n",
|
||
|
|
"u_hat0[:,1]=0.01\n",
|
||
|
|
"u_hat0=u_hat0.flatten(\"F\")\n",
|
||
|
|
"\n",
|
||
|
|
"# Optimization Bounds\n",
|
||
|
|
"# bnds=((v1_min,v1_max),...,(vp_min,vp_max),(w1_min,w1_max),...,(wp_min,wp_max))\n",
|
||
|
|
"\n",
|
||
|
|
"bnds=((v-2*vb,v+vb),)\n",
|
||
|
|
"for i in range (1,PRED_HZN):\n",
|
||
|
|
" bnds=bnds+((v-2*vb,v+vb),)\n",
|
||
|
|
" \n",
|
||
|
|
"bnds=bnds+((-wb,wb),)\n",
|
||
|
|
"for i in range (1,PRED_HZN):\n",
|
||
|
|
" bnds=bnds+((-wb,wb),)\n",
|
||
|
|
"\n",
|
||
|
|
"#while np.sum(np.abs(q[-1,0:2]-path[-1,0:2]))>0.1:\n",
|
||
|
|
"for i in range(50): \n",
|
||
|
|
" \n",
|
||
|
|
" start = time.time()\n",
|
||
|
|
" K = road_curve(curr_pos,track)\n",
|
||
|
|
" \n",
|
||
|
|
" # initial conditions for opt.\n",
|
||
|
|
" q0 = np.array([0,0,0,0,0])\n",
|
||
|
|
" q0[3] = f(q0[0],K)\n",
|
||
|
|
" q0[4] = - np.arctan(df(q0[0],K))\n",
|
||
|
|
"\n",
|
||
|
|
" #MPC LOOP\n",
|
||
|
|
" u_hat = minimize(objective,\n",
|
||
|
|
" u_hat0,\n",
|
||
|
|
" args=(q0,K,),\n",
|
||
|
|
" method='SLSQP',\n",
|
||
|
|
" bounds=bnds,\n",
|
||
|
|
" options={'maxiter':100}\n",
|
||
|
|
" ).x\n",
|
||
|
|
" \n",
|
||
|
|
" end = time.time()\n",
|
||
|
|
" opt_time.append(end-start)\n",
|
||
|
|
" \n",
|
||
|
|
" # store solution for plotting\n",
|
||
|
|
" x.append(x[-1]+u[0,0]*np.cos(theta[-1])*0.1)\n",
|
||
|
|
" y.append(y[-1]+u[0,0]*np.sin(theta[-1])*0.1)\n",
|
||
|
|
" theta.append(theta[-1]+u[0,1]*0.1)\n",
|
||
|
|
" \n",
|
||
|
|
" next_pos = [x[-1],y[-1],theta[-1]]\n",
|
||
|
|
" \n",
|
||
|
|
" # next initial condition\n",
|
||
|
|
" curr_pos=next_pos\n",
|
||
|
|
" u_hat0=u_hat\n",
|
||
|
|
"\n",
|
||
|
|
"print(\"SCIPY Optimization Time: Avrg: {:.4f}s Max: {:.4f}s Min: {:.4f}s\".format(np.mean(opt_time),\n",
|
||
|
|
" np.max(opt_time),\n",
|
||
|
|
" np.min(opt_time))) "
|
||
|
|
]
|
||
|
|
},
|
||
|
|
{
|
||
|
|
"cell_type": "code",
|
||
|
|
"execution_count": 84,
|
||
|
|
"metadata": {},
|
||
|
|
"outputs": [
|
||
|
|
{
|
||
|
|
"data": {
|
||
|
|
"image/png": "iVBORw0KGgoAAAANSUhEUgAABDAAAALICAYAAACJhQBYAAAABHNCSVQICAgIfAhkiAAAAAlwSFlzAAALEgAACxIB0t1+/AAAADh0RVh0U29mdHdhcmUAbWF0cGxvdGxpYiB2ZXJzaW9uMy4xLjMsIGh0dHA6Ly9tYXRwbG90bGliLm9yZy+AADFEAAAgAElEQVR4nOzde5TdZ33f+8+ePXdJMyNpRrLuvsqSAWOwjQO+YBPjOAklhBBOk0JKQxo4kMWBnlBMEkyCMREXB+oTaNLQhDbpaZuTAmnTy3JUfCGACWBjE7CwBfZIsmR7RtKMLjOz57L3+UPyYGP5rpnfvrxea7GWNLMlvosHa2+9/Xuep1Sr1WoBAAAAqGNtRQ8AAAAA8HQEDAAAAKDuCRgAAABA3RMwAAAAgLonYAAAAAB1T8AAAAAA6l570QMshr179xY9wrMyODiY0dHRosdgEVjr1mCdW4e1bg3WuXVY69ZgnVuDdW4sa9euPeHXPYEBAAAA1D0BAwAAAKh7AgYAAABQ9wQMAAAAoO4JGAAAAEDdEzAAAACAuidgAAAAAHVPwAAAAADqXvti/pd95jOfyR133JH+/v7ccMMNSZI///M/z7e+9a20t7dn9erVecc73pElS5Y84de+853vTHd3d9ra2lIul7Nt27bFHB0AAAAo0KIGjMsvvzxXX311Pv3pT89/7dxzz80v//Ivp1wu5y/+4i/yhS98IW9605tO+Os/+MEPpq+vb7HGBQAAAOrEom4hOeecc7J06dLHfe3FL35xyuVykmTz5s05cODAYo4EAAAANIBFfQLj6XzpS1/KK17xiif9/vXXX58kefWrX50rr7zySV+3ffv2bN++PUmybdu2DA4OntxBF1h7e3vDzcxzY61bg3VuHda6NVjn1mGtW4N1bg3WuTnUTcD4/Oc/n3K5nEsvvfSE37/uuuuyYsWKjI+P58Mf/nDWrl2bc84554SvvfLKKx8XOEZHRxdk5oUyODjYcDPz3Fjr1mCdW4e1bg3WuXVY69ZgnVuDdW4sa9euPeHX6+IWkltuuSXf+ta38q53vSulUumEr1mxYkWSpL+/PxdeeGF27ty5mCMCAAAABSo8YHz729/OX//1X+d973tfurq6TviaqampTE5Ozv/47rvvzsaNGxdzTAAAAKBAi7qF5FOf+lS+973v5fDhw3n729+eN77xjfnCF76Q2dnZXHfddUmSs846K7/+67+eAwcO5I//+I/z/ve/P+Pj4/nEJz6RJJmbm8sll1yS8847bzFHBwAAAApUqtVqtaKHWGh79+4teoRnxf6s1mGtW4N1bh3WujVY59ZhrVuDdW4N1rmx1PUZGAAAAABPRcAAAAAA6p6AAQAAANQ9AQMAAACoewIGAAAAUPcEDAAAAKDuCRgAAABA3RMwAAAAgLonYAAAAAB1T8AAAAAA6p6AAQAAAE2kVqvl4SPTGZ+aLXqUk6q96AEAAACA525mrpb7D07lnpHJ7BidzD0jkzk4OZu3nr8qr92youjxThoBAwAAABrI4cpcdszHionct38q03O1JMmqJR05d3Vvtgz15KVrlhQ86cklYAAAAECdqtVqefDwdHaMHHuyYsfIZPYcmk6SlEvJ6Su681NnDWTrUE+2DPZkZW9HwRMvHAEDAAAA6sT0XDU79/9oO8iOkckcqswlSZZ2tmXLYE+uOK0/W4Z6ctbK7nS1t87RlgIGAAAAFGRscvYxZ1dM5AcHpjJbPfa9tcs6c+G6pdky1JOtQz1Z19eZtlKp2IELJGAAAADAIqjWatk1Vpk/aHPHyGQeOjKTJOloK+Wsld157ZYV2XJ8O0h/t7+yP5b/NQAAAGABTM5Uc9/+H8WK749O5ujMsccr+rvL2TrUk5/ePJAtg705Y0VXOsqtsx3kuRAwAAAA4CQYOTrzmLMrJnL/wUqqtaSUZGN/Vy7Z1De/HeSUpR0ptfB2kOdCwAAAAIBnaa5aywNjldwzMjH/hMXoxGySpKtcyubBnrzhBSuzdagnmwd7srSzXPDEjU/AAAAAgKdxdHou33/M2RX37p/M1GwtSbKytz1bjz9ZsWWwN6ct70q5zdMVJ5uAAQAAAI9Rq9Xy0JGZ7Bj5UbDYNV5JLUlbKTlteVd+8vT+bBnqzdahngwt6Sh65JYgYAAAANDSZuaq+eHBY9tBHo0WY1NzSZLejracPdiTizcty9ahnpy1sic9HQ7bLIKAAQAAQEs5NDWbe0aPPVmxY2Qy9+2fykz12HaQU5Z25Lw1S7Jl8NiWkA39toPUCwEDAACAplWr1fLAgYl8bedY7jn+dMXew9NJkva25IwV3fmZzQPZOtSbLUM9Wd7jr8n1ysoAAADQNCqz1ezcP3X8OtNjW0IOT1eTJMu6ytky2JMrz+jP1qGenLGiO13ttoM0CgEDAACAhnVgcvZxZ1f88MBU5o7tBsn6vs5ctGFZLjxtKOt75rJuWWdKJdtBGpWAAQAAQEOYq9aya7zyo9tBRifz8JGZJElnuZSzVnbndVtXZOtQb84e6klfVzlJMjg4mNHR0SJH5yQQMAAAAKhLEzNzuXd06liwGJ3MvaOTmZg5th1keXc5W4Z687Obl2fLUE9OX96djrKnK5qZgAEAAEDharVaRo4e3w4yeuwJi+GxSqq1pJRk00BXXnlqX7YMHbsdZNWSDttBWoyAAQAAwKKbrdZy/8GpH20HGZnM/snZJEl3eymbB3vyiy9cma1Dvdm8sjtLOssFT0zRBAwAAAAW3JHK3PyTFTtGJ3Pf6GQqx0/bHOptzwtW9c4/XbFpoCvlNk9X8HgCBgAAACdVrVbLvsMzx4PFRO4Zmczu8ekkSVspOX15d1595kC2DvVky1BPBns7Cp6YRiBgAAAA8LzMzFWz88DU/FaQHSOTGa/MJUmWdLZly2BPLju1L1uHenLWyp50t7cVPDGNSMAAAADgWRmbmp0PFfeMTGbnganMVo9tB1mzrCPnr1uSrUO92TLYk/X9nWlz2CYngYABAADAk6rWatkzPv247SD7Ds8kSdrbSjlzRXdec/by+e0gA93+msnC8P8sAAAA5k3NVnPf/h/dDPL90ckcma4mSfq7ytky1JOrjp9fccaK7nSWbQdhcQgYAAAALWz/xMx8rLhnZDL3H5zK8ctBsqG/M6/YuCxbBnuydag3a5Z1pGQ7CAURMAAAAFrEXLWW4bHKY4LFREYmZpMkneVSNg/25OfPWZmtQz05e7Any7rKBU8MPyJgAAAANKmj03O5d/9U7hmZOL4dZCpTs8e2g6zoac/WoZ783PGzK05b3p32Nk9XUL8EDAAAgCZQq9XyyNHHbwcZHquklqStlGwa6MqrTu+b3w4ytKTddhAaioABAADQgGbmavnhwan5WLFjdDIHJ49tB+lpb8vZg915+YsGs2WoJ5sHu9PbYTsIjU3AAAAAaACHK3Pz51bsGJ3MffunMn38tM1VSzpy7ure+atMN/Z3pWw7CE1GwAAAAKgztVotDx6e/tHTFSOT2XNoOklSLiWnr+jO1WcNZMtQT7YM9mRlb0fBE8PCEzAAAAAKNj1Xzc79U7nnMdtBDlfmkiRLO9uyZbAnV5zWn61DPTlzZXe62tsKnhgWn4ABAACwyMYmZ+dDxT0jE/nBgakcvxwka5d15mXrls5vB1nX15k2h22CgAEAALCQqrVado1VjseKY9tBHjoykyTpaCvlrJXdee2WFdky1JOtgz3p6/bXNDgR/2QAAACcRJMz1dy3f3J+O8i9o5M5OnPs8Yr+7nK2DvXkpzcPZMtgb85Y0ZWOsu0g8EwIGAAAAM/DyNGZ+e0gO0Ymcv/BSqq1pJRkY39XLtnUd+zpiqGenLK0IyXbQeA5ETAAAACeoblqLfcfrGTH6MT8Exb7J2aTJN3tpWxe2ZM3vGBltg71ZPNgT5Z2lgueGJqHgAEAAPAkjkzP5d7HnF1x7/7JTM3WkiQre9uz9fiTFVsGe3Pa8q6U2zxdAQtFwAAAAEhSq9Xy0JGZ+VixY2Qyu8YrqSVpKyWnLe/KT54xkC2Dx6LF0JKOokeGliJgAAA
|
||
|
|
"text/plain": [
|
||
|
|
"<Figure size 1080x720 with 1 Axes>"
|
||
|
|
]
|
||
|
|
},
|
||
|
|
"metadata": {},
|
||
|
|
"output_type": "display_data"
|
||
|
|
}
|
||
|
|
],
|
||
|
|
"source": [
|
||
|
|
"plt.figure(figsize=(15,10))\n",
|
||
|
|
"\n",
|
||
|
|
"plt.plot(x[:],y[:])\n",
|
||
|
|
"plt.plot(track[0,:],track[1,:])\n",
|
||
|
|
"plt.ylabel('y')\n",
|
||
|
|
"plt.xlabel('x')\n",
|
||
|
|
"\n",
|
||
|
|
"plt.tight_layout()\n",
|
||
|
|
"\n",
|
||
|
|
"plt.show()"
|
||
|
|
]
|
||
|
|
},
|
||
|
|
{
|
||
|
|
"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
|
||
|
|
}
|