2020-07-01 00:21:27 +08:00
{
"cells": [
{
"cell_type": "code",
"execution_count": 1,
"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",
"* $v$ velocity of the robot\n",
"* $\\theta$ heading of the robot\n",
"\n",
"The inputs of the model are:\n",
"\n",
2020-08-06 17:21:47 +08:00
"* $a$ acceleration of the robot\n",
2020-10-23 18:23:12 +08:00
"* $\\delta$ steering of the robot\n",
2020-07-01 00:21:27 +08:00
"\n",
"These are the differential equations f(x,u) of the model:\n",
"\n",
"$\\dot{x} = f(x,u)$\n",
"\n",
"* $\\dot{x} = v\\cos{\\theta}$ \n",
"* $\\dot{y} = v\\sin{\\theta}$\n",
"* $\\dot{v} = a$\n",
"* $\\dot{\\theta} = \\frac{v\\tan{\\delta}}{L}$\n",
"\n",
"Discretize with forward Euler Integration for time step dt:\n",
"\n",
"${x_{t+1}} = x_{t} + f(x,u)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",
"* ${v_{t+1}} = v_{t} + a_tdt$\n",
"* ${\\theta_{t+1}} = \\theta_{t} + \\frac{v\\tan{\\delta}}{L} dt$\n",
"\n",
"----------------------\n",
"\n",
"The Model is **non-linear** and **time variant**, but the Numerical Optimizer requires a Linear sets of equations. To approximate the equivalent **LTI** State space model, the **Taylor's series expansion** is used around $\\bar{x}$ and $\\bar{u}$ (at each time step):\n",
"\n",
"$ f(x,u) \\approx f(\\bar{x},\\bar{u}) + \\frac{\\partial f(x,u)}{\\partial x}|_{x=\\bar{x},u=\\bar{u}}(x-\\bar{x}) + \\frac{\\partial f(x,u)}{\\partial u}|_{x=\\bar{x},u=\\bar{u}}(u-\\bar{u})$\n",
"\n",
"This can be rewritten usibg the State Space model form Ax+Bu :\n",
"\n",
"$ f(\\bar{x},\\bar{u}) + A|_{x=\\bar{x},u=\\bar{u}}(x-\\bar{x}) + B|_{x=\\bar{x},u=\\bar{u}}(u-\\bar{u})$\n",
"\n",
"Where:\n",
"\n",
"$\n",
"A =\n",
"\\quad\n",
"\\begin{bmatrix}\n",
2020-07-01 21:33:16 +08:00
"\\frac{\\partial f(x,u)}{\\partial x} & \\frac{\\partial f(x,u)}{\\partial y} & \\frac{\\partial f(x,u)}{\\partial v} & \\frac{\\partial f(x,u)}{\\partial \\theta} \\\\\n",
2020-07-01 00:21:27 +08:00
"\\end{bmatrix}\n",
"\\quad\n",
"=\n",
2020-07-01 21:33:16 +08:00
"\\displaystyle \\left[\\begin{matrix}0 & 0 & \\cos{\\left(\\theta \\right)} & - v \\sin{\\left(\\theta \\right)}\\\\0 & 0 & \\sin{\\left(\\theta \\right)} & v \\cos{\\left(\\theta \\right)}\\\\0 & 0 & 0 & 0\\\\0 & 0 & \\frac{\\tan{\\left(\\delta \\right)}}{L} & 0\\end{matrix}\\right]\n",
2020-07-01 00:21:27 +08:00
"$\n",
"\n",
"and\n",
"\n",
"$\n",
"B = \n",
"\\quad\n",
"\\begin{bmatrix}\n",
2020-07-01 21:33:16 +08:00
"\\frac{\\partial f(x,u)}{\\partial a} & \\frac{\\partial f(x,u)}{\\partial \\delta} \\\\\n",
2020-07-01 00:21:27 +08:00
"\\end{bmatrix}\n",
"\\quad\n",
"= \n",
2020-07-01 21:33:16 +08:00
"\\displaystyle \\left[\\begin{matrix}0 & 0\\\\0 & 0\\\\1 & 0\\\\0 & \\frac{v \\left(\\tan^{2}{\\left(\\delta \\right)} + 1\\right)}{L}\\end{matrix}\\right]\n",
2020-07-01 00:21:27 +08:00
"$\n",
"\n",
"are the *Jacobians*.\n",
"\n",
"\n",
"\n",
"So the discretized model is given by:\n",
"\n",
"$ x_{t+1} = x_t + (f(\\bar{x},\\bar{u}) + A|_{x=\\bar{x}}(x_t-\\bar{x}) + B|_{u=\\bar{u}}(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 LTI-equivalent kinematics model is:\n",
"\n",
"$ x_{t+1} = A'x_t + B' u_t + C' $\n",
"\n",
"with:\n",
"\n",
"$ A' = I+dtA|_{x=\\bar{x},u=\\bar{u}} $\n",
"\n",
"$ B' = dtB|_{x=\\bar{x},u=\\bar{u}} $\n",
"\n",
"$ C' = dt(f(\\bar{x},\\bar{u}) - A|_{x=\\bar{x},u=\\bar{u}}\\bar{x} - B|_{x=\\bar{x},u=\\bar{u}}\\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": 2,
"metadata": {},
"outputs": [],
"source": [
2020-10-06 17:13:11 +08:00
"\"\"\"\n",
"Control problem statement.\n",
"\"\"\"\n",
2020-07-01 00:21:27 +08:00
"\n",
"N = 4 #number of state variables\n",
"M = 2 #number of control variables\n",
"T = 20 #Prediction Horizon\n",
2020-10-26 23:49:51 +08:00
"DT = 0.2 #discretization step"
2020-07-01 00:21:27 +08:00
]
},
{
"cell_type": "code",
"execution_count": 3,
"metadata": {},
"outputs": [],
"source": [
"def get_linear_model(x_bar,u_bar):\n",
" \"\"\"\n",
2020-10-06 17:13:11 +08:00
" Computes the LTI approximated state space model x' = Ax + Bu + C\n",
2020-07-01 00:21:27 +08:00
" \"\"\"\n",
2020-10-06 17:13:11 +08:00
" \n",
" L=0.3 #vehicle wheelbase\n",
2020-07-01 00:21:27 +08:00
" \n",
" x = x_bar[0]\n",
" y = x_bar[1]\n",
" v = x_bar[2]\n",
" theta = x_bar[3]\n",
" \n",
" a = u_bar[0]\n",
" delta = u_bar[1]\n",
" \n",
" A = np.zeros((N,N))\n",
" A[0,2]=np.cos(theta)\n",
" A[0,3]=-v*np.sin(theta)\n",
2020-07-01 17:04:01 +08:00
" A[1,2]=np.sin(theta)\n",
2020-07-01 00:21:27 +08:00
" A[1,3]=v*np.cos(theta)\n",
" A[3,2]=v*np.tan(delta)/L\n",
2020-10-23 18:23:12 +08:00
" A_lin=np.eye(N)+DT*A\n",
2020-07-01 00:21:27 +08:00
" \n",
" B = np.zeros((N,M))\n",
" B[2,0]=1\n",
2020-07-01 17:04:01 +08:00
" B[3,1]=v/(L*np.cos(delta)**2)\n",
2020-10-23 18:23:12 +08:00
" B_lin=DT*B\n",
2020-07-01 00:21:27 +08:00
" \n",
" f_xu=np.array([v*np.cos(theta), v*np.sin(theta), a,v*np.tan(delta)/L]).reshape(N,1)\n",
2020-10-23 18:23:12 +08:00
" C_lin = DT*(f_xu - np.dot(A,x_bar.reshape(N,1)) - np.dot(B,u_bar.reshape(M,1)))\n",
2020-07-01 00:21:27 +08:00
" \n",
" return np.round(A_lin,4), np.round(B_lin,4), np.round(C_lin,4)"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"Motion Prediction: using scipy intergration"
]
},
{
"cell_type": "code",
"execution_count": 4,
"metadata": {},
"outputs": [],
"source": [
"# Define process model\n",
"# This uses the continuous model \n",
"def kinematics_model(x,t,u):\n",
" \"\"\"\n",
2020-10-06 17:13:11 +08:00
" Returns the set of ODE of the vehicle model.\n",
2020-07-01 00:21:27 +08:00
" \"\"\"\n",
" \n",
2020-10-06 17:13:11 +08:00
" L=0.3 #vehicle wheelbase\n",
2020-07-01 00:21:27 +08:00
" dxdt = x[2]*np.cos(x[3])\n",
" dydt = x[2]*np.sin(x[3])\n",
" dvdt = u[0]\n",
" dthetadt = x[2]*np.tan(u[1])/L\n",
"\n",
" dqdt = [dxdt,\n",
" dydt,\n",
" dvdt,\n",
" dthetadt]\n",
"\n",
" return dqdt\n",
"\n",
"def predict(x0,u):\n",
" \"\"\"\n",
" \"\"\"\n",
" \n",
2020-10-06 17:13:11 +08:00
" x_ = np.zeros((N,T+1))\n",
2020-07-01 00:21:27 +08:00
" \n",
2020-10-06 17:13:11 +08:00
" x_[:,0] = x0\n",
2020-07-01 00:21:27 +08:00
" \n",
" # solve ODE\n",
" for t in range(1,T+1):\n",
"\n",
2020-10-23 18:23:12 +08:00
" tspan = [0,DT]\n",
2020-07-01 00:21:27 +08:00
" x_next = odeint(kinematics_model,\n",
" x0,\n",
" tspan,\n",
" args=(u[:,t-1],))\n",
"\n",
" x0 = x_next[1]\n",
2020-10-06 17:13:11 +08:00
" x_[:,t]=x_next[1]\n",
2020-07-01 00:21:27 +08:00
" \n",
2020-10-06 17:13:11 +08:00
" return x_"
2020-07-01 00:21:27 +08:00
]
},
{
"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": 5,
"metadata": {},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
2021-04-01 22:26:39 +08:00
"CPU times: user 2.63 ms, sys: 844 µs, total: 3.47 ms\n",
"Wall time: 3.11 ms\n"
2020-07-01 00:21:27 +08:00
]
}
],
"source": [
"%%time\n",
"\n",
"u_bar = np.zeros((M,T))\n",
"u_bar[0,:] = 0.2 #m/ss\n",
"u_bar[1,:] = np.radians(-np.pi/4) #rad\n",
"\n",
"x0 = np.zeros(N)\n",
"x0[0] = 0\n",
"x0[1] = 1\n",
"x0[2] = 0\n",
"x0[3] = np.radians(0)\n",
"\n",
"x_bar=predict(x0,u_bar)"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"Check the model prediction"
]
},
{
"cell_type": "code",
"execution_count": 6,
"metadata": {},
"outputs": [
{
"data": {
2020-10-26 23:49:51 +08:00
"image/png": "iVBORw0KGgoAAAANSUhEUgAAAagAAACdCAYAAADhcuxqAAAAOXRFWHRTb2Z0d2FyZQBNYXRwbG90bGliIHZlcnNpb24zLjMuMSwgaHR0cHM6Ly9tYXRwbG90bGliLm9yZy/d3fzzAAAACXBIWXMAAAsTAAALEwEAmpwYAAAhAUlEQVR4nO3deVxVdf7H8dc5LAIugECSKOaa2Vhqko1JmGJj6BjTmIlpEpUpqaVNpfNrzMYwayLK3JrCJVpGc2sZ29REJ2tyT+WX22D1ywXxssgmy/n+/iB56AByQe49514+z8eDxwPuOVze93jvfXvOPef71ZRSCiGEEMJidLMDCCGEEDWRghJCCGFJUlBCCCEsSQpKCCGEJUlBCSGEsCQpKCGEEJZkqYIyDIOnnnqKefPmmR1FCCGEyTzNDnCxDRs2EBYWRnFxsV3rnzhxosbbg4ODyc7ObsxoV8xqmSRP3S6XqW3btk5O43iu8nqyWh6wXiZXy1Pb68kye1Bnz55l9+7dDB482OwoQgghLMAye1DLly9n7Nixl9172rhxIxs3bgRg3rx5BAcH17iep6dnrcvMYrVMkqduVswkhKtRpecp+WYLdP1NvX/XEgW1a9cu/P396dSpEwcPHqx1vejoaKKjo6t+rm2X0Wq7t2C9TJKnbk3tEJ8QjU1lHsFYmkLe6V/Q/7oQLbRdvX7fEgV16NAhdu7cyZ49eygtLaW4uJj58+czdepUs6MJIYSoJ1VehvrnKtSGD8C/NQHPvsq5epYTWKSgxowZw5gxYwA4ePAgH3/8sZSTEEK4IPXLjxhLU+Cn/6D99na00Q/TLPwazjXgCIklCkoIIYRrU0YF6ssPUevfAd/m6Il/Rut9yxXdp+UK6vrrr+f66683O4YQQgg7qTOnMJa+CkczoPct6GMT0VoFXPH9Wq6ghBBCuAalFCr9M9TqZaB7oCVMQ7tlIJqmNcr9S0EJIYSoN5VzFmPFfDi4B3r0Qh8/Ba11SKP+DSkoIYQQdlNKof6djnr/DSgvRxszEW3gnY2213QxKSghhBB2UefyMN5ZDLu3Q+fu6AmPo13luGsCpaCEEELUSe39FuPthVBciPbH8Wh3xKLpHg79m1JQQrgwwzCYMWMGrVu3ZsaMGRQUFJCSksKZM2cICQlh2rRptGjRwuyYwoWpokLUP95EfbMZ2ndEnz4Hrd01TvnblhksVoimwDAMu76UUnbd34UZAC5Yv349PXv2ZP78+fTs2ZP169c76JGIpkD97z6M56ag/r0Fbdgo9D+/7LRyAtmDEsKp4uLi7FrP29ubtLS0y65zYQaAu+++m08++QSAHTt2MHv2bACioqKYPXs2Y8eOvaLMoulR58+j1ixHffVPCA1Dn/ESWsduTs8hBSWEE3l7e/PKK69cdh2lFE899VSd91XTDAB5eXkEBgYCEBgYSH5+fq2/76qzA1gtD1gv05XkKT10gPzX5mCc/Bm/4aNoMXYiWjMfU/JIQQnhRL///e8JCan7WpHhw4dfdrm9MwBcjqvODmC1PGC9TA3Jo8rKUB+/j/psLQQGoT/xPOe738D5cwVwrsCheWqbHUAKSggnGjVqlF3rjRw58rLLa5sBwN/fn5ycHAIDA8nJyaFVq1aNEVu4OfV/mRipKfB/x9FujUa79yE0Xz+zY0lBCWGW06dP13i7l5cXAQEB6Hrt5zDVNgNAWloa6enpxMbGkp6eTkREhEOyC/egKipQn69FffQ+NG+BPvkZtBtvNjtWFSkoIUxyuSlldF3npptu4qGHHiIgIMDu+4yNjSUlJYXNmzcTHBzM9OnTGyGpcEfq9InKaTH+cwjtplvR7puE1tJae9xSUEKY5JFHHiEjI4ORI0dWHaNfvXo11157LT169ODdd98lNTWVJ5544rL3c/EMAC1btmTWrFnOiC9clDIM1JYNqDXLwdMb7aEn0G6+zSFDFV0puQ5KCJOsWrWKCRMmEBoaiqenJ6GhoTz88MOsWbOGsLAwEhMTycjIMDumcCPKdgbj1WdR7/8duv0G/bnX0ftFWbKcQPaghDCNUoozZ85ccqFtdnY2hmEA4OPjQ0VFhVnxhBtRSqG+2Yz6x5tgGGjjEtEif2fZYrpACkoIk8TExPDXv/6VgQMHEhQUhM1m46uvviImJgaA3bt3062b8y+OFO5F5edipC2Evf+Grj3QH3gcLSTU7Fh2kYISwiR33XUXHTp04JtvviEzM5OAgAAmTZpEr169ALj55pu5+WbrnFElXI/avR0jbRGUFKPd8wBa9AiHD/DamKSghDBRr169qgpJiMZiFORjpL6C+nYLhHdGT5iGFhZudqx6k4ISwiRlZWWsXr2ar7/+mnPnzrFixQr27dvHyZMnGTp0qNnxhItSB/dwNm0BKucs2u9Ho8WMQvN0zbd6S6TOzs5m4cKF5Obmomka0dHRVcfhhXBXK1aswGazMXXqVObOnQtA+/btWbFihRSUqDdVUlw5wOuWT/Fodw36xBlo13Q1O9YVsURBeXh4MG7cODp16kRxcTEzZszghhtuoF27dmZHE8JhvvvuO+bPn4+Pj0/V2VStW7fGZrOZnEy4GnU0A2Ppq5B9Gm3IXQQ9+Dhnz50zO9YVs0RBBQYGVo3A7OvrS1hYGDabTQpKuDVPT8+qU8ovyM/Pp2XLliYlEq5GlZWiPnwP9cU6aB2C/kQS2rW/QWvWDKSgGl9WVhaZmZl06dKl2jJXnR4ArJdJ8tTN0ZluueUWFixYQHx8PAA5OTksX76c/v37O+xvCvehfjpWOcDriZ/QIu9AG5WA5mP+AK+NSVP2Tt3pBCUlJTz77LPcfffd9OvXr871T5w4UePtVhv6HqyXSfLU7XKZapseoD7Ky8t555132LRpE6WlpXh7ezN48GDGjh2LpwkfarvK68lqecC5mVRFBerT1ahP/gEt/NHHT0br2de0PPZw+ek2ysvLSU5OJjIy0q5yEsLVeXp6Eh8fT3x8fNWhPatf2S/MpU7+X+UAr8ePoEVEot03Ea25+x4StkRBKaVYsmQJYWFhdU7UJoQrq22KDeCSmXHbtGnjjDjCRSjDQG3+BLX2bfBuhjbhKfSIAWbHcjhLFNShQ4fYunUr4eHhPPnkkwDExcXRp08fk5MJ0bguN8XGxVauXOngJMJVqLNZGMteg0P7oWdf9PsnowW0NjuWU1iioLp3786qVavMjiGEw11cPF999RX79+/nnnvuISQkhDNnzrB69Wp69uxpYkJhFUop1NcbUSvfAgXa+CmVs902ocPAligoIZqilStXMn/+fLy9vQG4+uqrmTBhAo899hgDBw40N5wwlcrLqRzgdd93cG1P9PipaMFN77CvFJQQJlFKkZWVdcn1fmfOnKl2bZRoWtTOf2G8uxjOn0e79yG0QcPR9KY5dZ8UlBAmGTZsWNV0GxdOw01PT2fYsGFmRxMmUIXnUO+9gfpuK1zTtXKA16ub9mAFUlBCmGTEiBGEh4fzzTffcPz48WrTbYimQ+3fhbHidSjIQ7vrPrQ7R6J5uM60GI4iBSWEiWS6jaZNlRShVi1FbfsCwjqgT/0LWnhns2NZRtM8sCmESTZt2mTXeps3b3ZwEmE2dfgAxnOPof61EW3oH9H/5xUpp/8iBSWEE7399tsopTAM47JfaWlpZkcVDqLKSjFWpWK8/D+gaehPzUX/43g0Ly+zo1mOHOITwolKSkoYPXp0net5yZuVW1LHj1ROi3HyZ7SBMWh/HI/m42t2LMtyu4IyPltD9r++pKKiAjSt8kv3AF0HDw/w9AIPT/DyqvzeyxvNuxl4N4NmzaCZL/j4gq8v+Pih+TYHv1+/mrcAvxZonvLmIRpmwYIFdq3XlC7GbApUeTlqwyrUP1dBq0D0x59Du7632bEsz+0KiqCr8LruBoySElCq8quiAqUMqKiA8jIoL4fS81BYAGWlqLJSOF9Sedv5kkvurqah3vUZL6F17u6cxyPcSkhIiNkRhJOpEz9V7jX9eBTtloFooyegNW9hdiyX4HYFpUdE4n/nHxo81LwyDCgtgZJiKC6GogIoLkQVFlR+X1gATfCKbiF
2020-07-01 00:21:27 +08:00
"text/plain": [
"<Figure size 432x288 with 2 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",
"\n",
"plt.tight_layout()\n",
"plt.show()"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"Motion Prediction: using the state space model"
]
},
{
"cell_type": "code",
"execution_count": 7,
"metadata": {},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
2021-04-01 22:26:39 +08:00
"CPU times: user 2.06 ms, sys: 567 µs, total: 2.63 ms\n",
"Wall time: 1.77 ms\n"
2020-07-01 00:21:27 +08:00
]
}
],
"source": [
"%%time\n",
"\n",
"u_bar = np.zeros((M,T))\n",
"u_bar[0,:] = 0.2 #m/s\n",
"u_bar[1,:] = np.radians(-np.pi/4) #rad\n",
"\n",
"x0 = np.zeros(N)\n",
"x0[0] = 0\n",
"x0[1] = 1\n",
"x0[2] = 0\n",
"x0[3] = np.radians(0)\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(N,1)\n",
" ut=u_bar[:,t-1].reshape(M,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": 8,
"metadata": {},
"outputs": [
{
"data": {
2020-10-26 23:49:51 +08:00
"image/png": "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
2020-07-01 00:21:27 +08:00
"text/plain": [
"<Figure size 432x288 with 2 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)')\n",
"#plt.xlabel('time')\n",
"\n",
"\n",
"plt.tight_layout()\n",
"plt.show()"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"The results are the same as expected, so the linearized model is equivalent as expected."
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"## PRELIMINARIES"
]
},
{
"cell_type": "code",
2020-10-23 23:31:57 +08:00
"execution_count": 9,
2020-07-01 00:21:27 +08:00
"metadata": {},
"outputs": [],
"source": [
"def compute_path_from_wp(start_xp, start_yp, step = 0.1):\n",
2020-10-06 17:13:11 +08:00
" \"\"\"\n",
" Computes a reference path given a set of waypoints\n",
" \"\"\"\n",
" \n",
2020-07-01 00:21:27 +08:00
" 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",
2020-10-23 17:43:10 +08:00
" \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,theta))\n",
2020-07-01 00:21:27 +08:00
"\n",
"\n",
"def get_nn_idx(state,path):\n",
2020-10-06 17:13:11 +08:00
" \"\"\"\n",
" Computes the index of the waypoint closest to vehicle\n",
" \"\"\"\n",
2020-07-01 00:21:27 +08:00
"\n",
" dx = state[0]-path[0,:]\n",
" dy = state[1]-path[1,:]\n",
2020-10-23 17:43:10 +08:00
" dist = np.hypot(dx,dy)\n",
2020-07-01 00:21:27 +08:00
" 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",
2020-10-23 17:43:10 +08:00
"def get_ref_trajectory(state, path, target_v):\n",
2020-10-06 17:13:11 +08:00
" \"\"\"\n",
" \"\"\"\n",
2020-10-23 17:43:10 +08:00
" xref = np.zeros((N, T + 1))\n",
" dref = np.zeros((1, T + 1))\n",
" \n",
" #sp = np.ones((1,T +1))*target_v #speed profile\n",
" \n",
" ncourse = path.shape[1]\n",
"\n",
2020-10-23 18:23:12 +08:00
" ind = get_nn_idx(state, path)\n",
2020-10-23 17:43:10 +08:00
"\n",
" xref[0, 0] = path[0,ind] #X\n",
" xref[1, 0] = path[1,ind] #Y\n",
2020-10-23 18:23:12 +08:00
" xref[2, 0] = target_v #sp[ind] #V\n",
" xref[3, 0] = path[2,ind] #Theta\n",
" dref[0, 0] = 0.0 # steer operational point should be 0\n",
2020-10-23 17:43:10 +08:00
" \n",
2020-10-23 22:18:36 +08:00
" dl = 0.05 # Waypoints spacing [m]\n",
2020-10-23 17:43:10 +08:00
" travel = 0.0\n",
"\n",
" for i in range(T + 1):\n",
2020-10-23 22:18:36 +08:00
" travel += abs(target_v) * DT #current V or target V?\n",
2020-10-23 17:43:10 +08:00
" dind = int(round(travel / dl))\n",
"\n",
" if (ind + dind) < ncourse:\n",
" xref[0, i] = path[0,ind + dind]\n",
" xref[1, i] = path[1,ind + dind]\n",
" xref[2, i] = target_v #sp[ind + dind]\n",
" xref[3, i] = path[2,ind + dind]\n",
" dref[0, i] = 0.0\n",
" else:\n",
" xref[0, i] = path[0,ncourse - 1]\n",
" xref[1, i] = path[1,ncourse - 1]\n",
2020-10-23 23:31:57 +08:00
" xref[2, i] = 0.0 #stop? #sp[ncourse - 1]\n",
2020-10-23 17:43:10 +08:00
" xref[3, i] = path[2,ncourse - 1]\n",
" dref[0, i] = 0.0\n",
"\n",
" return xref, dref"
2020-07-01 00:21:27 +08:00
]
},
{
"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": [
2020-10-23 17:43:10 +08:00
"<!--  -->\n",
2020-07-01 00:21:27 +08:00
"\n",
2020-10-23 17:43:10 +08:00
"<!--  -->"
2020-07-01 00:21:27 +08:00
]
},
{
"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} $"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"# Problem Formulation: Study case\n",
"\n",
2020-10-23 04:31:40 +08:00
"In this case, the objective function to minimize is given by:\n",
"\n",
2020-10-23 23:31:57 +08:00
"https://borrelli.me.berkeley.edu/pdfpub/IV_KinematicMPC_jason.pdf\n",
2020-07-01 00:21:27 +08:00
"\n",
"$\n",
"\\begin{equation}\n",
"\\begin{aligned}\n",
2020-10-30 01:40:16 +08:00
"\\min_{} \\quad & \\sum^{t+T-1}_{j=t} (x_{j} - x_{j,ref})^TQ(x_{j} - x_{j,ref}) + \\sum^{t+T-1}_{j=t+1} u^T_{j}Ru_{j} + (u_{j} - u_{j-1})^TP(u_{j} - u_{j-1}) \\\\\n",
2020-07-01 00:21:27 +08:00
"\\textrm{s.t.} \\quad & x(0) = x0\\\\\n",
2020-10-26 19:41:39 +08:00
" & x_{j+1} = Ax_{j}+Bu_{j} \\quad \\textrm{for} \\quad t<j<t+T-1 \\\\\n",
" & u_{MIN} < u_{j} < u_{MAX} \\quad \\textrm{for} \\quad t<j<t+T-1 \\\\\n",
" & \\dot{u}_{MIN} < \\frac{(u_{j} - u_{j-1})}{ts} < \\dot{u}_{MAX} \\quad \\textrm{for} \\quad t+1<j<t+T-1 \\\\\n",
2020-07-01 00:21:27 +08:00
"\\end{aligned}\n",
"\\end{equation}\n",
"$\n",
"\n",
"\n",
2020-10-26 19:41:39 +08:00
"Where R,P,Q are the cost matrices used to tune the response.\n"
2020-07-01 00:21:27 +08:00
]
},
{
"cell_type": "code",
2020-10-26 23:49:51 +08:00
"execution_count": 10,
2020-07-01 00:21:27 +08:00
"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",
2020-11-17 02:10:18 +08:00
"problem: variables n = 326, constraints m = 408\n",
" nnz(P) + nnz(A) = 1063\n",
2020-07-01 00:21:27 +08:00
"settings: linear system solver = qdldl,\n",
2020-10-23 18:23:12 +08:00
" eps_abs = 1.0e-05, eps_rel = 1.0e-05,\n",
2020-07-01 00:21:27 +08:00
" 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",
2021-04-01 22:26:39 +08:00
" 1 0.0000e+00 4.27e+00 4.67e+02 1.00e-01 3.75e-04s\n",
" 175 1.6965e+02 2.63e-05 3.49e-05 7.14e+00 1.85e-03s\n",
2020-07-01 00:21:27 +08:00
"\n",
"status: solved\n",
"solution polish: unsuccessful\n",
2020-11-17 02:10:18 +08:00
"number of iterations: 175\n",
"optimal objective: 169.6454\n",
2021-04-01 22:26:39 +08:00
"run time: 2.63e-03s\n",
2020-11-17 02:10:18 +08:00
"optimal rho estimate: 6.34e+00\n",
2020-07-01 00:21:27 +08:00
"\n",
2021-04-01 22:26:39 +08:00
"CPU times: user 119 ms, sys: 93 µs, total: 119 ms\n",
"Wall time: 116 ms\n"
2020-07-01 00:21:27 +08:00
]
}
],
"source": [
"%%time\n",
"\n",
2020-10-23 18:23:12 +08:00
"MAX_SPEED = 1.5 #m/s\n",
"MAX_STEER = np.radians(30) #rad\n",
2020-07-01 00:21:27 +08:00
"MAX_ACC = 1.0\n",
2020-07-01 17:04:01 +08:00
"REF_VEL=1.0\n",
2020-07-01 00:21:27 +08:00
"\n",
2020-07-01 17:04:01 +08:00
"track = compute_path_from_wp([0,3,6],\n",
2020-10-23 04:31:40 +08:00
" [0,0,0],0.05)\n",
2020-07-01 00:21:27 +08:00
"\n",
"# Starting Condition\n",
"x0 = np.zeros(N)\n",
2020-10-23 17:43:10 +08:00
"x0[0] = 0 #x\n",
"x0[1] = -0.25 #y\n",
"x0[2] = 0.0 #v\n",
"x0[3] = np.radians(-0) #yaw\n",
2020-07-01 00:21:27 +08:00
" \n",
"#starting guess\n",
"u_bar = np.zeros((M,T))\n",
2020-10-23 18:23:12 +08:00
"u_bar[0,:] = MAX_ACC/2 #a\n",
"u_bar[1,:] = 0.1 #delta\n",
2020-07-01 00:21:27 +08:00
"\n",
2020-10-23 04:31:40 +08:00
"# dynamics starting state w.r.t world frame\n",
2020-10-23 18:23:12 +08:00
"x_bar = np.zeros((N,T+1))\n",
"x_bar[:,0] = x0\n",
2020-10-23 04:31:40 +08:00
"\n",
2020-07-01 00:21:27 +08:00
"#prediction for linearization of costrains\n",
"for t in range (1,T+1):\n",
2020-10-23 18:23:12 +08:00
" xt = x_bar[:,t-1].reshape(N,1)\n",
" ut = u_bar[:,t-1].reshape(M,1)\n",
" A, B, C = get_linear_model(xt,ut)\n",
2020-07-01 00:21:27 +08:00
" xt_plus_one = np.squeeze(np.dot(A,xt)+np.dot(B,ut)+C)\n",
2020-10-23 18:23:12 +08:00
" x_bar[:,t] = xt_plus_one\n",
2020-07-01 00:21:27 +08:00
"\n",
2020-10-23 18:23:12 +08:00
"#CVXPY Linear MPC problem statement\n",
2020-07-01 00:21:27 +08:00
"x = cp.Variable((N, T+1))\n",
"u = cp.Variable((M, T))\n",
"cost = 0\n",
"constr = []\n",
"\n",
2020-10-23 18:23:12 +08:00
"# Cost Matrices\n",
2020-10-23 22:18:36 +08:00
"Q = np.diag([10,10,10,10]) #state error cost\n",
2020-10-23 18:23:12 +08:00
"Qf = np.diag([10,10,10,10]) #state final error cost\n",
"R = np.diag([10,10]) #input cost\n",
"R_ = np.diag([10,10]) #input rate of change cost\n",
2020-10-23 04:31:40 +08:00
"\n",
2020-10-23 18:23:12 +08:00
"#Get Reference_traj\n",
2020-10-26 19:41:39 +08:00
"x_ref, d_ref = get_ref_trajectory(x_bar[:,0], track, REF_VEL)\n",
2020-10-23 04:31:40 +08:00
"\n",
2020-10-23 18:23:12 +08:00
"#Prediction Horizon\n",
"for t in range(T):\n",
" \n",
" # Tracking Error\n",
2020-10-26 19:41:39 +08:00
" cost += cp.quad_form(x[:,t] - x_ref[:,t], Q)\n",
2020-10-23 04:31:40 +08:00
"\n",
2020-10-23 18:23:12 +08:00
" # Actuation effort\n",
2020-10-26 19:41:39 +08:00
" cost += cp.quad_form(u[:,t], R)\n",
2020-10-23 18:23:12 +08:00
" \n",
" # Actuation rate of change\n",
" if t < (T - 1):\n",
" cost += cp.quad_form(u[:, t + 1] - u[:, t], R_)\n",
"\n",
" # Kinrmatics Constrains (Linearized model)\n",
2020-10-26 19:41:39 +08:00
" A,B,C = get_linear_model(x_bar[:,t], u_bar[:,t])\n",
2020-10-23 18:23:12 +08:00
" constr += [x[:,t+1] == A@x[:,t] + B@u[:,t] + C.flatten()]\n",
"\n",
2020-11-17 02:10:18 +08:00
"#Final Point tracking\n",
"cost += cp.quad_form(x[:, T] - x_ref[:, T], Qf)\n",
"\n",
2020-10-23 18:23:12 +08:00
"# sums problem objectives and concatenates constraints.\n",
"constr += [x[:,0] == x_bar[:,0]] #starting condition\n",
2020-10-26 19:41:39 +08:00
"constr += [x[2,:] <= MAX_SPEED] #max speed\n",
"constr += [x[2,:] >= 0.0] #min_speed (not really needed)\n",
"constr += [cp.abs(u[0,:]) <= MAX_ACC] #max acc\n",
"constr += [cp.abs(u[1,:]) <= MAX_STEER] #max steer\n",
"# for t in range(T):\n",
"# if t < (T - 1):\n",
"# constr += [cp.abs(u[0,t] - u[0,t-1])/DT <= MAX_ACC] #max acc\n",
"# constr += [cp.abs(u[1,t] - u[1,t-1])/DT <= MAX_STEER] #max steer\n",
2020-07-01 00:21:27 +08:00
"\n",
"prob = cp.Problem(cp.Minimize(cost), constr)\n",
"solution = prob.solve(solver=cp.OSQP, verbose=True)"
]
},
{
"cell_type": "code",
2020-10-26 23:49:51 +08:00
"execution_count": 11,
2020-07-01 00:21:27 +08:00
"metadata": {},
"outputs": [
{
"data": {
2020-11-17 02:10:18 +08:00
"image/png": "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
2020-07-01 00:21:27 +08:00
"text/plain": [
2020-07-01 17:04:01 +08:00
"<Figure size 432x288 with 4 Axes>"
2020-07-01 00:21:27 +08:00
]
},
"metadata": {},
"output_type": "display_data"
}
],
"source": [
"x_mpc=np.array(x.value[0, :]).flatten()\n",
"y_mpc=np.array(x.value[1, :]).flatten()\n",
2020-07-01 17:04:01 +08:00
"v_mpc=np.array(x.value[2, :]).flatten()\n",
"theta_mpc=np.array(x.value[3, :]).flatten()\n",
"a_mpc=np.array(u.value[0, :]).flatten()\n",
2020-10-23 22:18:36 +08:00
"delta_mpc=np.array(u.value[1, :]).flatten()\n",
2020-07-01 00:21:27 +08:00
"\n",
"#simulate robot state trajectory for optimized U\n",
2020-10-23 23:31:57 +08:00
"x_traj=predict(x0, np.vstack((a_mpc,delta_mpc)))\n",
2020-07-01 00:21:27 +08:00
"\n",
"#plt.figure(figsize=(15,10))\n",
"#plot trajectory\n",
"plt.subplot(2, 2, 1)\n",
2020-10-23 22:18:36 +08:00
"plt.plot(track[0,:],track[1,:],\"b\")\n",
"plt.plot(x_ref[0,:],x_ref[1,:],\"g+\")\n",
2020-07-01 00:21:27 +08:00
"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",
2020-10-23 22:18:36 +08:00
"plt.subplot(2, 2, 3)\n",
2020-07-01 17:04:01 +08:00
"plt.plot(a_mpc)\n",
2020-10-23 22:18:36 +08:00
"plt.ylabel('a_in(t)')\n",
2020-07-01 00:21:27 +08:00
"#plt.xlabel('time')\n",
"\n",
"\n",
2020-10-23 22:18:36 +08:00
"plt.subplot(2, 2, 2)\n",
2020-07-01 17:04:01 +08:00
"plt.plot(theta_mpc)\n",
"plt.ylabel('theta(t)')\n",
2020-07-01 00:21:27 +08:00
"\n",
2020-10-23 22:18:36 +08:00
"plt.subplot(2, 2, 4)\n",
"plt.plot(delta_mpc)\n",
"plt.ylabel('d_in(t)')\n",
2020-07-01 00:21:27 +08:00
"\n",
"plt.tight_layout()\n",
"plt.show()"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
2020-11-17 02:10:18 +08:00
"## full track demo"
2020-07-01 00:21:27 +08:00
]
},
{
"cell_type": "code",
2021-03-02 22:25:29 +08:00
"execution_count": 12,
2020-07-01 00:21:27 +08:00
"metadata": {},
2020-07-01 17:04:01 +08:00
"outputs": [
2020-11-17 02:10:18 +08:00
{
"name": "stderr",
"output_type": "stream",
"text": [
"/home/marcello/.conda/envs/jupyter/lib/python3.8/site-packages/cvxpy/problems/problem.py:1054: UserWarning: Solution may be inaccurate. Try another solver, adjusting the solver settings, or solve with verbose=True for more information.\n",
" warnings.warn(\n"
]
},
2020-07-01 17:45:08 +08:00
{
2020-10-23 22:18:36 +08:00
"name": "stdout",
"output_type": "stream",
"text": [
2021-04-01 22:26:39 +08:00
"CVXPY Optimization Time: Avrg: 0.1606s Max: 0.2352s Min: 0.1437s\n"
2020-07-01 17:04:01 +08:00
]
}
],
2020-07-01 00:21:27 +08:00
"source": [
2020-07-01 22:59:13 +08:00
"track = compute_path_from_wp([0,3,4,6,10,12,14,6,1,0],\n",
2020-10-23 22:18:36 +08:00
" [0,0,2,4,3,3,-2,-6,-2,-2],0.05)\n",
2020-07-01 00:21:27 +08:00
"\n",
2020-10-26 23:49:51 +08:00
"# track = compute_path_from_wp([0,10,10,0],\n",
"# [0,0,1,1],0.05)\n",
2020-07-01 00:21:27 +08:00
"\n",
2020-10-26 19:41:39 +08:00
"sim_duration = 200 #time steps\n",
2020-07-01 00:21:27 +08:00
"opt_time=[]\n",
"\n",
"x_sim = np.zeros((N,sim_duration))\n",
"u_sim = np.zeros((M,sim_duration-1))\n",
"\n",
2020-10-23 22:18:36 +08:00
"MAX_SPEED = 1.5 #m/s\n",
2020-10-26 19:41:39 +08:00
"MAX_ACC = 1.0 #m/ss\n",
2020-10-26 23:49:51 +08:00
"MAX_D_ACC = 1.0 #m/sss\n",
2020-10-23 22:18:36 +08:00
"MAX_STEER = np.radians(30) #rad\n",
2020-10-26 23:49:51 +08:00
"MAX_D_STEER = np.radians(30) #rad/s\n",
2020-10-26 19:41:39 +08:00
"\n",
"REF_VEL = 1.0 #m/s\n",
2020-07-01 00:21:27 +08:00
"\n",
"# Starting Condition\n",
"x0 = np.zeros(N)\n",
2020-10-23 22:18:36 +08:00
"x0[0] = 0 #x\n",
"x0[1] = -0.25 #y\n",
"x0[2] = 0.0 #v\n",
"x0[3] = np.radians(-0) #yaw\n",
2020-07-01 00:21:27 +08:00
" \n",
"#starting guess\n",
"u_bar = np.zeros((M,T))\n",
2020-10-23 22:18:36 +08:00
"u_bar[0,:] = MAX_ACC/2 #a\n",
2020-10-26 19:41:39 +08:00
"u_bar[1,:] = 0.0 #delta\n",
2020-07-01 00:21:27 +08:00
"\n",
"for sim_time in range(sim_duration-1):\n",
" \n",
2020-10-26 23:49:51 +08:00
" iter_start = time.time()\n",
2020-07-01 00:21:27 +08:00
" \n",
2020-10-23 22:18:36 +08:00
" # dynamics starting state\n",
" x_bar = np.zeros((N,T+1))\n",
" x_bar[:,0] = x_sim[:,sim_time]\n",
2020-07-01 00:21:27 +08:00
" \n",
" #prediction for linearization of costrains\n",
" for t in range (1,T+1):\n",
2020-10-26 23:49:51 +08:00
" xt = x_bar[:,t-1].reshape(N,1)\n",
" ut = u_bar[:,t-1].reshape(M,1)\n",
" A,B,C = get_linear_model(xt,ut)\n",
2020-07-01 00:21:27 +08:00
" xt_plus_one = np.squeeze(np.dot(A,xt)+np.dot(B,ut)+C)\n",
2020-10-26 23:49:51 +08:00
" x_bar[:,t] = xt_plus_one\n",
2020-07-01 00:21:27 +08:00
" \n",
" #CVXPY Linear MPC problem statement\n",
" x = cp.Variable((N, T+1))\n",
" u = cp.Variable((M, T))\n",
2020-10-23 22:18:36 +08:00
" cost = 0\n",
" constr = []\n",
"\n",
" # Cost Matrices\n",
" Q = np.diag([20,20,10,0]) #state error cost\n",
2020-11-17 02:10:18 +08:00
" Qf = np.diag([30,30,30,0]) #state final error cost\n",
2020-10-23 22:18:36 +08:00
" R = np.diag([10,10]) #input cost\n",
" R_ = np.diag([10,10]) #input rate of change cost\n",
"\n",
" #Get Reference_traj\n",
" x_ref, d_ref = get_ref_trajectory(x_bar[:,0] ,track, REF_VEL)\n",
2020-07-01 00:21:27 +08:00
" \n",
" #Prediction Horizon\n",
" for t in range(T):\n",
"\n",
2020-10-23 22:18:36 +08:00
" # Tracking Error\n",
2020-10-26 19:41:39 +08:00
" cost += cp.quad_form(x[:,t] - x_ref[:,t], Q)\n",
2020-10-23 22:18:36 +08:00
"\n",
" # Actuation effort\n",
2020-10-26 19:41:39 +08:00
" cost += cp.quad_form(u[:,t], R)\n",
2020-07-01 00:21:27 +08:00
"\n",
" # Actuation rate of change\n",
" if t < (T - 1):\n",
2020-10-26 19:41:39 +08:00
" cost += cp.quad_form(u[:,t+1] - u[:,t], R_)\n",
2020-10-26 23:49:51 +08:00
" constr+= [cp.abs(u[0, t + 1] - u[0, t])/DT <= MAX_D_ACC] #max acc rate of change\n",
" constr += [cp.abs(u[1, t + 1] - u[1, t])/DT <= MAX_D_STEER] #max steer rate of change\n",
2020-10-23 22:18:36 +08:00
"\n",
2020-07-01 00:21:27 +08:00
" # Kinrmatics Constrains (Linearized model)\n",
2020-10-26 19:41:39 +08:00
" A,B,C = get_linear_model(x_bar[:,t], u_bar[:,t])\n",
2020-07-01 00:21:27 +08:00
" constr += [x[:,t+1] == A@x[:,t] + B@u[:,t] + C.flatten()]\n",
2020-11-17 02:10:18 +08:00
" \n",
" #Final Point tracking\n",
" cost += cp.quad_form(x[:, T] - x_ref[:, T], Qf)\n",
2020-07-01 00:21:27 +08:00
"\n",
" # sums problem objectives and concatenates constraints.\n",
2020-10-23 22:18:36 +08:00
" constr += [x[:,0] == x_bar[:,0]] #starting condition\n",
2020-10-26 19:41:39 +08:00
" constr += [x[2,:] <= MAX_SPEED] #max speed\n",
" constr += [x[2,:] >= 0.0] #min_speed (not really needed)\n",
" constr += [cp.abs(u[0,:]) <= MAX_ACC] #max acc\n",
" constr += [cp.abs(u[1,:]) <= MAX_STEER] #max steer\n",
2020-07-01 00:21:27 +08:00
" \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",
2020-10-26 19:41:39 +08:00
" u_bar = np.vstack((np.array(u.value[0,:]).flatten(),\n",
" (np.array(u.value[1,:]).flatten())))\n",
2020-07-01 00:21:27 +08:00
" \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",
2020-10-23 22:18:36 +08:00
" tspan = [0,DT]\n",
2020-07-01 00:21:27 +08:00
" 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",
2021-03-02 22:25:29 +08:00
"execution_count": 13,
2020-11-17 02:10:18 +08:00
"metadata": {},
"outputs": [
{
"data": {
"image/png": "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
"text/plain": [
"<Figure size 1080x720 with 5 Axes>"
]
},
"metadata": {},
"output_type": "display_data"
}
],
"source": [
"#plot trajectory\n",
"grid = plt.GridSpec(4, 5)\n",
"\n",
"plt.figure(figsize=(15,10))\n",
"\n",
"plt.subplot(grid[0:4, 0:4])\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, 4])\n",
"plt.plot(u_sim[0,:])\n",
"plt.ylabel('a(t) [m/ss]')\n",
"\n",
"plt.subplot(grid[1, 4])\n",
"plt.plot(x_sim[2,:])\n",
"plt.ylabel('v(t) [m/s]')\n",
"\n",
"plt.subplot(grid[2, 4])\n",
"plt.plot(np.degrees(u_sim[1,:]))\n",
"plt.ylabel('delta(t) [rad]')\n",
"\n",
"plt.subplot(grid[3, 4])\n",
"plt.plot(x_sim[3,:])\n",
"plt.ylabel('theta(t) [rad]')\n",
"\n",
"plt.tight_layout()\n",
"plt.show()"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"# Iterative Linearization"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
2021-03-04 18:49:28 +08:00
"The goal is to have a more accurate linearization of the diff equations. For every time step the optimization is iterativelly repeated using he previous optimization results **u_bar** to approximate the vehicle dynamics, instead of a random starting guess and/or the rsult at time t-1.\n",
"\n",
"In previous case the results at t-1 wer used to approimate the dynamics art time t!\n",
"\n",
"This maks the results less correlated but makes the controller slower!"
2020-11-17 02:10:18 +08:00
]
},
{
"cell_type": "code",
2021-03-02 22:25:29 +08:00
"execution_count": 14,
2020-11-17 02:10:18 +08:00
"metadata": {},
"outputs": [
{
"name": "stderr",
"output_type": "stream",
"text": [
"<ipython-input-9-700dd30d89f3>:41: RuntimeWarning: invalid value encountered in true_divide\n",
" v /= np.linalg.norm(v)\n"
]
},
{
"name": "stdout",
"output_type": "stream",
"text": [
2021-04-01 22:26:39 +08:00
"CVXPY Optimization Time: Avrg: 0.5925s Max: 0.8466s Min: 0.2902s\n"
2020-11-17 02:10:18 +08:00
]
}
],
"source": [
"track = compute_path_from_wp([0,3,4,6,10,12,14,6,1,0],\n",
" [0,0,2,4,3,3,-2,-6,-2,-2],0.05)\n",
"\n",
"# track = compute_path_from_wp([0,10,10,0],\n",
"# [0,0,1,1],0.05)\n",
"\n",
"sim_duration = 200 #time steps\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.5 #m/s\n",
"MAX_ACC = 1.0 #m/ss\n",
"MAX_D_ACC = 1.0 #m/sss\n",
"MAX_STEER = np.radians(30) #rad\n",
"MAX_D_STEER = np.radians(30) #rad/s\n",
"\n",
"REF_VEL = 1.0 #m/s\n",
"\n",
"# Starting Condition\n",
"x0 = np.zeros(N)\n",
"x0[0] = 0 #x\n",
"x0[1] = -0.25 #y\n",
"x0[2] = 0.0 #v\n",
"x0[3] = np.radians(-0) #yaw\n",
"\n",
"for sim_time in range(sim_duration-1):\n",
" \n",
" iter_start = time.time()\n",
2021-03-04 18:49:28 +08:00
" \n",
" #starting guess for ctrl\n",
" u_bar = np.zeros((M,T))\n",
" u_bar[0,:] = MAX_ACC/2 #a\n",
" u_bar[1,:] = 0.0 #delta \n",
" \n",
2020-11-17 02:10:18 +08:00
" for _ in range(5):\n",
" u_prev = u_bar\n",
" \n",
" # dynamics starting state\n",
" x_bar = np.zeros((N,T+1))\n",
" x_bar[:,0] = x_sim[:,sim_time]\n",
"\n",
" #prediction for linearization of costrains\n",
" for t in range (1,T+1):\n",
" xt = x_bar[:,t-1].reshape(N,1)\n",
" ut = u_bar[:,t-1].reshape(M,1)\n",
" A,B,C = get_linear_model(xt,ut)\n",
" xt_plus_one = np.squeeze(np.dot(A,xt)+np.dot(B,ut)+C)\n",
" x_bar[:,t] = xt_plus_one\n",
"\n",
" #CVXPY Linear MPC problem statement\n",
" x = cp.Variable((N, T+1))\n",
" u = cp.Variable((M, T))\n",
" cost = 0\n",
" constr = []\n",
"\n",
" # Cost Matrices\n",
" Q = np.diag([20,20,10,0]) #state error cost\n",
" Qf = np.diag([30,30,30,0]) #state final error cost\n",
" R = np.diag([10,10]) #input cost\n",
" R_ = np.diag([10,10]) #input rate of change cost\n",
"\n",
" #Get Reference_traj\n",
" x_ref, d_ref = get_ref_trajectory(x_bar[:,0] ,track, REF_VEL)\n",
"\n",
" #Prediction Horizon\n",
" for t in range(T):\n",
"\n",
" # Tracking Error\n",
" cost += cp.quad_form(x[:,t] - x_ref[:,t], Q)\n",
"\n",
" # Actuation effort\n",
" cost += cp.quad_form(u[:,t], R)\n",
"\n",
" # Actuation rate of change\n",
" if t < (T - 1):\n",
" cost += cp.quad_form(u[:,t+1] - u[:,t], R_)\n",
" constr+= [cp.abs(u[0, t + 1] - u[0, t])/DT <= MAX_D_ACC] #max acc rate of change\n",
" constr += [cp.abs(u[1, t + 1] - u[1, t])/DT <= MAX_D_STEER] #max steer rate of change\n",
"\n",
" # Kinrmatics Constrains (Linearized model)\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",
" #Final Point tracking\n",
" cost += cp.quad_form(x[:, T] - x_ref[:, T], Qf)\n",
"\n",
" # sums problem objectives and concatenates constraints.\n",
" constr += [x[:,0] == x_bar[:,0]] #starting condition\n",
" constr += [x[2,:] <= MAX_SPEED] #max speed\n",
" constr += [x[2,:] >= 0.0] #min_speed (not really needed)\n",
" constr += [cp.abs(u[0,:]) <= MAX_ACC] #max acc\n",
" constr += [cp.abs(u[1,:]) <= MAX_STEER] #max steer\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",
" #check how this solution differs from previous\n",
2021-03-04 18:49:28 +08:00
" #if the solutions are very\n",
2020-11-17 02:10:18 +08:00
" delta_u = np.sum(np.sum(np.abs(u_bar - u_prev),axis=0),axis=0)\n",
2021-03-04 18:49:28 +08:00
" if delta_u < 0.05:\n",
2020-11-17 02:10:18 +08:00
" break\n",
" \n",
" \n",
" # select u from best iteration\n",
" u_sim[:,sim_time] = u_bar[:,0]\n",
" \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",
" #reset u_bar? -> this simulates that we don use previous solution!\n",
" u_bar[0,:] = MAX_ACC/2 #a\n",
" u_bar[1,:] = 0.0 #delta\n",
" \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",
2021-03-02 22:25:29 +08:00
"execution_count": 15,
2020-07-01 00:21:27 +08:00
"metadata": {},
2020-10-23 22:18:36 +08:00
"outputs": [
{
"data": {
2021-03-04 18:49:28 +08:00
"image/png": "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
2020-10-23 22:18:36 +08:00
"text/plain": [
"<Figure size 1080x720 with 5 Axes>"
]
},
"metadata": {},
"output_type": "display_data"
}
],
2020-07-01 00:21:27 +08:00
"source": [
"#plot trajectory\n",
2020-07-01 17:45:08 +08:00
"grid = plt.GridSpec(4, 5)\n",
2020-07-01 00:21:27 +08:00
"\n",
"plt.figure(figsize=(15,10))\n",
"\n",
2020-07-01 17:45:08 +08:00
"plt.subplot(grid[0:4, 0:4])\n",
2020-07-01 00:21:27 +08:00
"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",
2020-07-01 17:45:08 +08:00
"plt.subplot(grid[0, 4])\n",
2020-07-01 00:21:27 +08:00
"plt.plot(u_sim[0,:])\n",
2020-07-01 17:45:08 +08:00
"plt.ylabel('a(t) [m/ss]')\n",
"\n",
"plt.subplot(grid[1, 4])\n",
"plt.plot(x_sim[2,:])\n",
2020-07-01 00:21:27 +08:00
"plt.ylabel('v(t) [m/s]')\n",
"\n",
2020-07-01 17:45:08 +08:00
"plt.subplot(grid[2, 4])\n",
2020-07-01 00:21:27 +08:00
"plt.plot(np.degrees(u_sim[1,:]))\n",
2020-10-23 23:31:57 +08:00
"plt.ylabel('delta(t) [rad]')\n",
2020-07-01 17:45:08 +08:00
"\n",
"plt.subplot(grid[3, 4])\n",
"plt.plot(x_sim[3,:])\n",
2020-10-23 23:31:57 +08:00
"plt.ylabel('theta(t) [rad]')\n",
2020-07-01 00:21:27 +08:00
"\n",
"plt.tight_layout()\n",
"plt.show()"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
2021-04-01 00:20:10 +08:00
"## V2 Use Dynamics w.r.t Robot Frame\n",
2021-03-02 22:25:29 +08:00
"\n",
"explanation here...\n",
"\n",
"benefits:\n",
2021-03-04 18:49:28 +08:00
"* slightly faster mpc convergence time -> more variables are 0, this helps the computation?\n",
"* no issues when vehicle heading ~PI in world"
2021-03-02 22:25:29 +08:00
]
},
{
"cell_type": "code",
2021-04-01 22:26:39 +08:00
"execution_count": 16,
2021-03-02 22:25:29 +08:00
"metadata": {},
"outputs": [],
"source": [
"def compute_path_from_wp(start_xp, start_yp, step = 0.1):\n",
" \"\"\"\n",
" Computes a reference path given a set of waypoints\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,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",
2021-04-01 00:20:10 +08:00
" # watch out to duplicate points!\n",
" final_xp=np.append(final_xp,fx(interp_range)[1:])\n",
" final_yp=np.append(final_yp,fy(interp_range)[1:])\n",
2021-03-02 22:25:29 +08:00
" \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,theta))\n",
"\n",
"\n",
"def get_nn_idx(state,path):\n",
" \"\"\"\n",
" Computes the index of the waypoint closest to vehicle\n",
" \"\"\"\n",
"\n",
" dx = state[0]-path[0,:]\n",
" dy = state[1]-path[1,:]\n",
" dist = np.hypot(dx,dy)\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 normalize_angle(angle):\n",
" \"\"\"\n",
" Normalize an angle to [-pi, pi]\n",
" \"\"\"\n",
" while angle > np.pi:\n",
" angle -= 2.0 * np.pi\n",
"\n",
" while angle < -np.pi:\n",
" angle += 2.0 * np.pi\n",
"\n",
" return angle\n",
"\n",
"def get_ref_trajectory(state, path, target_v):\n",
" \"\"\"\n",
" modified reference in robot frame\n",
" \"\"\"\n",
" xref = np.zeros((N, T + 1))\n",
" dref = np.zeros((1, T + 1))\n",
" \n",
" #sp = np.ones((1,T +1))*target_v #speed profile\n",
" \n",
" ncourse = path.shape[1]\n",
"\n",
" ind = get_nn_idx(state, path)\n",
" dx=path[0,ind] - state[0]\n",
" dy=path[1,ind] - state[1]\n",
" \n",
" xref[0, 0] = dx * np.cos(-state[3]) - dy * np.sin(-state[3]) #X\n",
" xref[1, 0] = dy * np.cos(-state[3]) + dx * np.sin(-state[3]) #Y\n",
" xref[2, 0] = target_v #V\n",
" xref[3, 0] = normalize_angle(path[2,ind]- state[3]) #Theta\n",
" dref[0, 0] = 0.0 # steer operational point should be 0\n",
" \n",
" dl = 0.05 # Waypoints spacing [m]\n",
" travel = 0.0\n",
" \n",
" for i in range(T + 1):\n",
" travel += abs(target_v) * DT #current V or target V?\n",
" dind = int(round(travel / dl))\n",
" \n",
" if (ind + dind) < ncourse:\n",
" dx=path[0,ind + dind] - state[0]\n",
" dy=path[1,ind + dind] - state[1]\n",
" \n",
" xref[0, i] = dx * np.cos(-state[3]) - dy * np.sin(-state[3])\n",
" xref[1, i] = dy * np.cos(-state[3]) + dx * np.sin(-state[3])\n",
" xref[2, i] = target_v #sp[ind + dind]\n",
" xref[3, i] = normalize_angle(path[2,ind + dind] - state[3])\n",
" dref[0, i] = 0.0\n",
" else:\n",
" dx=path[0,ncourse - 1] - state[0]\n",
" dy=path[1,ncourse - 1] - state[1]\n",
" \n",
" xref[0, i] = dx * np.cos(-state[3]) - dy * np.sin(-state[3])\n",
" xref[1, i] = dy * np.cos(-state[3]) + dx * np.sin(-state[3])\n",
" xref[2, i] = 0.0 #stop? #sp[ncourse - 1]\n",
" xref[3, i] = normalize_angle(path[2,ncourse - 1] - state[3])\n",
" dref[0, i] = 0.0\n",
"\n",
" return xref, dref"
]
},
{
"cell_type": "code",
2021-03-04 18:49:28 +08:00
"execution_count": 17,
2021-03-02 22:25:29 +08:00
"metadata": {},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
2021-04-01 22:26:39 +08:00
"CVXPY Optimization Time: Avrg: 0.1631s Max: 0.1989s Min: 0.1458s\n"
2021-03-02 22:25:29 +08:00
]
}
],
"source": [
"track = compute_path_from_wp([0,3,4,6,10,12,14,6,1,0],\n",
" [0,0,2,4,3,3,-2,-6,-2,-2],0.05)\n",
"\n",
"# track = compute_path_from_wp([0,10,10,0],\n",
"# [0,0,1,1],0.05)\n",
"\n",
"sim_duration = 200 #time steps\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.5 #m/s\n",
"MAX_ACC = 1.0 #m/ss\n",
"MAX_D_ACC = 1.0 #m/sss\n",
"MAX_STEER = np.radians(30) #rad\n",
"MAX_D_STEER = np.radians(30) #rad/s\n",
"\n",
"REF_VEL = 1.0 #m/s\n",
"\n",
"# Starting Condition\n",
"x0 = np.zeros(N)\n",
"x0[0] = 0 #x\n",
"x0[1] = -0.25 #y\n",
"x0[2] = 0.0 #v\n",
"x0[3] = np.radians(-0) #yaw\n",
" \n",
"#starting guess\n",
"u_bar = np.zeros((M,T))\n",
"u_bar[0,:] = MAX_ACC/2 #a\n",
"u_bar[1,:] = 0.0 #delta\n",
2021-03-04 18:49:28 +08:00
" \n",
2021-03-02 22:25:29 +08:00
"for sim_time in range(sim_duration-1):\n",
" \n",
" iter_start = time.time()\n",
" \n",
" # dynamics starting state w.r.t. robot are always null except vel \n",
" x_bar = np.zeros((N,T+1))\n",
" x_bar[2,0] = x_sim[2,sim_time]\n",
" \n",
" #prediction for linearization of costrains\n",
" for t in range (1,T+1):\n",
" xt = x_bar[:,t-1].reshape(N,1)\n",
" ut = u_bar[:,t-1].reshape(M,1)\n",
" A,B,C = get_linear_model(xt,ut)\n",
" xt_plus_one = np.squeeze(np.dot(A,xt)+np.dot(B,ut)+C)\n",
" x_bar[:,t] = xt_plus_one\n",
" \n",
" #CVXPY Linear MPC problem statement\n",
" x = cp.Variable((N, T+1))\n",
" u = cp.Variable((M, T))\n",
" cost = 0\n",
" constr = []\n",
"\n",
" # Cost Matrices\n",
2021-03-04 18:49:28 +08:00
" Q = np.diag([20,20,10,20]) #state error cost\n",
" Qf = np.diag([30,30,30,30]) #state final error cost\n",
2021-03-02 22:25:29 +08:00
" R = np.diag([10,10]) #input cost\n",
" R_ = np.diag([10,10]) #input rate of change cost\n",
"\n",
" #Get Reference_traj\n",
" #dont use x0 in this case\n",
" x_ref, d_ref = get_ref_trajectory(x_sim[:,sim_time] ,track, REF_VEL)\n",
" \n",
" #Prediction Horizon\n",
" for t in range(T):\n",
"\n",
" # Tracking Error\n",
" cost += cp.quad_form(x[:,t] - x_ref[:,t], Q)\n",
"\n",
" # Actuation effort\n",
" cost += cp.quad_form(u[:,t], R)\n",
"\n",
" # Actuation rate of change\n",
" if t < (T - 1):\n",
" cost += cp.quad_form(u[:,t+1] - u[:,t], R_)\n",
" constr+= [cp.abs(u[0, t + 1] - u[0, t])/DT <= MAX_D_ACC] #max acc rate of change\n",
" constr += [cp.abs(u[1, t + 1] - u[1, t])/DT <= MAX_D_STEER] #max steer rate of change\n",
"\n",
" # Kinrmatics Constrains (Linearized model)\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",
" #Final Point tracking\n",
" cost += cp.quad_form(x[:, T] - x_ref[:, T], Qf)\n",
"\n",
" # sums problem objectives and concatenates constraints.\n",
" constr += [x[:,0] == x_bar[:,0]] #starting condition\n",
" constr += [x[2,:] <= MAX_SPEED] #max speed\n",
" constr += [x[2,:] >= 0.0] #min_speed (not really needed)\n",
" constr += [cp.abs(u[0,:]) <= MAX_ACC] #max acc\n",
" constr += [cp.abs(u[1,:]) <= MAX_STEER] #max steer\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",
2021-03-04 18:49:28 +08:00
"execution_count": 18,
2021-03-02 22:25:29 +08:00
"metadata": {},
"outputs": [
{
"data": {
2021-04-01 22:26:39 +08:00
"image/png": "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
2021-03-02 22:25:29 +08:00
"text/plain": [
"<Figure size 1080x720 with 5 Axes>"
]
},
"metadata": {},
"output_type": "display_data"
}
],
"source": [
"#plot trajectory\n",
"grid = plt.GridSpec(4, 5)\n",
"\n",
"plt.figure(figsize=(15,10))\n",
"\n",
"plt.subplot(grid[0:4, 0:4])\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, 4])\n",
"plt.plot(u_sim[0,:])\n",
"plt.ylabel('a(t) [m/ss]')\n",
"\n",
"plt.subplot(grid[1, 4])\n",
"plt.plot(x_sim[2,:])\n",
"plt.ylabel('v(t) [m/s]')\n",
"\n",
"plt.subplot(grid[2, 4])\n",
"plt.plot(np.degrees(u_sim[1,:]))\n",
"plt.ylabel('delta(t) [rad]')\n",
"\n",
"plt.subplot(grid[3, 4])\n",
"plt.plot(x_sim[3,:])\n",
"plt.ylabel('theta(t) [rad]')\n",
"\n",
"plt.tight_layout()\n",
"plt.show()"
2020-07-01 00:21:27 +08:00
]
},
2021-04-01 00:20:10 +08:00
{
"cell_type": "markdown",
"metadata": {},
"source": [
"## V3 Add track constraints\n",
"inspried from -> https://arxiv.org/pdf/1711.07300.pdf\n",
"\n",
"explanation here...\n",
"\n",
"benefits:\n",
"* add a soft form of obstacle aoidance"
]
},
{
"cell_type": "code",
2021-04-01 22:26:39 +08:00
"execution_count": 19,
2021-04-01 00:20:10 +08:00
"metadata": {},
"outputs": [
{
"data": {
"text/plain": [
2021-04-01 22:26:39 +08:00
"[<matplotlib.lines.Line2D at 0x7f456f654400>]"
2021-04-01 00:20:10 +08:00
]
},
2021-04-01 22:26:39 +08:00
"execution_count": 19,
2021-04-01 00:20:10 +08:00
"metadata": {},
"output_type": "execute_result"
},
{
"data": {
2021-04-01 22:26:39 +08:00
"image/png": "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
2021-04-01 00:20:10 +08:00
"text/plain": [
"<Figure size 1080x720 with 1 Axes>"
]
},
"metadata": {},
"output_type": "display_data"
}
],
"source": [
2021-04-01 22:26:39 +08:00
"def generate_track_bounds(track,width=0.5):\n",
2021-04-01 00:20:10 +08:00
" \"\"\"\n",
" in world frame\n",
" \"\"\"\n",
" bounds_low=np.zeros((2, track.shape[1]))\n",
" bounds_upp=np.zeros((2, track.shape[1]))\n",
" \n",
" for idx in range(track.shape[1]):\n",
" x = track[0,idx]\n",
" y = track [1,idx]\n",
" th = track [2,idx]\n",
" \n",
" \"\"\"\n",
" trasform the points\n",
" \"\"\"\n",
" bounds_upp[0, idx] = 0 * np.cos(th) - width * np.sin(th) + x #X\n",
" bounds_upp[1, idx] = 0 * np.sin(th) + width * np.cos(th) + y #Y\n",
" \n",
" bounds_low[0, idx] = 0 * np.cos(th) - (-width) * np.sin(th) + x #X\n",
" bounds_low[1, idx] = 0 * np.sin(th) + (-width) * np.cos(th) + y #Y\n",
" \n",
" return bounds_low, bounds_upp\n",
"\n",
"track = compute_path_from_wp([0,3,4,6,10,12,14,6,1,0],\n",
" [0,0,2,4,3,3,-2,-6,-2,-2],0.05)\n",
"\n",
"lower, upper = generate_track_bounds(track)\n",
"\n",
"plt.figure(figsize=(15,10))\n",
"\n",
2021-04-01 22:26:39 +08:00
"plt.plot(track[0,:],track[1,:],\"b-\")\n",
"plt.plot(lower[0,:],lower[1,:],\"g-\")\n",
"plt.plot(upper[0,:],upper[1,:],\"r-\")"
2021-04-01 00:20:10 +08:00
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"the points can be used to generate the **halfplane constrains** for each reference point.\n",
"the issues (outliers points) should be gone after we are in vehicle frame...\n",
"\n",
"the halfplane constrains are defined given the line equation:\n",
"\n",
"**lower halfplane**\n",
"$$ a1x_1 + b1x_2 = c1 \\rightarrow a1x_1 + b1x_2 \\leq c1$$\n",
"\n",
"**upper halfplane**\n",
"$$ a2x_1 - b2x_2 = c2 \\rightarrow a2x_1 + b2x_2 \\leq c2$$\n",
"\n",
"we want to combine this in matrix form:\n",
"\n",
"$$\n",
"\\begin{bmatrix}\n",
"x_1 \\\\\n",
"x_2 \n",
"\\end{bmatrix}\n",
"\\begin{bmatrix}\n",
"a_1 & a_2\\\\\n",
"b_1 & b_2\n",
"\\end{bmatrix}\n",
"\\leq\n",
"\\begin{bmatrix}\n",
"c_1 \\\\\n",
"c_2 \n",
"\\end{bmatrix}\n",
"$$\n",
"\n",
"becouse our track points have known heading the coefficients can be computed from:\n",
"\n",
"$$ y - y' = \\frac{sin(\\theta)}{cos(\\theta)}(x - x') $$"
]
},
2021-04-01 22:26:39 +08:00
{
"cell_type": "code",
"execution_count": 53,
"metadata": {},
"outputs": [
{
"data": {
"text/plain": [
"(-1.0, 21.0, -0.21100000000000002, -0.189)"
]
},
"execution_count": 53,
"metadata": {},
"output_type": "execute_result"
},
{
"data": {
"image/png": "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
"text/plain": [
"<Figure size 360x360 with 1 Axes>"
]
},
"metadata": {},
"output_type": "display_data"
}
],
"source": [
"import numpy as np\n",
"import matplotlib.pyplot as plt\n",
"plt.style.use(\"ggplot\")\n",
"\n",
"def get_coeff(x,y,theta):\n",
" m = np.sin(theta)/np.cos(theta)\n",
" return(-m,1,y-m*x)\n",
"\n",
"#test -> assume point 10,1,pi/6\n",
"coeff = get_coeff(1,-0.2, 0.0)\n",
"y = []\n",
"pts = np.linspace(0,20,100)\n",
"\n",
"for x in pts:\n",
" y.append((-coeff[0]*x+coeff[2])/coeff[1])\n",
" \n",
"plt.figure(figsize=(5,5))\n",
"\n",
"plt.plot(pts,y,\"b-\")\n",
"plt.axis(\"equal\")"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"becouse the controller uses vhicle reference frame this rquire adapting -> the semiplane constraints must be gathetered from **x_ref points**\n",
"\n",
"*low and up are w.r.t vehicle y axis*"
]
},
{
"cell_type": "code",
"execution_count": 21,
"metadata": {},
"outputs": [],
"source": [
"def get_track_constrains(x_ref, width=0.5):\n",
" \"\"\"\n",
" x_ref has hape (4,T) -> [x,y,v,theta]_ref \n",
" \"\"\"\n",
" \n",
" #1-> get the upper and lower points\n",
" pts_low=np.zeros((3, x_ref.shape[1]))\n",
" pts_upp=np.zeros((3, x_ref.shape[1]))\n",
" \n",
" for idx in range(x_ref.shape[1]):\n",
" x = x_ref [0, idx]\n",
" y = x_ref [1, idx]\n",
" th = x_ref [3, idx]\n",
" \n",
" \"\"\"\n",
" trasform the points\n",
" \"\"\"\n",
" pts_upp[0, idx] = 0 * np.cos(th) - width * np.sin(th) + x #X\n",
" pts_upp[1, idx] = 0 * np.sin(th) + width * np.cos(th) + y #Y\n",
" pts_upp[2, idx] = th #heading\n",
" \n",
" pts_low[0, idx] = 0 * np.cos(th) - (-width) * np.sin(th) + x #X\n",
" pts_low[1, idx] = 0 * np.sin(th) + (-width) * np.cos(th) + y #Y\n",
" pts_low[2, idx] = th #heading\n",
" \n",
" #get coefficients ->(a,b,c)\n",
" coeff_low=np.zeros((3, x_ref.shape[1]))\n",
" coeff_upp=np.zeros((3, x_ref.shape[1]))\n",
" \n",
" for idx in range(pts_upp.shape[1]):\n",
" f = get_coeff(pts_low[0,idx],pts_low[1,idx],pts_low[2,idx])\n",
" coeff_low[0,idx]=f[0]\n",
" coeff_low[1,idx]=f[1]\n",
" coeff_low[2,idx]=f[2]\n",
" \n",
" f = get_coeff(pts_upp[0,idx],pts_upp[1,idx],pts_upp[2,idx])\n",
" coeff_upp[0,idx]=f[0]\n",
" coeff_upp[1,idx]=f[1]\n",
" coeff_upp[2,idx]=f[2]\n",
" \n",
" return coeff_low, coeff_upp\n",
" "
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"simpe u-turn test\n",
"\n",
"## 1-> NO BOUNDS"
]
},
{
"cell_type": "code",
"execution_count": 84,
"metadata": {},
"outputs": [
{
"name": "stderr",
"output_type": "stream",
"text": [
"/home/marcello/.conda/envs/jupyter/lib/python3.8/site-packages/ipykernel/ipkernel.py:287: DeprecationWarning: `should_run_async` will not call `transform_cell` automatically in the future. Please pass the result to `transformed_cell` argument and any exception that happen during thetransform in `preprocessing_exc_tuple` in IPython 7.17 and above.\n",
" and should_run_async(code)\n"
]
},
{
"name": "stdout",
"output_type": "stream",
"text": [
"CVXPY Optimization Time: Avrg: 0.1657s Max: 0.2079s Min: 0.1469s\n"
]
}
],
"source": [
"track = compute_path_from_wp([0,3,3,0],\n",
" [0,0,1,1],0.05)\n",
"\n",
"track_lower, track_upper = generate_track_bounds(track,0.12)\n",
"\n",
"sim_duration = 50 #time steps\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.5 #m/s\n",
"MAX_ACC = 1.0 #m/ss\n",
"MAX_D_ACC = 1.0 #m/sss\n",
"MAX_STEER = np.radians(30) #rad\n",
"MAX_D_STEER = np.radians(30) #rad/s\n",
"\n",
"REF_VEL = 1.0 #m/s\n",
"\n",
"# Starting Condition\n",
"x0 = np.zeros(N)\n",
"x0[0] = 0 #x\n",
"x0[1] = -0.25 #y\n",
"x0[2] = 0.0 #v\n",
"x0[3] = np.radians(-0) #yaw\n",
" \n",
"#starting guess\n",
"u_bar = np.zeros((M,T))\n",
"u_bar[0,:] = MAX_ACC/2 #a\n",
"u_bar[1,:] = 0.0 #delta\n",
" \n",
"for sim_time in range(sim_duration-1):\n",
" \n",
" iter_start = time.time()\n",
" \n",
" # dynamics starting state w.r.t. robot are always null except vel \n",
" x_bar = np.zeros((N,T+1))\n",
" x_bar[2,0] = x_sim[2,sim_time]\n",
" \n",
" #prediction for linearization of costrains\n",
" for t in range (1,T+1):\n",
" xt = x_bar[:,t-1].reshape(N,1)\n",
" ut = u_bar[:,t-1].reshape(M,1)\n",
" A,B,C = get_linear_model(xt,ut)\n",
" xt_plus_one = np.squeeze(np.dot(A,xt)+np.dot(B,ut)+C)\n",
" x_bar[:,t] = xt_plus_one\n",
" \n",
" #CVXPY Linear MPC problem statement\n",
" x = cp.Variable((N, T+1))\n",
" u = cp.Variable((M, T))\n",
" cost = 0\n",
" constr = []\n",
"\n",
" # Cost Matrices\n",
" Q = np.diag([20,20,10,20]) #state error cost\n",
" Qf = np.diag([30,30,30,30]) #state final error cost\n",
" R = np.diag([10,10]) #input cost\n",
" R_ = np.diag([10,10]) #input rate of change cost\n",
"\n",
" #Get Reference_traj\n",
" #dont use x0 in this case\n",
" x_ref, d_ref = get_ref_trajectory(x_sim[:,sim_time] ,track, REF_VEL)\n",
" \n",
" #Prediction Horizon\n",
" for t in range(T):\n",
"\n",
" # Tracking Error\n",
" cost += cp.quad_form(x[:,t] - x_ref[:,t], Q)\n",
"\n",
" # Actuation effort\n",
" cost += cp.quad_form(u[:,t], R)\n",
"\n",
" # Actuation rate of change\n",
" if t < (T - 1):\n",
" cost += cp.quad_form(u[:,t+1] - u[:,t], R_)\n",
" constr+= [cp.abs(u[0, t + 1] - u[0, t])/DT <= MAX_D_ACC] #max acc rate of change\n",
" constr += [cp.abs(u[1, t + 1] - u[1, t])/DT <= MAX_D_STEER] #max steer rate of change\n",
"\n",
" # Kinrmatics Constrains (Linearized model)\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",
" #Final Point tracking\n",
" cost += cp.quad_form(x[:, T] - x_ref[:, T], Qf)\n",
"\n",
" # sums problem objectives and concatenates constraints.\n",
" constr += [x[:,0] == x_bar[:,0]] #starting condition\n",
" constr += [x[2,:] <= MAX_SPEED] #max speed\n",
" constr += [x[2,:] >= 0.0] #min_speed (not really needed)\n",
" constr += [cp.abs(u[0,:]) <= MAX_ACC] #max acc\n",
" constr += [cp.abs(u[1,:]) <= MAX_STEER] #max steer\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": 85,
"metadata": {},
"outputs": [
{
"data": {
"image/png": "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
"text/plain": [
"<Figure size 1080x720 with 5 Axes>"
]
},
"metadata": {},
"output_type": "display_data"
}
],
"source": [
"#plot trajectory\n",
"grid = plt.GridSpec(4, 5)\n",
"\n",
"plt.figure(figsize=(15,10))\n",
"\n",
"plt.subplot(grid[0:4, 0:4])\n",
"plt.plot(track[0,:],track[1,:],\"b+\")\n",
"plt.plot(track_lower[0,:],track_lower[1,:],\"g-\")\n",
"plt.plot(track_upper[0,:],track_upper[1,:],\"r-\")\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, 4])\n",
"plt.plot(u_sim[0,:])\n",
"plt.ylabel('a(t) [m/ss]')\n",
"\n",
"plt.subplot(grid[1, 4])\n",
"plt.plot(x_sim[2,:])\n",
"plt.ylabel('v(t) [m/s]')\n",
"\n",
"plt.subplot(grid[2, 4])\n",
"plt.plot(np.degrees(u_sim[1,:]))\n",
"plt.ylabel('delta(t) [rad]')\n",
"\n",
"plt.subplot(grid[3, 4])\n",
"plt.plot(x_sim[3,:])\n",
"plt.ylabel('theta(t) [rad]')\n",
"\n",
"plt.tight_layout()\n",
"plt.show()"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"## 2-> WITH BOUNDS\n",
"if there is 90 deg turn the optimization fails!\n",
"if speed is too high it also fails ..."
]
},
{
"cell_type": "code",
"execution_count": 86,
"metadata": {},
"outputs": [
{
"name": "stderr",
"output_type": "stream",
"text": [
"/home/marcello/.conda/envs/jupyter/lib/python3.8/site-packages/ipykernel/ipkernel.py:287: DeprecationWarning: `should_run_async` will not call `transform_cell` automatically in the future. Please pass the result to `transformed_cell` argument and any exception that happen during thetransform in `preprocessing_exc_tuple` in IPython 7.17 and above.\n",
" and should_run_async(code)\n"
]
}
],
"source": [
"from scipy.signal import savgol_filter\n",
"def compute_path_from_wp(start_xp, start_yp, step = 0.1):\n",
" \"\"\"\n",
" Computes a reference path given a set of waypoints\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,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",
" # watch out to duplicate points!\n",
" final_xp=np.append(final_xp,fx(interp_range)[1:])\n",
" final_yp=np.append(final_yp,fy(interp_range)[1:])\n",
" \n",
" \"\"\"this smoothens up corners\"\"\"\n",
" window_size = 11 # Smoothening filter window\n",
" final_xp = savgol_filter(final_xp, window_size, 1)\n",
" final_yp = savgol_filter(final_yp, window_size, 1)\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,theta))\n"
]
},
{
"cell_type": "code",
"execution_count": 87,
"metadata": {},
"outputs": [
{
"name": "stderr",
"output_type": "stream",
"text": [
"/home/marcello/.conda/envs/jupyter/lib/python3.8/site-packages/cvxpy/problems/problem.py:1054: UserWarning: Solution may be inaccurate. Try another solver, adjusting the solver settings, or solve with verbose=True for more information.\n",
" warnings.warn(\n"
]
},
{
"name": "stdout",
"output_type": "stream",
"text": [
"CVXPY Optimization Time: Avrg: 0.1735s Max: 0.2559s Min: 0.1568s\n"
]
}
],
"source": [
"WIDTH=0.12\n",
"track = compute_path_from_wp([0,3,3,0],\n",
" [0,0,1,1],0.05)\n",
"\n",
"track_lower, track_upper = generate_track_bounds(track,WIDTH)\n",
"\n",
"sim_duration = 200 #time steps\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.5 #m/s\n",
"MAX_ACC = 1.0 #m/ss\n",
"MAX_D_ACC = 1.0 #m/sss\n",
"MAX_STEER = np.radians(30) #rad\n",
"MAX_D_STEER = np.radians(30) #rad/s\n",
"\n",
"REF_VEL = 0.4 #m/s\n",
"\n",
"# Starting Condition\n",
"x0 = np.zeros(N)\n",
"x0[0] = 0 #x\n",
"x0[1] = -WIDTH/2 #y\n",
"x0[2] = 0.0 #v\n",
"x0[3] = np.radians(-0) #yaw\n",
" \n",
"#starting guess\n",
"u_bar = np.zeros((M,T))\n",
"u_bar[0,:] = MAX_ACC/2 #a\n",
"u_bar[1,:] = 0.0 #delta\n",
" \n",
"for sim_time in range(sim_duration-1):\n",
" \n",
" iter_start = time.time()\n",
" \n",
" # dynamics starting state w.r.t. robot are always null except vel \n",
" x_bar = np.zeros((N,T+1))\n",
" x_bar[2,0] = x_sim[2,sim_time]\n",
" \n",
" #prediction for linearization of costrains\n",
" for t in range (1,T+1):\n",
" xt = x_bar[:,t-1].reshape(N,1)\n",
" ut = u_bar[:,t-1].reshape(M,1)\n",
" A,B,C = get_linear_model(xt,ut)\n",
" xt_plus_one = np.squeeze(np.dot(A,xt)+np.dot(B,ut)+C)\n",
" x_bar[:,t] = xt_plus_one\n",
" \n",
" #CVXPY Linear MPC problem statement\n",
" x = cp.Variable((N, T+1))\n",
" u = cp.Variable((M, T))\n",
" cost = 0\n",
" constr = []\n",
"\n",
" # Cost Matrices\n",
" Q = np.diag([20,20,10,20]) #state error cost\n",
" Qf = np.diag([30,30,30,30]) #state final error cost\n",
" R = np.diag([10,10]) #input cost\n",
" R_ = np.diag([10,10]) #input rate of change cost\n",
"\n",
" #Get Reference_traj\n",
" #dont use x0 in this case\n",
" x_ref, d_ref = get_ref_trajectory(x_sim[:,sim_time] ,track, REF_VEL)\n",
" \n",
" #Prediction Horizon\n",
" for t in range(T):\n",
"\n",
" # Tracking Error\n",
" cost += cp.quad_form(x[:,t] - x_ref[:,t], Q)\n",
"\n",
" # Actuation effort\n",
" cost += cp.quad_form(u[:,t], R)\n",
"\n",
" # Actuation rate of change\n",
" if t < (T - 1):\n",
" cost += cp.quad_form(u[:,t+1] - u[:,t], R_)\n",
" constr+= [cp.abs(u[0, t + 1] - u[0, t])/DT <= MAX_D_ACC] #max acc rate of change\n",
" constr += [cp.abs(u[1, t + 1] - u[1, t])/DT <= MAX_D_STEER] #max steer rate of change\n",
"\n",
" # Kinrmatics Constrains (Linearized model)\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",
" #Final Point tracking\n",
" cost += cp.quad_form(x[:, T] - x_ref[:, T], Qf)\n",
"\n",
" # sums problem objectives and concatenates constraints.\n",
" constr += [x[:,0] == x_bar[:,0]] #starting condition\n",
" constr += [x[2,:] <= MAX_SPEED] #max speed\n",
" constr += [x[2,:] >= 0.0] #min_speed (not really needed)\n",
" constr += [cp.abs(u[0,:]) <= MAX_ACC] #max acc\n",
" constr += [cp.abs(u[1,:]) <= MAX_STEER] #max steer\n",
" \n",
" #Track constrains\n",
" low,upp = get_track_constrains(x_ref,WIDTH)\n",
" for ii in range(low.shape[1]):\n",
" constr += [low[0,ii]*x[0,ii] + x[1,ii] >= low[2,ii]]\n",
" #constr += [upp[0,ii]*x[0,ii] + x[1,ii] <= upp[2,ii]]\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": 88,
"metadata": {},
"outputs": [
{
"data": {
"image/png": "iVBORw0KGgoAAAANSUhEUgAABDAAAALICAYAAACJhQBYAAAAOXRFWHRTb2Z0d2FyZQBNYXRwbG90bGliIHZlcnNpb24zLjMuMSwgaHR0cHM6Ly9tYXRwbG90bGliLm9yZy/d3fzzAAAACXBIWXMAAAsTAAALEwEAmpwYAADPvElEQVR4nOzde3xT9f0/8NfnJGna0gttAr0BKjcFRaVGIXhBpDInzrFNrXO6IducQ2UC8lUURKcoEwsK4m0y3PbdpboL7qtzsooDB/FnM8AbDqiiciktvUHpNcn5/P44TdrQtLRpkpOevp6PRx7JOed9zueTkxbNu5/P+yOklBJERERERERERHFM0bsDRERERERERESnwgQGEREREREREcU9JjCIiIiIiIiIKO4xgUFEREREREREcY8JDCIiIiIiIiKKe0xgEBEREREREVHcM+vdgVg4fPhwzNqy2+2oqqqKWXsUG/xcjYmfqzHxczUmfq7GxM/VmIz8uebm5urdhbgR6juWkT/7cPGeBAt1P3rze8URGEREREREREQU95jAICIiIiIiIqK4xwQGEREREREREcW9AVEDg4iIiIiIaKDZtWsXNmzYAFVVMX36dMyaNSvoeGNjI9asWYPq6mr4fD584xvfwLRp0/TpLFEPcAQGERERERGRwaiqivXr1+P+++/H6tWrsW3bNhw8eDAo5h//+AeGDRuGlStX4qGHHsJvfvMbeL3esNqTO7aj5oG5kI0Nkeg+UUhMYBARERERERlMWVkZsrOzkZWVBbPZjClTpqC0tDQoRgiB5uZmSCnR3NyMlJQUKEp4XxHlsTp4du8CvK0R6D1RaJxCQkREREREZDA1NTWw2WyBbZvNhn379gXFXHXVVXjiiSfwk5/8BE1NTZg/f36XCYySkhKUlJQAAFasWAG73R50vDE9HfUAMtPSYTrp2EBmNps73auBrK/3gwkMIiIiIiIig5FSdtonhAja/uCDD3DaaafhwQcfREVFBR555BGcddZZSE5O7nRuQUEBCgoKAttVVVVBx9WmZgBAzdFKCA70D7Db7Z3u1UAW6n7k5ub2+Hz+ZBERERERERmMzWZDdXV1YLu6uhoZGRlBMe+88w4mTZoEIQSys7MxdOhQHD58OLwGTSbt2ecLt8tEp8QEBhERERERkcGMGjUK5eXlqKyshNfrxfbt2+FwOIJi7HY7PvroIwBAXV0dDh8+jKFDh4bXoD+BoTKBQdHDKSREREREREQGYzKZMGfOHCxfvhyqqmLatGkYPnw4Nm3aBACYMWMGvvOd7+DZZ5/FwoULAQDf+973kJaWFlZ7wmSCBDgCg6KKCQwiIiIiIiIDys/PR35+ftC+GTNmBF5nZmZiyZIlkWnM1PbV0hfeMqxEPcEpJERERERERNQ3rIFBMcAEBhEREREREfUNR2BQDDCBQURERERERH3DERgUA0xgEBERERERUd8ERmAwgUHRwwQGERERERER9Y3CERgUfUxgEBERERERUd8EppCwBgZFDxMYRERERERE1DdtU0gkR2BQFDGBQURERERERH3DERgUA0xgEBERERERUd9wFRKKASYwiIiIiIiIqG8Cq5BwBAZFDxMYRERERERE1Df+ERgqR2BQ9DCBQURERERERH3DKSQUA0xgEBERERERUd9wCgnFABMYRERERERE1DccgUExwAQGERERERER9Q1HYFAMMIFBREREREREfaO0fbXkCAyKIrPeHSAiIiIiIqLI27VrFzZs2ABVVTF9+nTMmjWrU8wnn3yCl19+GT6fD6mpqXj44YfDaksIoU0jYQKDoogJDCIiIiIiIoNRVRXr16/HkiVLYLPZsHjxYjgcDgwbNiwQ09DQgJdeegkPPPAA7HY7jh071rdGTWYmMCiqOIWEiIiIiIjIYMrKypCdnY2srCyYzWZMmTIFpaWlQTH//ve/MWnSJNjtdgBAenp6n9oUZjNrYFBUcQQGERERERGRwdTU1MBmswW2bTYb9u3bFxRTXl4Or9eLhx56CE1NTbj66qsxderUkNcrKSlBSUkJAGDFihWBpEdHR01mWC0WpIU4NlCZzeaQ92qg6uv9YAKDiIiIiIjIYKSUnfYJIYK2fT4f9u/fj6VLl6K1tRVLlizBmDFjkJub2+ncgoICFBQUBLarqqo6N2o2o7nhBFpDHRug7HZ76Hs1QIW6H6F+3rrCBAYREREREZHB2Gw2VFdXB7arq6uRkZHRKSY1NRWJiYlITEzEuHHj8OWXX/bqC2UQ1sCgKGMNDCIiIiIiIoMZNWoUysvLUVlZCa/Xi+3bt8PhcATFOBwO/Pe//4XP50NLSwvKysqQl5cXdpvCZGINDIoqjsAgIiIiIiIyGJPJhDlz5mD58uVQVRXTpk3D8OHDsWnTJgDAjBkzMGzYMJx//vm45557oCgKrrjiCowYMaIPjZoBVY3QOyDqjAkMIiIiIiIiA8rPz0d+fn7QvhkzZgRtX3vttbj22msj06DJBMkRGBRFnEJCREREREREfaYto8oaGBQ9TGAQERERERFR35nMrIFBUcUEBhEREREREfUZR2BQtDGBQURERERERH3HERgUZUxgEBERERERUZ8JsxnwMoFB0cNVSIiIiIiIiGJk8+bNPYozmUyYOnVqlHsTYQlWwNOqdy/IwJjAICIiIiIiipEXX3wR48aNO2VcWVlZv0tgCEsC4PHo3Q0yMCYwiIiIiIiIYiQhIQHLli07Zdytt94ag95EluAIDIoy1sAgIiIiIiKKkV/84hc9inv88cej3JPIEwkJgJcjMCh6mMAgIiIiIiKKkZycnB7FZWdnR7knUWBJ4AgMiiomMIiIiIiIiHTw+uuv44svvgAA7N27Fz/96U9x5513Yu/evfp2LEwiIQFoZQKDoocJDCIiIiIiIh288cYbGDp0KADgD3/4A6655hp8+9vfxssvv6xvx8IkEqyA1wMppd5dIYNiAoOIiIiIiEgHjY2NSE5ORlNTE7744gt8/etfxxVXXIHDhw/r3bWwCEuC9oJ1MChKuAoJERERERGRDmw2G/bs2YMDBw5g3LhxUBQFjY2NUJR++nfmBKv27GnV6mEQRRgTGERERERERDq4+eabsWrVKpjNZixcuBAAsGPHDowePVrnnoVH+BMYra1Asr59IWNiAoOIiIiIiEgH+fn5eOGFF4L2TZ48GZMnT9apR30TmELClUgoSvrp2CQiIiIiIqL+7eDBg6irqwMANDc345VXXsHGjRvh8/n07ViYRAJrYFB0MYFBRERERESkg6effhqNjY0AgN/85jf49NNPsXfvXrz44osRuf6uXbvws5/9DHfddRc2btzYZVxZWRkKCwvx3nvv9a1BjsCgKOMUEiIiIiIiIh0cPXoUubm5kFKitLQURUVFSEhIwJ133tnna6uqivXr12PJkiWw2WxYvHgxHA4Hhg0b1inud7/7Hc4///w+tymsHWpgEEUBR2AQERERERHpwGKxoKmpCWVlZbDZbEhLS4PFYoHH0/cpGGVlZcjOzkZWVhbMZjOmTJmC0tLSTnFvvvkmJk2ahLS0tD63yRoYFG0cgUFERERERKSDiy++GD//+c/R1NSEq666CgCwf/9+DB06tM/Xrqmpgc1mC2zbbDbs27evU8z777+PZcuW4bnnnuv2eiUlJSgpKQEArFixAna7vVOMerwaAJCWnARriOMDkdlsDnmvBqq+3g8mMIiIiIiIiHQwe/ZsfPDBBzCZTDjnnHMAAEII/OAHP+jztaWUnfYJIYK2X375ZXzve9+Dopx6YH5BQQEKCgoC21VVVZ1i0k3a18vj1VUQIY4PRHa7PeS9GqhC3Y/c3Nwen88EBhERERERUQwtXboUEydORH5+Ps4777ygY6NGjYpIGzabDdXV1YHt6upqZGRkBMV89tlnePrppwEAx48fx86dO6EoCi666KKw2vRPIZGtrRCniCUKBxMYREREREREMXTLLbdgx44deO6553D8+HGcd955yM/Px7nnnovExMSItDFq1CiUl5ejsrISmZmZ2L59O+bNmxcUs27duqDXF1xwQdjJC6DDMqq
"text/plain": [
"<Figure size 1080x720 with 5 Axes>"
]
},
"metadata": {},
"output_type": "display_data"
}
],
"source": [
"#plot trajectory\n",
"grid = plt.GridSpec(4, 5)\n",
"\n",
"plt.figure(figsize=(15,10))\n",
"\n",
"plt.subplot(grid[0:4, 0:4])\n",
"plt.plot(track[0,:],track[1,:],\"b+\")\n",
"plt.plot(track_lower[0,:],track_lower[1,:],\"g.\")\n",
"plt.plot(track_upper[0,:],track_upper[1,:],\"r.\")\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, 4])\n",
"plt.plot(u_sim[0,:])\n",
"plt.ylabel('a(t) [m/ss]')\n",
"\n",
"plt.subplot(grid[1, 4])\n",
"plt.plot(x_sim[2,:])\n",
"plt.ylabel('v(t) [m/s]')\n",
"\n",
"\n",
"plt.subplot(grid[2, 4])\n",
"plt.plot(np.degrees(u_sim[1,:]))\n",
"plt.ylabel('delta(t) [rad]')\n",
"\n",
"plt.subplot(grid[3, 4])\n",
"plt.plot(x_sim[3,:])\n",
"plt.ylabel('theta(t) [rad]')\n",
"\n",
"plt.tight_layout()\n",
"plt.show()"
]
},
2020-07-01 00:21:27 +08:00
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": []
}
],
"metadata": {
"kernelspec": {
2020-10-23 04:31:40 +08:00
"display_name": "Python 3",
2020-07-01 00:21:27 +08:00
"language": "python",
2020-10-23 04:31:40 +08:00
"name": "python3"
2020-07-01 00:21:27 +08:00
},
"language_info": {
"codemirror_mode": {
"name": "ipython",
"version": 3
},
"file_extension": ".py",
"mimetype": "text/x-python",
"name": "python",
"nbconvert_exporter": "python",
"pygments_lexer": "ipython3",
2021-03-02 22:25:29 +08:00
"version": "3.8.5"
2020-07-01 00:21:27 +08:00
}
},
"nbformat": 4,
"nbformat_minor": 4
}
