1226 lines
185 KiB
Plaintext
1226 lines
185 KiB
Plaintext
|
{
|
||
|
|
"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",
|
||
|
|
"* $a$ acceleration of the robot\n",
|
||
|
|
"* $\\delta$ angular velocity of the robot\n",
|
||
|
|
"\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",
|
||
|
|
"\\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",
|
||
|
|
"\\end{bmatrix}\n",
|
||
|
|
"\\quad\n",
|
||
|
|
"=\n",
|
||
|
|
"\\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",
|
||
|
|
"$\n",
|
||
|
|
"\n",
|
||
|
|
"and\n",
|
||
|
|
"\n",
|
||
|
|
"$\n",
|
||
|
|
"B = \n",
|
||
|
|
"\\quad\n",
|
||
|
|
"\\begin{bmatrix}\n",
|
||
|
|
"\\frac{\\partial f(x,u)}{\\partial a} & \\frac{\\partial f(x,u)}{\\partial \\delta} \\\\\n",
|
||
|
|
"\\end{bmatrix}\n",
|
||
|
|
"\\quad\n",
|
||
|
|
"= \n",
|
||
|
|
"\\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",
|
||
|
|
"$\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": [
|
||
|
|
"\"\"\"\n",
|
||
|
|
"Control problem statement.\n",
|
||
|
|
"\"\"\"\n",
|
||
|
|
"\n",
|
||
|
|
"N = 4 #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": 3,
|
||
|
|
"metadata": {},
|
||
|
|
"outputs": [],
|
||
|
|
"source": [
|
||
|
|
"def get_linear_model(x_bar,u_bar):\n",
|
||
|
|
" \"\"\"\n",
|
||
|
|
" Computes the LTI approximated state space model x' = Ax + Bu + C\n",
|
||
|
|
" \"\"\"\n",
|
||
|
|
" \n",
|
||
|
|
" L=0.3 #vehicle wheelbase\n",
|
||
|
|
" \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",
|
||
|
|
" A[1,2]=np.sin(theta)\n",
|
||
|
|
" A[1,3]=v*np.cos(theta)\n",
|
||
|
|
" A[3,2]=v*np.tan(delta)/L\n",
|
||
|
|
" A_lin=np.eye(N)+dt*A\n",
|
||
|
|
" \n",
|
||
|
|
" B = np.zeros((N,M))\n",
|
||
|
|
" B[2,0]=1\n",
|
||
|
|
" B[3,1]=v/(L*np.cos(delta)**2)\n",
|
||
|
|
" B_lin=dt*B\n",
|
||
|
|
" \n",
|
||
|
|
" f_xu=np.array([v*np.cos(theta), v*np.sin(theta), a,v*np.tan(delta)/L]).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 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",
|
||
|
|
" Returns the set of ODE of the vehicle model.\n",
|
||
|
|
" \"\"\"\n",
|
||
|
|
" \n",
|
||
|
|
" L=0.3 #vehicle wheelbase\n",
|
||
|
|
" 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",
|
||
|
|
" x_ = np.zeros((N,T+1))\n",
|
||
|
|
" \n",
|
||
|
|
" x_[:,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_[:,t]=x_next[1]\n",
|
||
|
|
" \n",
|
||
|
|
" return x_"
|
||
|
|
]
|
||
|
|
},
|
||
|
|
{
|
||
|
|
"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": [
|
||
|
|
"CPU times: user 3.66 ms, sys: 83 µs, total: 3.74 ms\n",
|
||
|
|
"Wall time: 3.22 ms\n"
|
||
|
|
]
|
||
|
|
}
|
||
|
|
],
|
||
|
|
"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": {
|
||
|
|
"image/png": "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
|
||
|
|
"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": [
|
||
|
|
"CPU times: user 1.95 ms, sys: 4.06 ms, total: 6.01 ms\n",
|
||
|
|
"Wall time: 1.77 ms\n"
|
||
|
|
]
|
||
|
|
}
|
||
|
|
],
|
||
|
|
"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": {
|
||
|
|
"image/png": "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
|
||
|
|
"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",
|
||
|
|
"execution_count": 9,
|
||
|
|
"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",
|
||
|
|
" 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",
|
||
|
|
" 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.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",
|
||
|
|
" Computes the polynomial coefficents of the path ahead, in vehicle frame.\n",
|
||
|
|
" \"\"\"\n",
|
||
|
|
" \n",
|
||
|
|
" #given vehicle pos find lookahead waypoints\n",
|
||
|
|
" nn_idx=get_nn_idx(state,track)-1\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[3]) - dy * np.sin(-state[3]),\n",
|
||
|
|
" dy * np.cos(-state[3]) + dx * np.sin(-state[3]) ))\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",
|
||
|
|
" \"\"\"\n",
|
||
|
|
" Evaluates rank 3 polynomial\n",
|
||
|
|
" \"\"\"\n",
|
||
|
|
" return round(coeff[0]*x**3 + coeff[1]*x**2 + coeff[2]*x**1 + coeff[3]*x**0,6)\n",
|
||
|
|
"\n",
|
||
|
|
"# def f(x,coeff):\n",
|
||
|
|
"# \"\"\"\n",
|
||
|
|
"# Evaluates rank 5 polynomial\n",
|
||
|
|
"# \"\"\"\n",
|
||
|
|
"# return round(coeff[0]*x**5+coeff[1]*x**4+coeff[2]*x**3+coeff[3]*x**2+coeff[4]*x**1+coeff[5]*x**0,6)\n",
|
||
|
|
"\n",
|
||
|
|
"def df(x,coeff):\n",
|
||
|
|
" \"\"\"\n",
|
||
|
|
" Evaluates derivative of rank 3 polynomial\n",
|
||
|
|
" \"\"\"\n",
|
||
|
|
" return round(3*coeff[0]*x**2 + 2*coeff[1]*x**1 + coeff[2]*x**0,6)\n",
|
||
|
|
"\n",
|
||
|
|
"# def df(x,coeff):\n",
|
||
|
|
"# \"\"\"\n",
|
||
|
|
"# Evaluates derivative of rank 5 polynomial\n",
|
||
|
|
"# \"\"\"\n",
|
||
|
|
"# return round(5*coeff[0]*x**4 + 4*coeff[1]*x**3 +3*coeff[2]*x**2 + 2*coeff[3]*x**1 + coeff[4]*x**0,6)"
|
||
|
|
]
|
||
|
|
},
|
||
|
|
{
|
||
|
|
"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": [
|
||
|
|
"\n",
|
||
|
|
"\n",
|
||
|
|
""
|
||
|
|
]
|
||
|
|
},
|
||
|
|
{
|
||
|
|
"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",
|
||
|
|
"In this case, the objective function to minimize is given by: \n",
|
||
|
|
"\n",
|
||
|
|
"$\n",
|
||
|
|
"\\begin{equation}\n",
|
||
|
|
"\\begin{aligned}\n",
|
||
|
|
"\\min_{} \\quad & \\sum^{t+T-1}_{j=t} cte_{j|t,ref}^TQcte_{j,t,ref} + \\psi_{j|t,ref}^TP\\psi_{j|t,ref} + (v_{j|t} - v_{j|t,ref})^TK(v_{j|t} - v_{j|t,ref}) + 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",
|
||
|
|
" & a_{MIN} < a_{j|t} < a_{MAX} \\quad \\textrm{for} \\quad t<j<t+T-1 \\\\\n",
|
||
|
|
" & \\delta_{MIN} < \\delta_{j|t} < \\delta_{MAX} \\quad \\textrm{for} \\quad t<j<t+T-1 \\\\\n",
|
||
|
|
"\\end{aligned}\n",
|
||
|
|
"\\end{equation}\n",
|
||
|
|
"$\n",
|
||
|
|
"\n",
|
||
|
|
"The terns the sum of the **cross-track error**, **heading error**, **velocity error**, **actuaction effort**.\n",
|
||
|
|
"\n",
|
||
|
|
"Where R,P,K,Q are the cost matrices used to tune the response.\n",
|
||
|
|
"\n",
|
||
|
|
"## Error Formulation\n",
|
||
|
|
"\n",
|
||
|
|
"The track can be represented by fitting a curve trough its waypoints, using the vehicle position as reference\n",
|
||
|
|
"\n",
|
||
|
|
"\n",
|
||
|
|
"\n",
|
||
|
|
"A fitted cubic poly has the form:\n",
|
||
|
|
"\n",
|
||
|
|
"$\n",
|
||
|
|
"f = K_0 * x^3 + K_1 * x^2 + K_2 * x + K_3\n",
|
||
|
|
"$\n",
|
||
|
|
"\n",
|
||
|
|
"The derivative of a fitted cubic poly has the form:\n",
|
||
|
|
"\n",
|
||
|
|
"$\n",
|
||
|
|
"f' = 3.0 * K_0 * x^2 + 2.0 * K_1 * x + K_2\n",
|
||
|
|
"$\n",
|
||
|
|
"\n",
|
||
|
|
"\n",
|
||
|
|
"* **crosstrack error** cte: desired y-position - y-position of vehicle -> this is the value of the fitted polynomial\n",
|
||
|
|
"\n",
|
||
|
|
"* **heading error** epsi: desired heading - heading of vehicle -> is the inclination of tangent to the fitted polynomial\n",
|
||
|
|
"\n",
|
||
|
|
"Then using the fitted polynomial representation in vehicle frame the errors can be easily computed as:\n",
|
||
|
|
"\n",
|
||
|
|
"$\n",
|
||
|
|
"cte = f(px) \\\\\n",
|
||
|
|
"\\psi = -atan(f`(px)) \\\\\n",
|
||
|
|
"$"
|
||
|
|
]
|
||
|
|
},
|
||
|
|
{
|
||
|
|
"cell_type": "code",
|
||
|
|
"execution_count": 10,
|
||
|
|
"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 = 282, constraints m = 343\n",
|
||
|
|
" nnz(P) + nnz(A) = 910\n",
|
||
|
|
"settings: linear system solver = qdldl,\n",
|
||
|
|
" eps_abs = 1.0e-04, eps_rel = 1.0e-04,\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.04e+00 1.04e+02 1.00e-01 3.23e-04s\n",
|
||
|
|
" 50 3.3426e+00 2.85e-07 4.93e-08 1.00e-01 6.84e-04s\n",
|
||
|
|
"\n",
|
||
|
|
"status: solved\n",
|
||
|
|
"solution polish: unsuccessful\n",
|
||
|
|
"number of iterations: 50\n",
|
||
|
|
"optimal objective: 3.3426\n",
|
||
|
|
"run time: 9.06e-04s\n",
|
||
|
|
"optimal rho estimate: 1.76e-01\n",
|
||
|
|
"\n",
|
||
|
|
"CPU times: user 226 ms, sys: 5.12 ms, total: 231 ms\n",
|
||
|
|
"Wall time: 234 ms\n"
|
||
|
|
]
|
||
|
|
}
|
||
|
|
],
|
||
|
|
"source": [
|
||
|
|
"%%time\n",
|
||
|
|
"\n",
|
||
|
|
"MAX_SPEED = 1.25\n",
|
||
|
|
"MIN_SPEED = 0.75\n",
|
||
|
|
"MAX_STEER = 1.57\n",
|
||
|
|
"MAX_ACC = 1.0\n",
|
||
|
|
"REF_VEL=1.0\n",
|
||
|
|
"\n",
|
||
|
|
"track = compute_path_from_wp([0,3,6],\n",
|
||
|
|
" [0,0,0],0.5)\n",
|
||
|
|
"\n",
|
||
|
|
"# Starting Condition\n",
|
||
|
|
"x0 = np.zeros(N)\n",
|
||
|
|
"x0[0] = 0\n",
|
||
|
|
"x0[1] = -0.25\n",
|
||
|
|
"x0[2] = 0.0\n",
|
||
|
|
"x0[3] = np.radians(-0)\n",
|
||
|
|
" \n",
|
||
|
|
"#starting guess\n",
|
||
|
|
"u_bar = np.zeros((M,T))\n",
|
||
|
|
"u_bar[0,:]=0.5\n",
|
||
|
|
"u_bar[1,:]=0.1\n",
|
||
|
|
"\n",
|
||
|
|
"K=road_curve(x0,track)\n",
|
||
|
|
"\n",
|
||
|
|
"# dynamics starting state w.r.t vehicle frame\n",
|
||
|
|
"x_bar=np.zeros((N,T+1))\n",
|
||
|
|
"x_bar[2]=x0[2]\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",
|
||
|
|
"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 += 1*cp.sum_squares(x[2,t]-1.0) # move car to \n",
|
||
|
|
" cost += 100*cp.sum_squares(x[3,t]-np.arctan(df(x_bar[0,t],K))) # psi\n",
|
||
|
|
" cost += 1*cp.sum_squares(f(x_bar[0,t],K)-x[1,t]) # cte\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], 1*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 += [x[2, :] <= MAX_SPEED]\n",
|
||
|
|
"# constr += [x[2, :] >= MIN_SPEED]\n",
|
||
|
|
"constr += [cp.abs(u[0, :]) <= MAX_ACC]\n",
|
||
|
|
"constr += [cp.abs(u[1, :]) <= MAX_STEER]\n",
|
||
|
|
"\n",
|
||
|
|
"prob = cp.Problem(cp.Minimize(cost), constr)\n",
|
||
|
|
"solution = prob.solve(solver=cp.OSQP, verbose=True)"
|
||
|
|
]
|
||
|
|
},
|
||
|
|
{
|
||
|
|
"cell_type": "code",
|
||
|
|
"execution_count": 11,
|
||
|
|
"metadata": {},
|
||
|
|
"outputs": [
|
||
|
|
{
|
||
|
|
"data": {
|
||
|
|
"image/png": "iVBORw0KGgoAAAANSUhEUgAAAagAAAEYCAYAAAAJeGK1AAAABHNCSVQICAgIfAhkiAAAAAlwSFlzAAALEgAACxIB0t1+/AAAADh0RVh0U29mdHdhcmUAbWF0cGxvdGxpYiB2ZXJzaW9uMy4yLjIsIGh0dHA6Ly9tYXRwbG90bGliLm9yZy+WH4yJAAAgAElEQVR4nO3deXhU1f3H8fe52fdlJoCslU3BFSSCCAYkUKvUorUqWhURKWXfbIWioojikkJR0FoQ0dZWrIra2p800gKKlGBABFQSZJPFkI3s6z2/P4YEQvZt7p3J9/U888x278yHYW6+c8+59xyltdYIIYQQNmNYHUAIIYSoiRQoIYQQtiQFSgghhC1JgRJCCGFLUqCEEELYkhQoIYQQtuRrdYDmOn78eJX7TqeT9PR0i9I0nyfn9+TsUHP+jh07WpTGXs7fzirY8f/cbpkkT/1q285kD0oIIYQtSYESQghhS7Zq4ktPT2fFihVkZ2ejlCI+Pp4bb7zR6lhCiBqYH79L9pEDmMqAgEDwD4SAgLO3g4JQUU5wxEBUDMrPz+rIwsPYqkD5+Phwzz330L17dwoLC3n44Ye5/PLL6dy5s9XRhBDnK8inPO0EOj8PiouguBhKiuCc0dOqjKMWEQXRMajoGFfR6toD1auv674QNbBVgYqKiiIqKgqAoKAgOnXqRGZmphQoIWzIuOUeHA/OqtLhrrWG0hJXwSrMh8x0dOYpyDgFmafQmafQ3x+C3UlQWuIqYNExqJ59oVcf13XHrihDeh+EzQrUudLS0jh48CA9e/as8nhiYiKJiYkALFmyBKfTWeV5X1/fao95Ek/O78nZwfPz24FSCvwDXJewCGjXEVXDcrq8HI4dQqd8Dan70N9+Bds3uQpWUAjqkn6oISOhzxVSrNowWxaooqIiEhISGDduHMHBwVWei4+PJz4+vvL++YdL2vEQysbw5PyenB3kMHN3Uj4+ria+rj1gxGjXnlf6D+jUryFlL3rn5+gdn4KjHeraeNS1I6QpsA2yXYEqKysjISGBoUOHMnDgQKvjCCHcQCkFMR1QMR3gmuHosb9C7/of+tMN6A/eRH/4V7ikH8aQUXBFLMpXDrhoC2xVoLTWvPzyy3Tq1InRo0dbHUcIYRHl54eKHQKxQ9DpP6A/+wT9WSLmy0sgIho15m7U4BHS/OflbFWgvv32WzZv3kzXrl156KGHABg7diz9+/e3OJkQwirK2R71s7vQP70D9u7E/Oc69NoX0P/5COPOB1G9+lodUbQSWxWoiy++mHXr1lkdQwhhQ8rwgcsGYFx6FXr7ZvQ7azGffRg1YAjqtnEoRzurI4oWZqsCJYQQ9VFKoQbGoa8ciP74Xdfly+2oUWNQP7nN6niiBUkDrhDCI6mAQIyb78JY9BKq3zXof67DXPBrindttzqaaCFSoIQQHk1Fx2A8OAfj4WchOITsJ2a5+qlM0+poopmkQAkhvILqcTHG/OcJvG4Uev2fMV98Ep2fa3Us0QxSoITwIrt27WLGjBlMmzaN9evX17pcamoqd9xxB9u2bXNjutanAgIJn/Eo6u5JsG8X5qJZ6MOpVscSTSQFSggvYZomq1evZv78+SxdupTPPvuM77//vsbl/vKXv3DllVdakLL1KaUwht2I8dsloE3MJb/F3LLBNVqF8ChSoITwEqmpqXTo0IH27dvj6+vL4MGDSUpKqrbcv/71LwYOHEh4eLgFKd1HXdgbY8Ey6H0p+vUX0WuXo0uKrY4lGkEOMxfCS2RmZuJwOCrvOxwOUlJSqi2zfft2HnvsMV566aVaX6u+QZkr2HGA3SqZnE70E8vJX7eG/HWv4puRRuSCBIyQUGvy2IDd8tRFCpQQXqKmJiylqo4l/tprr3H33Xdj1DNEUH2DMlew4wDBNWYaOQYjuh2lf3qOUwumYMx8HOWmImW3z8hueaD2QZmlQAnhJRwOBxkZGZX3MzIyKudXq3DgwAH+8Ic/AJCTk8POnTsxDIOrr77arVmtoK4ajOE7D/PlJZgJv8OYtQgV5t3NnJ5O+qCE8BI9evTgxIkTpKWlUVZWxtatWxkwYECVZVasWFF5GTRoEBMmTGgTxamCuuJqjCkL4OQxzITfoXOyrI4k6iAFSggv4ePjw/jx41m8eDGzZs3immuuoUuXLmzYsIENGzZYHc821KX9MaY9AqdOYj73O3R2Rv0rCUso7eHHXh4/frzKfTu2rzaGJ+f35OwgExbW5fztrIId/88bmknv34u5/AmIiMSY82SrTYhot8/Ibnmg9u1M9qCEEG2S6n0JxqzHITcH89l56FMnrY4kziMFSgjRZqkeF2PMWQSFBZgJC9DZmVZHEuewVYFauXIlEyZMYM6cOVZHEUK0EapbT4zZT0Bejmv8vuIiqyOJM2xVoIYNG8b8+fOtjiGEaGNUt54YDz4ER77D/NPzaLPc6kgCmxWovn37EhrqvjO8hRCigroiFjX2QfhyO3rdq1bHEXjgibr1DcHiScN41MST83tydvD8/KL5jOE3YZ46if73+5jO9hjxN1sdqU3zuAJV3xAsdjyEsjE8Ob8nZwc5zFy4qNvuR6f/gF63Gu1oh+o3yOpIbZatmviEEMJqyjAwHpgDP+qFuep59MGU+lcSrUIKlBBCnEcFBGBM/R2ERWK+8AQ6/QerI7VJtipQy5YtY8GCBRw/fpxJkyaxceNGqyMJIdooFR6FMeMxKC/DXP4EuiDf6khtjq36oGbOnGl1BCGEqKQu6ILx63mYSx/FXLscY9LD1aYwEa3HVntQQghhN+riy1E/vw+SP0cnfmB1nDZFCpQQQtRDjRwDVw5Cv/MaOvVrq+O0GVKghBCiHkopjPunQ3QM5h+fReeetjpSmyAFSrQ4GSZGeCMVHIox6beuMftWJcj33A2kQIkWo8vLMP+5zjUqdLlsvML7qK49UHf9CvbtQv/jLavjeD0pUKJF6LQTZC2Ygl7/Z1RENJSWWB1JiFahhoxEXXM9+h9voffutDqOV5MCJZpFa425ZQPmEzMoO3IQNWEOxsSHUIFBVkcTolUopVB3/xo6dnWNNJF5yupIXstW50EJz6JzsjFffxG+3A4XX45jzuNk4WN1LI9VVlZGSkoKhw8fJj8/n5CQELp160avXr3w9ZVN1U5UQADGpN9iPjkH85XnMOYuRvn6WR3L68i3XjSaNk30tv+g//4aFBag7ngAdf1P8XG2Aw8eLNYqOTk5rF+/nk2bNhEaGkqnTp0IDAykqKiIf/3rX+Tl5REXF8eYMWMIDw+3Oq44Q3XojLpvGvqVZ9Hv/Rn1i/utjuR1pECJRtEHUzD/+kc4uB8u7I1x3zRUp25Wx/Jojz32GMOHD+e5554jOjq62vOZmZl8+umnPPbYYyxdutSChKI2RuwQzG93oze8h76kH6rvlVZH8ipSoESD6Jws9LtvoD9LhIgo1P0zUYOGoQzpxmyu5557rs4mvOjoaG6++WZuvPFGN6YSDaV+8QD62z2Ya5ZhPLYcFSp7uS1F/rqIOumSYswN6zEX/Bq97b+oH9+CsegljMHXS3FqIecWp1dfrXkm19dee036oWxKBQRgPDgHcnMwX38RrbXVkbyG/IURNdK5OZgf/g3ztw+g334VelyMsXA5xm33o4KCrY7ntTZt2lTj45s3b3ZzEtEYqmsP1C33wM5t6E//bXUcr9Gon2Rr164lLi6OH/3oR60UR1hNnzqJ/vd6V1NeSQlcHovx41ug1yUyinMrqphapry8vNo0M2lpaYSFhVkRSzSCGvkz9J4v0H/7E7rXJagOnayO5PEaVaDKy8tZvHgx4eHhDB06lKFDh+JwOForm3ATbZqwfw9688foHZ+BYaAGxaFG3YLq2NXqeG3Cli1bANeh5hW3K0RERDBlyhQrYolGUIaBMX4W5sJpmKsSMB5+FiXNss3is3DhwoUNXbhfv36MHj2amJgYdu/ezWuvvcaePXsA6NChQ7PbyHft2sXTTz/NRx99RElJCRdffHG96+Tm5la5n5AQRmxs4ycWS0gIY/Dgxo9+0NT1als3ODiYgoKCVnn
|
||
|
|
"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",
|
||
|
|
"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",
|
||
|
|
"w_mpc=np.array(u.value[1, :]).flatten()\n",
|
||
|
|
"\n",
|
||
|
|
"#simulate robot state trajectory for optimized U\n",
|
||
|
|
"x_traj=predict(x0, np.vstack((a_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(a_mpc)\n",
|
||
|
|
"plt.ylabel('a(t)')\n",
|
||
|
|
"#plt.xlabel('time')\n",
|
||
|
|
"\n",
|
||
|
|
"\n",
|
||
|
|
"plt.subplot(2, 2, 4)\n",
|
||
|
|
"plt.plot(theta_mpc)\n",
|
||
|
|
"plt.ylabel('theta(t)')\n",
|
||
|
|
"\n",
|
||
|
|
"plt.subplot(2, 2, 3)\n",
|
||
|
|
"plt.plot(v_mpc)\n",
|
||
|
|
"plt.ylabel('v(t)')\n",
|
||
|
|
"\n",
|
||
|
|
"plt.tight_layout()\n",
|
||
|
|
"plt.show()"
|
||
|
|
]
|
||
|
|
},
|
||
|
|
{
|
||
|
|
"cell_type": "markdown",
|
||
|
|
"metadata": {},
|
||
|
|
"source": [
|
||
|
|
"full track demo"
|
||
|
|
]
|
||
|
|
},
|
||
|
|
{
|
||
|
|
"cell_type": "code",
|
||
|
|
"execution_count": 50,
|
||
|
|
"metadata": {},
|
||
|
|
"outputs": [
|
||
|
|
{
|
||
|
|
"name": "stderr",
|
||
|
|
"output_type": "stream",
|
||
|
|
"text": [
|
||
|
|
"<ipython-input-9-526b0e6cd302>:29: RuntimeWarning: invalid value encountered in true_divide\n",
|
||
|
|
" v /= np.linalg.norm(v)\n",
|
||
|
|
"<ipython-input-50-8fcc0dfd7424>:34: RankWarning: Polyfit may be poorly conditioned\n",
|
||
|
|
" K=road_curve(x_sim[:,sim_time],track)\n"
|
||
|
|
]
|
||
|
|
},
|
||
|
|
{
|
||
|
|
"name": "stdout",
|
||
|
|
"output_type": "stream",
|
||
|
|
"text": [
|
||
|
|
"CVXPY Optimization Time: Avrg: 0.2259s Max: 0.3524s Min: 0.2046s\n"
|
||
|
|
]
|
||
|
|
}
|
||
|
|
],
|
||
|
|
"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.5)\n",
|
||
|
|
"\n",
|
||
|
|
"# track = compute_path_from_wp([0,5,7.5,10,12,13,13,10],\n",
|
||
|
|
"# [0,0,2.5,2.5,0,0,5,10],0.5)\n",
|
||
|
|
"\n",
|
||
|
|
"sim_duration = 175 #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.25\n",
|
||
|
|
"MIN_SPEED = 0.75\n",
|
||
|
|
"MAX_STEER = 1.57/2\n",
|
||
|
|
"MAX_ACC = 1.0\n",
|
||
|
|
"\n",
|
||
|
|
"# Starting Condition\n",
|
||
|
|
"x0 = np.zeros(N)\n",
|
||
|
|
"x0[0] = 0\n",
|
||
|
|
"x0[1] = -0.25\n",
|
||
|
|
"x0[2] = 0\n",
|
||
|
|
"x0[3] = 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\n",
|
||
|
|
"u_bar[1,:]=0.01\n",
|
||
|
|
"\n",
|
||
|
|
"for sim_time in range(sim_duration-1):\n",
|
||
|
|
" \n",
|
||
|
|
" iter_start=time.time()\n",
|
||
|
|
" K=road_curve(x_sim[:,sim_time],track)\n",
|
||
|
|
" \n",
|
||
|
|
" # dynamics starting state w.r.t vehicle frame\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",
|
||
|
|
" cost = 0\n",
|
||
|
|
" constr = []\n",
|
||
|
|
" x = cp.Variable((N, T+1))\n",
|
||
|
|
" u = cp.Variable((M, T))\n",
|
||
|
|
" \n",
|
||
|
|
" #Prediction Horizon\n",
|
||
|
|
" for t in range(T):\n",
|
||
|
|
"\n",
|
||
|
|
" cost += 30*cp.sum_squares(x[3,t]-np.arctan(df(x_bar[0,t],K))) # psi\n",
|
||
|
|
" cost += 20*cp.sum_squares(f(x_bar[0,t],K)-x[1,t]) # cte\n",
|
||
|
|
" cost += 10*cp.sum_squares(1.-x[2,t]) # desired v\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],10*np.eye(M))\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",
|
||
|
|
" # sums problem objectives and concatenates constraints.\n",
|
||
|
|
" constr += [x[:,0] == x_bar[:,0]] #<--watch out the start condition\n",
|
||
|
|
" constr += [x[2, :] <= MAX_SPEED]\n",
|
||
|
|
" constr += [x[2, :] >= 0.0]\n",
|
||
|
|
" constr += [cp.abs(u[0, :]) <= MAX_ACC]\n",
|
||
|
|
" constr += [cp.abs(u[1, :]) <= 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": 51,
|
||
|
|
"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) [deg/s]')\n",
|
||
|
|
"\n",
|
||
|
|
"plt.subplot(grid[3, 4])\n",
|
||
|
|
"plt.plot(x_sim[3,:])\n",
|
||
|
|
"plt.ylabel('theta(t) [m/s]')\n",
|
||
|
|
"\n",
|
||
|
|
"plt.tight_layout()\n",
|
||
|
|
"plt.show()"
|
||
|
|
]
|
||
|
|
},
|
||
|
|
{
|
||
|
|
"cell_type": "markdown",
|
||
|
|
"metadata": {},
|
||
|
|
"source": [
|
||
|
|
"## OBSTACLE AVOIDANCE\n",
|
||
|
|
"see dccp paper for reference"
|
||
|
|
]
|
||
|
|
},
|
||
|
|
{
|
||
|
|
"cell_type": "markdown",
|
||
|
|
"metadata": {},
|
||
|
|
"source": [
|
||
|
|
"import dccp\n",
|
||
|
|
"track = compute_path_from_wp([0,3,4,6,10,13],\n",
|
||
|
|
" [0,0,2,4,3,3],0.25)\n",
|
||
|
|
"\n",
|
||
|
|
"obstacles=np.array([[4,6],[2,4]])\n",
|
||
|
|
"obstacle_radius=0.5\n",
|
||
|
|
"\n",
|
||
|
|
"def to_vehic_frame(pt,pos_x,pos_y,theta):\n",
|
||
|
|
" dx = pt[0] - pos_x\n",
|
||
|
|
" dy = pt[1] - pos_y\n",
|
||
|
|
"\n",
|
||
|
|
" return [dx * np.cos(-theta) - dy * np.sin(-theta),\n",
|
||
|
|
" dy * np.cos(-theta) + dx * np.sin(-theta)]\n",
|
||
|
|
" \n",
|
||
|
|
"# track = compute_path_from_wp([0,5,7.5,10,12,13,13,10],\n",
|
||
|
|
"# [0,0,2.5,2.5,0,0,5,10],0.5)\n",
|
||
|
|
"\n",
|
||
|
|
"sim_duration = 80 #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.25\n",
|
||
|
|
"MIN_SPEED = 0.75\n",
|
||
|
|
"MAX_STEER_SPEED = 1.57\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",
|
||
|
|
" #compute coefficients\n",
|
||
|
|
" K=road_curve(x_sim[:,sim_time],track)\n",
|
||
|
|
" \n",
|
||
|
|
" #compute opstacles in ref frame\n",
|
||
|
|
" o_=[]\n",
|
||
|
|
" for j in range(2):\n",
|
||
|
|
" o_.append(to_vehic_frame(obstacles[:,j],x_sim[0,sim_time],x_sim[1,sim_time],x_sim[2,sim_time]) )\n",
|
||
|
|
" \n",
|
||
|
|
" # dynamics starting state w.r.t vehicle frame\n",
|
||
|
|
" x_bar=np.zeros((N,T+1))\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",
|
||
|
|
" cost = 0\n",
|
||
|
|
" constr = []\n",
|
||
|
|
" x = cp.Variable((N, T+1))\n",
|
||
|
|
" u = cp.Variable((M, T))\n",
|
||
|
|
" \n",
|
||
|
|
" #Prediction Horizon\n",
|
||
|
|
" for t in range(T):\n",
|
||
|
|
"\n",
|
||
|
|
" #cost += 30*cp.sum_squares(x[2,t]-np.arctan(df(x_bar[0,t],K))) # psi\n",
|
||
|
|
" cost += 50*cp.sum_squares(x[2,t]-np.arctan2(df(x_bar[0,t],K),x_bar[0,t])) # psi\n",
|
||
|
|
" cost += 20*cp.sum_squares(f(x_bar[0,t],K)-x[1,t]) # cte\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",
|
||
|
|
" # Actuation effort\n",
|
||
|
|
" cost += cp.quad_form( u[:, t],1*np.eye(M))\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",
|
||
|
|
" # Obstacle Avoidance Contrains\n",
|
||
|
|
" for j in range(2):\n",
|
||
|
|
" constr += [ cp.norm(x[0:2,t]-o_[j],2) >= obstacle_radius ]\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(method=\"dccp\", 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": "markdown",
|
||
|
|
"metadata": {},
|
||
|
|
"source": [
|
||
|
|
"#plot trajectory\n",
|
||
|
|
"grid = plt.GridSpec(2, 3)\n",
|
||
|
|
"\n",
|
||
|
|
"plt.figure(figsize=(15,10))\n",
|
||
|
|
"\n",
|
||
|
|
"ax=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",
|
||
|
|
"#obstacles\n",
|
||
|
|
"circle1=plt.Circle((obstacles[0,0], obstacles[1,0]), obstacle_radius, color='r')\n",
|
||
|
|
"circle2=plt.Circle((obstacles[0,1], obstacles[1,1]), obstacle_radius, color='r')\n",
|
||
|
|
"plt.axis(\"equal\")\n",
|
||
|
|
"plt.ylabel('y')\n",
|
||
|
|
"plt.xlabel('x')\n",
|
||
|
|
"\n",
|
||
|
|
"ax.add_artist(circle1)\n",
|
||
|
|
"ax.add_artist(circle2)\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",
|
||
|
|
"\n",
|
||
|
|
"plt.tight_layout()\n",
|
||
|
|
"plt.show()"
|
||
|
|
]
|
||
|
|
},
|
||
|
|
{
|
||
|
|
"cell_type": "code",
|
||
|
|
"execution_count": null,
|
||
|
|
"metadata": {},
|
||
|
|
"outputs": [],
|
||
|
|
"source": []
|
||
|
|
}
|
||
|
|
],
|
||
|
|
"metadata": {
|
||
|
|
"kernelspec": {
|
||
|
|
"display_name": "Python [conda env:.conda-jupyter] *",
|
||
|
|
"language": "python",
|
||
|
|
"name": "conda-env-.conda-jupyter-py"
|
||
|
|
},
|
||
|
|
"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.8.5"
|
||
|
|
}
|
||
|
|
},
|
||
|
|
"nbformat": 4,
|
||
|
|
"nbformat_minor": 4
|
||
|
|
}
|