mpc_python_learn/notebooks/MPC_cte_scipy.ipynb

732 lines
94 KiB
Plaintext

{
"cells": [
{
"cell_type": "code",
"execution_count": 1,
"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": 2,
"metadata": {},
"outputs": [],
"source": [
"# Define process model\n",
"def kinematics_model(q,t,u):\n",
" # arguments\n",
" # y = outputs\n",
" # t = time\n",
" # u = input value\n",
" # K = process gain\n",
" # tau = process time constant\n",
" x = q[0]\n",
" y = q[1]\n",
" theta = q[2]\n",
"\n",
" v = u[0]\n",
" w = u[1]\n",
"\n",
" # calculate derivative\n",
" dxdt = v*np.cos(theta)\n",
" dydt = v*np.sin(theta)\n",
" dthetadt = w\n",
"\n",
" dqdt = [dxdt,dydt,dthetadt]\n",
"\n",
" return dqdt"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"simulate"
]
},
{
"cell_type": "code",
"execution_count": 3,
"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.15\n",
"u[1,300:]=-0.15"
]
},
{
"cell_type": "code",
"execution_count": 4,
"metadata": {},
"outputs": [],
"source": [
"# initial conditions\n",
"q0 = [0,0,0]\n",
"\n",
"# store solution\n",
"x = np.empty_like(t)\n",
"y = np.empty_like(t)\n",
"theta = 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",
"\n",
"# solve ODE\n",
"for i in range(1,len(t)):\n",
" # span for next time step\n",
" tspan = [t[i-1],t[i]]\n",
" # solve for next step\n",
" q_t = odeint(kinematics_model,q0,tspan,args=(u[:,i],))\n",
" # store solution for plotting\n",
" x[i] = q_t[1][0]\n",
" y[i] = q_t[1][1]\n",
" theta[i] = q_t[1][2]\n",
" # next initial condition\n",
" q0 = q_t[1]"
]
},
{
"cell_type": "code",
"execution_count": 5,
"metadata": {},
"outputs": [
{
"data": {
"image/png": "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\n",
"text/plain": [
"<Figure size 432x288 with 4 Axes>"
]
},
"metadata": {
"needs_background": "light"
},
"output_type": "display_data"
}
],
"source": [
"\n",
"# plot results\n",
"# plt.plot(t,u,'g:',label='u(t)')\n",
"# plt.plot(t,x,'b-',label='x(t)')\n",
"# plt.plot(t,y,'r--',label='y(t)')\n",
"\n",
"#plot trajectory\n",
"plt.subplot(2, 2, 1)\n",
"plt.plot(x,y)\n",
"plt.ylabel('y')\n",
"plt.xlabel('x')\n",
"\n",
"#plot x(t)\n",
"plt.subplot(2, 2, 2)\n",
"plt.plot(t,x)\n",
"plt.ylabel('x(t)')\n",
"#plt.xlabel('time')\n",
"\n",
"\n",
"#plot y(t)\n",
"plt.subplot(2, 2, 3)\n",
"plt.plot(t,y)\n",
"plt.ylabel('y(t)')\n",
"#plt.xlabel('time')\n",
"\n",
"#plot theta(t)\n",
"plt.subplot(2, 2, 4)\n",
"plt.plot(t,theta)\n",
"plt.ylabel('theta(t)')\n",
"#plt.xlabel('time')\n",
"#plt.legend(loc='best')\n",
"\n",
"plt.tight_layout()\n",
"plt.show()"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"preliminaries:\n",
"* path - > waypoints\n",
"* error computation -> cte"
]
},
{
"cell_type": "code",
"execution_count": 24,
"metadata": {},
"outputs": [],
"source": [
"def calc_target_index(state,path):\n",
" \"\"\"\n",
" Finds the index of the closest waypoint.\n",
"\n",
" :param state: array_like, state of the vehicle [x_pos, y_pos, theta]\n",
" :param path: array_like, reference path ((x1, x2, ...), (y1, y2, ...)]\n",
" :returns: (int,float), nearest_index and cross track error\n",
" \"\"\"\n",
"\n",
" if np.shape(state)[0] is not 3 or np.shape(path)[0] is not 2:\n",
" raise ValueError(\"Input has wrong shape!\")\n",
"\n",
" # Search nearest point index by finding the point closest to the vehicle\n",
" # dx = [state[0] - icx for icx in path[0,:]]\n",
" # dy = [state[1] - icy for icy in path[1,:]]\n",
" # dist = [np.sqrt(idx ** 2 + idy ** 2) for (idx, idy) in zip(dx, dy)]\n",
" dx = state[0]-path[0,:]\n",
" dy = state[1]-path[1,:]\n",
" dist = np.sqrt(dx**2 + dy**2)\n",
" #nn_idx = dist.index(min(dist))\n",
" nn_idx = np.argmin(dist)\n",
"\n",
" try:\n",
"\n",
" # versor v from nearest wp -> next wp\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",
" # vector d from car position -> nearest wp\n",
" d = [path[0,nn_idx] - state[0],\n",
" path[1,nn_idx] - state[1]]\n",
"\n",
" # Get the scalar projection of d on v to compare direction\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",
" front_axle_vect = [np.cos(state[2] - np.pi / 2),\n",
" np.sin(state[2] - np.pi / 2)]\n",
"\n",
" # the cross-track error is given by the scalar projection of the car->wp vector onto the faxle versor\n",
" error_front_axle = np.dot([dx[target_idx], dy[target_idx]], front_axle_vect)\n",
"\n",
" return target_idx, error_front_axle"
]
},
{
"cell_type": "code",
"execution_count": 7,
"metadata": {},
"outputs": [],
"source": [
"def compute_path_from_wp(start_xp, start_yp, step = 0.25):\n",
" '''\n",
" Interpolation range is computed to assure one point every fixed distance step [m].\n",
" \n",
" :param start_xp: array_like, list of starting x coordinates\n",
" :param start_yp: array_like, list of starting y coordinates\n",
" :param step: float, interpolation distance [m] between consecutive waypoints\n",
" :returns: array_like, of shape (2,N)\n",
" '''\n",
"\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,section_len/delta)\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",
" #dx = np.append(0, np.diff(final_xp))\n",
" #dy = np.append(0, np.diff(final_yp))\n",
" #theta = np.arctan2(dy, dx)\n",
"\n",
" return np.vstack((final_xp,final_yp))"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"test path"
]
},
{
"cell_type": "code",
"execution_count": 8,
"metadata": {},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"(2, 91)\n"
]
},
{
"data": {
"image/png": "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\n",
"text/plain": [
"<Figure size 432x288 with 1 Axes>"
]
},
"metadata": {
"needs_background": "light"
},
"output_type": "display_data"
}
],
"source": [
"start_x=[0,5,7.5,8,10,15]\n",
"start_y=[0,0,5,10,10,12]\n",
"path = compute_path_from_wp(start_x,start_y)\n",
"\n",
"print(np.shape(path))\n",
"\n",
"plt.plot(path[0,:],path[1,:])\n",
"plt.show()"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"test cte"
]
},
{
"cell_type": "code",
"execution_count": 26,
"metadata": {},
"outputs": [
{
"data": {
"text/plain": [
"(1, -1.642756358806414)"
]
},
"execution_count": 26,
"metadata": {},
"output_type": "execute_result"
}
],
"source": [
"calc_target_index([0, 2, 0.75],path)"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"mpc"
]
},
{
"cell_type": "code",
"execution_count": 20,
"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",
" \n",
" # Clamp control horizon\n",
" elif k>CTRL_HZN:\n",
" u_hat[k,:] = u_hat[CTRL_HZN,:]\n",
"\n",
" ts_hat = [delta_t_hat*(k),delta_t_hat*(k+1)]\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 = odeint(kinematics_model,q_hat0,ts_hat,args=(u_hat[k,:],))\n",
" \n",
"# print(q_hat)\n",
" \n",
" q_hat0 = q_hat[-1,:]\n",
"\n",
" # Squared Error calculation\n",
" _,cte = calc_target_index(q_hat[-1,:],path)\n",
"# print(cte)\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",
" #delta_u_hat=np.sum(np.subtract([u_hat[2*k],u_hat[2*k+1]],[u_hat[2*(k-1)],u_hat[2*(k-1)+1]]))\n",
" se[k] = 20 * (cte)**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": 32,
"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 = 10 # Control Horizon\n",
"\n",
"delta_t_hat = 0.25 #time step"
]
},
{
"cell_type": "code",
"execution_count": 33,
"metadata": {},
"outputs": [
{
"name": "stderr",
"output_type": "stream",
"text": [
"/home/marcello/.local/lib/python3.5/site-packages/ipykernel_launcher.py:28: RuntimeWarning: invalid value encountered in true_divide\n"
]
},
{
"name": "stdout",
"output_type": "stream",
"text": [
" fun: 36.02837407165175\n",
" jac: array([-2.19157696e+00, -3.22256088e-02, 1.18650579e+00, -4.04749918e+00,\n",
" 1.49658680e-01, 6.93027973e-01, -8.36541653e-01, -2.46588659e+00,\n",
" 1.18037081e+00, 1.26046467e+00, -4.76961613e-01, 0.00000000e+00,\n",
" 0.00000000e+00, 0.00000000e+00, 0.00000000e+00, 0.00000000e+00,\n",
" 0.00000000e+00, 0.00000000e+00, 0.00000000e+00, 0.00000000e+00,\n",
" -2.14655476e+01, -2.45364900e+01, -2.71573195e+01, -3.44248009e+01,\n",
" -3.15746927e+01, -3.04034195e+01, -3.05026488e+01, -2.93093610e+01,\n",
" -2.17690802e+01, -1.74863310e+01, -8.52382469e+00, 0.00000000e+00,\n",
" 0.00000000e+00, 0.00000000e+00, 0.00000000e+00, 0.00000000e+00,\n",
" 0.00000000e+00, 0.00000000e+00, 0.00000000e+00, 0.00000000e+00])\n",
" message: 'Optimization terminated successfully.'\n",
" nfev: 1034\n",
" nit: 22\n",
" njev: 22\n",
" status: 0\n",
" success: True\n",
" x: array([ 1.36094105, 1.44124498, 1.49189215, 1.4944542 , 1.48896953,\n",
" 1.49211915, 1.43846883, 1.36823453, 1.34829146, 1.37191554,\n",
" 0.99609573, 0.99609573, 0.99609573, 0.99609573, 0.99609573,\n",
" 0.99609573, 0.99609573, 0.99609573, 0.99609573, 0.99609573,\n",
" -0.29866483, -0.28173159, -0.24925358, -0.24879537, -0.15900868,\n",
" -0.13969979, -0.13969797, -0.13969797, -0.13969797, -0.23856165,\n",
" 0.02904087, 0.02904087, 0.02904087, 0.02904087, 0.02904087,\n",
" 0.02904087, 0.02904087, 0.02904087, 0.02904087, 0.02904087])\n",
"time elapsed: 3.381047248840332\n"
]
}
],
"source": [
"q=np.array([[0,0.5],[0,0.4],[0,0.2]])\n",
"\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=(q[:,-1],),method='SLSQP',bounds=bnds,options={'maxiter':50})\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": 34,
"metadata": {},
"outputs": [
{
"data": {
"image/png": "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\n",
"text/plain": [
"<Figure size 432x288 with 1 Axes>"
]
},
"metadata": {
"needs_background": "light"
},
"output_type": "display_data"
}
],
"source": [
"u=solution.x.reshape(2,-1).T\n",
"x=[]\n",
"y=[]\n",
"theta=[]\n",
"q0=[0.5,0.4,0.2]\n",
"\n",
"#simulate q for optimized u_hat\n",
"def step(q0,u,delta):\n",
" \"\"\"\n",
" steps the simulated vehicle\n",
" \"\"\"\n",
" \n",
" # span for next time step\n",
" tspan = [delta*0,delta*1]\n",
" # solve for next step\n",
" q_t = odeint(kinematics_model,q0,tspan,args=(u,))\n",
" \n",
" return q_t[1]\n",
"\n",
"for i in range(1,np.shape(u)[0]):\n",
" \n",
" # step simulation of t\n",
" q_t = step(q0,u[i,:],delta_t_hat)\n",
" \n",
" # store solution for plotting\n",
" x.append(q_t[0])\n",
" y.append(q_t[1])\n",
" theta.append(q_t[2])\n",
" \n",
" # next initial condition\n",
" q0 = q_t\n",
" \n",
"plt.plot(x,y)\n",
"plt.plot(path[0,:],path[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": 185,
"metadata": {},
"outputs": [
{
"name": "stderr",
"output_type": "stream",
"text": [
"/home/marcello/.local/lib/python3.5/site-packages/ipykernel_launcher.py:28: RuntimeWarning: invalid value encountered in true_divide\n"
]
}
],
"source": [
"# Initialize state\n",
"# q=[x1, x2, ..., xn\n",
"# y1, y2, ..., yn\n",
"# theta1, theta2, ..., thetan]\n",
"\n",
"q=np.array([0,0.5,0.15]).reshape(3,-1)\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(110): \n",
" \n",
" start = time.time()\n",
" \n",
" #MPC LOOP\n",
" u_hat = minimize(objective,\n",
" u_hat0,\n",
" args=(q[:,-1],),\n",
" method='SLSQP',\n",
" bounds=bnds,\n",
" options={'maxiter':100}\n",
" ).x\n",
" \n",
" end = time.time()\n",
" #print(\"iter:1 time elapsed: {}\".format(end-start))\n",
" \n",
"# print(\"u_hat0 {}\".format(np.shape(u_hat0)))\n",
"# print(\"u_hat {}\".format(np.shape(u_hat)))\n",
"# print(\"bounds {}\".format(np.shape(bnds)))\n",
" \n",
" # \"move\" vehicle\n",
" q_next = odeint(kinematics_model,\n",
" q[:,-1], # q(t)\n",
" [0,delta_t_hat], # \n",
" args=([u_hat[0],u_hat[PRED_HZN]],) # v,w\n",
" )\n",
" \n",
" q = np.hstack((q,q_next[-1,:].reshape(3,-1)))\n",
" \n",
" # next initial condition\n",
" u_hat0=u_hat\n",
"\n",
" "
]
},
{
"cell_type": "code",
"execution_count": 186,
"metadata": {},
"outputs": [
{
"data": {
"image/png": "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\n",
"text/plain": [
"<Figure size 1080x720 with 1 Axes>"
]
},
"metadata": {
"needs_background": "light"
},
"output_type": "display_data"
}
],
"source": [
"plt.figure(figsize=(15,10))\n",
"\n",
"plt.plot(q[0,:],q[1,:])\n",
"plt.plot(path[0,:],path[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
}