diff --git a/data/mission.csv b/mission.csv similarity index 100% rename from data/mission.csv rename to mission.csv diff --git a/mission.py b/mission.py new file mode 100644 index 00000000..f941f8c5 --- /dev/null +++ b/mission.py @@ -0,0 +1,17 @@ +import pandas as pd + +class Mission: + def __init__(self, cave_height, cave_depth, reference): + self.cave_height = cave_height + self.cave_depth = cave_depth + self.reference = reference + + @classmethod + def from_csv(cls, filepath="mission.csv"): + data = pd.read_csv(filepath) + return cls( + cave_height=data["cave_height"].values, + cave_depth=data["cave_depth"].values, + reference=data["reference"].values + ) + diff --git a/notebooks/demo.ipynb b/notebooks/demo.ipynb index aa0c5cee..47215d0a 100644 --- a/notebooks/demo.ipynb +++ b/notebooks/demo.ipynb @@ -9,6 +9,16 @@ "# Add relevant Jupyter notebook extensions " ] }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "import sys\n", + "print(sys.executable)\n" + ] + }, { "cell_type": "code", "execution_count": null, @@ -42,13 +52,68 @@ "mission = Mission.from_csv(\"path/to/file\") # You must implement this method in the Mission class\n", "\n", "trajectory = closed_loop.simulate_with_random_disturbances(mission)\n", - "trajectory.plot_completed_mission(mission)" + "trajectory.plot_completed_mission(mission)\n" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "from mission import Mission\n", + "from uuv_mission.dynamic import ClosedLoop\n", + "from control import PDController\n", + "from submarine import Submarine\n", + "import numpy as np\n", + "\n", + "mission = Mission.from_csv(\"mission.csv\")\n", + "controller = PDController()\n", + "submarine = Submarine()\n", + "\n", + "system = ClosedLoop(submarine, controller)\n", + "\n", + "# simulate with zero disturbances\n", + "disturbances = np.zeros(len(mission.reference))\n", + "result = system.simulate(mission, disturbances)\n", + "\n", + "print(\"Simulation finished.\")\n", + "print(\"First few depths:\", result[\"positions\"][:5])\n", + "print(\"First few actions:\", result[\"actions\"][:5])\n" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "result = system.simulate(\n", + " Mission(reference=mission.reference[:100], # first 100 steps only\n", + " cave_height=mission.cave_height[:100],\n", + " cave_depth=mission.cave_depth[:100]),\n", + " np.zeros(100)\n", + ")" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "result = system.simulate(\n", + " Mission(reference=mission.reference[:20],\n", + " cave_height=mission.cave_height[:100],\n", + " cave_depth=mission.cave_depth[:100]),\n", + " np.zeros(100)\n", + ")\n" ] } ], "metadata": { "kernelspec": { - "display_name": "first-venv", + "display_name": ".venv", "language": "python", "name": "python3" }, @@ -62,7 +127,7 @@ "name": "python", "nbconvert_exporter": "python", "pygments_lexer": "ipython3", - "version": "3.9.12" + "version": "3.10.11" } }, "nbformat": 4, diff --git a/notebooks/depth_plot.png b/notebooks/depth_plot.png new file mode 100644 index 00000000..0837401d Binary files /dev/null and b/notebooks/depth_plot.png differ diff --git a/uuv_mission/dynamic.py b/uuv_mission/dynamic.py index c7c7ad53..35d34985 100644 --- a/uuv_mission/dynamic.py +++ b/uuv_mission/dynamic.py @@ -74,9 +74,17 @@ def random_mission(cls, duration: int, scale: float): return cls(reference, cave_height, cave_depth) @classmethod - def from_csv(cls, file_name: str): - # You are required to implement this method - pass + def from_csv(cls, file_name="../mission.csv"): + import pandas as pd + data = pd.read_csv(file_name) + return cls( + reference=data["reference"].values, + cave_height=data["cave_height"].values, + cave_depth=data["cave_depth"].values + ) + + + class ClosedLoop: @@ -84,23 +92,46 @@ def __init__(self, plant: Submarine, controller): self.plant = plant self.controller = controller - def simulate(self, mission: Mission, disturbances: np.ndarray) -> Trajectory: + def simulate(self, mission: Mission, disturbances: np.ndarray) -> dict: + import numpy as np T = len(mission.reference) - if len(disturbances) < T: - raise ValueError("Disturbances must be at least as long as mission duration") - - positions = np.zeros((T, 2)) + positions = np.zeros(T) actions = np.zeros(T) - self.plant.reset_state() + # Loop over each timestep for t in range(T): - positions[t] = self.plant.get_position() - observation_t = self.plant.get_depth() - # Call your controller here - self.plant.transition(actions[t], disturbances[t]) + if t % 10 == 0: + print(f"Step {t}, depth = {self.plant.get_depth():.2f}") + + # Reference and current depth + ref = mission.reference[t] + y = self.plant.get_depth() + + # Compute control signal using the PD controller + u = self.controller.compute(ref, y) + u = np.clip(u, -10, 10) + + # Apply disturbance if provided + if disturbances is not None and len(disturbances) > t: + disturbance = disturbances[t] + else: + disturbance = 0.0 + + # Update submarine model with control + disturbance + self.plant.transition(u, disturbance) + + # Store results + positions[t] = self.plant.get_depth() + actions[t] = u + + # Return results + return {"positions": positions, "actions": actions} + + + + - 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))