Updated README.md

README.md

@@ -1,2 +1,50 @@
 # mpc_python
-Python implementation of mpc controller for path tracking
+
+Python implementation of mpc controller for path tracking.
+
+## About
+
+The MPC is a model predictive path following controller which does follow a predefined reference path Xref and Yref by solving an optimization problem. The resulting optimization problem is shown in the following equation:
+
+MIN $ J(x(t),U) = \sum^{t+T-1}_{j=t} (x_{j,ref} - x_{j})^T_{j}Q(x_{j,ref} - x_{j}) + u^T_{j}Ru_{j} $
+
+s.t.
+
+$ x(0) = x0 $
+
+$ x_{j+1} = Ax_{j}+Bu_{j})$    for $t< j <t+T-1 $
+
+$ U_{MIN} < u_{j} < U_{MAX} $   for $t< j <t+T-1 $
+
+The vehicle dynamics are described by the differential drive model:
+
+* $\dot{x} = v\cos{\theta}$ 
+* $\dot{y} = v\sin{\theta}$
+* $\dot{\theta} = w$
+
+The state variables of the model are:
+
+* $x$ coordinate of the robot
+* $y$ coordinate of the robot
+* $\theta$ heading of the robot
+
+The inputs of the model are:
+
+* $v$ linear velocity of the robot
+* $w$ angular velocity of the robot
+
+## Demo
+
+![](img/demo.gif)
+
+To run the demo:
+
+```bash
+python3 mpc_demo/main.py
+```
+
+## Requirements
+
+```bash
+pip3 install --user --requirement requirements.txt
+```

mpc_demo/cvxpy_mpc.py

@@ -1,4 +1,6 @@
 import numpy as np
+np.seterr(divide='ignore', invalid='ignore')
+
 from scipy.integrate import odeint
 from scipy.interpolate import interp1d
 import cvxpy as cp

mpc_demo/main.py

@@ -34,7 +34,7 @@ class MPC():
         self.opt_u[1,:] = np.radians(0) #rad/s
 
         # Interpolated Path to follow given waypoints
-        self.path = compute_path_from_wp([0,20,30,30],[0,0,10,20])
+        self.path = compute_path_from_wp([0,10,12,2,4,14],[0,0,2,10,12,12])
 
         # Sim help vars
         self.sim_time=0
@@ -57,13 +57,17 @@ class MPC():
         while 1:
             if self.state is not None:
 
+                if np.sqrt((self.state[0]-self.path[0,-1])**2+(self.state[1]-self.path[1,-1])**2)<1:
+                    print("Success! Goal Reached")
+                    return
+
                 #optimization loop
                 start=time.time()
                 self.opt_u = optimize(self.state,
                                         self.opt_u,
                                         self.path)
 
-                print("CVXPY Optimization Time: {:.4f}s".format(time.time()-start))
+                # print("CVXPY Optimization Time: {:.4f}s".format(time.time()-start))
 
                 self.update_sim(self.opt_u[0,1],self.opt_u[1,1])
 
@@ -97,31 +101,78 @@ class MPC():
         plt.subplot(grid[0:2, 0:2])
         plt.title("MPC Simulation \n" + "Simulation elapsed time {}s".format(self.sim_time))
 
-        plt.plot(self.path[0,:],self.path[1,:], c='tab:orange', marker=".", label="reference track")
+        plt.plot(self.path[0,:],self.path[1,:], c='tab:orange',
-        plt.plot(self.x_history, self.y_history, c='tab:blue', marker=".", alpha=0.5, label="vehicle trajectory")
+                                                marker=".",
-        plt.plot(self.x_history[-1], self.y_history[-1], c='tab:blue', marker=".", markersize=12, label="vehicle position")
+                                                label="reference track")
-        plt.arrow(self.x_history[-1], self.y_history[-1],np.cos(self.h_history[-1]),np.sin(self.h_history[-1]),color='tab:blue',width=0.2,head_length=0.5)
+
+        plt.plot(self.x_history, self.y_history, c='tab:blue',
+                                                 marker=".",
+                                                 alpha=0.5,
+                                                 label="vehicle trajectory")
+
+        # plt.plot(self.x_history[-1], self.y_history[-1], c='tab:blue',
+        #                                                  marker=".",
+        #                                                  markersize=12,
+        #                                                  label="vehicle position")
+        # plt.arrow(self.x_history[-1],
+        #           self.y_history[-1],
+        #           np.cos(self.h_history[-1]),
+        #           np.sin(self.h_history[-1]),
+        #           color='tab:blue',
+        #           width=0.2,
+        #           head_length=0.5,
+        #           label="heading")
+
+        plot_car(self.x_history[-1], self.y_history[-1], self.h_history[-1])
+
         plt.ylabel('map y')
+        plt.yticks(np.arange(min(self.path[1,:])-1., max(self.path[1,:]+1.)+1, 1.0))
         plt.xlabel('map x')
-        plt.legend()
+        plt.xticks(np.arange(min(self.path[0,:])-1., max(self.path[0,:]+1.)+1, 1.0))
+        #plt.legend()
 
         plt.subplot(grid[0, 2])
         #plt.title("Linear Velocity {} m/s".format(self.v_history[-1]))
         plt.plot(self.v_history,c='tab:orange')
+        locs, _ = plt.xticks()
+        plt.xticks(locs[1:], locs[1:]*self.dt)
         plt.ylabel('v(t) [m/s]')
-        plt.xlabel('sample')
+        plt.xlabel('t [s]')
 
         plt.subplot(grid[1, 2])
         #plt.title("Angular Velocity {} m/s".format(self.w_history[-1]))
         plt.plot(np.degrees(self.w_history),c='tab:orange')
         plt.ylabel('w(t) [deg/s]')
-        plt.xlabel('sample')
+        locs, _ = plt.xticks()
+        plt.xticks(locs[1:], locs[1:]*self.dt)
+        plt.xlabel('t [s]')
 
         plt.tight_layout()
 
         plt.draw()
         plt.pause(0.1)
 
+
+def plot_car(x, y, yaw):
+    LENGTH = 1.0  # [m]
+    WIDTH = 0.5  # [m]
+    OFFSET = LENGTH/2  # [m]
+
+    outline = np.array([[-OFFSET, (LENGTH - OFFSET), (LENGTH - OFFSET), -OFFSET, -OFFSET],
+                        [WIDTH / 2, WIDTH / 2, - WIDTH / 2, -WIDTH / 2, WIDTH / 2]])
+
+    Rotm = np.array([[np.cos(yaw), np.sin(yaw)],
+                     [-np.sin(yaw), np.cos(yaw)]])
+
+    outline = (outline.T.dot(Rotm)).T
+
+    outline[0, :] += x
+    outline[1, :] += y
+
+    plt.plot(np.array(outline[0, :]).flatten(), np.array(outline[1, :]).flatten(), 'tab:blue')
+
+
+
 def do_sim():
     sim=MPC()
     try:

mpc_demo/utils.py

@@ -18,7 +18,7 @@ def compute_path_from_wp(start_xp, start_yp, step = 0.1):
     for idx in range(len(start_xp)-1):
         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)))
 
-        interp_range = np.linspace(0,1,section_len/delta)
+        interp_range = np.linspace(0,1,int(section_len/delta))
 
         fx=interp1d(np.linspace(0,1,2),start_xp[idx:idx+2],kind=1)
         fy=interp1d(np.linspace(0,1,2),start_yp[idx:idx+2],kind=1)

notebook/.ipynb_checkpoints/MPC_cvxpy-checkpoint.ipynb

@@ -122,13 +122,13 @@
     "\\quad\n",
     "$\n",
     "\n",
-    "This State Space Model is not linear (A,B are time changing), to linearize it the **Taylor's series expansion** is used around $\\bar{x}$ and $\\bar{u}$:\n",
+    "This State Space Model is **non-linear** (A,B are time changing), to linearize it the **Taylor's series expansion** is used around $\\bar{x}$ and $\\bar{u}$:\n",
     "\n",
     "$ \\dot{x} = f(x,u) \\approx f(\\bar{x},\\bar{u}) +  A(x-\\bar{x}) + B(u-\\bar{u})$\n",
     "\n",
     "So:\n",
     "\n",
-    "$ x{t+1} = x_t + (f(\\bar{x},\\bar{u}) +  A(x-\\bar{x}) + B(u-\\bar{u}) )dt $\n",
+    "$ x_{t+1} = x_t + (f(\\bar{x},\\bar{u}) +  A(x_t-\\bar{x}) + B(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",
@@ -157,6 +157,51 @@
     "-----------------"
    ]
   },
+  {
+   "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": {},
@@ -757,6 +802,7 @@
    "metadata": {},
    "source": [
     "![mpc](img/mpc_block_diagram.png)\n",
+    "\n",
     "![mpc](img/mpc_t.png)"
    ]
   },
@@ -778,9 +824,19 @@
     "\n",
     "$J(x(t),U) = \\sum^{t+T-1}_{j=t} x^T_{j|t}Qx_{j|t} + u^T_{j|t}Ru_{j|t}$\n",
     "\n",
-    "This accounts for quadratic error on deviation from 0 of the state and the control inputs sequences. Q and R are the **weight matrices** and are used to tune the response.\n",
+    "s.t.\n",
     "\n",
-    "Becouse the goal is tracking a **reference signal** such as a trajectory, the objective function is rewritten as:\n",
+    "$ x(0) = x0$\n",
+    "\n",
+    "$ x_{j+1|t} = Ax_{j|t}+Bu_{j|t})$    for $t<j<t+T-1 $\n",
+    "\n",
+    "Other linear constrains may be applied,for instance on the control variable:\n",
+    "\n",
+    "$ U_{MIN} < u_{j|t} < U_{MAX} $   for $t<j<t+T-1 $\n",
+    "\n",
+    "The objective fuction accounts for quadratic error on deviation from 0 of the state and the control inputs sequences. Q and R are the **weight matrices** and are used to tune the response.\n",
+    "\n",
+    "Because the goal is tracking a **reference signal** such as a trajectory, the objective function is rewritten as:\n",
     "\n",
     "$J(x(t),U) = \\sum^{t+T-1}_{j=t} \\delta x^T_{j|t}Q\\delta x_{j|t} + u^T_{j|t}Ru_{j|t}$\n",
     "\n",
@@ -796,11 +852,11 @@
     "\n",
     "s.t.\n",
     "\n",
-    "$ x_{j+1|t} = f(x_{j|t},{j|t}) t<j<t+T-1 $\n",
+    "$ x_{j+1|t} = f(x_{j|t},u_{j|t})$   for $t<j<t+T-1 $\n",
     "\n",
     "Other nonlinear constrains may be applied:\n",
     "\n",
-    "$ g(x_{j|t},{j|t})<0           t<j<t+T-1 $"
+    "$ g(x_{j|t},{j|t})<0$   for $t<j<t+T-1 $"
    ]
   },
   {
@@ -1176,9 +1232,9 @@
    "name": "python",
    "nbconvert_exporter": "python",
    "pygments_lexer": "ipython3",
-   "version": "3.5.2"
+   "version": "3.6.9"
   }
  },
  "nbformat": 4,
- "nbformat_minor": 2
+ "nbformat_minor": 4
 }

notebook/MPC_cvxpy.ipynb

@@ -122,13 +122,13 @@
     "\\quad\n",
     "$\n",
     "\n",
-    "This State Space Model is not linear (A,B are time changing), to linearize it the **Taylor's series expansion** is used around $\\bar{x}$ and $\\bar{u}$:\n",
+    "This State Space Model is **non-linear** (A,B are time changing), to linearize it the **Taylor's series expansion** is used around $\\bar{x}$ and $\\bar{u}$:\n",
     "\n",
     "$ \\dot{x} = f(x,u) \\approx f(\\bar{x},\\bar{u}) +  A(x-\\bar{x}) + B(u-\\bar{u})$\n",
     "\n",
     "So:\n",
     "\n",
-    "$ x{t+1} = x_t + (f(\\bar{x},\\bar{u}) +  A(x-\\bar{x}) + B(u-\\bar{u}) )dt $\n",
+    "$ x_{t+1} = x_t + (f(\\bar{x},\\bar{u}) +  A(x_t-\\bar{x}) + B(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",
@@ -157,6 +157,51 @@
     "-----------------"
    ]
   },
+  {
+   "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": {},
@@ -757,6 +802,7 @@
    "metadata": {},
    "source": [
     "![mpc](img/mpc_block_diagram.png)\n",
+    "\n",
     "![mpc](img/mpc_t.png)"
    ]
   },
@@ -778,9 +824,19 @@
     "\n",
     "$J(x(t),U) = \\sum^{t+T-1}_{j=t} x^T_{j|t}Qx_{j|t} + u^T_{j|t}Ru_{j|t}$\n",
     "\n",
-    "This accounts for quadratic error on deviation from 0 of the state and the control inputs sequences. Q and R are the **weight matrices** and are used to tune the response.\n",
+    "s.t.\n",
     "\n",
-    "Becouse the goal is tracking a **reference signal** such as a trajectory, the objective function is rewritten as:\n",
+    "$ x(0) = x0$\n",
+    "\n",
+    "$ x_{j+1|t} = Ax_{j|t}+Bu_{j|t})$    for $t<j<t+T-1 $\n",
+    "\n",
+    "Other linear constrains may be applied,for instance on the control variable:\n",
+    "\n",
+    "$ U_{MIN} < u_{j|t} < U_{MAX} $   for $t<j<t+T-1 $\n",
+    "\n",
+    "The objective fuction accounts for quadratic error on deviation from 0 of the state and the control inputs sequences. Q and R are the **weight matrices** and are used to tune the response.\n",
+    "\n",
+    "Because the goal is tracking a **reference signal** such as a trajectory, the objective function is rewritten as:\n",
     "\n",
     "$J(x(t),U) = \\sum^{t+T-1}_{j=t} \\delta x^T_{j|t}Q\\delta x_{j|t} + u^T_{j|t}Ru_{j|t}$\n",
     "\n",
@@ -796,11 +852,11 @@
     "\n",
     "s.t.\n",
     "\n",
-    "$ x_{j+1|t} = f(x_{j|t},{j|t}) t<j<t+T-1 $\n",
+    "$ x_{j+1|t} = f(x_{j|t},u_{j|t})$   for $t<j<t+T-1 $\n",
     "\n",
     "Other nonlinear constrains may be applied:\n",
     "\n",
-    "$ g(x_{j|t},{j|t})<0           t<j<t+T-1 $"
+    "$ g(x_{j|t},{j|t})<0$   for $t<j<t+T-1 $"
    ]
   },
   {
@@ -1176,9 +1232,9 @@
    "name": "python",
    "nbconvert_exporter": "python",
    "pygments_lexer": "ipython3",
-   "version": "3.5.2"
+   "version": "3.6.9"
   }
  },
  "nbformat": 4,
- "nbformat_minor": 2
+ "nbformat_minor": 4
 }

requirements.txt

@@ -0,0 +1,4 @@
+cvxpy==1.0.28
+numpy==1.18.1
+osqp==0.6.1
+scipy==1.4.1