{ "cells": [ { "cell_type": "code", "execution_count": 1, "metadata": {}, "outputs": [], "source": [ "import numpy as np\n", "from scipy.integrate import odeint\n", "from scipy.interpolate import interp1d\n", "import cvxpy as cp\n", "\n", "import matplotlib.pyplot as plt\n", "plt.style.use(\"ggplot\")\n", "\n", "import time" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "### kinematics model equations\n", "\n", "The variables of the model are:\n", "\n", "* $x$ coordinate of the robot\n", "* $y$ coordinate of the robot\n", "* $v$ velocity of the robot\n", "* $\\theta$ heading of the robot\n", "\n", "The inputs of the model are:\n", "\n", "* $v$ linear velocity of the robot\n", "* $\\delta$ angular velocity of the robot\n", "\n", "These are the differential equations f(x,u) of the model:\n", "\n", "$\\dot{x} = f(x,u)$\n", "\n", "* $\\dot{x} = v\\cos{\\theta}$ \n", "* $\\dot{y} = v\\sin{\\theta}$\n", "* $\\dot{v} = a$\n", "* $\\dot{\\theta} = \\frac{v\\tan{\\delta}}{L}$\n", "\n", "Discretize with forward Euler Integration for time step dt:\n", "\n", "${x_{t+1}} = x_{t} + f(x,u)dt$\n", "\n", "* ${x_{t+1}} = x_{t} + v_t\\cos{\\theta}dt$\n", "* ${y_{t+1}} = y_{t} + v_t\\sin{\\theta}dt$\n", "* ${v_{t+1}} = v_{t} + a_tdt$\n", "* ${\\theta_{t+1}} = \\theta_{t} + \\frac{v\\tan{\\delta}}{L} dt$\n", "\n", "----------------------\n", "\n", "The Model is **non-linear** and **time variant**, but the Numerical Optimizer requires a Linear sets of equations. To approximate the equivalent **LTI** State space model, the **Taylor's series expansion** is used around $\\bar{x}$ and $\\bar{u}$ (at each time step):\n", "\n", "$ f(x,u) \\approx f(\\bar{x},\\bar{u}) + \\frac{\\partial f(x,u)}{\\partial x}|_{x=\\bar{x},u=\\bar{u}}(x-\\bar{x}) + \\frac{\\partial f(x,u)}{\\partial u}|_{x=\\bar{x},u=\\bar{u}}(u-\\bar{u})$\n", "\n", "This can be rewritten usibg the State Space model form Ax+Bu :\n", "\n", "$ f(\\bar{x},\\bar{u}) + A|_{x=\\bar{x},u=\\bar{u}}(x-\\bar{x}) + B|_{x=\\bar{x},u=\\bar{u}}(u-\\bar{u})$\n", "\n", "Where:\n", "\n", "$\n", "A =\n", "\\quad\n", "\\begin{bmatrix}\n", "\\frac{\\partial f(x,u)}{\\partial x} & \\frac{\\partial f(x,u)}{\\partial y} & \\frac{\\partial f(x,u)}{\\partial \\theta} \\\\\n", "\\end{bmatrix}\n", "\\quad\n", "=\n", "\\quad\n", "\\begin{bmatrix}\n", "0 & 0 & -v\\sin{\\theta} \\\\\n", "0 & 0 & v\\cos{\\theta} \\\\\n", "0 & 0 & 0\\\\\n", "\\end{bmatrix}\n", "\\quad\n", "$\n", "\n", "and\n", "\n", "$\n", "B = \n", "\\quad\n", "\\begin{bmatrix}\n", "\\frac{\\partial f(x,u)}{\\partial v} & \\frac{\\partial f(x,u)}{\\partial \\delta} \\\\\n", "\\end{bmatrix}\n", "\\quad\n", "= \n", "\\quad\n", "\\begin{bmatrix}\n", "\\cos{\\theta} & 0 \\\\\n", "\\sin{\\theta} & 0 \\\\\n", "\\frac{\\tan{\\delta}}{L} & \\frac{v}{L{\\cos{\\delta}}^2}\n", "\\end{bmatrix}\n", "\\quad\n", "$\n", "\n", "are the *Jacobians*.\n", "\n", "\n", "\n", "So the discretized model is given by:\n", "\n", "$ x_{t+1} = x_t + (f(\\bar{x},\\bar{u}) + A|_{x=\\bar{x}}(x_t-\\bar{x}) + B|_{u=\\bar{u}}(u_t-\\bar{u}) )dt $\n", "\n", "$ x_{t+1} = (I+dtA)x_t + dtBu_t +dt(f(\\bar{x},\\bar{u}) - A\\bar{x} - B\\bar{u}))$\n", "\n", "The LTI-equivalent kinematics model is:\n", "\n", "$ x_{t+1} = A'x_t + B' u_t + C' $\n", "\n", "with:\n", "\n", "$ A' = I+dtA|_{x=\\bar{x},u=\\bar{u}} $\n", "\n", "$ B' = dtB|_{x=\\bar{x},u=\\bar{u}} $\n", "\n", "$ C' = dt(f(\\bar{x},\\bar{u}) - A|_{x=\\bar{x},u=\\bar{u}}\\bar{x} - B|_{x=\\bar{x},u=\\bar{u}}\\bar{u}) $" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "-----------------\n", "[About Taylor Series Expansion](https://courses.engr.illinois.edu/ece486/fa2017/documents/lecture_notes/state_space_p2.pdf):\n", "\n", "In order to linearize general nonlinear systems, we will use the Taylor Series expansion of functions.\n", "\n", "Typically it is possible to assume that the system is operating about some nominal\n", "state solution $\\bar{x}$ (possibly requires a nominal input $\\bar{u}$) called **equilibrium point**.\n", "\n", "Recall that the Taylor Series expansion of f(x) around the\n", "point $\\bar{x}$ is given by:\n", "\n", "$f(x)=f(\\bar{x}) + \\frac{df(x)}{dx}|_{x=\\bar{x}}(x-\\bar{x})$ + higher order terms...\n", "\n", "For x sufficiently close to $\\bar{x}$, these higher order terms will be very close to zero, and so we can drop them.\n", "\n", "The extension to functions of multiple states and inputs is very similar to the above procedure.Suppose the evolution of state x\n", "is given by:\n", "\n", "$\\dot{x} = f(x1, x2, . . . , xn, u1, u2, . . . , um) = Ax+Bu$\n", "\n", "Where:\n", "\n", "$ A =\n", "\\quad\n", "\\begin{bmatrix}\n", "\\frac{\\partial f(x,u)}{\\partial x1} & ... & \\frac{\\partial f(x,u)}{\\partial xn} \\\\\n", "\\end{bmatrix}\n", "\\quad\n", "$ and $ B = \\quad\n", "\\begin{bmatrix}\n", "\\frac{\\partial f(x,u)}{\\partial u1} & ... & \\frac{\\partial f(x,u)}{\\partial um} \\\\\n", "\\end{bmatrix}\n", "\\quad $\n", "\n", "Then:\n", "\n", "$f(x,u)=f(\\bar{x},\\bar{u}) + \\frac{df(x,u)}{dx}|_{x=\\bar{x}}(x-\\bar{x}) + \\frac{df(x,u)}{du}|_{u=\\bar{u}}(u-\\bar{u}) = f(\\bar{x},\\bar{u}) + A_{x=\\bar{x}}(x-\\bar{x}) + B_{u=\\bar{u}}(u-\\bar{u})$\n", "\n", "-----------------" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "### Kinematics Model" ] }, { "cell_type": "code", "execution_count": 2, "metadata": {}, "outputs": [], "source": [ "# Control problem statement.\n", "\n", "N = 4 #number of state variables\n", "M = 2 #number of control variables\n", "T = 20 #Prediction Horizon\n", "dt = 0.25 #discretization step" ] }, { "cell_type": "code", "execution_count": 3, "metadata": {}, "outputs": [], "source": [ "def get_linear_model(x_bar,u_bar):\n", " \"\"\"\n", " \"\"\"\n", " L=0.3\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[1,2]=np.sin(theta)\n", " A[0,3]=-v*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[0,0]=np.cos(theta)\n", " B[1,0]=np.sin(theta)\n", " B[2,0]=1\n", " B[3,1]=v/L*np.cos(delta)**2\n", " B_lin=dt*B\n", " \n", " f_xu=np.array([v*np.cos(theta), v*np.sin(theta), a,v*np.tan(delta)/L]).reshape(N,1)\n", " C_lin = dt*(f_xu - np.dot(A,x_bar.reshape(N,1)) - np.dot(B,u_bar.reshape(M,1)))\n", " \n", " return np.round(A_lin,4), np.round(B_lin,4), np.round(C_lin,4)" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "Motion Prediction: using scipy intergration" ] }, { "cell_type": "code", "execution_count": 4, "metadata": {}, "outputs": [], "source": [ "# Define process model\n", "# This uses the continuous model \n", "def kinematics_model(x,t,u):\n", " \"\"\"\n", " \"\"\"\n", " \n", " #[x,y,v,th]\n", " #[a,d]\n", " \n", " L=0.3\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_bar = np.zeros((N,T+1))\n", " \n", " x_bar[:,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_bar[:,t]=x_next[1]\n", " \n", " return x_bar" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "Validate the model, here the status w.r.t a straight line with constant heading 0" ] }, { "cell_type": "code", "execution_count": 5, "metadata": {}, "outputs": [ { "name": "stdout", "output_type": "stream", "text": [ "CPU times: user 3.65 ms, sys: 0 ns, total: 3.65 ms\n", "Wall time: 3.47 ms\n" ] } ], "source": [ "%%time\n", "\n", "u_bar = np.zeros((M,T))\n", "u_bar[0,:] = 0.2 #m/ss\n", "u_bar[1,:] = np.radians(-np.pi/4) #rad\n", "\n", "x0 = np.zeros(N)\n", "x0[0] = 0\n", "x0[1] = 1\n", "x0[2] = 0\n", "x0[3] = np.radians(0)\n", "\n", "x_bar=predict(x0,u_bar)" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "Check the model prediction" ] }, { "cell_type": "code", "execution_count": 6, "metadata": {}, "outputs": [ { "data": { "image/png": "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\n", "text/plain": [ "
" ] }, "metadata": {}, "output_type": "display_data" } ], "source": [ "#plot trajectory\n", "plt.subplot(2, 2, 1)\n", "plt.plot(x_bar[0,:],x_bar[1,:])\n", "plt.plot(np.linspace(0,10,T+1),np.zeros(T+1),\"b-\")\n", "plt.axis('equal')\n", "plt.ylabel('y')\n", "plt.xlabel('x')\n", "\n", "plt.subplot(2, 2, 2)\n", "plt.plot(np.degrees(x_bar[2,:]))\n", "plt.ylabel('theta(t) [deg]')\n", "#plt.xlabel('time')\n", "\n", "\n", "plt.tight_layout()\n", "plt.show()" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "Motion Prediction: using the state space model" ] }, { "cell_type": "code", "execution_count": 7, "metadata": {}, "outputs": [ { "name": "stdout", "output_type": "stream", "text": [ "CPU times: user 2.48 ms, sys: 0 ns, total: 2.48 ms\n", "Wall time: 1.85 ms\n" ] } ], "source": [ "%%time\n", "\n", "u_bar = np.zeros((M,T))\n", "u_bar[0,:] = 0.2 #m/s\n", "u_bar[1,:] = np.radians(-np.pi/4) #rad\n", "\n", "x0 = np.zeros(N)\n", "x0[0] = 0\n", "x0[1] = 1\n", "x0[2] = 0\n", "x0[3] = np.radians(0)\n", "\n", "x_bar=np.zeros((N,T+1))\n", "x_bar[:,0]=x0\n", "\n", "for t in range (1,T+1):\n", " xt=x_bar[:,t-1].reshape(N,1)\n", " ut=u_bar[:,t-1].reshape(M,1)\n", " \n", " A,B,C=get_linear_model(xt,ut)\n", " \n", " xt_plus_one = np.dot(A,xt)+np.dot(B,ut)+C\n", " \n", " x_bar[:,t]= np.squeeze(xt_plus_one)" ] }, { "cell_type": "code", "execution_count": 8, "metadata": {}, "outputs": [ { "data": { "image/png": "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\n", "text/plain": [ "
" ] }, "metadata": {}, "output_type": "display_data" } ], "source": [ "#plot trajectory\n", "plt.subplot(2, 2, 1)\n", "plt.plot(x_bar[0,:],x_bar[1,:])\n", "plt.plot(np.linspace(0,10,T+1),np.zeros(T+1),\"b-\")\n", "plt.axis('equal')\n", "plt.ylabel('y')\n", "plt.xlabel('x')\n", "\n", "plt.subplot(2, 2, 2)\n", "plt.plot(np.degrees(x_bar[2,:]))\n", "plt.ylabel('theta(t)')\n", "#plt.xlabel('time')\n", "\n", "\n", "plt.tight_layout()\n", "plt.show()" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "The results are the same as expected, so the linearized model is equivalent as expected." ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "## PRELIMINARIES" ] }, { "cell_type": "code", "execution_count": 9, "metadata": {}, "outputs": [], "source": [ "def compute_path_from_wp(start_xp, start_yp, step = 0.1):\n", " final_xp=[]\n", " final_yp=[]\n", " delta = step #[m]\n", "\n", " for idx in range(len(start_xp)-1):\n", " section_len = np.sum(np.sqrt(np.power(np.diff(start_xp[idx:idx+2]),2)+np.power(np.diff(start_yp[idx:idx+2]),2)))\n", "\n", " interp_range = np.linspace(0,1,np.floor(section_len/delta).astype(int))\n", " \n", " fx=interp1d(np.linspace(0,1,2),start_xp[idx:idx+2],kind=1)\n", " fy=interp1d(np.linspace(0,1,2),start_yp[idx:idx+2],kind=1)\n", " \n", " final_xp=np.append(final_xp,fx(interp_range))\n", " final_yp=np.append(final_yp,fy(interp_range))\n", "\n", " return np.vstack((final_xp,final_yp))\n", "\n", "def get_nn_idx(state,path):\n", "\n", " dx = state[0]-path[0,:]\n", " dy = state[1]-path[1,:]\n", " dist = np.sqrt(dx**2 + dy**2)\n", " nn_idx = np.argmin(dist)\n", "\n", " try:\n", " v = [path[0,nn_idx+1] - path[0,nn_idx],\n", " path[1,nn_idx+1] - path[1,nn_idx]] \n", " v /= np.linalg.norm(v)\n", "\n", " d = [path[0,nn_idx] - state[0],\n", " path[1,nn_idx] - state[1]]\n", "\n", " if np.dot(d,v) > 0:\n", " target_idx = nn_idx\n", " else:\n", " target_idx = nn_idx+1\n", "\n", " except IndexError as e:\n", " target_idx = nn_idx\n", "\n", " return target_idx\n", "\n", "def road_curve(state,track):\n", " \n", " #given vehicle pos find lookahead waypoints\n", " nn_idx=get_nn_idx(state,track)-1\n", " LOOKAHED=6\n", " lk_wp=track[:,nn_idx:nn_idx+LOOKAHED]\n", "\n", " #trasform lookahead waypoints to vehicle ref frame\n", " dx = lk_wp[0,:] - state[0]\n", " dy = lk_wp[1,:] - state[1]\n", "\n", " wp_vehicle_frame = np.vstack(( dx * np.cos(-state[3]) - dy * np.sin(-state[3]),\n", " dy * np.cos(-state[3]) + dx * np.sin(-state[3]) ))\n", "\n", " #fit poly\n", " return np.polyfit(wp_vehicle_frame[0,:], wp_vehicle_frame[1,:], 3, rcond=None, full=False, w=None, cov=False)\n", "\n", "def f(x,coeff):\n", " return round(coeff[0]*x**3 + coeff[1]*x**2 + coeff[2]*x**1 + coeff[3]*x**0,6)\n", "\n", "# def f(x,coeff):\n", "# return round(coeff[0]*x**5+coeff[1]*x**4+coeff[2]*x**3+coeff[3]*x**2+coeff[4]*x**1+coeff[5]*x**0,6)\n", "\n", "def df(x,coeff):\n", " return round(3*coeff[0]*x**2 + 2*coeff[1]*x**1 + coeff[2]*x**0,6)\n", "# def df(x,coeff):\n", "# return round(5*coeff[0]*x**4 + 4*coeff[1]*x**3 +3*coeff[2]*x**2 + 2*coeff[3]*x**1 + coeff[4]*x**0,6)" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "### MPC Problem formulation\n", "\n", "**Model Predictive Control** refers to the control approach of **numerically** solving a optimization problem at each time step. \n", "\n", "The controller generates a control signal over a fixed lenght T (Horizon) at each time step." ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "![mpc](img/mpc_block_diagram.png)\n", "\n", "![mpc](img/mpc_t.png)" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "#### Linear MPC Formulation\n", "\n", "Linear MPC makes use of the **LTI** (Linear time invariant) discrete state space model, wich represents a motion model used for Prediction.\n", "\n", "$x_{t+1} = Ax_t + Bu_t$\n", "\n", "The LTI formulation means that **future states** are linearly related to the current state and actuator signal. Hence, the MPC seeks to find a **control policy** U over a finite lenght horizon.\n", "\n", "$U={u_{t|t}, u_{t+1|t}, ...,u_{t+T|t}}$\n", "\n", "The objective function used minimize (drive the state to 0) is:\n", "\n", "$\n", "\\begin{equation}\n", "\\begin{aligned}\n", "\\min_{} \\quad & \\sum^{t+T-1}_{j=t} x^T_{j|t}Qx_{j|t} + u^T_{j|t}Ru_{j|t}\\\\\n", "\\textrm{s.t.} \\quad & x(0) = x0\\\\\n", " & x_{j+1|t} = Ax_{j|t}+Bu_{j|t}) \\quad \\textrm{for} \\quad t this is the value of the fitted polynomial\n", "\n", "* **heading error** epsi: desired heading - heading of vehicle -> is the inclination of tangent to the fitted polynomial\n", "\n", "Then using the fitted polynomial representation in vehicle frame the errors can be easily computed as:\n", "\n", "$\n", "cte = f(px) \\\\\n", "\\psi = -atan(f`(px)) \\\\\n", "$" ] }, { "cell_type": "code", "execution_count": 22, "metadata": {}, "outputs": [ { "name": "stdout", "output_type": "stream", "text": [ "-----------------------------------------------------------------\n", " OSQP v0.6.0 - Operator Splitting QP Solver\n", " (c) Bartolomeo Stellato, Goran Banjac\n", " University of Oxford - Stanford University 2019\n", "-----------------------------------------------------------------\n", "problem: variables n = 282, constraints m = 364\n", " nnz(P) + nnz(A) = 975\n", "settings: linear system solver = qdldl,\n", " eps_abs = 1.0e-04, eps_rel = 1.0e-04,\n", " eps_prim_inf = 1.0e-04, eps_dual_inf = 1.0e-04,\n", " rho = 1.00e-01 (adaptive),\n", " sigma = 1.00e-06, alpha = 1.60, max_iter = 10000\n", " check_termination: on (interval 25),\n", " scaling: on, scaled_termination: off\n", " warm start: on, polish: on, time_limit: off\n", "\n", "iter objective pri res dua res rho time\n", " 1 0.0000e+00 1.00e+00 1.00e+02 1.00e-01 3.06e-04s\n", " 50 3.9564e-01 4.47e-10 6.69e-10 1.00e-01 6.77e-04s\n", "\n", "status: solved\n", "solution polish: unsuccessful\n", "number of iterations: 50\n", "optimal objective: 0.3956\n", "run time: 9.19e-04s\n", "optimal rho estimate: 4.67e-02\n", "\n", "CPU times: user 284 ms, sys: 0 ns, total: 284 ms\n", "Wall time: 281 ms\n" ] } ], "source": [ "%%time\n", "\n", "MAX_SPEED = 1.25\n", "MIN_SPEED = 0.75\n", "MAX_STEER = 1.57\n", "MAX_ACC = 1.0\n", "\n", "track = compute_path_from_wp([0,3],\n", " [0,0],0.5)\n", "\n", "# Starting Condition\n", "x0 = np.zeros(N)\n", "x0[0] = 0\n", "x0[1] = -0.25\n", "x0[2] = 1\n", "x0[3] = np.radians(-0)\n", " \n", "#starting guess\n", "u_bar = np.zeros((M,T))\n", "u_bar[0,:]=0.01\n", "u_bar[1,:]=0.01\n", "\n", " \n", "K=road_curve(x0,track)\n", "\n", "# dynamics starting state w.r.t vehicle frame\n", "x_bar=np.zeros((N,T+1))\n", "x_bar[2]=x0[2]\n", "#prediction for linearization of costrains\n", "for t in range (1,T+1):\n", " xt=x_bar[:,t-1].reshape(N,1)\n", " ut=u_bar[:,t-1].reshape(M,1)\n", " A,B,C=get_linear_model(xt,ut)\n", " xt_plus_one = np.squeeze(np.dot(A,xt)+np.dot(B,ut)+C)\n", " x_bar[:,t]= xt_plus_one\n", " \n", "\n", "x = cp.Variable((N, T+1))\n", "u = cp.Variable((M, T))\n", "\n", "#CVXPY Linear MPC problem statement\n", "cost = 0\n", "constr = []\n", "\n", "for t in range(T):\n", " \n", " cost += 1*cp.sum_squares(x[3,t]-np.arctan(df(x_bar[0,t],K))) # psi\n", " cost += 1*cp.sum_squares(f(x_bar[0,t],K)-x[1,t]) # cte\n", " # Actuation effort\n", " cost += cp.quad_form( u[:, t],1*np.eye(M))\n", " \n", " # Actuation rate of change\n", " if t < (T - 1):\n", " cost += cp.quad_form(u[:, t + 1] - u[:, t], 1*np.eye(M))\n", " \n", " # KINEMATICS constrains\n", " A,B,C=get_linear_model(x_bar[:,t],u_bar[:,t])\n", " constr += [x[:,t+1] == A@x[:,t] + B@u[:,t] + C.flatten()]\n", " \n", "# sums problem objectives and concatenates constraints.\n", "constr += [x[:,0] == x_bar[:,0]] #<--watch out the start condition\n", "constr += [x[2, :] <= MAX_SPEED]\n", "constr += [x[2, :] >= MIN_SPEED]\n", "constr += [cp.abs(u[0, :]) <= MAX_ACC]\n", "constr += [cp.abs(u[1, :]) <= MAX_STEER]\n", "\n", "prob = cp.Problem(cp.Minimize(cost), constr)\n", "solution = prob.solve(solver=cp.OSQP, verbose=True)" ] }, { "cell_type": "code", "execution_count": 24, "metadata": {}, "outputs": [ { "data": { "image/png": "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\n", "text/plain": [ "
" ] }, "metadata": {}, "output_type": "display_data" } ], "source": [ "x_mpc=np.array(x.value[0, :]).flatten()\n", "y_mpc=np.array(x.value[1, :]).flatten()\n", "theta_mpc=np.array(x.value[2, :]).flatten()\n", "v_mpc=np.array(u.value[0, :]).flatten()\n", "w_mpc=np.array(u.value[1, :]).flatten()\n", "\n", "#simulate robot state trajectory for optimized U\n", "x_traj=predict(x0, np.vstack((v_mpc,w_mpc)))\n", "\n", "#plt.figure(figsize=(15,10))\n", "#plot trajectory\n", "plt.subplot(2, 2, 1)\n", "plt.plot(track[0,:],track[1,:],\"b+\")\n", "plt.plot(x_traj[0,:],x_traj[1,:])\n", "plt.axis(\"equal\")\n", "plt.ylabel('y')\n", "plt.xlabel('x')\n", "\n", "#plot v(t)\n", "plt.subplot(2, 2, 2)\n", "plt.plot(v_mpc)\n", "plt.ylabel('v(t)')\n", "#plt.xlabel('time')\n", "\n", "#plot w(t)\n", "# plt.subplot(2, 2, 3)\n", "# plt.plot(w_mpc)\n", "# plt.ylabel('w(t)')\n", "#plt.xlabel('time')\n", "\n", "# plt.subplot(2, 2, 3)\n", "# plt.plot(psi_mpc)\n", "# plt.ylabel('psi(t)')\n", "\n", "# plt.subplot(2, 2, 4)\n", "# plt.plot(cte_mpc)\n", "# plt.ylabel('cte(t)')\n", "\n", "plt.tight_layout()\n", "plt.show()" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "full track demo" ] }, { "cell_type": "code", "execution_count": null, "metadata": {}, "outputs": [], "source": [ "track = compute_path_from_wp([0,3,4,6,10,13],\n", " [0,0,2,4,3,3],0.25)\n", "\n", "# track = compute_path_from_wp([0,5,7.5,10,12,13,13,10],\n", "# [0,0,2.5,2.5,0,0,5,10],0.5)\n", "\n", "sim_duration = 80 #time steps\n", "opt_time=[]\n", "\n", "x_sim = np.zeros((N,sim_duration))\n", "u_sim = np.zeros((M,sim_duration-1))\n", "\n", "MAX_SPEED = 1.25\n", "MIN_SPEED = 0.75\n", "MAX_STEER = 1.57\n", "MAX_ACC = 1.0\n", "\n", "# Starting Condition\n", "x0 = np.zeros(N)\n", "x0[0] = 0\n", "x0[1] = -0.25\n", "x0[2] = np.radians(-0)\n", "x_sim[:,0]=x0\n", " \n", "#starting guess\n", "u_bar = np.zeros((M,T))\n", "u_bar[0,:]=0.5*(MAX_SPEED+MIN_SPEED)\n", "u_bar[1,:]=0.1\n", "\n", "for sim_time in range(sim_duration-1):\n", " \n", " iter_start=time.time()\n", " \n", " K=road_curve(x_sim[:,sim_time],track)\n", " \n", " # dynamics starting state w.r.t vehicle frame\n", " x_bar=np.zeros((N,T+1))\n", " \n", " #prediction for linearization of costrains\n", " for t in range (1,T+1):\n", " xt=x_bar[:,t-1].reshape(N,1)\n", " ut=u_bar[:,t-1].reshape(M,1)\n", " A,B,C=get_linear_model(xt,ut)\n", " xt_plus_one = np.squeeze(np.dot(A,xt)+np.dot(B,ut)+C)\n", " x_bar[:,t]= xt_plus_one\n", " \n", " #CVXPY Linear MPC problem statement\n", " cost = 0\n", " constr = []\n", " x = cp.Variable((N, T+1))\n", " u = cp.Variable((M, T))\n", " \n", " #Prediction Horizon\n", " for t in range(T):\n", "\n", " #cost += 30*cp.sum_squares(x[2,t]-np.arctan(df(x_bar[0,t],K))) # psi\n", " cost += 50*cp.sum_squares(x[2,t]-np.arctan2(df(x_bar[0,t],K),x_bar[0,t])) # psi\n", " cost += 20*cp.sum_squares(f(x_bar[0,t],K)-x[1,t]) # cte\n", "\n", " # Actuation rate of change\n", " if t < (T - 1):\n", " cost += cp.quad_form(u[:, t + 1] - u[:, t], 100*np.eye(M))\n", " \n", " # Actuation effort\n", " cost += cp.quad_form( u[:, t],1*np.eye(M))\n", " \n", " # Kinrmatics Constrains (Linearized model)\n", " A,B,C=get_linear_model(x_bar[:,t],u_bar[:,t])\n", " constr += [x[:,t+1] == A@x[:,t] + B@u[:,t] + C.flatten()]\n", "\n", " # sums problem objectives and concatenates constraints.\n", " constr += [x[:,0] == x_bar[:,0]] #<--watch out the start condition\n", " constr += [u[0, :] <= MAX_SPEED]\n", " constr += [u[0, :] >= MIN_SPEED]\n", " constr += [cp.abs(u[1, :]) <= MAX_STEER_SPEED]\n", " \n", " # Solve\n", " prob = cp.Problem(cp.Minimize(cost), constr)\n", " solution = prob.solve(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": null, "metadata": {}, "outputs": [], "source": [ "#plot trajectory\n", "grid = plt.GridSpec(2, 3)\n", "\n", "plt.figure(figsize=(15,10))\n", "\n", "plt.subplot(grid[0:2, 0:2])\n", "plt.plot(track[0,:],track[1,:],\"b+\")\n", "plt.plot(x_sim[0,:],x_sim[1,:])\n", "plt.axis(\"equal\")\n", "plt.ylabel('y')\n", "plt.xlabel('x')\n", "\n", "plt.subplot(grid[0, 2])\n", "plt.plot(u_sim[0,:])\n", "plt.ylabel('v(t) [m/s]')\n", "\n", "plt.subplot(grid[1, 2])\n", "plt.plot(np.degrees(u_sim[1,:]))\n", "plt.ylabel('w(t) [deg/s]')\n", "\n", "plt.tight_layout()\n", "plt.show()" ] }, { "cell_type": "markdown", "metadata": {}, "source": [ "## OBSTACLE AVOIDANCE\n", "see dccp paper for reference" ] }, { "cell_type": "code", "execution_count": null, "metadata": {}, "outputs": [], "source": [ "import dccp\n", "track = compute_path_from_wp([0,3,4,6,10,13],\n", " [0,0,2,4,3,3],0.25)\n", "\n", "obstacles=np.array([[4,6],[2,4]])\n", "obstacle_radius=0.5\n", "\n", "def to_vehic_frame(pt,pos_x,pos_y,theta):\n", " dx = pt[0] - pos_x\n", " dy = pt[1] - pos_y\n", "\n", " return [dx * np.cos(-theta) - dy * np.sin(-theta),\n", " dy * np.cos(-theta) + dx * np.sin(-theta)]\n", " \n", "# track = compute_path_from_wp([0,5,7.5,10,12,13,13,10],\n", "# [0,0,2.5,2.5,0,0,5,10],0.5)\n", "\n", "sim_duration = 80 #time steps\n", "opt_time=[]\n", "\n", "x_sim = np.zeros((N,sim_duration))\n", "u_sim = np.zeros((M,sim_duration-1))\n", "\n", "MAX_SPEED = 1.25\n", "MIN_SPEED = 0.75\n", "MAX_STEER_SPEED = 1.57\n", "\n", "# Starting Condition\n", "x0 = np.zeros(N)\n", "x0[0] = 0\n", "x0[1] = -0.25\n", "x0[2] = np.radians(-0)\n", "x_sim[:,0]=x0\n", " \n", "#starting guess\n", "u_bar = np.zeros((M,T))\n", "u_bar[0,:]=0.5*(MAX_SPEED+MIN_SPEED)\n", "u_bar[1,:]=0.00\n", "\n", "for sim_time in range(sim_duration-1):\n", " \n", " iter_start=time.time()\n", " \n", " #compute coefficients\n", " K=road_curve(x_sim[:,sim_time],track)\n", " \n", " #compute opstacles in ref frame\n", " o_=[]\n", " for j in range(2):\n", " o_.append(to_vehic_frame(obstacles[:,j],x_sim[0,sim_time],x_sim[1,sim_time],x_sim[2,sim_time]) )\n", " \n", " # dynamics starting state w.r.t vehicle frame\n", " x_bar=np.zeros((N,T+1))\n", " \n", " #prediction for linearization of costrains\n", " for t in range (1,T+1):\n", " xt=x_bar[:,t-1].reshape(N,1)\n", " ut=u_bar[:,t-1].reshape(M,1)\n", " A,B,C=get_linear_model(xt,ut)\n", " xt_plus_one = np.squeeze(np.dot(A,xt)+np.dot(B,ut)+C)\n", " x_bar[:,t]= xt_plus_one\n", " \n", " #CVXPY Linear MPC problem statement\n", " cost = 0\n", " constr = []\n", " x = cp.Variable((N, T+1))\n", " u = cp.Variable((M, T))\n", " \n", " #Prediction Horizon\n", " for t in range(T):\n", "\n", " #cost += 30*cp.sum_squares(x[2,t]-np.arctan(df(x_bar[0,t],K))) # psi\n", " cost += 50*cp.sum_squares(x[2,t]-np.arctan2(df(x_bar[0,t],K),x_bar[0,t])) # psi\n", " cost += 20*cp.sum_squares(f(x_bar[0,t],K)-x[1,t]) # cte\n", "\n", " # Actuation rate of change\n", " if t < (T - 1):\n", " cost += cp.quad_form(u[:, t + 1] - u[:, t], 100*np.eye(M))\n", " \n", " # Actuation effort\n", " cost += cp.quad_form( u[:, t],1*np.eye(M))\n", " \n", " # Kinrmatics Constrains (Linearized model)\n", " A,B,C=get_linear_model(x_bar[:,t],u_bar[:,t])\n", " constr += [x[:,t+1] == A@x[:,t] + B@u[:,t] + C.flatten()]\n", " \n", " # Obstacle Avoidance Contrains\n", " for j in range(2):\n", " constr += [ cp.norm(x[0:2,t]-o_[j],2) >= obstacle_radius ]\n", "\n", " # sums problem objectives and concatenates constraints.\n", " constr += [x[:,0] == x_bar[:,0]] #<--watch out the start condition\n", " constr += [u[0, :] <= MAX_SPEED]\n", " constr += [u[0, :] >= MIN_SPEED]\n", " constr += [cp.abs(u[1, :]) <= MAX_STEER_SPEED]\n", " \n", " # Solve\n", " prob = cp.Problem(cp.Minimize(cost), constr)\n", " solution = prob.solve(method=\"dccp\", verbose=False)\n", " \n", " #retrieved optimized U and assign to u_bar to linearize in next step\n", " u_bar=np.vstack((np.array(u.value[0, :]).flatten(),\n", " (np.array(u.value[1, :]).flatten())))\n", " \n", " u_sim[:,sim_time] = u_bar[:,0]\n", " \n", " # Measure elpased time to get results from cvxpy\n", " opt_time.append(time.time()-iter_start)\n", " \n", " # move simulation to t+1\n", " tspan = [0,dt]\n", " x_sim[:,sim_time+1] = odeint(kinematics_model,\n", " x_sim[:,sim_time],\n", " tspan,\n", " args=(u_bar[:,0],))[1]\n", " \n", "print(\"CVXPY Optimization Time: Avrg: {:.4f}s Max: {:.4f}s Min: {:.4f}s\".format(np.mean(opt_time),\n", " np.max(opt_time),\n", " np.min(opt_time))) " ] }, { "cell_type": "code", "execution_count": null, "metadata": {}, "outputs": [], "source": [ "#plot trajectory\n", "grid = plt.GridSpec(2, 3)\n", "\n", "plt.figure(figsize=(15,10))\n", "\n", "ax=plt.subplot(grid[0:2, 0:2])\n", "plt.plot(track[0,:],track[1,:],\"b+\")\n", "plt.plot(x_sim[0,:],x_sim[1,:])\n", "#obstacles\n", "circle1=plt.Circle((obstacles[0,0], obstacles[1,0]), obstacle_radius, color='r')\n", "circle2=plt.Circle((obstacles[0,1], obstacles[1,1]), obstacle_radius, color='r')\n", "plt.axis(\"equal\")\n", "plt.ylabel('y')\n", "plt.xlabel('x')\n", "\n", "ax.add_artist(circle1)\n", "ax.add_artist(circle2)\n", "\n", "plt.subplot(grid[0, 2])\n", "plt.plot(u_sim[0,:])\n", "plt.ylabel('v(t) [m/s]')\n", "\n", "plt.subplot(grid[1, 2])\n", "plt.plot(np.degrees(u_sim[1,:]))\n", "plt.ylabel('w(t) [deg/s]')\n", "\n", "\n", "plt.tight_layout()\n", "plt.show()" ] }, { "cell_type": "code", "execution_count": null, "metadata": {}, "outputs": [], "source": [] } ], "metadata": { "kernelspec": { "display_name": "Python [conda env:jupyter] *", "language": "python", "name": "conda-env-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.3" } }, "nbformat": 4, "nbformat_minor": 4 }