12345qiupeng
/
mpc_python_learn
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity
mpc_python_learn/notebooks/3.0-MPC-v3-track-constrains...

1098 lines
366 KiB
Plaintext
Raw Normal View History

forther r&d in track constrains and plots tidy up
2021-04-14 18:38:30 +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": "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.2 #discretization step\n",
"\n",
"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)\n",
"\n",
"\"\"\"\n",
"the ODE is used to update the simulation given the mpc results\n",
"I use this insted of using the LTI twice\n",
"\"\"\"\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_\n",
"\n",
"\n",
"\"\"\"\n",
"MODIFIED TO INCLUDE FRAME TRANSFORMATION\n",
"\"\"\"\n",
"def compute_path_from_wp(start_xp, start_yp, step = 0.1):\n",
" \"\"\"\n",
" Computes a reference path given a set of waypoints\n",
" \"\"\"\n",
" \n",
" final_xp=[]\n",
" final_yp=[]\n",
" delta = step #[m]\n",
"\n",
" for idx in range(len(start_xp)-1):\n",
" section_len = np.sum(np.sqrt(np.power(np.diff(start_xp[idx:idx+2]),2)+np.power(np.diff(start_yp[idx:idx+2]),2)))\n",
"\n",
" interp_range = np.linspace(0,1,np.floor(section_len/delta).astype(int))\n",
" \n",
" fx=interp1d(np.linspace(0,1,2),start_xp[idx:idx+2],kind=1)\n",
" fy=interp1d(np.linspace(0,1,2),start_yp[idx:idx+2],kind=1)\n",
" \n",
" # watch out to duplicate points!\n",
" final_xp=np.append(final_xp,fx(interp_range)[1:])\n",
" final_yp=np.append(final_yp,fy(interp_range)[1:])\n",
" \n",
" dx = np.append(0, np.diff(final_xp))\n",
" dy = np.append(0, np.diff(final_yp))\n",
" theta = np.arctan2(dy, dx)\n",
"\n",
" return np.vstack((final_xp,final_yp,theta))\n",
"\n",
"\n",
"def get_nn_idx(state,path):\n",
" \"\"\"\n",
" Computes the index of the waypoint closest to vehicle\n",
" \"\"\"\n",
"\n",
" dx = state[0]-path[0,:]\n",
" dy = state[1]-path[1,:]\n",
" dist = np.hypot(dx,dy)\n",
" nn_idx = np.argmin(dist)\n",
"\n",
" try:\n",
" v = [path[0,nn_idx+1] - path[0,nn_idx],\n",
" path[1,nn_idx+1] - path[1,nn_idx]] \n",
" v /= np.linalg.norm(v)\n",
"\n",
" d = [path[0,nn_idx] - state[0],\n",
" path[1,nn_idx] - state[1]]\n",
"\n",
" if np.dot(d,v) > 0:\n",
" target_idx = nn_idx\n",
" else:\n",
" target_idx = nn_idx+1\n",
"\n",
" except IndexError as e:\n",
" target_idx = nn_idx\n",
"\n",
" return target_idx\n",
"\n",
"def normalize_angle(angle):\n",
" \"\"\"\n",
" Normalize an angle to [-pi, pi]\n",
" \"\"\"\n",
" while angle > np.pi:\n",
" angle -= 2.0 * np.pi\n",
"\n",
" while angle < -np.pi:\n",
" angle += 2.0 * np.pi\n",
"\n",
" return angle\n",
"\n",
"def get_ref_trajectory(state, path, target_v):\n",
" \"\"\"\n",
" modified reference in robot frame\n",
" \"\"\"\n",
" xref = np.zeros((N, T + 1))\n",
" dref = np.zeros((1, T + 1))\n",
" \n",
" #sp = np.ones((1,T +1))*target_v #speed profile\n",
" \n",
" ncourse = path.shape[1]\n",
"\n",
" ind = get_nn_idx(state, path)\n",
" dx=path[0,ind] - state[0]\n",
" dy=path[1,ind] - state[1]\n",
" \n",
" xref[0, 0] = dx * np.cos(-state[3]) - dy * np.sin(-state[3]) #X\n",
" xref[1, 0] = dy * np.cos(-state[3]) + dx * np.sin(-state[3]) #Y\n",
" xref[2, 0] = target_v #V\n",
" xref[3, 0] = normalize_angle(path[2,ind]- state[3]) #Theta\n",
" dref[0, 0] = 0.0 # steer operational point should be 0\n",
" \n",
" dl = 0.05 # Waypoints spacing [m]\n",
" travel = 0.0\n",
" \n",
" for i in range(T + 1):\n",
" travel += abs(target_v) * DT #current V or target V?\n",
" dind = int(round(travel / dl))\n",
" \n",
" if (ind + dind) < ncourse:\n",
" dx=path[0,ind + dind] - state[0]\n",
" dy=path[1,ind + dind] - state[1]\n",
" \n",
" xref[0, i] = dx * np.cos(-state[3]) - dy * np.sin(-state[3])\n",
" xref[1, i] = dy * np.cos(-state[3]) + dx * np.sin(-state[3])\n",
" xref[2, i] = target_v #sp[ind + dind]\n",
" xref[3, i] = normalize_angle(path[2,ind + dind] - state[3])\n",
" dref[0, i] = 0.0\n",
" else:\n",
" dx=path[0,ncourse - 1] - state[0]\n",
" dy=path[1,ncourse - 1] - state[1]\n",
" \n",
" xref[0, i] = dx * np.cos(-state[3]) - dy * np.sin(-state[3])\n",
" xref[1, i] = dy * np.cos(-state[3]) + dx * np.sin(-state[3])\n",
" xref[2, i] = 0.0 #stop? #sp[ncourse - 1]\n",
" xref[3, i] = normalize_angle(path[2,ncourse - 1] - state[3])\n",
" dref[0, i] = 0.0\n",
"\n",
" return xref, dref"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"## SMOOTHEN PATH\n",
"\n",
"I use a corner smoothing tecnique to help the line-finding proplem -> this way the line does not change from 0 to pi/2 instantly"
]
},
{
"cell_type": "code",
"execution_count": 3,
"metadata": {},
"outputs": [],
"source": [
"from scipy.signal import savgol_filter\n",
"def compute_path_from_wp(start_xp, start_yp, step = 0.1):\n",
" \"\"\"\n",
" Computes a reference path given a set of waypoints\n",
" \"\"\"\n",
" \n",
" final_xp=[]\n",
" final_yp=[]\n",
" delta = step #[m]\n",
"\n",
" for idx in range(len(start_xp)-1):\n",
" section_len = np.sum(np.sqrt(np.power(np.diff(start_xp[idx:idx+2]),2)+np.power(np.diff(start_yp[idx:idx+2]),2)))\n",
"\n",
" interp_range = np.linspace(0,1,np.floor(section_len/delta).astype(int))\n",
" \n",
" fx=interp1d(np.linspace(0,1,2),start_xp[idx:idx+2],kind=1)\n",
" fy=interp1d(np.linspace(0,1,2),start_yp[idx:idx+2],kind=1)\n",
" \n",
" # watch out to duplicate points!\n",
" final_xp=np.append(final_xp,fx(interp_range)[1:])\n",
" final_yp=np.append(final_yp,fy(interp_range)[1:])\n",
" \n",
" \"\"\"this smoothens up corners\"\"\"\n",
less cringy readme
2021-04-14 22:17:16 +08:00
" window_size = 7 # Smoothening filter window\n",
forther r&d in track constrains and plots tidy up
2021-04-14 18:38:30 +08:00
" final_xp = savgol_filter(final_xp, window_size, 1)\n",
" final_yp = savgol_filter(final_yp, window_size, 1)\n",
" \n",
" dx = np.append(0, np.diff(final_xp))\n",
" dy = np.append(0, np.diff(final_yp))\n",
" theta = np.arctan2(dy, dx)\n",
"\n",
" return np.vstack((final_xp,final_yp,theta))\n"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"## V3 Add track constraints\n",
"inspried from -> https://arxiv.org/pdf/1711.07300.pdf\n",
"\n",
"explanation here...\n",
"\n",
"benefits:\n",
"* add a soft form of obstacle aoidance"
]
},
{
"cell_type": "code",
"execution_count": 4,
"metadata": {},
"outputs": [
{
"data": {
"text/plain": [
less cringy readme
2021-04-14 22:17:16 +08:00
"[<matplotlib.lines.Line2D at 0x7f10ac8425e0>]"
forther r&d in track constrains and plots tidy up
2021-04-14 18:38:30 +08:00
]
},
"execution_count": 4,
"metadata": {},
"output_type": "execute_result"
},
{
"data": {
less cringy readme
2021-04-14 22:17:16 +08:00
"image/png": "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
forther r&d in track constrains and plots tidy up
2021-04-14 18:38:30 +08:00
"text/plain": [
"<Figure size 1080x720 with 1 Axes>"
]
},
"metadata": {},
"output_type": "display_data"
}
],
"source": [
"def generate_track_bounds(track,width=0.5):\n",
" \"\"\"\n",
" in world frame\n",
" \"\"\"\n",
" bounds_low=np.zeros((2, track.shape[1]))\n",
" bounds_upp=np.zeros((2, track.shape[1]))\n",
" \n",
" for idx in range(track.shape[1]):\n",
" x = track[0,idx]\n",
" y = track [1,idx]\n",
" th = track [2,idx]\n",
" \n",
" \"\"\"\n",
" trasform the points\n",
" \"\"\"\n",
" bounds_upp[0, idx] = 0 * np.cos(th) - width * np.sin(th) + x #X\n",
" bounds_upp[1, idx] = 0 * np.sin(th) + width * np.cos(th) + y #Y\n",
" \n",
" bounds_low[0, idx] = 0 * np.cos(th) - (-width) * np.sin(th) + x #X\n",
" bounds_low[1, idx] = 0 * np.sin(th) + (-width) * np.cos(th) + y #Y\n",
" \n",
" return bounds_low, bounds_upp\n",
"\n",
"track = compute_path_from_wp([0,3,4,6,10,12,14,6,1,0],\n",
" [0,0,2,4,3,3,-2,-6,-2,-2],0.05)\n",
"\n",
"lower, upper = generate_track_bounds(track)\n",
"\n",
"plt.figure(figsize=(15,10))\n",
"\n",
"plt.plot(track[0,:],track[1,:],\"b-\")\n",
"plt.plot(lower[0,:],lower[1,:],\"g-\")\n",
"plt.plot(upper[0,:],upper[1,:],\"r-\")"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"the points can be used to generate the **halfplane constrains** for each reference point.\n",
"the issues (outliers points) should be gone after we are in vehicle frame...\n",
"\n",
"the halfplane constrains are defined given the line equation:\n",
"\n",
"**lower halfplane**\n",
"$$ a1x_1 + b1x_2 = c1 \\rightarrow a1x_1 + b1x_2 \\leq c1$$\n",
"\n",
"**upper halfplane**\n",
"$$ a2x_1 - b2x_2 = c2 \\rightarrow a2x_1 + b2x_2 \\leq c2$$\n",
"\n",
"we want to combine this in matrix form:\n",
"\n",
"$$\n",
"\\begin{bmatrix}\n",
"x_1 \\\\\n",
"x_2 \n",
"\\end{bmatrix}\n",
"\\begin{bmatrix}\n",
"a_1 & a_2\\\\\n",
"b_1 & b_2\n",
"\\end{bmatrix}\n",
"\\leq\n",
"\\begin{bmatrix}\n",
"c_1 \\\\\n",
"c_2 \n",
"\\end{bmatrix}\n",
"$$\n",
"\n",
"becouse our track points have known heading the coefficients can be computed from:\n",
"\n",
"$$ y - y' = \\frac{sin(\\theta)}{cos(\\theta)}(x - x') $$\n",
"\n",
"we have:\n",
"\n",
"$$\n",
"-tan(\\theta)x + y = - tan(\\theta)x' + y'\n",
"$$\n",
"where:\n",
"* $ a = -tan(\\theta) $\n",
"* $ b = 1 $\n",
"* $ c = - tan(\\theta)x' + y' $"
]
},
{
"cell_type": "code",
"execution_count": 5,
"metadata": {},
"outputs": [
{
"data": {
"text/plain": [
"(-2.0, 2.0)"
]
},
"execution_count": 5,
"metadata": {},
"output_type": "execute_result"
},
{
"data": {
"image/png": "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
"text/plain": [
"<Figure size 360x360 with 1 Axes>"
]
},
"metadata": {},
"output_type": "display_data"
}
],
"source": [
"import numpy as np\n",
"import matplotlib.pyplot as plt\n",
"plt.style.use(\"ggplot\")\n",
"\n",
"def get_coeff(x,y,theta):\n",
" m = np.sin(theta)/np.cos(theta)\n",
" return(-m,1,y-m*x)\n",
"\n",
"#test -> assume point 10,1,pi/6\n",
"#coeff = get_coeff(1,-1, np.pi/2)\n",
"coeff = get_coeff(1,-1, np.pi/4)\n",
"y = []\n",
"pts = np.linspace(0,20,100)\n",
"\n",
"for x in pts:\n",
" y.append((-coeff[0]*x+coeff[2])/coeff[1])\n",
" \n",
"plt.figure(figsize=(5,5))\n",
"plt.plot(pts,y,\"b-\")\n",
"\n",
"plt.xlim((-2, 2))\n",
"plt.ylim((-2, 2))"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"## WARN TANGENT BREAKS AROUND PI/2?\n",
"force the equation to x = val"
]
},
{
"cell_type": "code",
"execution_count": 6,
"metadata": {},
"outputs": [
{
"data": {
"text/plain": [
"(-2.0, 2.0)"
]
},
"execution_count": 6,
"metadata": {},
"output_type": "execute_result"
},
{
"data": {
"image/png": "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
"text/plain": [
"<Figure size 360x360 with 1 Axes>"
]
},
"metadata": {},
"output_type": "display_data"
}
],
"source": [
"# def get_coeff(x,y,theta):\n",
" \n",
"# if (theta - np.pi/2) < 0.01:\n",
"# #print (\"WARN -> theta is 90, tan is: \" + str(theta))\n",
"# # eq is x = val\n",
"# m = 0\n",
"# return (1,1e-6,x)\n",
"# else:\n",
"# m = np.sin(theta)/np.cos(theta)\n",
"# return(-m,1,y-m*x)\n",
" \n",
"#test -> assume point 10,1,pi/6\n",
"coeff = get_coeff(1,-1, np.pi/2)\n",
"y = []\n",
"pts = np.linspace(0,20,100)\n",
"\n",
"for x in pts:\n",
" y.append((-coeff[0]*x+coeff[2])/coeff[1])\n",
" \n",
"plt.figure(figsize=(5,5))\n",
"\n",
"plt.plot(pts,y,\"b-\")\n",
"plt.axis(\"equal\")\n",
"plt.xlim((-2, 2))\n",
"plt.ylim((-2, 2))"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"becouse the controller uses vhicle reference frame this rquire adapting -> the semiplane constraints must be gathetered from **x_ref points**\n",
"\n",
"*low and up are w.r.t vehicle y axis*"
]
},
{
"cell_type": "code",
"execution_count": 7,
"metadata": {},
"outputs": [],
"source": [
"def get_track_constrains(x_ref, width=0.5):\n",
" \"\"\"\n",
" x_ref has hape (4,T) -> [x,y,v,theta]_ref \n",
" \"\"\"\n",
" \n",
" #1-> get the upper and lower points\n",
" pts_low=np.zeros((3, x_ref.shape[1]))\n",
" pts_upp=np.zeros((3, x_ref.shape[1]))\n",
" \n",
" for idx in range(x_ref.shape[1]):\n",
" x = x_ref [0, idx]\n",
" y = x_ref [1, idx]\n",
" th = x_ref [3, idx]\n",
" \n",
" \"\"\"\n",
" trasform the points\n",
" \"\"\"\n",
" pts_upp[0, idx] = 0 * np.cos(th) - width * np.sin(th) + x #X\n",
" pts_upp[1, idx] = 0 * np.sin(th) + width * np.cos(th) + y #Y\n",
" pts_upp[2, idx] = th #heading\n",
" \n",
" pts_low[0, idx] = 0 * np.cos(th) - (-width) * np.sin(th) + x #X\n",
" pts_low[1, idx] = 0 * np.sin(th) + (-width) * np.cos(th) + y #Y\n",
" pts_low[2, idx] = th #heading\n",
" \n",
" #get coefficients ->(a,b,c)\n",
" coeff_low=np.zeros((3, x_ref.shape[1]))\n",
" coeff_upp=np.zeros((3, x_ref.shape[1]))\n",
" \n",
" for idx in range(pts_upp.shape[1]):\n",
" f = get_coeff(pts_low[0,idx],pts_low[1,idx],pts_low[2,idx])\n",
" coeff_low[0,idx]=f[0]\n",
" coeff_low[1,idx]=f[1]\n",
" coeff_low[2,idx]=f[2]\n",
" \n",
" f = get_coeff(pts_upp[0,idx],pts_upp[1,idx],pts_upp[2,idx])\n",
" coeff_upp[0,idx]=f[0]\n",
" coeff_upp[1,idx]=f[1]\n",
" coeff_upp[2,idx]=f[2]\n",
" \n",
" return coeff_low, coeff_upp\n",
" "
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"## MPC INTEGRATION\n",
"\n",
"compare the results with and without"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"simpe u-turn test\n",
"\n",
"## 1-> NO BOUNDS"
]
},
{
"cell_type": "code",
"execution_count": 8,
"metadata": {},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
less cringy readme
2021-04-14 22:17:16 +08:00
"CVXPY Optimization Time: Avrg: 0.1719s Max: 0.2054s Min: 0.1581s\n"
forther r&d in track constrains and plots tidy up
2021-04-14 18:38:30 +08:00
]
}
],
"source": [
"track = compute_path_from_wp([0,3,3,0],\n",
" [0,0,1,1],0.05)\n",
"\n",
"track_lower, track_upper = generate_track_bounds(track,0.12)\n",
"\n",
"sim_duration = 50 #time steps\n",
"opt_time=[]\n",
"\n",
"x_sim = np.zeros((N,sim_duration))\n",
"u_sim = np.zeros((M,sim_duration-1))\n",
"\n",
"MAX_SPEED = 1.5 #m/s\n",
"MAX_ACC = 1.0 #m/ss\n",
"MAX_D_ACC = 1.0 #m/sss\n",
"MAX_STEER = np.radians(30) #rad\n",
"MAX_D_STEER = np.radians(30) #rad/s\n",
"\n",
less cringy readme
2021-04-14 22:17:16 +08:00
"REF_VEL = 0.5 #m/s\n",
forther r&d in track constrains and plots tidy up
2021-04-14 18:38:30 +08:00
"\n",
"# Starting Condition\n",
"x0 = np.zeros(N)\n",
"x0[0] = 0 #x\n",
"x0[1] = -0.05 #y\n",
"x0[2] = 0.0 #v\n",
"x0[3] = np.radians(-0) #yaw\n",
"x_sim[:,0] = x0 #simulation_starting conditions\n",
" \n",
"#starting guess\n",
"u_bar = np.zeros((M,T))\n",
"u_bar[0,:] = MAX_ACC/2 #a\n",
"u_bar[1,:] = 0.0 #delta\n",
" \n",
"for sim_time in range(sim_duration-1):\n",
" \n",
" iter_start = time.time()\n",
" \n",
" # dynamics starting state w.r.t. robot are always null except vel \n",
" x_bar = np.zeros((N,T+1))\n",
" x_bar[2,0] = x_sim[2,sim_time]\n",
" \n",
" #prediction for linearization of costrains\n",
" for t in range (1,T+1):\n",
" xt = x_bar[:,t-1].reshape(N,1)\n",
" ut = u_bar[:,t-1].reshape(M,1)\n",
" A,B,C = get_linear_model(xt,ut)\n",
" xt_plus_one = np.squeeze(np.dot(A,xt)+np.dot(B,ut)+C)\n",
" x_bar[:,t] = xt_plus_one\n",
" \n",
" #CVXPY Linear MPC problem statement\n",
" x = cp.Variable((N, T+1))\n",
" u = cp.Variable((M, T))\n",
" cost = 0\n",
" constr = []\n",
"\n",
" # Cost Matrices\n",
" Q = np.diag([20,20,10,20]) #state error cost\n",
" Qf = np.diag([30,30,30,30]) #state final error cost\n",
" R = np.diag([10,10]) #input cost\n",
" R_ = np.diag([10,10]) #input rate of change cost\n",
"\n",
" #Get Reference_traj\n",
" #dont use x0 in this case\n",
" x_ref, d_ref = get_ref_trajectory(x_sim[:,sim_time] ,track, REF_VEL)\n",
" \n",
" #Prediction Horizon\n",
" for t in range(T):\n",
"\n",
" # Tracking Error\n",
" cost += cp.quad_form(x[:,t] - x_ref[:,t], Q)\n",
"\n",
" # Actuation effort\n",
" cost += cp.quad_form(u[:,t], R)\n",
"\n",
" # Actuation rate of change\n",
" if t < (T - 1):\n",
" cost += cp.quad_form(u[:,t+1] - u[:,t], R_)\n",
" constr+= [cp.abs(u[0, t + 1] - u[0, t])/DT <= MAX_D_ACC] #max acc rate of change\n",
" constr += [cp.abs(u[1, t + 1] - u[1, t])/DT <= MAX_D_STEER] #max steer rate of change\n",
"\n",
" # Kinrmatics Constrains (Linearized model)\n",
" A,B,C = get_linear_model(x_bar[:,t], u_bar[:,t])\n",
" constr += [x[:,t+1] == A@x[:,t] + B@u[:,t] + C.flatten()]\n",
" \n",
" #Final Point tracking\n",
" cost += cp.quad_form(x[:, T] - x_ref[:, T], Qf)\n",
"\n",
" # sums problem objectives and concatenates constraints.\n",
" constr += [x[:,0] == x_bar[:,0]] #starting condition\n",
" constr += [x[2,:] <= MAX_SPEED] #max speed\n",
" constr += [x[2,:] >= 0.0] #min_speed (not really needed)\n",
" constr += [cp.abs(u[0,:]) <= MAX_ACC] #max acc\n",
" constr += [cp.abs(u[1,:]) <= MAX_STEER] #max steer\n",
" \n",
" # Solve\n",
" prob = cp.Problem(cp.Minimize(cost), constr)\n",
" solution = prob.solve(solver=cp.OSQP, verbose=False)\n",
" \n",
" #retrieved optimized U and assign to u_bar to linearize in next step\n",
" u_bar = np.vstack((np.array(u.value[0,:]).flatten(),\n",
" (np.array(u.value[1,:]).flatten())))\n",
" \n",
" u_sim[:,sim_time] = u_bar[:,0]\n",
" \n",
" # Measure elpased time to get results from cvxpy\n",
" opt_time.append(time.time()-iter_start)\n",
" \n",
" # move simulation to t+1\n",
" tspan = [0,DT]\n",
" x_sim[:,sim_time+1] = odeint(kinematics_model,\n",
" x_sim[:,sim_time],\n",
" tspan,\n",
" args=(u_bar[:,0],))[1]\n",
" \n",
"print(\"CVXPY Optimization Time: Avrg: {:.4f}s Max: {:.4f}s Min: {:.4f}s\".format(np.mean(opt_time),\n",
" np.max(opt_time),\n",
" np.min(opt_time))) "
]
},
{
"cell_type": "code",
"execution_count": 9,
"metadata": {},
"outputs": [
{
"data": {
less cringy readme
2021-04-14 22:17:16 +08:00
"image/png": "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
forther r&d in track constrains and plots tidy up
2021-04-14 18:38:30 +08:00
"text/plain": [
"<Figure size 1080x720 with 5 Axes>"
]
},
"metadata": {},
"output_type": "display_data"
}
],
"source": [
"#plot trajectory\n",
"grid = plt.GridSpec(4, 5)\n",
"\n",
"plt.figure(figsize=(15,10))\n",
"\n",
"plt.subplot(grid[0:4, 0:4])\n",
"plt.plot(track[0,:],track[1,:],\"b+\")\n",
"plt.plot(track_lower[0,:],track_lower[1,:],\"g-\")\n",
"plt.plot(track_upper[0,:],track_upper[1,:],\"r-\")\n",
"plt.plot(x_sim[0,:],x_sim[1,:])\n",
"plt.axis(\"equal\")\n",
"plt.ylabel('y')\n",
"plt.xlabel('x')\n",
"\n",
"plt.subplot(grid[0, 4])\n",
"plt.plot(u_sim[0,:])\n",
"plt.ylabel('a(t) [m/ss]')\n",
"\n",
"plt.subplot(grid[1, 4])\n",
"plt.plot(x_sim[2,:])\n",
"plt.ylabel('v(t) [m/s]')\n",
"\n",
"plt.subplot(grid[2, 4])\n",
"plt.plot(np.degrees(u_sim[1,:]))\n",
"plt.ylabel('delta(t) [rad]')\n",
"\n",
"plt.subplot(grid[3, 4])\n",
"plt.plot(x_sim[3,:])\n",
"plt.ylabel('theta(t) [rad]')\n",
"\n",
"plt.tight_layout()\n",
"plt.show()"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"## 2-> WITH BOUNDS\n",
"if there is 90 deg turn the optimization fails!\n",
"if speed is too high it also fails ..."
]
},
{
"cell_type": "code",
less cringy readme
2021-04-14 22:17:16 +08:00
"execution_count": 12,
forther r&d in track constrains and plots tidy up
2021-04-14 18:38:30 +08:00
"metadata": {},
"outputs": [
{
"name": "stdout",
"output_type": "stream",
"text": [
less cringy readme
2021-04-14 22:17:16 +08:00
"CVXPY Optimization Time: Avrg: 0.1803s Max: 0.2286s Min: 0.1630s\n"
forther r&d in track constrains and plots tidy up
2021-04-14 18:38:30 +08:00
]
}
],
"source": [
less cringy readme
2021-04-14 22:17:16 +08:00
"WIDTH=0.18\n",
"REF_VEL = 0.4 #m/s\n",
forther r&d in track constrains and plots tidy up
2021-04-14 18:38:30 +08:00
"\n",
"computed_coeff = []\n",
"\n",
"track = compute_path_from_wp([0,3,3,0],\n",
" [0,0,1,1],0.05)\n",
"\n",
"track_lower, track_upper = generate_track_bounds(track,WIDTH)\n",
"\n",
"sim_duration = 200 #time steps\n",
"opt_time=[]\n",
"\n",
"x_sim = np.zeros((N,sim_duration))\n",
"u_sim = np.zeros((M,sim_duration-1))\n",
"\n",
"MAX_SPEED = 1.5 #m/s\n",
"MAX_ACC = 1.0 #m/ss\n",
"MAX_D_ACC = 1.0 #m/sss\n",
"MAX_STEER = np.radians(30) #rad\n",
"MAX_D_STEER = np.radians(30) #rad/s\n",
"\n",
"\n",
"# Starting Condition\n",
"x0 = np.zeros(N)\n",
"x0[0] = 0 #x\n",
less cringy readme
2021-04-14 22:17:16 +08:00
"x0[1] = WIDTH/2 #y\n",
forther r&d in track constrains and plots tidy up
2021-04-14 18:38:30 +08:00
"x0[2] = 0.0 #v\n",
"x0[3] = np.radians(-0) #yaw\n",
"x_sim[:,0] = x0 #simulation_starting conditions\n",
" \n",
"#starting guess\n",
"u_bar = np.zeros((M,T))\n",
"u_bar[0,:] = MAX_ACC/2 #a\n",
"u_bar[1,:] = 0.0 #delta\n",
" \n",
"for sim_time in range(sim_duration-1):\n",
" \n",
" iter_start = time.time()\n",
" \n",
" # dynamics starting state w.r.t. robot are always null except vel \n",
" x_bar = np.zeros((N,T+1))\n",
" x_bar[2,0] = x_sim[2,sim_time]\n",
" \n",
" #prediction for linearization of costrains\n",
" for t in range (1,T+1):\n",
" xt = x_bar[:,t-1].reshape(N,1)\n",
" ut = u_bar[:,t-1].reshape(M,1)\n",
" A,B,C = get_linear_model(xt,ut)\n",
" xt_plus_one = np.squeeze(np.dot(A,xt)+np.dot(B,ut)+C)\n",
" x_bar[:,t] = xt_plus_one\n",
" \n",
" #CVXPY Linear MPC problem statement\n",
" x = cp.Variable((N, T+1))\n",
" u = cp.Variable((M, T))\n",
" cost = 0\n",
" constr = []\n",
"\n",
" # Cost Matrices\n",
" Q = np.diag([20,20,10,20]) #state error cost\n",
" Qf = np.diag([30,30,30,30]) #state final error cost\n",
" R = np.diag([10,10]) #input cost\n",
" R_ = np.diag([10,10]) #input rate of change cost\n",
"\n",
" #Get Reference_traj\n",
" #dont use x0 in this case\n",
" x_ref, d_ref = get_ref_trajectory(x_sim[:,sim_time] ,track, REF_VEL)\n",
" \n",
" #Prediction Horizon\n",
" for t in range(T):\n",
"\n",
" # Tracking Error\n",
" cost += cp.quad_form(x[:,t] - x_ref[:,t], Q)\n",
"\n",
" # Actuation effort\n",
" cost += cp.quad_form(u[:,t], R)\n",
"\n",
" # Actuation rate of change\n",
" if t < (T - 1):\n",
" cost += cp.quad_form(u[:,t+1] - u[:,t], R_)\n",
" constr+= [cp.abs(u[0, t + 1] - u[0, t])/DT <= MAX_D_ACC] #max acc rate of change\n",
" constr += [cp.abs(u[1, t + 1] - u[1, t])/DT <= MAX_D_STEER] #max steer rate of change\n",
"\n",
" # Kinrmatics Constrains (Linearized model)\n",
" A,B,C = get_linear_model(x_bar[:,t], u_bar[:,t])\n",
" constr += [x[:,t+1] == A@x[:,t] + B@u[:,t] + C.flatten()]\n",
" \n",
" #Final Point tracking\n",
" cost += cp.quad_form(x[:, T] - x_ref[:, T], Qf)\n",
"\n",
" # sums problem objectives and concatenates constraints.\n",
" constr += [x[:,0] == x_bar[:,0]] #starting condition\n",
" constr += [x[2,:] <= MAX_SPEED] #max speed\n",
" constr += [x[2,:] >= 0.0] #min_speed (not really needed)\n",
" constr += [cp.abs(u[0,:]) <= MAX_ACC] #max acc\n",
" constr += [cp.abs(u[1,:]) <= MAX_STEER] #max steer\n",
" \n",
" #Track constrains\n",
" low,upp = get_track_constrains(x_ref,WIDTH)\n",
" computed_coeff.append((low,upp))\n",
" for ii in range(low.shape[1]):\n",
" constr += [low[0,ii]*x[0,ii] + x[1,ii] >= low[2,ii]]\n",
" #constr += [upp[0,ii]*x[0,ii] + x[1,ii] <= upp[2,ii]]\n",
" \n",
" # Solve\n",
" prob = cp.Problem(cp.Minimize(cost), constr)\n",
" solution = prob.solve(solver=cp.OSQP, verbose=False)\n",
" \n",
" #retrieved optimized U and assign to u_bar to linearize in next step\n",
" u_bar = np.vstack((np.array(u.value[0,:]).flatten(),\n",
" (np.array(u.value[1,:]).flatten())))\n",
" \n",
" u_sim[:,sim_time] = u_bar[:,0]\n",
" \n",
" # Measure elpased time to get results from cvxpy\n",
" opt_time.append(time.time()-iter_start)\n",
" \n",
" # move simulation to t+1\n",
" tspan = [0,DT]\n",
" x_sim[:,sim_time+1] = odeint(kinematics_model,\n",
" x_sim[:,sim_time],\n",
" tspan,\n",
" args=(u_bar[:,0],))[1]\n",
" \n",
"print(\"CVXPY Optimization Time: Avrg: {:.4f}s Max: {:.4f}s Min: {:.4f}s\".format(np.mean(opt_time),\n",
" np.max(opt_time),\n",
" np.min(opt_time))) "
]
},
{
"cell_type": "code",
less cringy readme
2021-04-14 22:17:16 +08:00
"execution_count": 19,
forther r&d in track constrains and plots tidy up
2021-04-14 18:38:30 +08:00
"metadata": {},
"outputs": [
{
"data": {
less cringy readme
2021-04-14 22:17:16 +08:00
"image/png": "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
forther r&d in track constrains and plots tidy up
2021-04-14 18:38:30 +08:00
"text/plain": [
"<Figure size 1080x720 with 5 Axes>"
]
},
less cringy readme
2021-04-14 22:17:16 +08:00
"metadata": {
"needs_background": "light"
},
forther r&d in track constrains and plots tidy up
2021-04-14 18:38:30 +08:00
"output_type": "display_data"
}
],
"source": [
less cringy readme
2021-04-14 22:17:16 +08:00
"%matplotlib inline\n",
forther r&d in track constrains and plots tidy up
2021-04-14 18:38:30 +08:00
"#plot trajectory\n",
"grid = plt.GridSpec(4, 5)\n",
"\n",
"plt.figure(figsize=(15,10))\n",
"\n",
"plt.subplot(grid[0:4, 0:4])\n",
"plt.plot(track[0,:],track[1,:],\"b+\")\n",
"plt.plot(track_lower[0,:],track_lower[1,:],\"g.\")\n",
"plt.plot(track_upper[0,:],track_upper[1,:],\"r.\")\n",
"plt.plot(x_sim[0,:],x_sim[1,:])\n",
"plt.axis(\"equal\")\n",
"plt.ylabel('y')\n",
"plt.xlabel('x')\n",
"\n",
"plt.subplot(grid[0, 4])\n",
"plt.plot(u_sim[0,:])\n",
"plt.ylabel('a(t) [m/ss]')\n",
"\n",
"plt.subplot(grid[1, 4])\n",
"plt.plot(x_sim[2,:])\n",
"plt.ylabel('v(t) [m/s]')\n",
"\n",
"\n",
"plt.subplot(grid[2, 4])\n",
"plt.plot(np.degrees(u_sim[1,:]))\n",
"plt.ylabel('delta(t) [rad]')\n",
"\n",
"plt.subplot(grid[3, 4])\n",
"plt.plot(x_sim[3,:])\n",
"plt.ylabel('theta(t) [rad]')\n",
"\n",
"plt.tight_layout()\n",
"plt.show()"
]
},
{
"cell_type": "markdown",
"metadata": {},
"source": [
"## VISUALIZE THE COMPUTED HALF-PLANES"
]
},
{
"cell_type": "code",
less cringy readme
2021-04-14 22:17:16 +08:00
"execution_count": 14,
forther r&d in track constrains and plots tidy up
2021-04-14 18:38:30 +08:00
"metadata": {},
"outputs": [
{
"data": {
less cringy readme
2021-04-14 22:17:16 +08:00
"image/png": "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
forther r&d in track constrains and plots tidy up
2021-04-14 18:38:30 +08:00
"text/plain": [
"<Figure size 1080x1080 with 9 Axes>"
]
},
"metadata": {},
"output_type": "display_data"
}
],
"source": [
"import numpy as np\n",
"import matplotlib.pyplot as plt\n",
"plt.style.use(\"ggplot\")\n",
"\n",
"\n",
"def plot_low_upp(low, up):\n",
" \"\"\"\n",
" low and upp arre arrays of shape (3,N)\n",
" each column represent the couefficient at each\n",
" step of the optimization\n",
" \"\"\"\n",
" \n",
" #for low,up in zip(low, up):\n",
" for idx in range(low.shape[1]):\n",
" \n",
" ll = low[:,idx]\n",
" uu = up[:,idx]\n",
" \n",
" x = np.linspace(-2,2,100)\n",
"\n",
" #low\n",
" y = []\n",
" for xx in x:\n",
" y.append((-ll[0]*xx+ll[2])/ll[1])\n",
"\n",
" plt.plot(x,y,\"b-\")\n",
"\n",
" #high\n",
" y = []\n",
" for xx in x:\n",
" y.append((-uu[0]*xx+uu[2])/uu[1])\n",
"\n",
" plt.plot(x,y,\"r-\")\n",
"\n",
" plt.xlim((-2, 2))\n",
" plt.ylim((-2, 2))\n",
"\n",
"def plot_lines():\n",
" \"\"\"\n",
" sample randomly from computed coeff\n",
" and plot grid\n",
" \"\"\"\n",
" \n",
" plt.figure(figsize=(15,15))\n",
" indices = np.random.choice(len(computed_coeff), 9)\n",
"\n",
" for i in range(len(indices)):\n",
" plt.subplot(3,3,i+1)\n",
" plt.title(\"t = \"+str(indices[i]))\n",
" plot_low_upp(computed_coeff[indices[i]][0]\n",
" ,computed_coeff[indices[i]][1])\n",
" \n",
" plt.tight_layout()\n",
" plt.show()\n",
"\n",
"plot_lines()"
]
},
{
"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
}