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

1282 lines
288 KiB
Plaintext
Raw Normal View History

WIP: racecar mpc
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",
added input delay in sim
2020-08-06 17:21:47 +08:00
"* $a$ acceleration of the robot\n",
new cost function
2020-10-23 18:23:12 +08:00
"* $\\delta$ steering of the robot\n",
WIP: racecar mpc
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",
updated racecar files
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",
WIP: racecar mpc
2020-07-01 00:21:27 +08:00
"\\end{bmatrix}\n",
"\\quad\n",
"=\n",
updated racecar files
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",
WIP: racecar mpc
2020-07-01 00:21:27 +08:00
"$\n",
"\n",
"and\n",
"\n",
"$\n",
"B = \n",
"\\quad\n",
"\\begin{bmatrix}\n",
updated racecar files
2020-07-01 21:33:16 +08:00
"\\frac{\\partial f(x,u)}{\\partial a} & \\frac{\\partial f(x,u)}{\\partial \\delta} \\\\\n",
WIP: racecar mpc
2020-07-01 00:21:27 +08:00
"\\end{bmatrix}\n",
"\\quad\n",
"= \n",
updated racecar files
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",
WIP: racecar mpc
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": [
Added docstrings in utils functions
2020-10-06 17:13:11 +08:00
"\"\"\"\n",
"Control problem statement.\n",
"\"\"\"\n",
WIP: racecar mpc
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",
rewrite constrains for clarity
2020-10-26 23:49:51 +08:00
"DT = 0.2 #discretization step"
WIP: racecar mpc
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",
Added docstrings in utils functions
2020-10-06 17:13:11 +08:00
" Computes the LTI approximated state space model x' = Ax + Bu + C\n",
WIP: racecar mpc
2020-07-01 00:21:27 +08:00
" \"\"\"\n",
Added docstrings in utils functions
2020-10-06 17:13:11 +08:00
" \n",
" L=0.3 #vehicle wheelbase\n",
WIP: racecar mpc
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",
fixed racecar mpc matrices
2020-07-01 17:04:01 +08:00
" A[1,2]=np.sin(theta)\n",
WIP: racecar mpc
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",
new cost function
2020-10-23 18:23:12 +08:00
" A_lin=np.eye(N)+DT*A\n",
WIP: racecar mpc
2020-07-01 00:21:27 +08:00
" \n",
" B = np.zeros((N,M))\n",
" B[2,0]=1\n",
fixed racecar mpc matrices
2020-07-01 17:04:01 +08:00
" B[3,1]=v/(L*np.cos(delta)**2)\n",
new cost function
2020-10-23 18:23:12 +08:00
" B_lin=DT*B\n",
WIP: racecar mpc
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",
new cost function
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",
WIP: racecar mpc
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",
Added docstrings in utils functions
2020-10-06 17:13:11 +08:00
" Returns the set of ODE of the vehicle model.\n",
WIP: racecar mpc
2020-07-01 00:21:27 +08:00
" \"\"\"\n",
" \n",
Added docstrings in utils functions
2020-10-06 17:13:11 +08:00
" L=0.3 #vehicle wheelbase\n",
WIP: racecar mpc
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",
Added docstrings in utils functions
2020-10-06 17:13:11 +08:00
" x_ = np.zeros((N,T+1))\n",
WIP: racecar mpc
2020-07-01 00:21:27 +08:00
" \n",
Added docstrings in utils functions
2020-10-06 17:13:11 +08:00
" x_[:,0] = x0\n",
WIP: racecar mpc
2020-07-01 00:21:27 +08:00
" \n",
" # solve ODE\n",
" for t in range(1,T+1):\n",
"\n",
new cost function
2020-10-23 18:23:12 +08:00
" tspan = [0,DT]\n",
WIP: racecar mpc
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",
Added docstrings in utils functions
2020-10-06 17:13:11 +08:00
" x_[:,t]=x_next[1]\n",
WIP: racecar mpc
2020-07-01 00:21:27 +08:00
" \n",
Added docstrings in utils functions
2020-10-06 17:13:11 +08:00
" return x_"
WIP: racecar mpc
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": [
checking out iterative linearization from PythonRobotics
2020-11-17 02:10:18 +08:00
"CPU times: user 3.06 ms, sys: 0 ns, total: 3.06 ms\n",
"Wall time: 2.72 ms\n"
WIP: racecar mpc
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": {
rewrite constrains for clarity
2020-10-26 23:49:51 +08:00
"image/png": "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
WIP: racecar mpc
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": [
checking out iterative linearization from PythonRobotics
2020-11-17 02:10:18 +08:00
"CPU times: user 0 ns, sys: 2.51 ms, total: 2.51 ms\n",
"Wall time: 1.74 ms\n"
WIP: racecar mpc
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": {
rewrite constrains for clarity
2020-10-26 23:49:51 +08:00
"image/png": "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
WIP: racecar mpc
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",
fixed paper link
2020-10-23 23:31:57 +08:00
"execution_count": 9,
WIP: racecar mpc
2020-07-01 00:21:27 +08:00
"metadata": {},
"outputs": [],
"source": [
"def compute_path_from_wp(start_xp, start_yp, step = 0.1):\n",
Added docstrings in utils functions
2020-10-06 17:13:11 +08:00
" \"\"\"\n",
" Computes a reference path given a set of waypoints\n",
" \"\"\"\n",
" \n",
WIP: racecar mpc
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",
added trajectory generation method
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",
WIP: racecar mpc
2020-07-01 00:21:27 +08:00
"\n",
"\n",
"def get_nn_idx(state,path):\n",
Added docstrings in utils functions
2020-10-06 17:13:11 +08:00
" \"\"\"\n",
" Computes the index of the waypoint closest to vehicle\n",
" \"\"\"\n",
WIP: racecar mpc
2020-07-01 00:21:27 +08:00
"\n",
" dx = state[0]-path[0,:]\n",
" dy = state[1]-path[1,:]\n",
added trajectory generation method
2020-10-23 17:43:10 +08:00
" dist = np.hypot(dx,dy)\n",
WIP: racecar mpc
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",
added trajectory generation method
2020-10-23 17:43:10 +08:00
"def get_ref_trajectory(state, path, target_v):\n",
Added docstrings in utils functions
2020-10-06 17:13:11 +08:00
" \"\"\"\n",
" \"\"\"\n",
added trajectory generation method
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",
new cost function
2020-10-23 18:23:12 +08:00
" ind = get_nn_idx(state, path)\n",
added trajectory generation method
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",
new cost function
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",
added trajectory generation method
2020-10-23 17:43:10 +08:00
" \n",
heading error issues hotfixes
2020-10-23 22:18:36 +08:00
" dl = 0.05 # Waypoints spacing [m]\n",
added trajectory generation method
2020-10-23 17:43:10 +08:00
" travel = 0.0\n",
"\n",
" for i in range(T + 1):\n",
heading error issues hotfixes
2020-10-23 22:18:36 +08:00
" travel += abs(target_v) * DT #current V or target V?\n",
added trajectory generation method
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",
fixed paper link
2020-10-23 23:31:57 +08:00
" xref[2, i] = 0.0 #stop? #sp[ncourse - 1]\n",
added trajectory generation method
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"
WIP: racecar mpc
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": [
added trajectory generation method
2020-10-23 17:43:10 +08:00
"<!-- ![mpc](img/mpc_block_diagram.png) -->\n",
WIP: racecar mpc
2020-07-01 00:21:27 +08:00
"\n",
added trajectory generation method
2020-10-23 17:43:10 +08:00
"<!-- ![mpc](img/mpc_t.png) -->"
WIP: racecar mpc
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",
starting new notebook to implement cost function from to paper
2020-10-23 04:31:40 +08:00
"In this case, the objective function to minimize is given by:\n",
"\n",
fixed paper link
2020-10-23 23:31:57 +08:00
"https://borrelli.me.berkeley.edu/pdfpub/IV_KinematicMPC_jason.pdf\n",
WIP: racecar mpc
2020-07-01 00:21:27 +08:00
"\n",
"$\n",
"\\begin{equation}\n",
"\\begin{aligned}\n",
updated README to latest version
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",
WIP: racecar mpc
2020-07-01 00:21:27 +08:00
"\\textrm{s.t.} \\quad & x(0) = x0\\\\\n",
added constrains max rate of change
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",
WIP: racecar mpc
2020-07-01 00:21:27 +08:00
"\\end{aligned}\n",
"\\end{equation}\n",
"$\n",
"\n",
"\n",
added constrains max rate of change
2020-10-26 19:41:39 +08:00
"Where R,P,Q are the cost matrices used to tune the response.\n"
WIP: racecar mpc
2020-07-01 00:21:27 +08:00
]
},
{
"cell_type": "code",
rewrite constrains for clarity
2020-10-26 23:49:51 +08:00
"execution_count": 10,
WIP: racecar mpc
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",
checking out iterative linearization from PythonRobotics
2020-11-17 02:10:18 +08:00
"problem: variables n = 326, constraints m = 408\n",
" nnz(P) + nnz(A) = 1063\n",
WIP: racecar mpc
2020-07-01 00:21:27 +08:00
"settings: linear system solver = qdldl,\n",
new cost function
2020-10-23 18:23:12 +08:00
" eps_abs = 1.0e-05, eps_rel = 1.0e-05,\n",
WIP: racecar mpc
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",
checking out iterative linearization from PythonRobotics
2020-11-17 02:10:18 +08:00
" 1 0.0000e+00 4.27e+00 4.67e+02 1.00e-01 3.72e-04s\n",
" 175 1.6965e+02 2.63e-05 3.49e-05 7.14e+00 1.83e-03s\n",
WIP: racecar mpc
2020-07-01 00:21:27 +08:00
"\n",
"status: solved\n",
"solution polish: unsuccessful\n",
checking out iterative linearization from PythonRobotics
2020-11-17 02:10:18 +08:00
"number of iterations: 175\n",
"optimal objective: 169.6454\n",
"run time: 2.07e-03s\n",
"optimal rho estimate: 6.34e+00\n",
WIP: racecar mpc
2020-07-01 00:21:27 +08:00
"\n",
checking out iterative linearization from PythonRobotics
2020-11-17 02:10:18 +08:00
"CPU times: user 120 ms, sys: 4.12 ms, total: 125 ms\n",
"Wall time: 127 ms\n"
WIP: racecar mpc
2020-07-01 00:21:27 +08:00
]
}
],
"source": [
"%%time\n",
"\n",
new cost function
2020-10-23 18:23:12 +08:00
"MAX_SPEED = 1.5 #m/s\n",
"MAX_STEER = np.radians(30) #rad\n",
WIP: racecar mpc
2020-07-01 00:21:27 +08:00
"MAX_ACC = 1.0\n",
fixed racecar mpc matrices
2020-07-01 17:04:01 +08:00
"REF_VEL=1.0\n",
WIP: racecar mpc
2020-07-01 00:21:27 +08:00
"\n",
fixed racecar mpc matrices
2020-07-01 17:04:01 +08:00
"track = compute_path_from_wp([0,3,6],\n",
starting new notebook to implement cost function from to paper
2020-10-23 04:31:40 +08:00
" [0,0,0],0.05)\n",
WIP: racecar mpc
2020-07-01 00:21:27 +08:00
"\n",
"# Starting Condition\n",
"x0 = np.zeros(N)\n",
added trajectory generation method
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",
WIP: racecar mpc
2020-07-01 00:21:27 +08:00
" \n",
"#starting guess\n",
"u_bar = np.zeros((M,T))\n",
new cost function
2020-10-23 18:23:12 +08:00
"u_bar[0,:] = MAX_ACC/2 #a\n",
"u_bar[1,:] = 0.1 #delta\n",
WIP: racecar mpc
2020-07-01 00:21:27 +08:00
"\n",
starting new notebook to implement cost function from to paper
2020-10-23 04:31:40 +08:00
"# dynamics starting state w.r.t world frame\n",
new cost function
2020-10-23 18:23:12 +08:00
"x_bar = np.zeros((N,T+1))\n",
"x_bar[:,0] = x0\n",
starting new notebook to implement cost function from to paper
2020-10-23 04:31:40 +08:00
"\n",
WIP: racecar mpc
2020-07-01 00:21:27 +08:00
"#prediction for linearization of costrains\n",
"for t in range (1,T+1):\n",
new cost function
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",
WIP: racecar mpc
2020-07-01 00:21:27 +08:00
" xt_plus_one = np.squeeze(np.dot(A,xt)+np.dot(B,ut)+C)\n",
new cost function
2020-10-23 18:23:12 +08:00
" x_bar[:,t] = xt_plus_one\n",
WIP: racecar mpc
2020-07-01 00:21:27 +08:00
"\n",
new cost function
2020-10-23 18:23:12 +08:00
"#CVXPY Linear MPC problem statement\n",
WIP: racecar mpc
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",
new cost function
2020-10-23 18:23:12 +08:00
"# Cost Matrices\n",
heading error issues hotfixes
2020-10-23 22:18:36 +08:00
"Q = np.diag([10,10,10,10]) #state error cost\n",
new cost function
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",
starting new notebook to implement cost function from to paper
2020-10-23 04:31:40 +08:00
"\n",
new cost function
2020-10-23 18:23:12 +08:00
"#Get Reference_traj\n",
added constrains max rate of change
2020-10-26 19:41:39 +08:00
"x_ref, d_ref = get_ref_trajectory(x_bar[:,0], track, REF_VEL)\n",
starting new notebook to implement cost function from to paper
2020-10-23 04:31:40 +08:00
"\n",
new cost function
2020-10-23 18:23:12 +08:00
"#Prediction Horizon\n",
"for t in range(T):\n",
" \n",
" # Tracking Error\n",
added constrains max rate of change
2020-10-26 19:41:39 +08:00
" cost += cp.quad_form(x[:,t] - x_ref[:,t], Q)\n",
starting new notebook to implement cost function from to paper
2020-10-23 04:31:40 +08:00
"\n",
new cost function
2020-10-23 18:23:12 +08:00
" # Actuation effort\n",
added constrains max rate of change
2020-10-26 19:41:39 +08:00
" cost += cp.quad_form(u[:,t], R)\n",
new cost function
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",
added constrains max rate of change
2020-10-26 19:41:39 +08:00
" A,B,C = get_linear_model(x_bar[:,t], u_bar[:,t])\n",
new cost function
2020-10-23 18:23:12 +08:00
" constr += [x[:,t+1] == A@x[:,t] + B@u[:,t] + C.flatten()]\n",
"\n",
checking out iterative linearization from PythonRobotics
2020-11-17 02:10:18 +08:00
"#Final Point tracking\n",
"cost += cp.quad_form(x[:, T] - x_ref[:, T], Qf)\n",
"\n",
new cost function
2020-10-23 18:23:12 +08:00
"# sums problem objectives and concatenates constraints.\n",
"constr += [x[:,0] == x_bar[:,0]] #starting condition\n",
added constrains max rate of change
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",
WIP: racecar mpc
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",
rewrite constrains for clarity
2020-10-26 23:49:51 +08:00
"execution_count": 11,
WIP: racecar mpc
2020-07-01 00:21:27 +08:00
"metadata": {},
"outputs": [
{
"data": {
checking out iterative linearization from PythonRobotics
2020-11-17 02:10:18 +08:00
"image/png": "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
WIP: racecar mpc
2020-07-01 00:21:27 +08:00
"text/plain": [
fixed racecar mpc matrices
2020-07-01 17:04:01 +08:00
"<Figure size 432x288 with 4 Axes>"
WIP: racecar mpc
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",
fixed racecar mpc matrices
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",
heading error issues hotfixes
2020-10-23 22:18:36 +08:00
"delta_mpc=np.array(u.value[1, :]).flatten()\n",
WIP: racecar mpc
2020-07-01 00:21:27 +08:00
"\n",
"#simulate robot state trajectory for optimized U\n",
fixed paper link
2020-10-23 23:31:57 +08:00
"x_traj=predict(x0, np.vstack((a_mpc,delta_mpc)))\n",
WIP: racecar mpc
2020-07-01 00:21:27 +08:00
"\n",
"#plt.figure(figsize=(15,10))\n",
"#plot trajectory\n",
"plt.subplot(2, 2, 1)\n",
heading error issues hotfixes
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",
WIP: racecar mpc
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",
heading error issues hotfixes
2020-10-23 22:18:36 +08:00
"plt.subplot(2, 2, 3)\n",
fixed racecar mpc matrices
2020-07-01 17:04:01 +08:00
"plt.plot(a_mpc)\n",
heading error issues hotfixes
2020-10-23 22:18:36 +08:00
"plt.ylabel('a_in(t)')\n",
WIP: racecar mpc
2020-07-01 00:21:27 +08:00
"#plt.xlabel('time')\n",
"\n",
"\n",
heading error issues hotfixes
2020-10-23 22:18:36 +08:00
"plt.subplot(2, 2, 2)\n",
fixed racecar mpc matrices
2020-07-01 17:04:01 +08:00
"plt.plot(theta_mpc)\n",
"plt.ylabel('theta(t)')\n",
WIP: racecar mpc
2020-07-01 00:21:27 +08:00
"\n",
heading error issues hotfixes
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",
WIP: racecar mpc
2020-07-01 00:21:27 +08:00
"\n",
"plt.tight_layout()\n",
"plt.show()"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
checking out iterative linearization from PythonRobotics
2020-11-17 02:10:18 +08:00
"## full track demo"
WIP: racecar mpc
2020-07-01 00:21:27 +08:00
]
},
{
"cell_type": "code",
checking out iterative linearization from PythonRobotics
2020-11-17 02:10:18 +08:00
"execution_count": 18,
WIP: racecar mpc
2020-07-01 00:21:27 +08:00
"metadata": {},
fixed racecar mpc matrices
2020-07-01 17:04:01 +08:00
"outputs": [
checking out iterative linearization from PythonRobotics
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"
]
},
master of plots
2020-07-01 17:45:08 +08:00
{
heading error issues hotfixes
2020-10-23 22:18:36 +08:00
"name": "stdout",
"output_type": "stream",
"text": [
checking out iterative linearization from PythonRobotics
2020-11-17 02:10:18 +08:00
"CVXPY Optimization Time: Avrg: 0.1708s Max: 0.2599s Min: 0.1510s\n"
fixed racecar mpc matrices
2020-07-01 17:04:01 +08:00
]
}
],
WIP: racecar mpc
2020-07-01 00:21:27 +08:00
"source": [
plotting sim racecar
2020-07-01 22:59:13 +08:00
"track = compute_path_from_wp([0,3,4,6,10,12,14,6,1,0],\n",
heading error issues hotfixes
2020-10-23 22:18:36 +08:00
" [0,0,2,4,3,3,-2,-6,-2,-2],0.05)\n",
WIP: racecar mpc
2020-07-01 00:21:27 +08:00
"\n",
rewrite constrains for clarity
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",
WIP: racecar mpc
2020-07-01 00:21:27 +08:00
"\n",
added constrains max rate of change
2020-10-26 19:41:39 +08:00
"sim_duration = 200 #time steps\n",
WIP: racecar mpc
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",
heading error issues hotfixes
2020-10-23 22:18:36 +08:00
"MAX_SPEED = 1.5 #m/s\n",
added constrains max rate of change
2020-10-26 19:41:39 +08:00
"MAX_ACC = 1.0 #m/ss\n",
rewrite constrains for clarity
2020-10-26 23:49:51 +08:00
"MAX_D_ACC = 1.0 #m/sss\n",
heading error issues hotfixes
2020-10-23 22:18:36 +08:00
"MAX_STEER = np.radians(30) #rad\n",
rewrite constrains for clarity
2020-10-26 23:49:51 +08:00
"MAX_D_STEER = np.radians(30) #rad/s\n",
added constrains max rate of change
2020-10-26 19:41:39 +08:00
"\n",
"REF_VEL = 1.0 #m/s\n",
WIP: racecar mpc
2020-07-01 00:21:27 +08:00
"\n",
"# Starting Condition\n",
"x0 = np.zeros(N)\n",
heading error issues hotfixes
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",
WIP: racecar mpc
2020-07-01 00:21:27 +08:00
" \n",
"#starting guess\n",
"u_bar = np.zeros((M,T))\n",
heading error issues hotfixes
2020-10-23 22:18:36 +08:00
"u_bar[0,:] = MAX_ACC/2 #a\n",
added constrains max rate of change
2020-10-26 19:41:39 +08:00
"u_bar[1,:] = 0.0 #delta\n",
WIP: racecar mpc
2020-07-01 00:21:27 +08:00
"\n",
"for sim_time in range(sim_duration-1):\n",
" \n",
rewrite constrains for clarity
2020-10-26 23:49:51 +08:00
" iter_start = time.time()\n",
WIP: racecar mpc
2020-07-01 00:21:27 +08:00
" \n",
heading error issues hotfixes
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",
WIP: racecar mpc
2020-07-01 00:21:27 +08:00
" \n",
" #prediction for linearization of costrains\n",
" for t in range (1,T+1):\n",
rewrite constrains for clarity
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",
WIP: racecar mpc
2020-07-01 00:21:27 +08:00
" xt_plus_one = np.squeeze(np.dot(A,xt)+np.dot(B,ut)+C)\n",
rewrite constrains for clarity
2020-10-26 23:49:51 +08:00
" x_bar[:,t] = xt_plus_one\n",
WIP: racecar mpc
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",
heading error issues hotfixes
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",
checking out iterative linearization from PythonRobotics
2020-11-17 02:10:18 +08:00
" Qf = np.diag([30,30,30,0]) #state final error cost\n",
heading error issues hotfixes
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",
WIP: racecar mpc
2020-07-01 00:21:27 +08:00
" \n",
" #Prediction Horizon\n",
" for t in range(T):\n",
"\n",
heading error issues hotfixes
2020-10-23 22:18:36 +08:00
" # Tracking Error\n",
added constrains max rate of change
2020-10-26 19:41:39 +08:00
" cost += cp.quad_form(x[:,t] - x_ref[:,t], Q)\n",
heading error issues hotfixes
2020-10-23 22:18:36 +08:00
"\n",
" # Actuation effort\n",
added constrains max rate of change
2020-10-26 19:41:39 +08:00
" cost += cp.quad_form(u[:,t], R)\n",
WIP: racecar mpc
2020-07-01 00:21:27 +08:00
"\n",
" # Actuation rate of change\n",
" if t < (T - 1):\n",
added constrains max rate of change
2020-10-26 19:41:39 +08:00
" cost += cp.quad_form(u[:,t+1] - u[:,t], R_)\n",
rewrite constrains for clarity
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",
heading error issues hotfixes
2020-10-23 22:18:36 +08:00
"\n",
WIP: racecar mpc
2020-07-01 00:21:27 +08:00
" # Kinrmatics Constrains (Linearized model)\n",
added constrains max rate of change
2020-10-26 19:41:39 +08:00
" A,B,C = get_linear_model(x_bar[:,t], u_bar[:,t])\n",
WIP: racecar mpc
2020-07-01 00:21:27 +08:00
" constr += [x[:,t+1] == A@x[:,t] + B@u[:,t] + C.flatten()]\n",
checking out iterative linearization from PythonRobotics
2020-11-17 02:10:18 +08:00
" \n",
" #Final Point tracking\n",
" cost += cp.quad_form(x[:, T] - x_ref[:, T], Qf)\n",
WIP: racecar mpc
2020-07-01 00:21:27 +08:00
"\n",
" # sums problem objectives and concatenates constraints.\n",
heading error issues hotfixes
2020-10-23 22:18:36 +08:00
" constr += [x[:,0] == x_bar[:,0]] #starting condition\n",
added constrains max rate of change
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",
WIP: racecar mpc
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",
added constrains max rate of change
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",
WIP: racecar mpc
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",
heading error issues hotfixes
2020-10-23 22:18:36 +08:00
" tspan = [0,DT]\n",
WIP: racecar mpc
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",
checking out iterative linearization from PythonRobotics
2020-11-17 02:10:18 +08:00
"execution_count": 20,
"metadata": {},
"outputs": [
{
"data": {
"image/png": "iVBORw0KGgoAAAANSUhEUgAABDAAAALICAYAAACJhQBYAAAAOXRFWHRTb2Z0d2FyZQBNYXRwbG90bGliIHZlcnNpb24zLjMuMSwgaHR0cHM6Ly9tYXRwbG90bGliLm9yZy/d3fzzAAAACXBIWXMAAAsTAAALEwEAmpwYAAEAAElEQVR4nOzdd3xT9f7H8ddJ073oAGpD2UPBAQUUEUWgotcJTtybi+Ck+ruCKCqiKBZw4bgizuu+4lasAxD0skVFRmW2BUpbRkt3z/n9EVJa2kJ3kvb9fDx4JDnz0xzaJJ98vp+vYVmWhYiIiIiIiIiIB7O5OwARERERERERkaNRAkNEREREREREPJ4SGCIiIiIiIiLi8ZTAEBERERERERGPpwSGiIiIiIiIiHg8JTBERERERERExOPZ3R1AfaSnp7vt3NHR0WRmZrrt/HJ0ukbeQdfJ8+kaeT5dI8+na+QddJ08nzuvUWxsrFvO607Vfd7ypN8VT4oFFM/RVBVPbX63VIEhIiIiIiIiIh5PCQwRERERERER8XhePYRERERERESkuZs9ezYrV64kPDycpKSkSusty2Lu3LmsWrUKf39/xo4dS+fOnQFYvXo1c+fOxTRNhg0bxogRI5o4epGGowoMERERERERD3bmmWcyceLEatevWrWKnTt38uyzzzJ69GheffVVAEzTZM6cOUycOJGZM2eyePFiUlNTmypskQanCgwREREREREP1rNnTzIyMqpdv3z5cs444wwMw6B79+4cOHCAPXv2sHv3bmJiYmjbti0AAwcOZNmyZbRr165OcVj79mC+Mp1sX1/Mnn2wDR9Rp+OI1JUSGCIiIiIiIl4sOzub6OjossdRUVFkZ2eTnZ1NVFRUheUbN26s9jjJyckkJycDMG3atArHBCj1Mdjn60tp2lZse7OIvuqWBv5Jas9ut1eK050Uz5HVNx4lMERERERERLyYZVmVlhmGUe3y6iQkJJCQkFD2uMrpN+9+BP+P5pK/ONkjpuf0hmlC3ckb4qnNNKpKYIiIiIiIiHixqKioCh8Ks7KyiIiIoKSkhKysrErL68sWFg4HDmCZJoZNbRWl6eh/m4iIiIiIiBfr168fCxcuxLIsNmzYQFBQEBEREXTp0oUdO3aQkZFBSUkJS5YsoV+/fvU+nxEaDpYJ+QcaIHqRmlMFhoiIiIiIiAebNWsWa9euJScnhzFjxnD55ZdTUlICwPDhw+nTpw8rV67kzjvvxM/Pj7FjxwLg4+PDTTfdxNSpUzFNkyFDhhAXF1fveGyhYc47uTkQHFrv44nUlBIYIiIiIiIiHuzuu+8+4nrDMLjllqobasbHxxMfH9+g8dhCw513cvdD25r3LxCpLw0hERERERERkRqzhbZy3jmQ49Y4pOVRAkNERERERERqzDg4hMTKVQJDmpYSGCIiIiIiIlJjtrByQ0hEmpASGCIiIiIiIlJjRlAIGDY4kOvuUKSFUQJDREREREREaswwDPDzg5Iid4ciLYwSGCIiIiIiIlI7dl8oVgJDmpYSGCIiIiIiIlI7vr5QUuLuKKSFUQJDREREREREakcVGOIGSmCIiIiIiIhI7fj6QXGxu6OQFkYJDBEREREREakdux2rRAkMaVpKYIiIiIiIiEjtqAJD3EAJDBEREREREakduy+oAkOamN3dAYiIiIiIiMiRrV69mrlz52KaJsOGDWPEiBEV1n/22WcsWrQIANM0SU1NZc6cOYSEhDBu3DgCAgKw2Wz4+Pgwbdq0+gdk94X8A/U/jkgtKIEhIiIiIiLiwUzTZM6cOUyaNImoqCgmTJhAv379aNeuXdk2F154IRdeeCEAy5cv58svvyQkJKRs/eTJkwkLC2u4oHx9IUcVGNK0NIRERERERETEg6WkpBATE0Pbtm2x2+0MHDiQZcuWVbv94sWLOe200xo1JsPuqx4Y0uRUgSEiIiIiIuLBsrOziYqKKnscFRXFxo0bq9y2sLCQ1atXc/PNN1dYPnXqVADOOussEhISqtw3OTmZ5ORkAKZNm0Z0dHSV29ntdvxDQym2zGq3aSp2u93tMZSneI6svvEogSEiIiIiIuLBLMuqtMwwjCq3XbFiBT169KgwfGTKlClERkayb98+HnvsMWJjY+nZs2elfRMSEiokNzIzM6s8R3R0NIWlJlZBQbXbNJXo6Gi3x1Ce4jmyquKJjY2t8f4aQiIiIiIiIuLBoqKiyMrKKnuclZVFREREldsuXryYQYMGVVgWGRkJQHh4OP379yclJaX+QdntmoVEmpwSGCIiIiIiIh6sS5cu7Nixg4yMDEpKSliyZAn9+vWrtF1eXh5r166tsK6goID8/Pyy+2vWrKF9+/b1D8rXD4qL6n8ckVrQEBIREREREREP5uPjw0033cTUqVMxTZMhQ4YQFxfH/PnzARg+fDgAS5cu5aSTTiIgIKBs33379vH0008DUFpayqBBg+jdu3f9g7L7qgJDmpwSGCIiIiIiIh4uPj6e+Pj4CstciQuXM888kzPPPLPCsrZt2zJ9+vSGD8jXF0wTq7QUw8en4Y8vUgUNIREREREREZHasfs6b1WFIU1ICQwRERERERGpHV8/560SGNKElMAQERERERGR2nFVYKiRpzQhJTBERERERESkdsoSGKrAkKajBIaIiIiIiIjUjq96YEjTUwJDREREREREasVQBYa4gRIYIiIiIiIiUjuuJp7qgSFNSAkMERERERERqR1fNfGUpqcEhoiIiIiIiNSOn7/zVgkMaUJKYIiIiIiIiEjt+B0cQlKkBIY0Hbu7AxAREREREfE2P/zwQ4228/HxYfDgwY0cjRscrMCwigox3ByKtBxKYIiIiIiIiNTSK6+8wnHHHXfU7VJSUpp1AoPiQvfGIS2KEhgiIiIiIiK15Ofnx+TJk4+63Y033tgE0biB78EERpESGNJ0lMAQERERERGppSeffLJG2z3xxBMNcr7Vq1czd+5cTNNk2LBhjBgxosL6P//8k6eeeoo2bdoAcMopp3DppZfWaN86cVVgqAeGNCElMERERERERGrpmGOOqdF2MTEx9T6XaZrMmTOHSZMmERUVxYQJE+jXrx/t2rWrsN1xxx3H/fffX6d9a81uB8NQBYY0Kc1CIiIiIiIiUg9ffPEFW7ZsAWDDhg3cdttt3H777WzYsKFBjp+SkkJMTAxt27bFbrczcOBAli1b1uj7HolhGM4qDCUwpAmpAkNERERERKQevvzyS4YOHQrAu+++y/nnn09gYCCvv/46jz/+eL2Pn52dTVRUVNnjqKgoNm7cWGm7DRs2cN999xEREcG1115LXFxcjfcFSE5OJjk5GYBp06YRHR1d5XZ2u53o6Ggy/AMI8LERVs12TcEVi6dQPEdW33iUwBAREREREamHvLw8goKCyM/PZ8uWLTz44IPYbDbefPPNBjm+ZVmVlhlGxclLO3XqxOzZswkICGDlypVMnz6dZ599tkb7uiQkJJCQkFD2ODMzs8rtoqOjyczMxLL7UrBvH0XVbNcUXLF4CsVzZFXFExsbW+P9NYRERERERESkHqKioli/fj2LFy/muOOOw2azkZeXh83WMB+3oqKiyMrKKnuclZVFREREhW2CgoIICAgAID4+ntLSUvbv31+jfevMzx+K1cRTmo4SGCIiIiIiIvVwzTXXMGPGDD755JOymT9WrlxJ165dG+T4Xbp0YceOHWRkZFBSUsKSJUvo169fhW327t1bVm2RkpKCaZqEhobWaN868/PDUg8MaUIaQiIiIiIiIlIP8fHxvPzyyxWWDRgwgAEDBjTI8X18fLjpppuYOnUqpmkyZMgQ4uLimD9/PgDDhw/n119/Zf78+fj4+ODn58fdd9+NYRjV7tsgVIEhTUwJDBERERERkXpITU0lJCSEVq1aUVBQwGeffYbNZuOCCy7Abm+Yj1zx8fHEx8dXWDZ8+PCy++eccw7nnHNOjfdtEH7+UFjQ8McVqYaGkIiIiIiIiNTDM888Q15eHgBvvvkmf/31Fxs2bOCVV15xc2SNzNcPCjWERJqOKjBERERERETqYffu3cTGxmJZFsuWLSM
"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": [
"explanation here ..."
]
},
{
"cell_type": "code",
"execution_count": 31,
"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": [
"CVXPY Optimization Time: Avrg: 0.6106s Max: 0.9317s Min: 0.3077s\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.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",
"\n",
"for sim_time in range(sim_duration-1):\n",
" \n",
" iter_start = time.time()\n",
" 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",
" delta_u = np.sum(np.sum(np.abs(u_bar - u_prev),axis=0),axis=0)\n",
" if delta_u < 0.1:\n",
" 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",
"execution_count": 32,
WIP: racecar mpc
2020-07-01 00:21:27 +08:00
"metadata": {},
heading error issues hotfixes
2020-10-23 22:18:36 +08:00
"outputs": [
{
"data": {
checking out iterative linearization from PythonRobotics
2020-11-17 02:10:18 +08:00
"image/png": "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
heading error issues hotfixes
2020-10-23 22:18:36 +08:00
"text/plain": [
"<Figure size 1080x720 with 5 Axes>"
]
},
"metadata": {},
"output_type": "display_data"
}
],
WIP: racecar mpc
2020-07-01 00:21:27 +08:00
"source": [
"#plot trajectory\n",
master of plots
2020-07-01 17:45:08 +08:00
"grid = plt.GridSpec(4, 5)\n",
WIP: racecar mpc
2020-07-01 00:21:27 +08:00
"\n",
"plt.figure(figsize=(15,10))\n",
"\n",
master of plots
2020-07-01 17:45:08 +08:00
"plt.subplot(grid[0:4, 0:4])\n",
WIP: racecar mpc
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",
master of plots
2020-07-01 17:45:08 +08:00
"plt.subplot(grid[0, 4])\n",
WIP: racecar mpc
2020-07-01 00:21:27 +08:00
"plt.plot(u_sim[0,:])\n",
master of plots
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",
WIP: racecar mpc
2020-07-01 00:21:27 +08:00
"plt.ylabel('v(t) [m/s]')\n",
"\n",
master of plots
2020-07-01 17:45:08 +08:00
"plt.subplot(grid[2, 4])\n",
WIP: racecar mpc
2020-07-01 00:21:27 +08:00
"plt.plot(np.degrees(u_sim[1,:]))\n",
fixed paper link
2020-10-23 23:31:57 +08:00
"plt.ylabel('delta(t) [rad]')\n",
master of plots
2020-07-01 17:45:08 +08:00
"\n",
"plt.subplot(grid[3, 4])\n",
"plt.plot(x_sim[3,:])\n",
fixed paper link
2020-10-23 23:31:57 +08:00
"plt.ylabel('theta(t) [rad]')\n",
WIP: racecar mpc
2020-07-01 00:21:27 +08:00
"\n",
"plt.tight_layout()\n",
"plt.show()"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"## OBSTACLE AVOIDANCE\n",
"see dccp paper for reference"
]
},
{
"cell_type": "code",
"execution_count": null,
"metadata": {},
"outputs": [],
"source": []
}
],
"metadata": {
"kernelspec": {
starting new notebook to implement cost function from to paper
2020-10-23 04:31:40 +08:00
"display_name": "Python 3",
WIP: racecar mpc
2020-07-01 00:21:27 +08:00
"language": "python",
starting new notebook to implement cost function from to paper
2020-10-23 04:31:40 +08:00
"name": "python3"
WIP: racecar mpc
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",
added trajectory generation method
2020-10-23 17:43:10 +08:00
"version": "3.8.5"
WIP: racecar mpc
2020-07-01 00:21:27 +08:00
}
},
"nbformat": 4,
"nbformat_minor": 4
}