1095 lines
424 KiB
Plaintext
1095 lines
424 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": "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",
|
||
|
|
" window_size = 11 # Smoothening filter window\n",
|
||
|
|
" final_xp = savgol_filter(final_xp, window_size, 1)\n",
|
||
|
|
" final_yp = savgol_filter(final_yp, window_size, 1)\n",
|
||
|
|
" \n",
|
||
|
|
" dx = np.append(0, np.diff(final_xp))\n",
|
||
|
|
" dy = np.append(0, np.diff(final_yp))\n",
|
||
|
|
" theta = np.arctan2(dy, dx)\n",
|
||
|
|
"\n",
|
||
|
|
" return np.vstack((final_xp,final_yp,theta))\n"
|
||
|
|
]
|
||
|
|
},
|
||
|
|
{
|
||
|
|
"cell_type": "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": [
|
||
|
|
"[<matplotlib.lines.Line2D at 0x7f3b8df30610>]"
|
||
|
|
]
|
||
|
|
},
|
||
|
|
"execution_count": 4,
|
||
|
|
"metadata": {},
|
||
|
|
"output_type": "execute_result"
|
||
|
|
},
|
||
|
|
{
|
||
|
|
"data": {
|
||
|
|
"image/png": "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
|
||
|
|
"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": [
|
||
|
|
"CVXPY Optimization Time: Avrg: 0.1672s Max: 0.2184s Min: 0.1477s\n"
|
||
|
|
]
|
||
|
|
}
|
||
|
|
],
|
||
|
|
"source": [
|
||
|
|
"track = compute_path_from_wp([0,3,3,0],\n",
|
||
|
|
" [0,0,1,1],0.05)\n",
|
||
|
|
"\n",
|
||
|
|
"track_lower, track_upper = generate_track_bounds(track,0.12)\n",
|
||
|
|
"\n",
|
||
|
|
"sim_duration = 50 #time steps\n",
|
||
|
|
"opt_time=[]\n",
|
||
|
|
"\n",
|
||
|
|
"x_sim = np.zeros((N,sim_duration))\n",
|
||
|
|
"u_sim = np.zeros((M,sim_duration-1))\n",
|
||
|
|
"\n",
|
||
|
|
"MAX_SPEED = 1.5 #m/s\n",
|
||
|
|
"MAX_ACC = 1.0 #m/ss\n",
|
||
|
|
"MAX_D_ACC = 1.0 #m/sss\n",
|
||
|
|
"MAX_STEER = np.radians(30) #rad\n",
|
||
|
|
"MAX_D_STEER = np.radians(30) #rad/s\n",
|
||
|
|
"\n",
|
||
|
|
"REF_VEL = 1.0 #m/s\n",
|
||
|
|
"\n",
|
||
|
|
"# Starting Condition\n",
|
||
|
|
"x0 = np.zeros(N)\n",
|
||
|
|
"x0[0] = 0 #x\n",
|
||
|
|
"x0[1] = -0.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": {
|
||
|
|
"image/png": "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
|
||
|
|
"text/plain": [
|
||
|
|
"<Figure size 1080x720 with 5 Axes>"
|
||
|
|
]
|
||
|
|
},
|
||
|
|
"metadata": {},
|
||
|
|
"output_type": "display_data"
|
||
|
|
}
|
||
|
|
],
|
||
|
|
"source": [
|
||
|
|
"#plot trajectory\n",
|
||
|
|
"grid = plt.GridSpec(4, 5)\n",
|
||
|
|
"\n",
|
||
|
|
"plt.figure(figsize=(15,10))\n",
|
||
|
|
"\n",
|
||
|
|
"plt.subplot(grid[0:4, 0:4])\n",
|
||
|
|
"plt.plot(track[0,:],track[1,:],\"b+\")\n",
|
||
|
|
"plt.plot(track_lower[0,:],track_lower[1,:],\"g-\")\n",
|
||
|
|
"plt.plot(track_upper[0,:],track_upper[1,:],\"r-\")\n",
|
||
|
|
"plt.plot(x_sim[0,:],x_sim[1,:])\n",
|
||
|
|
"plt.axis(\"equal\")\n",
|
||
|
|
"plt.ylabel('y')\n",
|
||
|
|
"plt.xlabel('x')\n",
|
||
|
|
"\n",
|
||
|
|
"plt.subplot(grid[0, 4])\n",
|
||
|
|
"plt.plot(u_sim[0,:])\n",
|
||
|
|
"plt.ylabel('a(t) [m/ss]')\n",
|
||
|
|
"\n",
|
||
|
|
"plt.subplot(grid[1, 4])\n",
|
||
|
|
"plt.plot(x_sim[2,:])\n",
|
||
|
|
"plt.ylabel('v(t) [m/s]')\n",
|
||
|
|
"\n",
|
||
|
|
"plt.subplot(grid[2, 4])\n",
|
||
|
|
"plt.plot(np.degrees(u_sim[1,:]))\n",
|
||
|
|
"plt.ylabel('delta(t) [rad]')\n",
|
||
|
|
"\n",
|
||
|
|
"plt.subplot(grid[3, 4])\n",
|
||
|
|
"plt.plot(x_sim[3,:])\n",
|
||
|
|
"plt.ylabel('theta(t) [rad]')\n",
|
||
|
|
"\n",
|
||
|
|
"plt.tight_layout()\n",
|
||
|
|
"plt.show()"
|
||
|
|
]
|
||
|
|
},
|
||
|
|
{
|
||
|
|
"cell_type": "markdown",
|
||
|
|
"metadata": {},
|
||
|
|
"source": [
|
||
|
|
"## 2-> WITH BOUNDS\n",
|
||
|
|
"if there is 90 deg turn the optimization fails!\n",
|
||
|
|
"if speed is too high it also fails ..."
|
||
|
|
]
|
||
|
|
},
|
||
|
|
{
|
||
|
|
"cell_type": "code",
|
||
|
|
"execution_count": 13,
|
||
|
|
"metadata": {},
|
||
|
|
"outputs": [
|
||
|
|
{
|
||
|
|
"name": "stdout",
|
||
|
|
"output_type": "stream",
|
||
|
|
"text": [
|
||
|
|
"CVXPY Optimization Time: Avrg: 0.1760s Max: 0.2117s Min: 0.1617s\n"
|
||
|
|
]
|
||
|
|
}
|
||
|
|
],
|
||
|
|
"source": [
|
||
|
|
"WIDTH=0.25\n",
|
||
|
|
"REF_VEL = 0.2 #m/s\n",
|
||
|
|
"\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",
|
||
|
|
"x0[1] = -WIDTH/2 #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",
|
||
|
|
" #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",
|
||
|
|
"execution_count": 14,
|
||
|
|
"metadata": {},
|
||
|
|
"outputs": [
|
||
|
|
{
|
||
|
|
"data": {
|
||
|
|
"image/png": "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
|
||
|
|
"text/plain": [
|
||
|
|
"<Figure size 1080x720 with 5 Axes>"
|
||
|
|
]
|
||
|
|
},
|
||
|
|
"metadata": {},
|
||
|
|
"output_type": "display_data"
|
||
|
|
}
|
||
|
|
],
|
||
|
|
"source": [
|
||
|
|
"#plot trajectory\n",
|
||
|
|
"grid = plt.GridSpec(4, 5)\n",
|
||
|
|
"\n",
|
||
|
|
"plt.figure(figsize=(15,10))\n",
|
||
|
|
"\n",
|
||
|
|
"plt.subplot(grid[0:4, 0:4])\n",
|
||
|
|
"plt.plot(track[0,:],track[1,:],\"b+\")\n",
|
||
|
|
"plt.plot(track_lower[0,:],track_lower[1,:],\"g.\")\n",
|
||
|
|
"plt.plot(track_upper[0,:],track_upper[1,:],\"r.\")\n",
|
||
|
|
"plt.plot(x_sim[0,:],x_sim[1,:])\n",
|
||
|
|
"plt.axis(\"equal\")\n",
|
||
|
|
"plt.ylabel('y')\n",
|
||
|
|
"plt.xlabel('x')\n",
|
||
|
|
"\n",
|
||
|
|
"plt.subplot(grid[0, 4])\n",
|
||
|
|
"plt.plot(u_sim[0,:])\n",
|
||
|
|
"plt.ylabel('a(t) [m/ss]')\n",
|
||
|
|
"\n",
|
||
|
|
"plt.subplot(grid[1, 4])\n",
|
||
|
|
"plt.plot(x_sim[2,:])\n",
|
||
|
|
"plt.ylabel('v(t) [m/s]')\n",
|
||
|
|
"\n",
|
||
|
|
"\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",
|
||
|
|
"execution_count": 15,
|
||
|
|
"metadata": {},
|
||
|
|
"outputs": [
|
||
|
|
{
|
||
|
|
"data": {
|
||
|
|
"image/png": "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
|
||
|
|
"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
|
||
|
|
}
|