diff --git a/.gitignore b/.gitignore index 508562dc..d56c9507 100644 --- a/.gitignore +++ b/.gitignore @@ -1,3 +1,4 @@ .DS_Store .vscode/ -__pycache__/ \ No newline at end of file +__pycache__/ +.idea/ \ No newline at end of file diff --git a/notebooks/demo.ipynb b/notebooks/demo.ipynb index aa0c5cee..345dbbbc 100644 --- a/notebooks/demo.ipynb +++ b/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, diff --git a/report/SubMission Report.pdf b/report/SubMission Report.pdf new file mode 100644 index 00000000..3659e16d Binary files /dev/null and b/report/SubMission Report.pdf differ diff --git a/report/completed_mission.png b/report/completed_mission.png new file mode 100644 index 00000000..49d6d1dc Binary files /dev/null and b/report/completed_mission.png differ diff --git a/report/report.tex b/report/report.tex new file mode 100644 index 00000000..10a495cb --- /dev/null +++ b/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} \ No newline at end of file diff --git a/requirements.txt b/requirements.txt index 608462be..ae056dd2 100644 --- a/requirements.txt +++ b/requirements.txt @@ -1,3 +1,4 @@ matplotlib==3.9.2 numpy==2.1.1 pandas==2.2.3 +cvxpy==1.1.15 \ No newline at end of file diff --git a/uuv_mission/__init__.py b/uuv_mission/__init__.py index e69de29b..483c4ff6 100644 --- a/uuv_mission/__init__.py +++ b/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'] \ No newline at end of file diff --git a/uuv_mission/control.py b/uuv_mission/control.py new file mode 100644 index 00000000..eb592b9e --- /dev/null +++ b/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 \ No newline at end of file diff --git a/uuv_mission/dynamic.py b/uuv_mission/dynamic.py index c7c7ad53..b2314dfa 100644 --- a/uuv_mission/dynamic.py +++ b/uuv_mission/dynamic.py @@ -7,16 +7,16 @@ class Submarine: def __init__(self): - self.mass = 1 + self.mass = 1. self.drag = 0.1 - self.actuator_gain = 1 + self.actuator_gain = 1. - self.dt = 1 # Time step for discrete time simulation + self.dt = 1. # Time step for discrete time simulation - self.pos_x = 0 - self.pos_y = 0 - self.vel_x = 1 # Constant velocity in x direction - self.vel_y = 0 + self.pos_x = 0. + self.pos_y = 0. + self.vel_x = 1. # Constant velocity in x direction + self.vel_y = 0. def transition(self, action: float, disturbance: float): @@ -34,11 +34,21 @@ def get_position(self) -> tuple: return self.pos_x, self.pos_y def reset_state(self): - self.pos_x = 0 - self.pos_y = 0 - self.vel_x = 1 - self.vel_y = 0 - + self.pos_x = 0. + self.pos_y = 0. + self.vel_x = 1. + self.vel_y = 0. + + def get_dynamics(self): + + A = np.array([[0, 1], [0, -self.drag / self.mass]]) + B = np.array([[0], [self.actuator_gain / self.mass]]) + C = np.array([[1, 0]]) + D = np.array([[0]]) + + return A, B, C, D + + class Trajectory: def __init__(self, position: np.ndarray): self.position = position @@ -75,8 +85,11 @@ def random_mission(cls, duration: int, scale: float): @classmethod def from_csv(cls, file_name: str): - # You are required to implement this method - pass + data = np.genfromtxt(file_name, delimiter=',', skip_header=1) + reference = data[:, 0] + cave_height = data[:, 1] + cave_depth = data[:, 2] + return cls(reference, cave_height, cave_depth) class ClosedLoop: @@ -95,13 +108,21 @@ def simulate(self, mission: Mission, disturbances: np.ndarray) -> Trajectory: self.plant.reset_state() for t in range(T): - positions[t] = self.plant.get_position() + positions[t] = self.plant.get_position() # for plotting only + + # Assume the two observable states are position and velocity observation_t = self.plant.get_depth() - # Call your controller here - self.plant.transition(actions[t], disturbances[t]) + velocity_t = observation_t - positions[t-1, 1] if t > 0 else 0 + x0 = np.array([observation_t, velocity_t]) + + reference_t = mission.reference[t] + action_t = self.controller.compute_control_action(x0, reference_t) + actions[t] = action_t + + self.plant.transition(action_t, disturbances[t]) return Trajectory(positions) def simulate_with_random_disturbances(self, mission: Mission, variance: float = 0.5) -> Trajectory: disturbances = np.random.normal(0, variance, len(mission.reference)) - return self.simulate(mission, disturbances) + return self.simulate(mission, disturbances) \ No newline at end of file diff --git a/uuv_mission/terrain.py b/uuv_mission/terrain.py index a69f4932..24ac42bf 100644 --- a/uuv_mission/terrain.py +++ b/uuv_mission/terrain.py @@ -40,7 +40,7 @@ def plot_reference_and_terrain(reference, upper, lower): plt.show() def write_mission_to_csv(mission, file_name): - # Create a DataFrame from the Mission object's attributes + data = { 'reference': mission.reference, 'cave_height': mission.cave_height, @@ -48,8 +48,4 @@ def write_mission_to_csv(mission, file_name): } df = pd.DataFrame(data) - # Write the DataFrame to a CSV file - df.to_csv(file_name, index=False) - - - \ No newline at end of file + df.to_csv(file_name, index=False) \ No newline at end of file diff --git a/uuv_mission/test.py b/uuv_mission/test.py new file mode 100644 index 00000000..7038b1e2 --- /dev/null +++ b/uuv_mission/test.py @@ -0,0 +1,17 @@ +# Import relevant modules +from uuv_mission import Submarine, Mission, ClosedLoop, PDController + + +sub = Submarine() + +# Instantiate your controller (depending on your implementation) +A, B, C, D = sub.get_dynamics() +pd_controller = PDController(A, B, C, D) + +closed_loop = ClosedLoop(sub, pd_controller) +mission = Mission.from_csv( + 'C:/Users/cvest/Claudio/Oxford/3rd Year/B1/b1-coding-practical-mt24/data/mission.csv' +) + +trajectory = closed_loop.simulate_with_random_disturbances(mission) +trajectory.plot_completed_mission(mission) \ No newline at end of file