12345qiupeng
/
mpc_python_learn
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity
mpc_python_learn/notebook/MPC_cte_cvxpy.ipynb

1085 lines
179 KiB
Plaintext
Raw Normal View History

WIP: fixed equations for cte
2020-04-22 22:32:10 +08:00
{
"cells": [
{
"cell_type": "code",
"execution_count": 69,
"metadata": {},
"outputs": [],
"source": [
"import numpy as np\n",
"from scipy.integrate import odeint\n",
"from scipy.interpolate import interp1d\n",
"import cvxpy as cp\n",
"\n",
"import matplotlib.pyplot as plt\n",
"plt.style.use(\"ggplot\")\n",
"\n",
"import time"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"### kinematics model equations\n",
"\n",
"The variables of the model are:\n",
"\n",
"* $x$ coordinate of the robot\n",
"* $y$ coordinate of the robot\n",
"* $\\theta$ heading of the robot\n",
"* $\\psi$ heading error = $\\psi = \\theta_{ref} - \\theta$\n",
"* $cte$ crosstrack error = lateral distance of the robot from the track w.r.t robot frame, cte is a function of the track and x\n",
"\n",
"The inputs of the model are:\n",
"\n",
"* $v$ linear velocity of the robot\n",
"* $w$ angular velocity of the robot\n",
"\n",
"These are the differential equations f(x,u) of the model:\n",
"\n",
"* $\\dot{x} = v\\cos{\\theta}$ \n",
"* $\\dot{y} = v\\sin{\\theta}$\n",
"* $\\dot{\\theta} = w$\n",
"* $\\dot{\\psi} = -w$\n",
"* $\\dot{cte} = v\\sin{\\psi}$\n",
"\n",
"The **Continuous** State Space Model is\n",
"\n",
"$ {\\dot{x}} = Ax + Bu $\n",
"\n",
"with:\n",
"\n",
"$ A =\n",
"\\quad\n",
"\\begin{bmatrix}\n",
"\\frac{\\partial f(x,u)}{\\partial x} & \\frac{\\partial f(x,u)}{\\partial y} & \\frac{\\partial f(x,u)}{\\partial \\theta} & \\frac{\\partial f(x,u)}{\\partial \\psi} & \\frac{\\partial f(x,u)}{\\partial cte} \\\\\n",
"\\end{bmatrix}\n",
"\\quad\n",
"=\n",
"\\quad\n",
"\\begin{bmatrix}\n",
"0 & 0 & -vsin(\\theta) & 0 & 0 \\\\\n",
"0 & 0 & vcos(\\theta) & 0 & 0 \\\\\n",
"0 & 0 & 0 & 0 & 0 \\\\\n",
"0 & 0 & 0 & 0 & 0 \\\\\n",
"0 & 0 & 0 & vcos(\\psi) & 0 \n",
"\\end{bmatrix}\n",
"\\quad $\n",
"\n",
"\n",
"$ B = \\quad\n",
"\\begin{bmatrix}\n",
"\\frac{\\partial f(x,u)}{\\partial v} & \\frac{\\partial f(x,u)}{\\partial w} \\\\\n",
"\\end{bmatrix}\n",
"\\quad\n",
"=\n",
"\\quad\n",
"\\begin{bmatrix}\n",
"\\cos{\\theta_t} & 0 \\\\\n",
"\\sin{\\theta_t} & 0 \\\\\n",
"0 & 1 \\\\\n",
"0 & -1 \\\\\n",
"\\sin{(\\psi_t)} & 0 \n",
"\\end{bmatrix}\n",
"\\quad $\n",
"\n",
"discretize with forward Euler Integration for time step dt:\n",
"\n",
"* ${x_{t+1}} = x_{t} + v_t\\cos{\\theta}*dt$\n",
"* ${y_{t+1}} = y_{t} + v_t\\sin{\\theta}*dt$\n",
"* ${\\theta_{t+1}} = \\theta_{t} + w_t*dt$\n",
"* ${\\psi_{t+1}} = \\psi_{t} - w_t*dt$\n",
"* ${cte_{t+1}} = cte_{t} + v_t\\sin{\\psi}*dt$\n",
"\n",
"The **Discrete** State Space Model is then:\n",
"\n",
"${x_{t+1}} = Ax_t + Bu_t $\n",
"\n",
"with:\n",
"\n",
"$\n",
"A = \\quad\n",
"\\begin{bmatrix}\n",
"1 & 0 & -v\\sin{\\theta}dt & 0 & 0 \\\\\n",
"0 & 1 & v\\cos{\\theta}dt & 0 & 0 \\\\\n",
"0 & 0 & 1 & 0 & 0 \\\\\n",
"0 & 0 & 0 & 1 & 0 \\\\\n",
"0 & 0 & 0 & vcos{(\\psi)}dt & 1 \n",
"\\end{bmatrix}\n",
"\\quad\n",
"$\n",
"\n",
"$\n",
"B = \\quad\n",
"\\begin{bmatrix}\n",
"\\cos{\\theta_t}dt & 0 \\\\\n",
"\\sin{\\theta_t}dt & 0 \\\\\n",
"0 & dt \\\\\n",
"0 & -dt \\\\\n",
"\\sin{\\psi_t}dt & 0 \n",
"\\end{bmatrix}\n",
"\\quad\n",
"$\n",
"\n",
"This State Space Model is **non-linear** (A,B are time changing), to linearize it the **Taylor's series expansion** is used around $\\bar{x}$ and $\\bar{u}$:\n",
"\n",
"$ \\dot{x} = f(x,u) \\approx f(\\bar{x},\\bar{u}) + A(x-\\bar{x}) + B(u-\\bar{u})$\n",
"\n",
"So:\n",
"\n",
"$ x_{t+1} = x_t + (f(\\bar{x},\\bar{u}) + A(x_t-\\bar{x}) + B(u_t-\\bar{u}) )dt $\n",
"\n",
"$ x_{t+1} = (I+dtA)x_t + dtBu_t +dt(f(\\bar{x},\\bar{u}) - A\\bar{x} - B\\bar{u}))$\n",
"\n",
"The Discrete linearized kinematics model is\n",
"\n",
"$ x_{t+1} = A'x_t + B' u_t + C' $\n",
"\n",
"with:\n",
"\n",
"$ A' = I+dtA $\n",
"\n",
"$ B' = dtB $\n",
"\n",
"$ C' = dt(f(\\bar{x},\\bar{u}) - A\\bar{x} - B\\bar{u}) $"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"-----------------\n",
"[About Taylor Series Expansion](https://courses.engr.illinois.edu/ece486/fa2017/documents/lecture_notes/state_space_p2.pdf):\n",
"\n",
"In order to linearize general nonlinear systems, we will use the Taylor Series expansion of functions.\n",
"\n",
"Typically it is possible to assume that the system is operating about some nominal\n",
"state solution $\\bar{x}$ (possibly requires a nominal input $\\bar{u}$) called **equilibrium point**.\n",
"\n",
"Recall that the Taylor Series expansion of f(x) around the\n",
"point $\\bar{x}$ is given by:\n",
"\n",
"$f(x)=f(\\bar{x}) + \\frac{df(x)}{dx}|_{x=\\bar{x}}(x-\\bar{x})$ + higher order terms...\n",
"\n",
"For x sufficiently close to $\\bar{x}$, these higher order terms will be very close to zero, and so we can drop them.\n",
"\n",
"The extension to functions of multiple states and inputs is very similar to the above procedure.Suppose the evolution of state x\n",
"is given by:\n",
"\n",
"$\\dot{x} = f(x1, x2, . . . , xn, u1, u2, . . . , um) = Ax+Bu$\n",
"\n",
"Where:\n",
"\n",
"$ A =\n",
"\\quad\n",
"\\begin{bmatrix}\n",
"\\frac{\\partial f(x,u)}{\\partial x1} & ... & \\frac{\\partial f(x,u)}{\\partial xn} \\\\\n",
"\\end{bmatrix}\n",
"\\quad\n",
"$ and $ B = \\quad\n",
"\\begin{bmatrix}\n",
"\\frac{\\partial f(x,u)}{\\partial u1} & ... & \\frac{\\partial f(x,u)}{\\partial um} \\\\\n",
"\\end{bmatrix}\n",
"\\quad $\n",
"\n",
"Then:\n",
"\n",
"$f(x,u)=f(\\bar{x},\\bar{u}) + \\frac{df(x,u)}{dx}|_{x=\\bar{x}}(x-\\bar{x}) + \\frac{df(x,u)}{du}|_{u=\\bar{u}}(u-\\bar{u}) = f(\\bar{x},\\bar{u}) + A_{x=\\bar{x}}(x-\\bar{x}) + B_{u=\\bar{u}}(u-\\bar{u})$\n",
"\n",
"-----------------"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"### Kinematics Model"
]
},
{
"cell_type": "code",
"execution_count": 70,
"metadata": {},
"outputs": [],
"source": [
"# Control problem statement.\n",
"\n",
"N = 5 #number of state variables\n",
"M = 2 #number of control variables\n",
"T = 20 #Prediction Horizon\n",
"dt = 0.25 #discretization step"
]
},
{
"cell_type": "code",
"execution_count": 71,
"metadata": {},
"outputs": [],
"source": [
"def get_linear_model(x_bar,u_bar):\n",
" \"\"\"\n",
" \"\"\"\n",
" \n",
" x = x_bar[0]\n",
" y = x_bar[1]\n",
" theta = x_bar[2]\n",
" psi = x_bar[3]\n",
" cte = x_bar[4]\n",
" \n",
" v = u_bar[0]\n",
" w = u_bar[1]\n",
" \n",
" A = np.zeros((N,N))\n",
" A[0,2]=-v*np.sin(theta)\n",
" A[1,2]=v*np.cos(theta)\n",
" A[4,3]=v*np.cos(psi)\n",
" A_lin=np.eye(N)+dt*A\n",
" \n",
" B = np.zeros((N,M))\n",
" B[0,0]=np.cos(theta)\n",
" B[1,0]=np.sin(theta)\n",
" B[2,1]=1\n",
" B[3,1]=-1\n",
" B[4,0]=np.sin(psi)\n",
" B_lin=dt*B\n",
" \n",
" f_xu=np.array([v*np.cos(theta),v*np.sin(theta),w,-w,v*np.sin(-psi)]).reshape(N,1)\n",
" C_lin = dt*(f_xu - np.dot(A,x_bar.reshape(N,1)) - np.dot(B,u_bar.reshape(M,1)))\n",
" \n",
" return A_lin,B_lin,C_lin"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"Motion Prediction: using scipy intergration"
]
},
{
"cell_type": "code",
"execution_count": 72,
"metadata": {},
"outputs": [],
"source": [
"# Define process model\n",
"def kinematics_model(x,t,u):\n",
" \"\"\"\n",
" \"\"\"\n",
"\n",
" dxdt = u[0]*np.cos(x[2])\n",
" dydt = u[0]*np.sin(x[2])\n",
" dthetadt = u[1]\n",
" dpsidt = -u[1]\n",
" dctedt = u[0]*np.sin(-x[3])\n",
"\n",
" dqdt = [dxdt,\n",
" dydt,\n",
" dthetadt,\n",
" dpsidt,\n",
" dctedt]\n",
"\n",
" return dqdt\n",
"\n",
"def predict(x0,u):\n",
" \"\"\"\n",
" \"\"\"\n",
" \n",
" x_bar = np.zeros((N,T+1))\n",
" \n",
" x_bar[:,0] = x0\n",
" \n",
" # solve ODE\n",
" for t in range(1,T+1):\n",
"\n",
" tspan = [0,dt]\n",
" x_next = odeint(kinematics_model,\n",
" x0,\n",
" tspan,\n",
" args=(u[:,t-1],))\n",
"\n",
" x0 = x_next[1]\n",
" x_bar[:,t]=x_next[1]\n",
" \n",
" return x_bar"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"Validate the model, here the status w.r.t a straight line with constant heading 0"
]
},
{
"cell_type": "code",
"execution_count": 73,
"metadata": {},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"CPU times: user 7 ms, sys: 0 ns, total: 7 ms\n",
"Wall time: 5.8 ms\n"
]
}
],
"source": [
"%%time\n",
"\n",
"u_bar = np.zeros((M,T))\n",
"u_bar[0,:] = 1 #m/s\n",
"u_bar[1,:] = np.radians(-10) #rad/s\n",
"\n",
"x0 = np.zeros(N)\n",
"x0[0] = 0\n",
"x0[1] = 1\n",
"x0[2] = np.radians(0)\n",
"x0[3] = 0\n",
"x0[4] = 1\n",
"\n",
"x_bar=predict(x0,u_bar)"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"Check the model prediction"
]
},
{
"cell_type": "code",
"execution_count": 74,
"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_bar[0,:],x_bar[1,:])\n",
"plt.plot(np.linspace(0,10,T+1),np.zeros(T+1),\"b-\")\n",
"plt.axis('equal')\n",
"plt.ylabel('y')\n",
"plt.xlabel('x')\n",
"\n",
"plt.subplot(2, 2, 2)\n",
"plt.plot(np.degrees(x_bar[2,:]))\n",
"plt.ylabel('theta(t) [deg]')\n",
"#plt.xlabel('time')\n",
"\n",
"plt.subplot(2, 2, 3)\n",
"plt.plot(np.degrees(x_bar[3,:]))\n",
"plt.ylabel('psi(t) [deg]')\n",
"#plt.xlabel('time')\n",
"\n",
"plt.subplot(2, 2, 4)\n",
"plt.plot(x_bar[4,:])\n",
"plt.ylabel('cte(t)')\n",
"\n",
"plt.tight_layout()\n",
"plt.show()"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"the results seems valid:\n",
"* the cte is correct\n",
"* theta error is correct"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"Motion Prediction: using the state space model"
]
},
{
"cell_type": "code",
"execution_count": 75,
"metadata": {},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"CPU times: user 3.28 ms, sys: 461 µs, total: 3.74 ms\n",
"Wall time: 2.68 ms\n"
]
}
],
"source": [
"%%time\n",
"\n",
"u_bar = np.zeros((M,T))\n",
"u_bar[0,:] = 1 #m/s\n",
"u_bar[1,:] = np.radians(-10) #rad/s\n",
"\n",
"x0 = np.zeros(N)\n",
"x0[0] = 0\n",
"x0[1] = 1\n",
"x0[2] = np.radians(0)\n",
"x0[3] = 0\n",
"x0[4] = 1\n",
"\n",
"x_bar=np.zeros((N,T+1))\n",
"x_bar[:,0]=x0\n",
"\n",
"for t in range (1,T+1):\n",
" xt=x_bar[:,t-1].reshape(5,1)\n",
" ut=u_bar[:,t-1].reshape(2,1)\n",
" \n",
" A,B,C=get_linear_model(xt,ut)\n",
" \n",
" xt_plus_one = np.dot(A,xt)+np.dot(B,ut)+C\n",
" \n",
" x_bar[:,t]= np.squeeze(xt_plus_one)"
]
},
{
"cell_type": "code",
"execution_count": 76,
"metadata": {},
"outputs": [
{
"data": {
"image/png": "iVBORw0KGgoAAAANSUhEUgAAAaIAAAEYCAYAAAAeWvJ8AAAABHNCSVQICAgIfAhkiAAAAAlwSFlzAAALEgAACxIB0t1+/AAAADh0RVh0U29mdHdhcmUAbWF0cGxvdGxpYiB2ZXJzaW9uMy4xLjMsIGh0dHA6Ly9tYXRwbG90bGliLm9yZy+AADFEAAAgAElEQVR4nOzdeVxU5f7A8c9zQEVEdvct13LLDdMsxQXLbCNLzdJy6Za5kLaZlomZaxqJW5vh1i1bjPZF9LqU18IUzfSqqKUmhgIiiChwnt8f8xMlQBBhzszwfb9evS4zc+acr8/l8J15lu+jtNYaIYQQwiKG1QEIIYQo3yQRCSGEsJQkIiGEEJaSRCSEEMJSkoiEEEJYShKREEIIS7lbHcC1On78eIHPBwYGcurUKTtH45ikLS4prC1q165tQTTOQe6xoklbXFKSe8zpE5EQwiYuLo6oqChM06RXr16EhobmeT0rK4uFCxdy6NAhqlatyrhx46hevbpF0QpxiXTNCeECTNNk6dKlTJo0iYiICH766SeOHTuW55j169dTpUoVFixYwJ133sn7779vUbRC5CWJSAgXEB8fT82aNalRowbu7u506dKF2NjYPMds27aN7t27A9C5c2d2795NSQqr6Mxz5Mx9kfM7Y4s+WIhikK45IVxAcnIyAQEBuY8DAgI4cOBAoce4ubnh6elJWloa3t7eeY6LiYkhJiYGgFmzZhEYGJjn9eyEY5xOP8Pp8KfwvGsAXoOfRFWqVBb/LKfh7u6er53Kq5K0hSQiIUQeISEhhISE5D7ON/BcwQM9aS6Vv/mIjK8+IuPX/2KMGI9q0MTOkToOmaxwSUkmK0jXnBAuwN/fn6SkpNzHSUlJ+Pv7F3pMTk4OGRkZVK1atUTXUxUr4f3YeIzxU+FcBubM5zC//gidk1Pyf4QotyQRCeECGjduTEJCAomJiWRnZ7NlyxaCgoLyHNOhQwc2bNgAwNatW2nZsiVKqWu6rmrRDiN8Aap9F3T0KszXJqITC57uLURhJBEJ4QLc3NwYPnw406dPZ/z48dx8883Uq1eP1atXs23bNgB69uxJeno6Y8eO5auvvuLhhx8ulWurKl4Yjz+HeuwZSDiK+co4zE3flWgihCiflLPvRySL7YombXGJLGi9eldzj+nkU5jL5sPendA6COPRsSgfP3uEaSm5xy6RMSIhhKWUfyDGuKmoBx+H/+3CDB+L3v5fq8MSDk4SkRCiVCnDwOh1F8bkCPCvhrlkJmbUfPS5DKtDEw5KEpEQokyoWvUwJs5B3TkA/d//YE4NQ+/fbXVYwgFJIhJClBnlXgEjdDDGhFng5oY590XMT5ahs7KsDk04EElEQogypxrfgDH5DVTX29Hfr8Gc8Qz62GGrwxIOQhKREMIulEdljCGjMMZOhjOnMac/g/n9GrQpi2DLO0lEQgi7Ujd2xAhfAK2D0J8sw5z3EvrU31aHJSwkiUgIYXeqqg/GkxNRQ5+CI4cwp4Zhblkni2DLKUlEQghLKKUwbumFMSUS6jVER83HfHMWOu2M1aEJO5NEJISwlAqsgfHsdNQDQ2FnLGb4GPRv26wOS9iRJCIhhOWU4YZxez+MF+dBVR/MyFcwVy1Gn8+0OjRhBw6zH9GpU6dYtGgRp0+fRilFSEgIffv2tTosIYQdqXoNMV6ch45+H702Gr13J8bw8ajGN1gdmihDDpOI3NzcGDJkCI0aNeLcuXO88MIL3HjjjdStW9fq0IRwaOnp6URERHDy5EmqVavG+PHj8fLyynPMH3/8wTvvvMO5c+cwDIN+/frRpUsXiyK+MlWhIqr/MPSNHTHfi8Cc/QLqzv6oOwei3B3mT5YoRQ7TNefn50ejRo0AqFy5MnXq1CE5OdniqIRwfNHR0bRu3ZrIyEhat25NdHR0vmMqVqzImDFjeP3115k0aRLLli3j7NmzFkRbfOr6VhhTIlGdu6O/Wo0563l0wjGrwxJlwCE/XiQmJnL48GGaNMm/9XBMTAwxMTEAzJo1q9C90WUP+UukLS5xxbaIjY0lPDwcgODgYMLDwxk8eHCeYy4vwe/v74+Pjw9nzpyhSpUq9gz1qinPKqjh49BtbsJctQhz2jjUA0NR3fuiDIf5HC2ukcMloszMTObNm8fQoUPx9PTM93pISAghISG5jwvbA0T2B7lE2uISV9yPKDU1FT8/254/vr6+pKamXvH4+Ph4srOzqVGjRoGvO+SHvdvvIadjF84snsmFD96mwp4deI99CbeAava5fhFc8QNOSZWkLRwqEWVnZzNv3jy6du1Kp06drA5HCIcxbdo0Tp8+ne/5Bx98MM9jpdQVt/9OSUlhwYIFjB49GqOQbxSO/GFPP/ECatP3XPhoKafCHkYNfhKjY1e7xlAQ+bB3SUk+7DlMItJa8+abb1KnTh3uuusuq8MRwqFMnjy50Nd8fHxISUnBz8+PlJQUvL29CzwuIyODWbNmMWjQIJo1a1ZWoZYppRQquA/6hhsxl76Ofvs1zLhfUA89gariVfQJhENymE7Wffv2sWnTJnbv3s1zzz3Hc889x/bt260OSwiHFxQUxMaNGwHYuHEjHTt2zHdMdnY2c+fOpVu3bnTu3NneIZY6VaM2xoTZqHsfQv/6o22vo707rQ5LlJDDfCO64YYb+Oijj6wOQwinExoaSkREBOvXr8+dvg1w8OBB1q5dy8iRI9myZQt79+4lLS2NDRs2ADB69Giuu+466wK/RsrNDXXXg+hWHTCXvo75+mRUyD2o+4agKlayOjxxFZR28iqDx48fL/B56bO9RNriElecrFDWnOEe0+fPo9csR6//CmrVw3jsaVT9xna7viO1hdWceoxICFd05swZNm3axPbt2/nzzz/JyMjA09OTBg0a0LZtW7p3717omI4oPlWpEmrQ47ZFsMvmY854FnX3INQd96MMN6vDE0WQRCREGXn//ff58ccfadeuHT179qROnTpUrlyZc+fO8ddff7Fnzx4mTJjArbfeysMPP2x1uC5BtWyHEb4AvWoJOnoV+rdtthJB1WtZHZq4AklEQpSRgIAAIiMjqVChQr7XGjZsyK233sqFCxdYv369BdG5LlWlKjz+HLTthH7/TcxXnkINGIHqetsVp7YL6zjMrDkhXE2fPn1yk1BBa4DANqW6T58+9gyrXFBKYXQKxgiPhEbXo1cuwlz4KvpMitWhiQJIIhLCDp566qkCn784w02UDeVfDWPcVNTAx2DvTswpY9E7tlodlvgHl+ya0//bxQVvb/TFUieGG1SqBJU8oFJlqOqNqlDR2iBFuVLQ5NSMjIxCqxuI0qMMAxVyD7pFW9s078UzULeEoAY+hqqcv4yYsD+XTETmG1NIycm58kGVPaGqL/gHoqrVhMAaqBq1oXYDqF4L5SYzbcS1e/LJJwG4cOFC7s8Xpaenc8stt1gRVrmkatfHmPga+ssP0d9+iv7fLttEhmYtrQ6t3HPJRGQ8/So+Pt6Xij/m5MD5TNtuj+fPQdoZSEuF1BR08kl03M+QlkruZ1b3ClCnAaphM2h0Parx9VCtlgx0iqs2duxYtNbMnDmTsWPH5nnN19dX1i/ZmXKvgLpvCLp1kG2vo7mTULf3Q93zEKqASSXCPmRB6//T5zPhxF/ov/6Av/5EHzkEhw/YEheAXyCqeRto3gbVsh2qqk8pRG8fstjuEqsWtJ4/f55KlZxztb8zLGgtCZ15Dv3RUvTmH6BuQ9si2DoNSnQuZ2+L0iQLWq+BquQBDRqjGlxaja3NHDh+FB2/F/2/nbZvTlvWoZUBTZuj2nVGteuCcpBS9MKxfPPNN/Tu3ZsKFSoUmoSysrJYu3Ytffv2tXN0QnlURj0yBt2mE+bySMxXx9vKA4XcK3sd2ZkkoitQhhvUvQ5V9zrofoctMf15CL3rF/SOrejVS9Grl0KzVqjO3VEdbkF5OvZGY8J+Tp8+TVhYGO3ataNFixbUrl0bDw8PMjMzOX78OHv27GHHjh0EBwdbHWq5ptp0xJi6EHPFQvTHUehd2zCGPYUKqG51aOWGdM1dA514HP3LZvT
"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_bar[0,:],x_bar[1,:])\n",
"plt.plot(np.linspace(0,10,T+1),np.zeros(T+1),\"b-\")\n",
"plt.axis('equal')\n",
"plt.ylabel('y')\n",
"plt.xlabel('x')\n",
"\n",
"plt.subplot(2, 2, 2)\n",
"plt.plot(x_bar[2,:])\n",
"plt.ylabel('theta(t)')\n",
"#plt.xlabel('time')\n",
"\n",
"plt.subplot(2, 2, 3)\n",
"plt.plot(x_bar[3,:])\n",
"plt.ylabel('psi(t)')\n",
"#plt.xlabel('time')\n",
"\n",
"plt.subplot(2, 2, 4)\n",
"plt.plot(x_bar[4,:])\n",
"plt.ylabel('cte(t)')\n",
"\n",
"plt.tight_layout()\n",
"plt.show()"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"The results are the same as expected"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"------------------\n",
"\n",
"the kinematics model predictits psi and cte for constant heading references. So, for non-constant paths appropriate functions have to be developed.\n",
"\n",
"-----------------"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"## PRELIMINARIES"
]
},
{
"cell_type": "code",
"execution_count": 77,
"metadata": {},
"outputs": [],
"source": [
"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": "markdown",
"metadata": {},
"source": [
"test it"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"### MPC Problem formulation\n",
"\n",
"**Model Predictive Control** refers to the control approach of **numerically** solving a optimization problem at each time step. \n",
"\n",
"The controller generates a control signal over a fixed lenght T (Horizon) at each time step."
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"![mpc](img/mpc_block_diagram.png)\n",
"\n",
"![mpc](img/mpc_t.png)"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"#### Linear MPC Formulation\n",
"\n",
"Linear MPC makes use of the **LTI** (Linear time invariant) discrete state space model, wich represents a motion model used for Prediction.\n",
"\n",
"$x_{t+1} = Ax_t + Bu_t$\n",
"\n",
"The LTI formulation means that **future states** are linearly related to the current state and actuator signal. Hence, the MPC seeks to find a **control policy** U over a finite lenght horizon.\n",
"\n",
"$U={u_{t|t}, u_{t+1|t}, ...,u_{t+T|t}}$\n",
"\n",
"The objective function used minimize (drive the state to 0) is:\n",
"\n",
"$\n",
"\\begin{equation}\n",
"\\begin{aligned}\n",
"\\min_{} \\quad & \\sum^{t+T-1}_{j=t} x^T_{j|t}Qx_{j|t} + u^T_{j|t}Ru_{j|t}\\\\\n",
"\\textrm{s.t.} \\quad & x(0) = x0\\\\\n",
" & x_{j+1|t} = Ax_{j|t}+Bu_{j|t}) \\quad \\textrm{for} \\quad t<j<t+T-1 \\\\\n",
"\\end{aligned}\n",
"\\end{equation}\n",
"$\n",
"\n",
"Other linear constrains may be applied,for instance on the control variable:\n",
"\n",
"$ U_{MIN} < u_{j|t} < U_{MAX} \\quad \\textrm{for} \\quad t<j<t+T-1 $\n",
"\n",
"The objective fuction accounts for quadratic error on deviation from 0 of the state and the control inputs sequences. Q and R are the **weight matrices** and are used to tune the response.\n",
"\n",
"Because the goal is tracking a **reference signal** such as a trajectory, the objective function is rewritten as:\n",
"\n",
"$\n",
"\\begin{equation}\n",
"\\begin{aligned}\n",
"\\min_{} \\quad & \\sum^{t+T-1}_{j=t} \\delta x^T_{j|t}Q\\delta x_{j|t} + u^T_{j|t}Ru_{j|t}\n",
"\\end{aligned}\n",
"\\end{equation}\n",
"$\n",
"\n",
"where the error w.r.t desired state is accounted for:\n",
"\n",
"$ \\delta x = x_{j,t,ref} - x_{j,t} $\n",
"\n",
"#### Non-Linear MPC Formulation\n",
"\n",
"In general cases, the objective function is any non-differentiable non-linear function of states and inputs over a finite horizon T. In this case the constrains include nonlinear dynamics of motion.\n",
"\n",
"$\n",
"\\begin{equation}\n",
"\\begin{aligned}\n",
"\\min_{} \\quad & \\sum^{t+T}_{j=t} C(x_{j|t},{j|t})\\\\\n",
"\\textrm{s.t.} \\quad & x(0) = x0\\\\\n",
" & x_{j+1|t} = f(x_{j|t},u_{j|t}) \\quad \\textrm{for} \\quad t<j<t+T-1 \\\\\n",
"\\end{aligned}\n",
"\\end{equation}\n",
"$\n",
"\n",
"Other nonlinear constrains may be applied:\n",
"\n",
"$ g(x_{j|t},{j|t})<0 \\quad \\textrm{for} \\quad t<j<t+T-1 $"
]
},
{
"cell_type": "code",
"execution_count": 106,
"metadata": {},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"[ 0. 0. 0. -0.17453294 -1.015427 ]\n"
]
}
],
"source": [
"track = compute_path_from_wp([0,5,10],[0,0,2])\n",
"\n",
"# Constrains\n",
"MAX_SPEED = 2.5\n",
"MIN_SPEED = 0.5\n",
"MAX_STEER_SPEED = 1.57/2\n",
"\n",
"# Starting Condition\n",
"x0 = np.zeros(N)\n",
"x0[0] = 0\n",
"x0[1] = 1\n",
"x0[2] = np.radians(-10)\n",
"\n",
"#X0 is needed ONLY to initialize road curve\n",
"K=road_curve(x0,track)\n",
"\n",
"#starting guess\n",
"u_bar = np.zeros((M,T))\n",
"u_bar[0,:]=0.5*(MAX_SPEED+MIN_SPEED)\n",
"u_bar[1,:]=0.0\n",
"\n",
"# Linearized Model for Prediction##############\n",
"x_bar=np.zeros((N,T+1))\n",
"\n",
"#starting state is 0,0,0,psi,cte\n",
"x_bar[0,0]=0\n",
"x_bar[1,0]=0\n",
"x_bar[2,0]=0\n",
"# x_bar[3,0]=x_bar[2,0]-np.arctan2(df(x_bar[0,0],K),x_bar[0,0])\n",
"x_bar[3,0]=x_bar[2,0]-np.arctan(df(x_bar[0,0],K))\n",
"x_bar[4,0]=f(x_bar[0,0],K)-x_bar[1,0]\n",
"\n",
"print(x_bar[:,0])\n",
"\n",
"for t in range (1,T+1):\n",
" xt=x_bar[:,t-1].reshape(5,1)\n",
" ut=u_bar[:,t-1].reshape(2,1)\n",
" \n",
" A,B,C=get_linear_model(xt,ut)\n",
" \n",
" xt_plus_one = np.squeeze(np.dot(A,xt)+np.dot(B,ut)+C)\n",
"# xt_plus_one[3]=xt_plus_one[2]-np.arctan2(df(xt_plus_one[0],K),xt_plus_one[0])\n",
" xt_plus_one[3]=xt_plus_one[2]-np.arctan(df(xt_plus_one[0],K))\n",
" xt_plus_one[4]=f(xt_plus_one[0],K)-xt_plus_one[1]\n",
" \n",
" x_bar[:,t]= xt_plus_one\n",
"#################################################\n"
]
},
{
"cell_type": "code",
"execution_count": 107,
"metadata": {},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"-----------------------------------------------------------------\n",
" OSQP v0.6.0 - Operator Splitting QP Solver\n",
" (c) Bartolomeo Stellato, Goran Banjac\n",
" University of Oxford - Stanford University 2019\n",
"-----------------------------------------------------------------\n",
"problem: variables n = 283, constraints m = 323\n",
" nnz(P) + nnz(A) = 857\n",
"settings: linear system solver = qdldl,\n",
" eps_abs = 1.0e-05, eps_rel = 1.0e-05,\n",
" eps_prim_inf = 1.0e-04, eps_dual_inf = 1.0e-04,\n",
" rho = 1.00e-01 (adaptive),\n",
" sigma = 1.00e-06, alpha = 1.60, max_iter = 10000\n",
" check_termination: on (interval 25),\n",
" scaling: on, scaled_termination: off\n",
" warm start: on, polish: on, time_limit: off\n",
"\n",
"iter objective pri res dua res rho time\n",
" 1 0.0000e+00 1.02e+00 1.21e+02 1.00e-01 3.61e-04s\n",
" 125 1.4749e+03 9.83e-06 4.40e-03 7.08e+01 1.50e-03s\n",
"\n",
"status: solved\n",
"solution polish: unsuccessful\n",
"number of iterations: 125\n",
"optimal objective: 1474.8916\n",
"run time: 1.98e-03s\n",
"optimal rho estimate: 1.51e+01\n",
"\n",
"CPU times: user 230 ms, sys: 4.14 ms, total: 234 ms\n",
"Wall time: 230 ms\n"
]
}
],
"source": [
"%%time\n",
"\n",
"x = cp.Variable((N, T+1))\n",
"u = cp.Variable((M, T))\n",
"\n",
"#CVXPY Linear MPC problem statement\n",
"cost = 0\n",
"constr = []\n",
"\n",
"for t in range(T):\n",
" \n",
" # Cost function\n",
" cost += 10*cp.sum_squares( x[3, t]) # psi\n",
" cost += 500*cp.sum_squares( x[4, t]) # cte\n",
" \n",
" # Actuation effort\n",
" cost += cp.quad_form( u[:, t],1*np.eye(M))\n",
" \n",
" # Actuation rate of change\n",
" if t < (T - 1):\n",
" cost += cp.quad_form(u[:, t + 1] - u[:, t], 100*np.eye(M))\n",
" \n",
" # KINEMATICS constrains\n",
" A,B,C=get_linear_model(x_bar[:,t],u_bar[:,t])\n",
" constr += [x[:,t+1] == A*x[:,t] + B*u[:,t] + C.flatten()]\n",
" \n",
"# sums problem objectives and concatenates constraints.\n",
"constr += [x[:,0] == x_bar[:,0]] #<--watch out the start condition\n",
"constr += [u[0, :] <= MAX_SPEED]\n",
"constr += [u[0, :] >= MIN_SPEED]\n",
"constr += [cp.abs(u[1, :]) <= MAX_STEER_SPEED]\n",
"\n",
"prob = cp.Problem(cp.Minimize(cost), constr)\n",
"solution = prob.solve(solver=cp.OSQP, verbose=True)"
]
},
{
"cell_type": "code",
"execution_count": 108,
"metadata": {},
"outputs": [
{
"data": {
"image/png": "iVBORw0KGgoAAAANSUhEUgAAAagAAAEYCAYAAAAJeGK1AAAABHNCSVQICAgIfAhkiAAAAAlwSFlzAAALEgAACxIB0t1+/AAAADh0RVh0U29mdHdhcmUAbWF0cGxvdGxpYiB2ZXJzaW9uMy4xLjMsIGh0dHA6Ly9tYXRwbG90bGliLm9yZy+AADFEAAAgAElEQVR4nOzdeXhU5dnH8e8zmQQICdkhkLAGwr6FsO8SlCpVpO4rSutrQVGxttqKaCmVVhaLqLWKQrEL1QrWXSKyCCJhFQKShEW2QMhCSEhClvO8f4yJCVlJZuacSe7PdXmRmXNmzi/jTO45z3kWpbXWCCGEEBZjMzuAEEIIURUpUEIIISxJCpQQQghLkgIlhBDCkqRACSGEsCQpUEIIISzJbnYAVzt9+nSF26GhoaSnp5uUpiIrZQHJU5vL87Rr187ENO53+WeplNX/P5nNannAepmq+yzJGZQQQghLkgIlhBDCkhp9E58QQtREp59FH9gNyQdQYyehuvYyO5L4gUcWKMMwePLJJwkODubJJ580O44QwoPoSwVwaB86cTc6cTecPeXY4GVH792O7VfzUR2izA0pAA8tUB9//DERERHk5+ebHUUI4SH0+QyMFUvh0D4oLgYfH4jugxo3CdU7Bpo1x/jTkxgvPovtN39CtWlanWCsyOOuQWVkZLBr1y4mTJhgdhQhhAfRH78Dh/ahrvoptsd+j+3Ff+L1yLPY4m5AtW2PCg7D9tjvATCWPIPOtE4vt6bK4wrUihUruOuuu1BKmR1FCOEhdO4F9JZ41NCx2G6+D9VrAMrbp9J+KjwC2yPPwsUcjBfnonMvuD+sKONRTXw7d+4kICCALl26kJiYWOU+8fHxxMfHA7BgwQJCQ0MrbLfb7ZXuM4uVsoDkqY3V8oi60xs/hcJC1MQpte6rOkZhe2gOxotzMZb+HtvseajmLdyQUlzOowrUoUOH2LFjB7t376awsJD8/HyWLl3KrFmzyvaJi4sjLi6u7Pblg9GsNEDNSllA8tSmqQ/U9VS6qAj95UfQeyAqomOdHqO698H2f7/GePV5jFefx/bQHJS3t4uTist5VIG64447uOOOOwBITEzkgw8+qFCchBDicnr7RsjOwnb/Y1f0ODVgKOreh9Fv/QW9fDE88CuUzctFKUVVPO4alBBC1JXWGv35WojsBD37X/HjbSMmoG6Zjt65Bf3uCqfnEzXzqDOo8nr37k3v3r3NjiGE5ezZs4e33noLwzCYMGECU6ZUvO6yYsWKsmu4hYWFZGdns2LFChOSukHiLjh9HHXfo/XuWGWbeAPG2VPo+A/Qw69Cte/s5JCiOh5boIQQlRmGwfLly3n66acJCQnhqaeeIjY2lsjIyLJ9pk2bVvbzJ598wtGjR01I6h7G52shMBg1ZHSDnkfdeA965xaMf7/uGMgrvYjdQpr4hGhEUlJSCA8Pp02bNtjtdkaMGEFCQkK1+2/ZsoVRo0a5MaH76BNH4eBe1FU/Rdkb1sFBtfRD3Xg3JO1H79jipISiNnIGJUQjkpmZSUhISNntkJAQkpOTq9z33LlzpKWl0adPnyq31zZko5TVut+X5sn+xytcat6C0Btvx+bXqsHPq2+4ncwt8RjvrSRk/DV17nputdcHrJmpKlKghGiitmzZwrBhw7DZqm5IqW3IRikrDgc4l3wIY/M61LhrySwohALn5NM334/xpyc594+/YbvhzjrnsdLrA9bLJOtBCdEEBAcHk5GRUXY7IyOD4ODgKvfdunUrI0eOdFc0t9LrPwRDoyb81KnPq7r2Qg0Zi/70PfS5M059blGZFCghGpGoqChSU1NJS0ujuLiYrVu3EhsbW2m/U6dOcfHiRaKjo01I6VpG/kX0xk9RMcNRYeFOf3510zTw8sJ4502nP7eoSJr4hGhEvLy8uP/++5k/fz6GYTB+/Hjat2/P6tWriYqKKitWW7ZsYcSIEY2yN1pB/IeQfxF1de3TGtWHCgpBXXszes0q9IHdqF4DXXIcIQVKiEYnJiaGmJiYCvfdeuutFW7fcsst7ozkNrqkhIsf/ge69kJ16e6y46iJN6C/Wofx7zewPfMXlF3+lLqCNPEJIRqP3V9jpKVic9HZUynl7YPt1p9D6gn0ho9ceqymTAqUEKLR0Du2YAsJg/6DXX+wfoOhTwz6f/9CXzjv+uM1QVKghBCNgtYanbQfnz4xbpnUVSnlOIsqvIRes8rlx2uKpEAJIRqHs6cgJxvvXgPcdkgVHoma8FP0lnj09yluO25TIQVKCNEo6CTHBLg+vd3bq05ddyv4tcL49+tord167MZOCpQQonFI2g+tAvFq196th1W+LR3z9KUcRCdsduuxGzspUEIIj+e4/pSI6tbblLFdauQE6BCFfncF+tIltx+/sZICJYTwfOlnISsdulc98a2rKZuXo8NEVjr6s/+akqExkgIlhPB4Otlx/Ul1M28RUxXdGzV4tGOevoxzpuVoTKRACSE8X1IitPSHdh1MjaF+Ng0A/d8VpuZoLKRACSE8nk7aD916oapZOsRdVEgYatJUdMLmsl6Fov6kQAkhPJrOyoBzZ0xt3itPXfMzCArFWP062igxO45H86gClZ6eznPPPcdjjz3G7Nmz+fjjj82OJJqQefNcPzuBuHI6aT8AyqQOEpdTzZo5luQ4fgS95Quz43g0jypQXl5e3H333SxZsoT58+fz2WefcfLkSbNjiUZs0SJ/Fi3yB+APf5ACZUnJidC8BUR2NjtJGTV4NHTtiV6zCuNirtlxPJZHFaigoCC6dOkCQIsWLYiIiCAzM9PkVKKxKS1Iixb5s3ix4z9hXTopEbr2RHlZ5wuEUgrbbb+A3AtcfOcts+N4LI9dxCQtLY2jR4/StWvXCvfHx8cTHx8PwIIFCwgNDa2w3W63V7rPLFbKApIHHM14ixd78fzzzVi82Kfs/oiIdhX+ffrpEubNc2s0UQWdkw2pJ1DDx5sdpRLVsStqZBx5H/4H24DhqMhOZkfyOB5ZoAoKCli0aBHTpk3D19e3wra4uDji4uLKbqenp1fYHhoaWuk+s1gpCzTdPIsW+fP44zllZ0wAzZr5VLnv7Nk5PP54zg+32rk8m6iFBcY/1URNvQe1fyfG6wux/W4RyqeZ2ZE8ikc18QEUFxezaNEiRo8ezdChQ82OIzzU5c145YtTVWbPdhSlH4uTsAKdlAg+PtCpa+07m0D5B9Bq1tNw+jj6XWnqu1IedQalteavf/0rERERTJ482ew4wgOVFqbFix1nTaVFqbbrTI8/nlPpbF2YTyfthy49UHZvs6NUq9nAYY4l4te9j+41EDVAvljXlUcVqEOHDrFp0yY6dOjAE088AcDtt99OTEyMycmElZUWpfIFCX68nlSV2bMd+5aeOQHMmVOChVpAmzx9MRdOHkNNvs3sKLVSN96DPrQPY+VSbJ2WogJDzI7kETyqQPXo0YP//Oc/ZscQHqCq60p1Vf46kzTpWVjKQdDaMuOfaqK8vbH94lcY8x7DePNFbI8+Z/qsF55AXiHR6JQvSuWLU02F6tSp04AUJ0+ik/eD3Q6do82OUicqPBJ12y/g4F7052vMjuMRpECJRqH8gNrSQlRTEx5ULEql/1qlKBUUFJCRkUFBQYHZUSxLJyVCp2iP6hmnRk2EmBHotW+jjyWbHcfyPKqJT4jyqmrGq60nXun2qoqS2cXp+PHjxMfHs2vXLs6d+3G5htatWzNgwAAmTpxIhw7mztZtFbogH75PQU36mdlRrohSCts9D2H8PsnR9XzOElRz6XxTHSlQwuNU1xOvNuULkFWKUqkXX3yRkydPMmLECB5++GEiIiJo0aIF+fn5nDp1igMHDrB06VIiIyN59NFHzY5rvsPfgWFYdvxTTVRLP2zTZ2MsfBr9r9dR9z1idiTLkgIlPEJ9euKdOnWaiIh2FXriWaUgXW706NEMGjSo0v1+fn50796d7t27c+ONN7Jz504T0lmPTkoEmw269jA7Sr2o6D6o625Gf7gao1NXbOOvMzuSJUmBEpZW2oxX17Oky7uHW+m6Uk3KF6fk5GS
"text/plain": [
"<Figure size 432x288 with 4 Axes>"
]
},
"metadata": {},
"output_type": "display_data"
}
],
"source": [
"x_mpc=np.array(x.value[0, :]).flatten()\n",
"y_mpc=np.array(x.value[1, :]).flatten()\n",
"theta_mpc=np.array(x.value[2, :]).flatten()\n",
"psi_mpc=np.array(x.value[3, :]).flatten()\n",
"cte_mpc=np.array(x.value[4, :]).flatten()\n",
"v_mpc=np.array(u.value[0, :]).flatten()\n",
"w_mpc=np.array(u.value[1, :]).flatten()\n",
"\n",
"#simulate robot state trajectory for optimized U\n",
"x_traj=predict(x0, np.vstack((v_mpc,w_mpc)))\n",
"\n",
"#plt.figure(figsize=(15,10))\n",
"#plot trajectory\n",
"plt.subplot(2, 2, 1)\n",
"plt.plot(track[0,:],track[1,:],\"b+\")\n",
"plt.plot(x_traj[0,:],x_traj[1,:])\n",
"plt.axis(\"equal\")\n",
"plt.ylabel('y')\n",
"plt.xlabel('x')\n",
"\n",
"#plot v(t)\n",
"plt.subplot(2, 2, 2)\n",
"plt.plot(v_mpc)\n",
"plt.ylabel('v(t)')\n",
"#plt.xlabel('time')\n",
"\n",
"#plot w(t)\n",
"# plt.subplot(2, 2, 3)\n",
"# plt.plot(w_mpc)\n",
"# plt.ylabel('w(t)')\n",
"#plt.xlabel('time')\n",
"\n",
"plt.subplot(2, 2, 3)\n",
"plt.plot(psi_mpc)\n",
"plt.ylabel('psi(t)')\n",
"\n",
"plt.subplot(2, 2, 4)\n",
"plt.plot(cte_mpc)\n",
"plt.ylabel('cte(t)')\n",
"\n",
"plt.tight_layout()\n",
"plt.show()"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"full track demo"
]
},
{
"cell_type": "code",
"execution_count": 97,
"metadata": {},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
"CVXPY Optimization Time: Avrg: 0.1852s Max: 0.2253s Min: 0.1567s\n"
]
}
],
"source": [
"track = compute_path_from_wp([0,3,4,6],\n",
" [0,0,2,1])\n",
"\n",
"sim_duration = 20\n",
"opt_time=[]\n",
"\n",
"x_sim = np.zeros((N,sim_duration))\n",
"u_sim = np.zeros((M,sim_duration-1))\n",
"\n",
"MAX_SPEED = 1.25\n",
"MIN_SPEED = 0.75\n",
"MAX_STEER_SPEED = 1.57/2\n",
"\n",
"# Starting Condition\n",
"x0 = np.zeros(N)\n",
"x0[0] = 0\n",
"x0[1] = -0.25\n",
"x0[2] = np.radians(-0)\n",
"x_sim[:,0]=x0\n",
" \n",
"#starting guess\n",
"u_bar = np.zeros((M,T))\n",
"u_bar[0,:]=0.5*(MAX_SPEED+MIN_SPEED)\n",
"u_bar[1,:]=0.00\n",
"\n",
"for sim_time in range(sim_duration-1):\n",
" \n",
" iter_start=time.time()\n",
" \n",
" K=road_curve(x_sim[:,sim_time],track)\n",
" \n",
" # dynamics starting state\n",
" x_bar=np.zeros((N,T+1))\n",
" x_bar[3,0]=x_bar[2,0]-np.arctan2(df(x_bar[0,0],K),x_bar[0,0])\n",
" x_bar[4,0]=f(x_bar[0,0],K)-x_bar[1,0]\n",
" \n",
" #x_bar[:,0]=x_sim[:,sim_time]\n",
" for t in range (1,T+1):\n",
" xt=x_bar[:,t-1].reshape(5,1)\n",
" ut=u_bar[:,t-1].reshape(2,1)\n",
"\n",
" A,B,C=get_linear_model(xt,ut)\n",
"\n",
" xt_plus_one = np.squeeze(np.dot(A,xt)+np.dot(B,ut)+C)\n",
"\n",
" xt_plus_one[3]=xt_plus_one[2]-np.arctan2(df(xt_plus_one[0],K),xt_plus_one[0])\n",
" xt_plus_one[4]=f(xt_plus_one[0],K)-xt_plus_one[1]\n",
" \n",
" x_bar[:,t]= xt_plus_one\n",
" \n",
" #CVXPY Linear MPC problem statement\n",
" cost = 0\n",
" constr = []\n",
" x = cp.Variable((N, T+1))\n",
" u = cp.Variable((M, T))\n",
" \n",
" for t in range(T):\n",
"\n",
" # Tracking\n",
" cost += 10*cp.sum_squares( x[3, t]) # psi\n",
" cost += 50*cp.sum_squares( x[4, t]) # cte\n",
"\n",
" # Actuation rate of change\n",
" if t < (T - 1):\n",
" cost += cp.quad_form(u[:, t + 1] - u[:, t], 10*np.eye(M))\n",
" \n",
" # Actuation effort\n",
" cost += cp.quad_form( u[:, t],1*np.eye(M))\n",
" \n",
" # Constrains\n",
" A,B,C=get_linear_model(x_bar[:,t],u_bar[:,t])\n",
" constr += [x[:,t+1] == A*x[:,t] + B*u[:,t] + C.flatten()]\n",
"\n",
" # sums problem objectives and concatenates constraints.\n",
" constr += [x[:,0] == x_bar[:,0]] #<--watch out the start condition\n",
" constr += [u[0, :] <= MAX_SPEED]\n",
" constr += [u[0, :] >= MIN_SPEED]\n",
" constr += [cp.abs(u[1, :]) <= MAX_STEER_SPEED]\n",
" \n",
" # Solve\n",
" prob = cp.Problem(cp.Minimize(cost), constr)\n",
" solution = prob.solve(solver=cp.OSQP, verbose=False)\n",
" \n",
" #retrieved optimized U and assign to u_bar to linearize in next step\n",
" u_bar=np.vstack((np.array(u.value[0, :]).flatten(),\n",
" (np.array(u.value[1, :]).flatten())))\n",
" \n",
" u_sim[:,sim_time] = u_bar[:,0]\n",
" \n",
" # Measure elpased time to get results from cvxpy\n",
" opt_time.append(time.time()-iter_start)\n",
" \n",
" # move simulation to t+1\n",
" tspan = [0,dt]\n",
" x_sim[:,sim_time+1] = odeint(kinematics_model,\n",
" x_sim[:,sim_time],\n",
" tspan,\n",
" args=(u_bar[:,0],))[1]\n",
" \n",
"print(\"CVXPY 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": 98,
"metadata": {},
"outputs": [
{
"data": {
"image/png": "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
"text/plain": [
"<Figure size 1080x720 with 3 Axes>"
]
},
"metadata": {},
"output_type": "display_data"
}
],
"source": [
"#plot trajectory\n",
"grid = plt.GridSpec(2, 3)\n",
"\n",
"plt.figure(figsize=(15,10))\n",
"\n",
"plt.subplot(grid[0:2, 0:2])\n",
"plt.plot(track[0,:],track[1,:],\"b+\")\n",
"plt.plot(x_sim[0,:],x_sim[1,:])\n",
"plt.axis(\"equal\")\n",
"plt.ylabel('y')\n",
"plt.xlabel('x')\n",
"\n",
"plt.subplot(grid[0, 2])\n",
"plt.plot(u_sim[0,:])\n",
"plt.ylabel('v(t) [m/s]')\n",
"\n",
"plt.subplot(grid[1, 2])\n",
"plt.plot(np.degrees(u_sim[1,:]))\n",
"plt.ylabel('w(t) [deg/s]')\n",
"\n",
"plt.tight_layout()\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
}