1126 lines
312 KiB
Plaintext
1126 lines
312 KiB
Plaintext
|
{
|
||
|
|
"cells": [
|
||
|
|
{
|
||
|
|
"cell_type": "code",
|
||
|
|
"execution_count": null,
|
||
|
|
"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": [
|
||
|
|
"## 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": 19,
|
||
|
|
"metadata": {},
|
||
|
|
"outputs": [
|
||
|
|
{
|
||
|
|
"data": {
|
||
|
|
"text/plain": [
|
||
|
|
"[<matplotlib.lines.Line2D at 0x7f35d2a74b80>]"
|
||
|
|
]
|
||
|
|
},
|
||
|
|
"execution_count": 19,
|
||
|
|
"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') $$"
|
||
|
|
]
|
||
|
|
},
|
||
|
|
{
|
||
|
|
"cell_type": "code",
|
||
|
|
"execution_count": 57,
|
||
|
|
"metadata": {},
|
||
|
|
"outputs": [
|
||
|
|
{
|
||
|
|
"data": {
|
||
|
|
"text/plain": [
|
||
|
|
"(-2.0, 2.0)"
|
||
|
|
]
|
||
|
|
},
|
||
|
|
"execution_count": 57,
|
||
|
|
"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",
|
||
|
|
"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": 56,
|
||
|
|
"metadata": {},
|
||
|
|
"outputs": [
|
||
|
|
{
|
||
|
|
"data": {
|
||
|
|
"text/plain": [
|
||
|
|
"(-2.0, 2.0)"
|
||
|
|
]
|
||
|
|
},
|
||
|
|
"execution_count": 56,
|
||
|
|
"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": 21,
|
||
|
|
"metadata": {},
|
||
|
|
"outputs": [],
|
||
|
|
"source": [
|
||
|
|
"def get_track_constrains(x_ref, width=0.5):\n",
|
||
|
|
" \"\"\"\n",
|
||
|
|
" x_ref has hape (4,T) -> [x,y,v,theta]_ref \n",
|
||
|
|
" \"\"\"\n",
|
||
|
|
" \n",
|
||
|
|
" #1-> get the upper and lower points\n",
|
||
|
|
" pts_low=np.zeros((3, x_ref.shape[1]))\n",
|
||
|
|
" pts_upp=np.zeros((3, x_ref.shape[1]))\n",
|
||
|
|
" \n",
|
||
|
|
" for idx in range(x_ref.shape[1]):\n",
|
||
|
|
" x = x_ref [0, idx]\n",
|
||
|
|
" y = x_ref [1, idx]\n",
|
||
|
|
" th = x_ref [3, idx]\n",
|
||
|
|
" \n",
|
||
|
|
" \"\"\"\n",
|
||
|
|
" trasform the points\n",
|
||
|
|
" \"\"\"\n",
|
||
|
|
" pts_upp[0, idx] = 0 * np.cos(th) - width * np.sin(th) + x #X\n",
|
||
|
|
" pts_upp[1, idx] = 0 * np.sin(th) + width * np.cos(th) + y #Y\n",
|
||
|
|
" pts_upp[2, idx] = th #heading\n",
|
||
|
|
" \n",
|
||
|
|
" pts_low[0, idx] = 0 * np.cos(th) - (-width) * np.sin(th) + x #X\n",
|
||
|
|
" pts_low[1, idx] = 0 * np.sin(th) + (-width) * np.cos(th) + y #Y\n",
|
||
|
|
" pts_low[2, idx] = th #heading\n",
|
||
|
|
" \n",
|
||
|
|
" #get coefficients ->(a,b,c)\n",
|
||
|
|
" coeff_low=np.zeros((3, x_ref.shape[1]))\n",
|
||
|
|
" coeff_upp=np.zeros((3, x_ref.shape[1]))\n",
|
||
|
|
" \n",
|
||
|
|
" for idx in range(pts_upp.shape[1]):\n",
|
||
|
|
" f = get_coeff(pts_low[0,idx],pts_low[1,idx],pts_low[2,idx])\n",
|
||
|
|
" coeff_low[0,idx]=f[0]\n",
|
||
|
|
" coeff_low[1,idx]=f[1]\n",
|
||
|
|
" coeff_low[2,idx]=f[2]\n",
|
||
|
|
" \n",
|
||
|
|
" f = get_coeff(pts_upp[0,idx],pts_upp[1,idx],pts_upp[2,idx])\n",
|
||
|
|
" coeff_upp[0,idx]=f[0]\n",
|
||
|
|
" coeff_upp[1,idx]=f[1]\n",
|
||
|
|
" coeff_upp[2,idx]=f[2]\n",
|
||
|
|
" \n",
|
||
|
|
" return coeff_low, coeff_upp\n",
|
||
|
|
" "
|
||
|
|
]
|
||
|
|
},
|
||
|
|
{
|
||
|
|
"cell_type": "markdown",
|
||
|
|
"metadata": {},
|
||
|
|
"source": [
|
||
|
|
"## MPC INTEGRATION\n",
|
||
|
|
"\n",
|
||
|
|
"compare the results with and without"
|
||
|
|
]
|
||
|
|
},
|
||
|
|
{
|
||
|
|
"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": [
|
||
|
|
"simpe u-turn test\n",
|
||
|
|
"\n",
|
||
|
|
"## 1-> NO BOUNDS"
|
||
|
|
]
|
||
|
|
},
|
||
|
|
{
|
||
|
|
"cell_type": "code",
|
||
|
|
"execution_count": 22,
|
||
|
|
"metadata": {},
|
||
|
|
"outputs": [
|
||
|
|
{
|
||
|
|
"name": "stdout",
|
||
|
|
"output_type": "stream",
|
||
|
|
"text": [
|
||
|
|
"CVXPY Optimization Time: Avrg: 0.1656s Max: 0.2061s Min: 0.1480s\n"
|
||
|
|
]
|
||
|
|
}
|
||
|
|
],
|
||
|
|
"source": [
|
||
|
|
"track = compute_path_from_wp([0,3,3,0],\n",
|
||
|
|
" [0,0,1,1],0.05)\n",
|
||
|
|
"\n",
|
||
|
|
"track_lower, track_upper = generate_track_bounds(track,0.12)\n",
|
||
|
|
"\n",
|
||
|
|
"sim_duration = 50 #time steps\n",
|
||
|
|
"opt_time=[]\n",
|
||
|
|
"\n",
|
||
|
|
"x_sim = np.zeros((N,sim_duration))\n",
|
||
|
|
"u_sim = np.zeros((M,sim_duration-1))\n",
|
||
|
|
"\n",
|
||
|
|
"MAX_SPEED = 1.5 #m/s\n",
|
||
|
|
"MAX_ACC = 1.0 #m/ss\n",
|
||
|
|
"MAX_D_ACC = 1.0 #m/sss\n",
|
||
|
|
"MAX_STEER = np.radians(30) #rad\n",
|
||
|
|
"MAX_D_STEER = np.radians(30) #rad/s\n",
|
||
|
|
"\n",
|
||
|
|
"REF_VEL = 1.0 #m/s\n",
|
||
|
|
"\n",
|
||
|
|
"# Starting Condition\n",
|
||
|
|
"x0 = np.zeros(N)\n",
|
||
|
|
"x0[0] = 0 #x\n",
|
||
|
|
"x0[1] = -0.25 #y\n",
|
||
|
|
"x0[2] = 0.0 #v\n",
|
||
|
|
"x0[3] = np.radians(-0) #yaw\n",
|
||
|
|
" \n",
|
||
|
|
"#starting guess\n",
|
||
|
|
"u_bar = np.zeros((M,T))\n",
|
||
|
|
"u_bar[0,:] = MAX_ACC/2 #a\n",
|
||
|
|
"u_bar[1,:] = 0.0 #delta\n",
|
||
|
|
" \n",
|
||
|
|
"for sim_time in range(sim_duration-1):\n",
|
||
|
|
" \n",
|
||
|
|
" iter_start = time.time()\n",
|
||
|
|
" \n",
|
||
|
|
" # dynamics starting state w.r.t. robot are always null except vel \n",
|
||
|
|
" x_bar = np.zeros((N,T+1))\n",
|
||
|
|
" x_bar[2,0] = x_sim[2,sim_time]\n",
|
||
|
|
" \n",
|
||
|
|
" #prediction for linearization of costrains\n",
|
||
|
|
" for t in range (1,T+1):\n",
|
||
|
|
" xt = x_bar[:,t-1].reshape(N,1)\n",
|
||
|
|
" ut = u_bar[:,t-1].reshape(M,1)\n",
|
||
|
|
" A,B,C = get_linear_model(xt,ut)\n",
|
||
|
|
" xt_plus_one = np.squeeze(np.dot(A,xt)+np.dot(B,ut)+C)\n",
|
||
|
|
" x_bar[:,t] = xt_plus_one\n",
|
||
|
|
" \n",
|
||
|
|
" #CVXPY Linear MPC problem statement\n",
|
||
|
|
" x = cp.Variable((N, T+1))\n",
|
||
|
|
" u = cp.Variable((M, T))\n",
|
||
|
|
" cost = 0\n",
|
||
|
|
" constr = []\n",
|
||
|
|
"\n",
|
||
|
|
" # Cost Matrices\n",
|
||
|
|
" Q = np.diag([20,20,10,20]) #state error cost\n",
|
||
|
|
" Qf = np.diag([30,30,30,30]) #state final error cost\n",
|
||
|
|
" R = np.diag([10,10]) #input cost\n",
|
||
|
|
" R_ = np.diag([10,10]) #input rate of change cost\n",
|
||
|
|
"\n",
|
||
|
|
" #Get Reference_traj\n",
|
||
|
|
" #dont use x0 in this case\n",
|
||
|
|
" x_ref, d_ref = get_ref_trajectory(x_sim[:,sim_time] ,track, REF_VEL)\n",
|
||
|
|
" \n",
|
||
|
|
" #Prediction Horizon\n",
|
||
|
|
" for t in range(T):\n",
|
||
|
|
"\n",
|
||
|
|
" # Tracking Error\n",
|
||
|
|
" cost += cp.quad_form(x[:,t] - x_ref[:,t], Q)\n",
|
||
|
|
"\n",
|
||
|
|
" # Actuation effort\n",
|
||
|
|
" cost += cp.quad_form(u[:,t], R)\n",
|
||
|
|
"\n",
|
||
|
|
" # Actuation rate of change\n",
|
||
|
|
" if t < (T - 1):\n",
|
||
|
|
" cost += cp.quad_form(u[:,t+1] - u[:,t], R_)\n",
|
||
|
|
" constr+= [cp.abs(u[0, t + 1] - u[0, t])/DT <= MAX_D_ACC] #max acc rate of change\n",
|
||
|
|
" constr += [cp.abs(u[1, t + 1] - u[1, t])/DT <= MAX_D_STEER] #max steer rate of change\n",
|
||
|
|
"\n",
|
||
|
|
" # Kinrmatics Constrains (Linearized model)\n",
|
||
|
|
" A,B,C = get_linear_model(x_bar[:,t], u_bar[:,t])\n",
|
||
|
|
" constr += [x[:,t+1] == A@x[:,t] + B@u[:,t] + C.flatten()]\n",
|
||
|
|
" \n",
|
||
|
|
" #Final Point tracking\n",
|
||
|
|
" cost += cp.quad_form(x[:, T] - x_ref[:, T], Qf)\n",
|
||
|
|
"\n",
|
||
|
|
" # sums problem objectives and concatenates constraints.\n",
|
||
|
|
" constr += [x[:,0] == x_bar[:,0]] #starting condition\n",
|
||
|
|
" constr += [x[2,:] <= MAX_SPEED] #max speed\n",
|
||
|
|
" constr += [x[2,:] >= 0.0] #min_speed (not really needed)\n",
|
||
|
|
" constr += [cp.abs(u[0,:]) <= MAX_ACC] #max acc\n",
|
||
|
|
" constr += [cp.abs(u[1,:]) <= MAX_STEER] #max steer\n",
|
||
|
|
" \n",
|
||
|
|
" # Solve\n",
|
||
|
|
" prob = cp.Problem(cp.Minimize(cost), constr)\n",
|
||
|
|
" solution = prob.solve(solver=cp.OSQP, verbose=False)\n",
|
||
|
|
" \n",
|
||
|
|
" #retrieved optimized U and assign to u_bar to linearize in next step\n",
|
||
|
|
" u_bar = np.vstack((np.array(u.value[0,:]).flatten(),\n",
|
||
|
|
" (np.array(u.value[1,:]).flatten())))\n",
|
||
|
|
" \n",
|
||
|
|
" u_sim[:,sim_time] = u_bar[:,0]\n",
|
||
|
|
" \n",
|
||
|
|
" # Measure elpased time to get results from cvxpy\n",
|
||
|
|
" opt_time.append(time.time()-iter_start)\n",
|
||
|
|
" \n",
|
||
|
|
" # move simulation to t+1\n",
|
||
|
|
" tspan = [0,DT]\n",
|
||
|
|
" x_sim[:,sim_time+1] = odeint(kinematics_model,\n",
|
||
|
|
" x_sim[:,sim_time],\n",
|
||
|
|
" tspan,\n",
|
||
|
|
" args=(u_bar[:,0],))[1]\n",
|
||
|
|
" \n",
|
||
|
|
"print(\"CVXPY Optimization Time: Avrg: {:.4f}s Max: {:.4f}s Min: {:.4f}s\".format(np.mean(opt_time),\n",
|
||
|
|
" np.max(opt_time),\n",
|
||
|
|
" np.min(opt_time))) "
|
||
|
|
]
|
||
|
|
},
|
||
|
|
{
|
||
|
|
"cell_type": "code",
|
||
|
|
"execution_count": 23,
|
||
|
|
"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": 24,
|
||
|
|
"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": "code",
|
||
|
|
"execution_count": 62,
|
||
|
|
"metadata": {},
|
||
|
|
"outputs": [
|
||
|
|
{
|
||
|
|
"name": "stderr",
|
||
|
|
"output_type": "stream",
|
||
|
|
"text": [
|
||
|
|
"/home/marcello/.conda/envs/jupyter/lib/python3.8/site-packages/cvxpy/problems/problem.py:1054: UserWarning: Solution may be inaccurate. Try another solver, adjusting the solver settings, or solve with verbose=True for more information.\n",
|
||
|
|
" warnings.warn(\n"
|
||
|
|
]
|
||
|
|
},
|
||
|
|
{
|
||
|
|
"name": "stdout",
|
||
|
|
"output_type": "stream",
|
||
|
|
"text": [
|
||
|
|
"CVXPY Optimization Time: Avrg: 0.1837s Max: 0.2651s Min: 0.1593s\n"
|
||
|
|
]
|
||
|
|
}
|
||
|
|
],
|
||
|
|
"source": [
|
||
|
|
"WIDTH=0.12\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",
|
||
|
|
"REF_VEL = 0.4 #m/s\n",
|
||
|
|
"\n",
|
||
|
|
"# Starting Condition\n",
|
||
|
|
"x0 = np.zeros(N)\n",
|
||
|
|
"x0[0] = 0 #x\n",
|
||
|
|
"x0[1] = -WIDTH/2 #y\n",
|
||
|
|
"x0[2] = 0.0 #v\n",
|
||
|
|
"x0[3] = np.radians(-0) #yaw\n",
|
||
|
|
" \n",
|
||
|
|
"#starting guess\n",
|
||
|
|
"u_bar = np.zeros((M,T))\n",
|
||
|
|
"u_bar[0,:] = MAX_ACC/2 #a\n",
|
||
|
|
"u_bar[1,:] = 0.0 #delta\n",
|
||
|
|
" \n",
|
||
|
|
"for sim_time in range(sim_duration-1):\n",
|
||
|
|
" \n",
|
||
|
|
" iter_start = time.time()\n",
|
||
|
|
" \n",
|
||
|
|
" # dynamics starting state w.r.t. robot are always null except vel \n",
|
||
|
|
" x_bar = np.zeros((N,T+1))\n",
|
||
|
|
" x_bar[2,0] = x_sim[2,sim_time]\n",
|
||
|
|
" \n",
|
||
|
|
" #prediction for linearization of costrains\n",
|
||
|
|
" for t in range (1,T+1):\n",
|
||
|
|
" xt = x_bar[:,t-1].reshape(N,1)\n",
|
||
|
|
" ut = u_bar[:,t-1].reshape(M,1)\n",
|
||
|
|
" A,B,C = get_linear_model(xt,ut)\n",
|
||
|
|
" xt_plus_one = np.squeeze(np.dot(A,xt)+np.dot(B,ut)+C)\n",
|
||
|
|
" x_bar[:,t] = xt_plus_one\n",
|
||
|
|
" \n",
|
||
|
|
" #CVXPY Linear MPC problem statement\n",
|
||
|
|
" x = cp.Variable((N, T+1))\n",
|
||
|
|
" u = cp.Variable((M, T))\n",
|
||
|
|
" cost = 0\n",
|
||
|
|
" constr = []\n",
|
||
|
|
"\n",
|
||
|
|
" # Cost Matrices\n",
|
||
|
|
" Q = np.diag([20,20,10,20]) #state error cost\n",
|
||
|
|
" Qf = np.diag([30,30,30,30]) #state final error cost\n",
|
||
|
|
" R = np.diag([10,10]) #input cost\n",
|
||
|
|
" R_ = np.diag([10,10]) #input rate of change cost\n",
|
||
|
|
"\n",
|
||
|
|
" #Get Reference_traj\n",
|
||
|
|
" #dont use x0 in this case\n",
|
||
|
|
" x_ref, d_ref = get_ref_trajectory(x_sim[:,sim_time] ,track, REF_VEL)\n",
|
||
|
|
" \n",
|
||
|
|
" #Prediction Horizon\n",
|
||
|
|
" for t in range(T):\n",
|
||
|
|
"\n",
|
||
|
|
" # Tracking Error\n",
|
||
|
|
" cost += cp.quad_form(x[:,t] - x_ref[:,t], Q)\n",
|
||
|
|
"\n",
|
||
|
|
" # Actuation effort\n",
|
||
|
|
" cost += cp.quad_form(u[:,t], R)\n",
|
||
|
|
"\n",
|
||
|
|
" # Actuation rate of change\n",
|
||
|
|
" if t < (T - 1):\n",
|
||
|
|
" cost += cp.quad_form(u[:,t+1] - u[:,t], R_)\n",
|
||
|
|
" constr+= [cp.abs(u[0, t + 1] - u[0, t])/DT <= MAX_D_ACC] #max acc rate of change\n",
|
||
|
|
" constr += [cp.abs(u[1, t + 1] - u[1, t])/DT <= MAX_D_STEER] #max steer rate of change\n",
|
||
|
|
"\n",
|
||
|
|
" # Kinrmatics Constrains (Linearized model)\n",
|
||
|
|
" A,B,C = get_linear_model(x_bar[:,t], u_bar[:,t])\n",
|
||
|
|
" constr += [x[:,t+1] == A@x[:,t] + B@u[:,t] + C.flatten()]\n",
|
||
|
|
" \n",
|
||
|
|
" #Final Point tracking\n",
|
||
|
|
" cost += cp.quad_form(x[:, T] - x_ref[:, T], Qf)\n",
|
||
|
|
"\n",
|
||
|
|
" # sums problem objectives and concatenates constraints.\n",
|
||
|
|
" constr += [x[:,0] == x_bar[:,0]] #starting condition\n",
|
||
|
|
" constr += [x[2,:] <= MAX_SPEED] #max speed\n",
|
||
|
|
" constr += [x[2,:] >= 0.0] #min_speed (not really needed)\n",
|
||
|
|
" constr += [cp.abs(u[0,:]) <= MAX_ACC] #max acc\n",
|
||
|
|
" constr += [cp.abs(u[1,:]) <= MAX_STEER] #max steer\n",
|
||
|
|
" \n",
|
||
|
|
" #Track constrains\n",
|
||
|
|
" low,upp = get_track_constrains(x_ref,WIDTH)\n",
|
||
|
|
" 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": 63,
|
||
|
|
"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": 66,
|
||
|
|
"metadata": {},
|
||
|
|
"outputs": [
|
||
|
|
{
|
||
|
|
"data": {
|
||
|
|
"image/png": "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
|
||
|
|
"text/plain": [
|
||
|
|
"<Figure size 360x360 with 4 Axes>"
|
||
|
|
]
|
||
|
|
},
|
||
|
|
"metadata": {},
|
||
|
|
"output_type": "display_data"
|
||
|
|
}
|
||
|
|
],
|
||
|
|
"source": [
|
||
|
|
"import numpy as np\n",
|
||
|
|
"import matplotlib.pyplot as plt\n",
|
||
|
|
"plt.style.use(\"ggplot\")\n",
|
||
|
|
"\n",
|
||
|
|
"times = np.linspace(1,len(computed_coeff)/10,4).astype(int)\n",
|
||
|
|
"\n",
|
||
|
|
"plt.figure(figsize=(5,5))\n",
|
||
|
|
"pts = np.linspace(-2,2,100)\n",
|
||
|
|
"\n",
|
||
|
|
"\"\"\"\n",
|
||
|
|
"this needs tydy up badly...\n",
|
||
|
|
"\"\"\"\n",
|
||
|
|
"\n",
|
||
|
|
"plt.subplot(2, 2, 1)\n",
|
||
|
|
"c1 = computed_coeff[times[0]][0]\n",
|
||
|
|
"c2 = computed_coeff[times[0]][1]\n",
|
||
|
|
"for idx in range(c.shape[1]):\n",
|
||
|
|
" #low\n",
|
||
|
|
" coeff = c1[:,idx]\n",
|
||
|
|
" y = []\n",
|
||
|
|
"\n",
|
||
|
|
" for x in pts:\n",
|
||
|
|
" y.append((-coeff[0]*x+coeff[2])/coeff[1])\n",
|
||
|
|
" \n",
|
||
|
|
" plt.plot(pts,y,\"b-\")\n",
|
||
|
|
" \n",
|
||
|
|
" #high\n",
|
||
|
|
" coeff = c2[:,idx]\n",
|
||
|
|
" y = []\n",
|
||
|
|
"\n",
|
||
|
|
" for x in pts:\n",
|
||
|
|
" y.append((-coeff[0]*x+coeff[2])/coeff[1])\n",
|
||
|
|
" \n",
|
||
|
|
" plt.plot(pts,y,\"r-\")\n",
|
||
|
|
" plt.xlim((-2, 2))\n",
|
||
|
|
" plt.ylim((-2, 2))\n",
|
||
|
|
"\n",
|
||
|
|
"\n",
|
||
|
|
"plt.subplot(2, 2, 2)\n",
|
||
|
|
"c1 = computed_coeff[times[1]][0]\n",
|
||
|
|
"c2 = computed_coeff[times[1]][1]\n",
|
||
|
|
"for idx in range(c.shape[1]):\n",
|
||
|
|
" #low\n",
|
||
|
|
" coeff = c1[:,idx]\n",
|
||
|
|
" y = []\n",
|
||
|
|
" for x in pts:\n",
|
||
|
|
" y.append((-coeff[0]*x+coeff[2])/coeff[1])\n",
|
||
|
|
" \n",
|
||
|
|
" plt.plot(pts,y,\"b-\")\n",
|
||
|
|
" \n",
|
||
|
|
" #high\n",
|
||
|
|
" coeff = c2[:,idx]\n",
|
||
|
|
" y = []\n",
|
||
|
|
" for x in pts:\n",
|
||
|
|
" y.append((-coeff[0]*x+coeff[2])/coeff[1])\n",
|
||
|
|
" \n",
|
||
|
|
" plt.plot(pts,y,\"r-\")\n",
|
||
|
|
" plt.xlim((-2, 2))\n",
|
||
|
|
" plt.ylim((-2, 2))\n",
|
||
|
|
"\n",
|
||
|
|
"\n",
|
||
|
|
"plt.subplot(2, 2, 3)\n",
|
||
|
|
"c1 = computed_coeff[times[2]][0]\n",
|
||
|
|
"c2 = computed_coeff[times[2]][1]\n",
|
||
|
|
"for idx in range(c.shape[1]):\n",
|
||
|
|
" #low\n",
|
||
|
|
" coeff = c1[:,idx]\n",
|
||
|
|
" y = []\n",
|
||
|
|
"\n",
|
||
|
|
" for x in pts:\n",
|
||
|
|
" y.append((-coeff[0]*x+coeff[2])/coeff[1])\n",
|
||
|
|
" \n",
|
||
|
|
" plt.plot(pts,y,\"b-\")\n",
|
||
|
|
" \n",
|
||
|
|
" #high\n",
|
||
|
|
" coeff = c2[:,idx]\n",
|
||
|
|
" y = []\n",
|
||
|
|
" for x in pts:\n",
|
||
|
|
" y.append((-coeff[0]*x+coeff[2])/coeff[1])\n",
|
||
|
|
" \n",
|
||
|
|
" plt.plot(pts,y,\"r-\")\n",
|
||
|
|
" plt.xlim((-2, 2))\n",
|
||
|
|
" plt.ylim((-2, 2))\n",
|
||
|
|
"\n",
|
||
|
|
"plt.subplot(2, 2, 4)\n",
|
||
|
|
"c1 = computed_coeff[times[3]][0]\n",
|
||
|
|
"c2 = computed_coeff[times[3]][1]\n",
|
||
|
|
"for idx in range(c.shape[1]):\n",
|
||
|
|
" #low\n",
|
||
|
|
" coeff = c1[:,idx]\n",
|
||
|
|
" y = []\n",
|
||
|
|
"\n",
|
||
|
|
" for x in pts:\n",
|
||
|
|
" y.append((-coeff[0]*x+coeff[2])/coeff[1])\n",
|
||
|
|
" \n",
|
||
|
|
" plt.plot(pts,y,\"b-\")\n",
|
||
|
|
" \n",
|
||
|
|
" #high\n",
|
||
|
|
" coeff = c2[:,idx]\n",
|
||
|
|
" y = []\n",
|
||
|
|
" for x in pts:\n",
|
||
|
|
" y.append((-coeff[0]*x+coeff[2])/coeff[1])\n",
|
||
|
|
" \n",
|
||
|
|
" plt.plot(pts,y,\"r-\")\n",
|
||
|
|
" plt.xlim((-2, 2))\n",
|
||
|
|
" plt.ylim((-2, 2))\n",
|
||
|
|
"\n",
|
||
|
|
"plt.tight_layout()\n",
|
||
|
|
"plt.show()"
|
||
|
|
]
|
||
|
|
}
|
||
|
|
],
|
||
|
|
"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
|
||
|
|
}
|