implementation and optimization

uuv_mission/__init__.py

@@ -0,0 +1,4 @@
+from .dynamic import Submarine
+from .dynamic import ClosedLoop
+from .dynamic import Mission
+from .control import controller

uuv_mission/control.py

@@ -0,0 +1,12 @@
+# implementation of a PD controller
+def controller(y0, y1, r0, r1):
+	# proportional gain
+	Kp = 0.18
+	# derivative gain
+	Kd = 0.65
+	# error
+	e0 = r0 - y0
+	e1 = r1 - y1
+	# action
+	u = Kp * e0 + Kd * (e0 - e1)
+	return u

uuv_mission/dynamic.py

@@ -3,105 +3,111 @@
 import numpy as np
 import matplotlib.pyplot as plt
 from .terrain import generate_reference_and_limits
+import pandas as pd
 
 class Submarine:
-    def __init__(self):
-
-        self.mass = 1
-        self.drag = 0.1
-        self.actuator_gain = 1
-
-        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
-
-
-    def transition(self, action: float, disturbance: float):
-        self.pos_x += self.vel_x * self.dt
-        self.pos_y += self.vel_y * self.dt
-
-        force_y = -self.drag * self.vel_y + self.actuator_gain * (action + disturbance)
-        acc_y = force_y / self.mass
-        self.vel_y += acc_y * self.dt
-
-    def get_depth(self) -> float:
-        return self.pos_y
-    
-    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
-    
+	def __init__(self):
+
+		self.mass = 1
+		self.drag = 0.1
+		self.actuator_gain = 1
+
+		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
+
+
+	def transition(self, action: float, disturbance: float):
+		self.pos_x += self.vel_x * self.dt
+		self.pos_y += self.vel_y * self.dt
+
+		force_y = -self.drag * self.vel_y + self.actuator_gain * (action + disturbance)
+		acc_y = force_y / self.mass
+		self.vel_y += acc_y * self.dt
+
+	def get_depth(self) -> float:
+		return self.pos_y
+	
+	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
+	
 class Trajectory:
-    def __init__(self, position: np.ndarray):
-        self.position = position  
-        
-    def plot(self):
-        plt.plot(self.position[:, 0], self.position[:, 1])
-        plt.show()
-
-    def plot_completed_mission(self, mission: Mission):
-        x_values = np.arange(len(mission.reference))
-        min_depth = np.min(mission.cave_depth)
-        max_height = np.max(mission.cave_height)
-
-        plt.fill_between(x_values, mission.cave_height, mission.cave_depth, color='blue', alpha=0.3)
-        plt.fill_between(x_values, mission.cave_depth, min_depth*np.ones(len(x_values)), 
-                         color='saddlebrown', alpha=0.3)
-        plt.fill_between(x_values, max_height*np.ones(len(x_values)), mission.cave_height, 
-                         color='saddlebrown', alpha=0.3)
-        plt.plot(self.position[:, 0], self.position[:, 1], label='Trajectory')
-        plt.plot(mission.reference, 'r', linestyle='--', label='Reference')
-        plt.legend(loc='upper right')
-        plt.show()
+	def __init__(self, position: np.ndarray):
+		self.position = position  
+		
+	def plot(self):
+		plt.plot(self.position[:, 0], self.position[:, 1])
+		plt.show()
+
+	def plot_completed_mission(self, mission: Mission):
+		x_values = np.arange(len(mission.reference))
+		min_depth = np.min(mission.cave_depth)
+		max_height = np.max(mission.cave_height)
+
+		plt.fill_between(x_values, mission.cave_height, mission.cave_depth, color='blue', alpha=0.3)
+		plt.fill_between(x_values, mission.cave_depth, min_depth*np.ones(len(x_values)), 
+						 color='saddlebrown', alpha=0.3)
+		plt.fill_between(x_values, max_height*np.ones(len(x_values)), mission.cave_height, 
+						 color='saddlebrown', alpha=0.3)
+		plt.plot(self.position[:, 0], self.position[:, 1], label='Trajectory')
+		plt.plot(mission.reference, 'r', linestyle='--', label='Reference')
+		plt.legend(loc='upper right')
+		plt.show()
 
 @dataclass
 class Mission:
-    reference: np.ndarray
-    cave_height: np.ndarray
-    cave_depth: np.ndarray
+	reference: np.ndarray
+	cave_height: np.ndarray
+	cave_depth: np.ndarray
 
-    @classmethod
-    def random_mission(cls, duration: int, scale: float):
-        (reference, cave_height, cave_depth) = generate_reference_and_limits(duration, scale)
-        return cls(reference, cave_height, cave_depth)
+	@classmethod
+	def random_mission(cls, duration: int, scale: float):
+		(reference, cave_height, cave_depth) = generate_reference_and_limits(duration, scale)
+		return cls(reference, cave_height, cave_depth)
 
-    @classmethod
-    def from_csv(cls, file_name: str):
-        # You are required to implement this method
-        pass
+	@classmethod
+	def from_csv(cls, file_name: str):
+		f = pd.read_csv(file_name)
+		reference = f['reference'].values
+		cave_height = f['cave_height'].values
+		cave_depth = f['cave_depth'].values
+		return cls(reference, cave_height, cave_depth)
 
 
 class ClosedLoop:
-    def __init__(self, plant: Submarine, controller):
-        self.plant = plant
-        self.controller = controller
-
-    def simulate(self,  mission: Mission, disturbances: np.ndarray) -> Trajectory:
-
-        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))
-        actions = np.zeros(T)
-        self.plant.reset_state()
-
-        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])
-
-        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)
+	def __init__(self, plant: Submarine, controller):
+		self.plant = plant
+		self.controller = controller
+
+	def simulate(self,  mission: Mission, disturbances: np.ndarray) -> Trajectory:
+
+		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))
+		actions = np.zeros(T)
+		self.plant.reset_state()
+
+		for t in range(T):
+			positions[t] = self.plant.get_position()
+			observation_t = self.plant.get_depth()
+			# Call your controller here
+			actions[t] = self.controller(positions[t][1], positions[t-1][1],
+				mission.reference[t], mission.reference[t-1])
+			self.plant.transition(actions[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)