From b3593ab10037a28d3f2bca57cae0ad38e84ebf69 Mon Sep 17 00:00:00 2001
From: mcarfagno <m.carfagno@outlook.com>
Date: Tue, 7 Apr 2020 16:45:11 +0100
Subject: [PATCH] Updated LATEX equations in notebook

---
 .ipynb_checkpoints/README-checkpoint.md       |  50 +++++
 README.md                                     |   2 +-
 .../.ipynb_checkpoints/main-checkpoint.py     | 184 ++++++++++++++++++
 .../MPC_cvxpy-checkpoint.ipynb                |  42 ++--
 notebook/MPC_cvxpy.ipynb                      |  42 ++--
 notebook/MPC_scipy.ipynb                      |   4 +-
 6 files changed, 291 insertions(+), 33 deletions(-)
 create mode 100644 .ipynb_checkpoints/README-checkpoint.md
 create mode 100755 mpc_demo/.ipynb_checkpoints/main-checkpoint.py

diff --git a/.ipynb_checkpoints/README-checkpoint.md b/.ipynb_checkpoints/README-checkpoint.md
new file mode 100644
index 0000000..0296083
--- /dev/null
+++ b/.ipynb_checkpoints/README-checkpoint.md
@@ -0,0 +1,50 @@
+# mpc_python
+
+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
+```
diff --git a/README.md b/README.md
index 0296083..aff415d 100644
--- a/README.md
+++ b/README.md
@@ -6,7 +6,7 @@ Python implementation of mpc controller for path tracking.
 
 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} $
+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.
 
diff --git a/mpc_demo/.ipynb_checkpoints/main-checkpoint.py b/mpc_demo/.ipynb_checkpoints/main-checkpoint.py
new file mode 100755
index 0000000..a05b363
--- /dev/null
+++ b/mpc_demo/.ipynb_checkpoints/main-checkpoint.py
@@ -0,0 +1,184 @@
+#! /usr/bin/env python
+
+import numpy as np
+import matplotlib.pyplot as plt
+from matplotlib import animation
+
+from utils import compute_path_from_wp
+from cvxpy_mpc import optimize
+
+import sys
+import time
+
+# Robot Starting position
+SIM_START_X=0
+SIM_START_Y=2
+SIM_START_H=0
+
+# Classes
+class MPC():
+
+    def __init__(self):
+
+        # State for the robot mathematical model [x,y,heading]
+        self.state =  [SIM_START_X, SIM_START_Y, SIM_START_H]
+        # Sim step
+        self.dt = 0.25
+
+        # starting guess output
+        N = 5 #number of state variables
+        M = 2 #number of control variables
+        T = 20 #Prediction Horizon
+        self.opt_u =  np.zeros((M,T))
+        self.opt_u[0,:] = 1 #m/s
+        self.opt_u[1,:] = np.radians(0) #rad/s
+
+        # Interpolated Path to follow given waypoints
+        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
+        self.x_history=[]
+        self.y_history=[]
+        self.h_history=[]
+        self.v_history=[]
+        self.w_history=[]
+
+        #Initialise plot
+        plt.style.use("ggplot")
+        self.fig = plt.figure()
+        plt.ion()
+        plt.show()
+
+    def run(self):
+        '''
+        '''
+
+        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))
+
+                self.update_sim(self.opt_u[0,1],self.opt_u[1,1])
+
+                self.plot_sim()
+
+    def update_sim(self,lin_v,ang_v):
+        '''
+        Updates state.
+
+        :param lin_v: float
+        :param ang_v: float
+        '''
+
+        self.state[0] = self.state[0] +lin_v*np.cos(self.state[2])*self.dt
+        self.state[1] = self.state[1] +lin_v*np.sin(self.state[2])*self.dt
+        self.state[2] = self.state[2] +ang_v*self.dt
+
+    def plot_sim(self):
+
+        self.sim_time = self.sim_time+self.dt
+        self.x_history.append(self.state[0])
+        self.y_history.append(self.state[1])
+        self.h_history.append(self.state[2])
+        self.v_history.append(self.opt_u[0,1])
+        self.w_history.append(self.opt_u[1,1])
+
+        plt.clf()
+
+        grid = plt.GridSpec(2, 3)
+
+        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.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.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('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]')
+        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:
+        sim.run()
+    except Exception as e:
+        sys.exit(e)
+
+if __name__ == '__main__':
+    do_sim()
diff --git a/notebook/.ipynb_checkpoints/MPC_cvxpy-checkpoint.ipynb b/notebook/.ipynb_checkpoints/MPC_cvxpy-checkpoint.ipynb
index bac2213..8583868 100644
--- a/notebook/.ipynb_checkpoints/MPC_cvxpy-checkpoint.ipynb
+++ b/notebook/.ipynb_checkpoints/MPC_cvxpy-checkpoint.ipynb
@@ -822,37 +822,49 @@
     "\n",
     "The objective function used minimize (drive the state to 0) is:\n",
     "\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",
-    "s.t.\n",
-    "\n",
-    "$ x(0) = x0$\n",
-    "\n",
-    "$ x_{j+1|t} = Ax_{j|t}+Bu_{j|t})$    for $t<j<t+T-1 $\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} t<j<t+T-1    \\\\\n",
+    "\\end{aligned}\n",
+    "\\end{equation}\n",
+    "$\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",
+    "$ U_{MIN} < u_{j|t} < U_{MAX}    \\quad  \\textrm{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",
+    "\\begin{equation}\n",
+    "\\begin{aligned}\n",
+    "\\min_{} \\quad &  \\sum^{t+T-1}_{j=t} \\delta x^T_{j|t}Q\\delta x_{j|t} + u^T_{j|t}Ru_{j|t}\n",
+    "\\end{aligned}\n",
+    "\\end{equation}\n",
+    "$\n",
     "\n",
     "where the error w.r.t desired state is accounted for:\n",
     "\n",
-    "$\\delta x = x_{j,t,ref} - x_{j,t}$\n",
+    "$ \\delta x = x_{j,t,ref} - x_{j,t} $\n",
     "\n",
     "#### Non-Linear MPC Formulation\n",
     "\n",
     "In general cases, the objective function is any non-differentiable non-linear function of states and inputs over a finite horizon T. In this case the constrains include nonlinear dynamics of motion.\n",
     "\n",
-    "$J(x(t),U) = \\sum^{t+T}_{j=t} C(x_{j|t},{j|t})$\n",
-    "\n",
-    "s.t.\n",
-    "\n",
-    "$ x_{j+1|t} = f(x_{j|t},u_{j|t})$   for $t<j<t+T-1 $\n",
+    "$\n",
+    "\\begin{equation}\n",
+    "\\begin{aligned}\n",
+    "\\min_{} \\quad &  \\sum^{t+T}_{j=t} C(x_{j|t},{j|t})\\\\\n",
+    "\\textrm{s.t.} \\quad &  x(0) = x0\\\\\n",
+    "  & x_{j+1|t} = f(x_{j|t},u_{j|t}) \\quad  \\textrm{for} t<j<t+T-1    \\\\\n",
+    "\\end{aligned}\n",
+    "\\end{equation}\n",
+    "$\n",
     "\n",
     "Other nonlinear constrains may be applied:\n",
     "\n",
diff --git a/notebook/MPC_cvxpy.ipynb b/notebook/MPC_cvxpy.ipynb
index bac2213..8583868 100644
--- a/notebook/MPC_cvxpy.ipynb
+++ b/notebook/MPC_cvxpy.ipynb
@@ -822,37 +822,49 @@
     "\n",
     "The objective function used minimize (drive the state to 0) is:\n",
     "\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",
-    "s.t.\n",
-    "\n",
-    "$ x(0) = x0$\n",
-    "\n",
-    "$ x_{j+1|t} = Ax_{j|t}+Bu_{j|t})$    for $t<j<t+T-1 $\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} t<j<t+T-1    \\\\\n",
+    "\\end{aligned}\n",
+    "\\end{equation}\n",
+    "$\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",
+    "$ U_{MIN} < u_{j|t} < U_{MAX}    \\quad  \\textrm{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",
+    "\\begin{equation}\n",
+    "\\begin{aligned}\n",
+    "\\min_{} \\quad &  \\sum^{t+T-1}_{j=t} \\delta x^T_{j|t}Q\\delta x_{j|t} + u^T_{j|t}Ru_{j|t}\n",
+    "\\end{aligned}\n",
+    "\\end{equation}\n",
+    "$\n",
     "\n",
     "where the error w.r.t desired state is accounted for:\n",
     "\n",
-    "$\\delta x = x_{j,t,ref} - x_{j,t}$\n",
+    "$ \\delta x = x_{j,t,ref} - x_{j,t} $\n",
     "\n",
     "#### Non-Linear MPC Formulation\n",
     "\n",
     "In general cases, the objective function is any non-differentiable non-linear function of states and inputs over a finite horizon T. In this case the constrains include nonlinear dynamics of motion.\n",
     "\n",
-    "$J(x(t),U) = \\sum^{t+T}_{j=t} C(x_{j|t},{j|t})$\n",
-    "\n",
-    "s.t.\n",
-    "\n",
-    "$ x_{j+1|t} = f(x_{j|t},u_{j|t})$   for $t<j<t+T-1 $\n",
+    "$\n",
+    "\\begin{equation}\n",
+    "\\begin{aligned}\n",
+    "\\min_{} \\quad &  \\sum^{t+T}_{j=t} C(x_{j|t},{j|t})\\\\\n",
+    "\\textrm{s.t.} \\quad &  x(0) = x0\\\\\n",
+    "  & x_{j+1|t} = f(x_{j|t},u_{j|t}) \\quad  \\textrm{for} t<j<t+T-1    \\\\\n",
+    "\\end{aligned}\n",
+    "\\end{equation}\n",
+    "$\n",
     "\n",
     "Other nonlinear constrains may be applied:\n",
     "\n",
diff --git a/notebook/MPC_scipy.ipynb b/notebook/MPC_scipy.ipynb
index a089768..11d0c03 100644
--- a/notebook/MPC_scipy.ipynb
+++ b/notebook/MPC_scipy.ipynb
@@ -723,9 +723,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
 }