Mission class update

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
+        )
+

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,

uuv_mission/dynamic.py

@@ -74,33 +74,64 @@ 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:
     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))