Complete assignment

.gitignore

@@ -1,3 +1,4 @@
 .DS_Store
 .vscode/
-__pycache__/
+__pycache__/
+.idea/

notebooks/demo.ipynb

@@ -1,12 +1,29 @@
 {
  "cells": [
+  {
+   "cell_type": "markdown",
+   "metadata": {},
+   "source": [
+    "# Demo Script\n",
+    "\n",
+    "This file can be used to test out functionality of the code"
+   ]
+  },
+  {
+   "cell_type": "markdown",
+   "metadata": {},
+   "source": [
+    "### Load extensions and check PATH"
+   ]
+  },
   {
    "cell_type": "code",
    "execution_count": 1,
    "metadata": {},
    "outputs": [],
    "source": [
-    "# Add relevant Jupyter notebook extensions "
+    "%load_ext autoreload\n",
+    "%autoreload 2"
    ]
   },
   {
@@ -15,17 +32,38 @@
    "metadata": {},
    "outputs": [],
    "source": [
-    "# You can double-check your Python path like this...\n",
-    "import sys  \n",
-    "print(sys.path)"
+    "import sys\n",
+    "print(\"\\n\".join(sys.path))"
    ]
   },
   {
    "cell_type": "markdown",
    "metadata": {},
    "source": [
-    "# Simulate closed-loop\n",
-    "After implementing your control functionality, you can simulate the closed-loop with code that looks something like this..."
+    "### Test data extraction method"
+   ]
+  },
+  {
+   "cell_type": "code",
+   "execution_count": 3,
+   "metadata": {},
+   "outputs": [],
+   "source": [
+    "# Script to test Mission class\n",
+    "from uuv_mission import Submarine, Mission, Trajectory\n",
+    "\n",
+    "submarine = Submarine()\n",
+    "random_mission = Mission.random_mission(duration=100, scale=40)\n",
+    "mission = Mission.from_csv('C:/Users/cvest/Claudio/Oxford/3rd Year/B1/b1-coding-practical-mt24/data/mission.csv')\n",
+    "# Trajectory.plot_completed_mission(submarine, mission) # to check data extraction"
+   ]
+  },
+  {
+   "cell_type": "markdown",
+   "metadata": {},
+   "source": [
+    "## Simulate closed-loop\n",
+    "### Test the PD controller"
    ]
   },
   {
@@ -34,21 +72,43 @@
    "metadata": {},
    "outputs": [],
    "source": [
-    "# Import relevant modules\n",
+    "from uuv_mission import Submarine, Mission, ClosedLoop, PDController\n",
     "\n",
     "sub = Submarine()\n",
-    "# Instantiate your controller (depending on your implementation)\n",
-    "closed_loop = ClosedLoop(sub, controller)\n",
-    "mission = Mission.from_csv(\"path/to/file\") # You must implement this method in the Mission class\n",
+    "\n",
+    "A, B, C, D = sub.get_dynamics()\n",
+    "pd_controller = PDController(A, B, C, D)\n",
+    "\n",
+    "closed_loop = ClosedLoop(sub, pd_controller)\n",
+    "mission = Mission.from_csv(\n",
+    "    'C:/Users/cvest/Claudio/Oxford/3rd Year/B1/b1-coding-practical-mt24/data/mission.csv'\n",
+    ")\n",
+    "random_mission = Mission.random_mission(duration=100, scale=40) # to test on random mission\n",
     "\n",
     "trajectory = closed_loop.simulate_with_random_disturbances(mission)\n",
     "trajectory.plot_completed_mission(mission)"
    ]
+  },
+  {
+   "cell_type": "markdown",
+   "metadata": {},
+   "source": [
+    "### Test the MPC controller"
+   ]
+  },
+  {
+   "cell_type": "code",
+   "execution_count": null,
+   "metadata": {},
+   "outputs": [],
+   "source": [
+    "# Decided to leave this as report was already getting long"
+   ]
   }
  ],
  "metadata": {
   "kernelspec": {
-   "display_name": "first-venv",
+   "display_name": "a2e",
    "language": "python",
    "name": "python3"
   },
@@ -62,7 +122,7 @@
    "name": "python",
    "nbconvert_exporter": "python",
    "pygments_lexer": "ipython3",
-   "version": "3.9.12"
+   "version": "3.11.4"
   }
  },
  "nbformat": 4,

report/report.tex

@@ -0,0 +1,162 @@
+\documentclass[hidelinks]{article}
+%
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+% START CUSTOM INCLUDES & DEFINITIONS
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+%
+\usepackage{amsmath}
+\usepackage{parskip} %noident everywhere
+\usepackage{hyperref} % Show hyperlinks - claudio
+\hypersetup{
+    colorlinks = true
+    linkcolor = blue
+    urlcolor = red
+    }
+% Block diagrams
+\usepackage{tikz}
+\usetikzlibrary{shapes.geometric, arrows, calc}
+\tikzstyle{block} = [rectangle, draw,
+    text centered, rounded corners, minimum height=3em, minimum width=6em]
+\tikzstyle{sum} = [draw, circle, node distance=1cm]
+\tikzstyle{input} = [coordinate]
+\tikzstyle{output} = [coordinate]
+\tikzstyle{arrow} = [draw, -latex']
+%
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+% END CUSTOM INCLUDES & DEFINITIONS
+%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
+%
+\pdfobjcompresslevel=0
+%
+\title{\vspace{-4cm} Submarine Mission Report}
+\author{\vspace{-2cm} Claudio Vestini}
+\date{}
+\begin{document}
+\maketitle
+%
+\paragraph{Motivation}
+This brief report will concern the B1 submarine coding practical, where
+we were tasked with designing the controller to guide a Submarine through its cave mission by tracking a given reference to avoid collisions with the cave boundaries.
+Preliminary steps taken before starting development:
+%
+\begin{enumerate}
+    \item Create and activate a virtual environment with the given requirements (numpy, matplotlib and pandas packages).
+    \item Fork project repository to my GitHub account.
+    \item Set up a .env file to add local packages onto Python PATH.
+    \item Make sure running files do not give any errors before branching off `main'.
+\end{enumerate}
+%
+\paragraph{Mission Data}
+The first task was to obtain mission data from the given .csv file. I achieved this through the following steps:
+%
+\begin{enumerate}
+    \setcounter{enumi}{4}
+    \item Create a new branch to modify the Mission class within.
+    \item Implement a new classmethod to extract each column of the mission.csv file into a separate variable, then return the data as an instance of the Mission class.
+    \item Test the new functionality by using the Trajectory class's plotting methods; then merge the branch back into `main', and delete the unused branch.
+\end{enumerate}
+%
+\paragraph{Controller}
+The design of the controller necessitated a thorough understanding of the Submarine and ClosedLoop classes, both of which were to be modified.
+For full analysis, check Apppendix~\ref{appendix}. I completed my implementation as follows:
+%
+\begin{enumerate}
+    \setcounter{enumi}{7}
+    \item Create a new branch to avoid pushing error prone code to `main'.
+    \item Add a new method to the Submarine class to obtain the state space dynamics via matrices $A$, $B$, $C$ and $D$.
+    \item Inside of a new module named control.py, create a new Controller class, initialised with the four matrices above. Create a subclass PDController, which inherits the dynamics, and possesses additional class variables $K_P$ and $K_D$. For this subclass, develop a class method to compute the next control action $u[t]$ given observation $y[t]$ and reference $r[t]$.
+    \item Modify the ClosedLoop class to correctly call the controller within the simulate method.
+    \item Test the controller by using the given demo.ipynb file. Merge and delete the branch.
+\end{enumerate}
+My decision to use a hierarchical class system proved effective as it kept the codebase modular. It also allows for future development of any other type of controller: I subsequently developed another subclass MPCController, which was better at tracking the reference but required more computational effort.
+%
+\newpage
+\appendix
+\section{System Equations} \label{appendix}
+To implement the controller, I first analysed the given code to infer the system's dynamics. The submarine progresses at constant speed in the $x$ direction, so needs only be controlled in the y direction (another hint to this is that we only have one set of reference values).
+\par By inspection of the transition method, given drag $D$, velocity $\frac{dy}{dt}$, actuator gain $K$, input $u(t)$ and disturbance $d(t)$, the force in the vertical direction is given by:
+%
+\begin{equation}
+    F_y = - D \cdot \frac{dy}{dt} + K \cdot [u(t) + d(t)] \label{eq:force}
+\end{equation}
+%
+\par Combining \eqref{eq:force} with Newton's second law we obtain acceleration:
+%
+\begin{equation}
+    \frac{d^2y}{dt^2} = -\frac{D}{m} \cdot \frac{dy}{dt} + \frac{K}{m} \cdot [u(t) + d(t)] \label{eq:accelleration}
+\end{equation}
+%
+\par It is then very simple to obtain all matrices for the (continuous) state space dynamics of the Plant in canonic form:
+\begin{equation}
+    \begin{aligned}
+        \dot{x}(t) &= A x(t) + B [u(t) + d(t)] \\
+        y(t) &= C x(t) + D u(t) \label{eq:plant}
+    \end{aligned}
+\end{equation}
+%
+as:
+%
+\begin{equation}
+    \displaystyle
+    A = \begin{bmatrix}
+            0 & 1 \\
+            0 & -\frac{D}{m}
+        \end{bmatrix}; \quad
+    B = \begin{bmatrix}
+            0 \\
+            \frac{K}{m}
+        \end{bmatrix}; \quad
+    C = \begin{bmatrix}
+            1 \\
+            0
+          \end{bmatrix}; \quad
+    D = \begin{bmatrix}
+            0
+        \end{bmatrix} \label{eq:matrices}
+\end{equation}
+%
+\par Our system operates in discrete time, hence the PD Controller takes the standard form:
+%
+\begin{equation}
+u[t] = K_P \cdot e[t] + K_D \cdot (e[t] - e[t-1]) \label{eq:controller}
+\end{equation}
+%
+\par The system diagram in discrete time is then:
+%
+\begin{center}
+\begin{tikzpicture}[auto, node distance=2cm]
+    % Nodes
+    \node [input] (input) {};
+    \node [sum, right of=input, node distance=1.5cm] (sum) {$\Sigma$};
+    \node [block, right of=sum, node distance=2.5cm] (controller) {Controller};
+    \node [sum, right of=controller, node distance=2.5cm] (sum2) {$\Sigma$};
+    \node [block, right of=sum2, node distance=2cm] (plant) {Plant};
+    \node [coordinate, right of=plant, node distance=1.5cm] (feedback) {};
+    \node [output, right of=plant, node distance=2.5cm] (output) {};
+
+    % Disturbance input node
+    \node [input, above of=sum2, node distance=2cm] (disturbance) {};
+
+    % Labels for summing junctions
+    \node at ($(sum) + (-0.4,0.4)$) {+}; % Plus sign at r[t] input
+    \node at ($(sum) + (-0.4,-0.4)$) {--}; % Minus sign at y[t] input
+    \node at ($(sum2) + (-0.4,0.4)$) {+}; % Plus sign at d[t] input
+    \node at ($(sum2) + (-0.4,-0.4)$) {+}; % Plus sign at u[t] input
+
+    % Arrows
+    \draw [arrow] (input) -- node {$r[t]$} (sum);
+    \draw [arrow] (sum) -- node {$e[t]$} (controller);
+    \draw [arrow] (controller) -- node {$u[t]$} (sum2);
+    \draw [arrow] (sum2) -- node {} (plant);
+    \draw [arrow] (plant) -- node {$y[t]$} (output);
+    \draw [arrow] (disturbance) -- node [near end] {$d[t]$} (sum2);
+
+    % Feedback
+    \draw [arrow] (feedback) |- ++(0,-2) -| node[pos=0.85] {$y[t]$} (sum);
+
+\end{tikzpicture}
+\end{center}
+\vspace{0.2cm}
+where the Plant is specified in equations~\eqref{eq:plant} and~\eqref{eq:matrices} and the Controller is given by equation~\eqref{eq:controller}, with gains $K_P = 0.15$ and $K_D = 0.6$ (as per requirements).
+%
+\end{document}

requirements.txt

@@ -1,3 +1,4 @@
 matplotlib==3.9.2
 numpy==2.1.1
 pandas==2.2.3
+cvxpy==1.1.15

uuv_mission/__init__.py

@@ -0,0 +1,5 @@
+from .dynamic import Submarine, Mission, ClosedLoop, Trajectory
+from .control import PDController, MPCController
+
+__all__ = ['Submarine', 'Mission', 'ClosedLoop', 'Trajectory',
+           'PDController', 'MPCController']

uuv_mission/control.py

@@ -0,0 +1,59 @@
+import numpy as np
+import cvxpy as cp
+
+class Controller:
+    def __init__(self, A: np.ndarray, B: np.ndarray, C: np.ndarray, D: np.ndarray):
+
+        self.A = A
+        self.B = B
+        self.C = C
+        self.D = D
+
+class PDController(Controller):
+    def __init__(self, A: np.ndarray, B: np.ndarray, C: np.ndarray, D: np.ndarray,
+                 Kp: float = 0.15, Kd: float = 0.6):
+
+        super().__init__(A, B, C, D)
+        self.Kp = Kp
+        self.Kd = Kd
+        self.previous_error = 0.
+
+    def compute_control_action(self, x0: np.ndarray, reference: float) -> float:
+
+        error = reference - self.C @ x0
+        derivative = (error - self.previous_error)
+
+        u = self.Kp * error + self.Kd * derivative
+        self.previous_error = error
+
+        return u.item()
+
+class MPCController(Controller):
+    def __init__(self, A: np.ndarray, B: np.ndarray, C: np.ndarray, D: np.ndarray,
+                 horizon: int, Q: np.ndarray, R: np.ndarray):
+
+        super().__init__(A, B, C, D)
+        self.horizon = horizon
+        self.Q = Q
+        self.R = R
+
+    def compute_control_action(self, x0: np.ndarray, reference: np.ndarray) -> np.ndarray:
+
+        x = cp.Variable((self.A.shape[0], self.horizon + 1))
+        u = cp.Variable((self.B.shape[1], self.horizon))
+        y = cp.Variable((self.C.shape[0], self.horizon + 1))
+
+        cost = 0
+        constraints = [x[:, 0] == x0]
+        for t in range(self.horizon):
+            cost += cp.quad_form(y[:, t] - reference[:, t], self.Q) + cp.quad_form(u[:, t], self.R)
+            constraints += [
+                x[:, t + 1] == self.A @ x[:, t] + self.B @ u[:, t],
+                y[:, t] == self.C @ x[:, t] + self.D @ u[:, t]
+            ]
+
+        problem = cp.Problem(cp.Minimize(cost), constraints)
+
+        problem.solve()
+
+        return u[:, 0].value # return first control action