732 lines
94 KiB
Plaintext
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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
|
||
|
|
"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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
|
||
|
|
"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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
|
||
|
|
"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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
|
||
|
|
"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
|
||
|
|
}
|