From a8ba044b7b67fb5fd7559399b657b47d35cf2f54 Mon Sep 17 00:00:00 2001 From: = <=> Date: Tue, 15 Oct 2024 12:47:38 +0100 Subject: [PATCH 1/7] commit with Mission.reference working --- uuv_mission/dynamic.py | 21 ++++++++++++++++++++- 1 file changed, 20 insertions(+), 1 deletion(-) diff --git a/uuv_mission/dynamic.py b/uuv_mission/dynamic.py index c7c7ad53..200ee141 100644 --- a/uuv_mission/dynamic.py +++ b/uuv_mission/dynamic.py @@ -2,7 +2,8 @@ from dataclasses import dataclass import numpy as np import matplotlib.pyplot as plt -from .terrain import generate_reference_and_limits +#from .terrain import generate_reference_and_limits +import csv class Submarine: def __init__(self): @@ -68,6 +69,14 @@ class Mission: cave_height: np.ndarray cave_depth: np.ndarray + #Initialising the arrays + @classmethod + def __init__(self, reference, cave_height, cave_depth): + self.reference = reference + self.cave_height = cave_height + self.cave_depth = cave_depth + + @classmethod def random_mission(cls, duration: int, scale: float): (reference, cave_height, cave_depth) = generate_reference_and_limits(duration, scale) @@ -76,6 +85,12 @@ def random_mission(cls, duration: int, scale: float): @classmethod def from_csv(cls, file_name: str): # You are required to implement this method + with open("data/%s.csv" % (file_name)) as file: + for row in file: + Mission.reference.append(row.split()[0]) + + print(Mission.reference) + pass @@ -105,3 +120,7 @@ def simulate(self, mission: Mission, disturbances: np.ndarray) -> Trajectory: 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) + +#Initialise the class mission with empty arrays +Mission([],[],[]) +Mission.from_csv("mission") \ No newline at end of file From 7fa6d3e37abdd498a8e3c9ac18c5835e469ce2ab Mon Sep 17 00:00:00 2001 From: = <=> Date: Tue, 15 Oct 2024 12:58:11 +0100 Subject: [PATCH 2/7] commit with all reference, height and depth successfully stored --- uuv_mission/dynamic.py | 15 ++++++++++++--- 1 file changed, 12 insertions(+), 3 deletions(-) diff --git a/uuv_mission/dynamic.py b/uuv_mission/dynamic.py index 200ee141..16f8db13 100644 --- a/uuv_mission/dynamic.py +++ b/uuv_mission/dynamic.py @@ -87,9 +87,16 @@ def from_csv(cls, file_name: str): # You are required to implement this method with open("data/%s.csv" % (file_name)) as file: for row in file: - Mission.reference.append(row.split()[0]) + Mission.reference.append(row.split(',')[0]) + Mission.cave_height.append(row.split(',')[1]) + + #Each row in the csv actually ends with /n, which we need to remove + full_value = row.split(',')[2] + shortened_value = full_value[:-1] + Mission.cave_depth.append(shortened_value) + + return Mission - print(Mission.reference) pass @@ -123,4 +130,6 @@ def simulate_with_random_disturbances(self, mission: Mission, variance: float = #Initialise the class mission with empty arrays Mission([],[],[]) -Mission.from_csv("mission") \ No newline at end of file + +Mission.from_csv("mission") +#Now Mission.reference has a list of all the refernce values, Mission.cave_depth has all of the depths and Mission.cave_height has all the heights in arrays From fdb83bb6b902fc8621de0b3b92c49421f5ba59ab Mon Sep 17 00:00:00 2001 From: = <=> Date: Tue, 15 Oct 2024 17:49:19 +0100 Subject: [PATCH 3/7] end of monday, control.py written but not implemented --- notebooks/demo.ipynb | 1 + uuv_mission/control.py | 12 ++++++++++++ uuv_mission/dynamic.py | 14 +++++++++++--- 3 files changed, 24 insertions(+), 3 deletions(-) create mode 100644 uuv_mission/control.py diff --git a/notebooks/demo.ipynb b/notebooks/demo.ipynb index aa0c5cee..e881ba74 100644 --- a/notebooks/demo.ipynb +++ b/notebooks/demo.ipynb @@ -36,6 +36,7 @@ "source": [ "# Import relevant modules\n", "\n", + "\n", "sub = Submarine()\n", "# Instantiate your controller (depending on your implementation)\n", "closed_loop = ClosedLoop(sub, controller)\n", diff --git a/uuv_mission/control.py b/uuv_mission/control.py new file mode 100644 index 00000000..9413e06b --- /dev/null +++ b/uuv_mission/control.py @@ -0,0 +1,12 @@ +import numpy as np +import matplotlib.pyplot as plt +import pandas as pd + +def PD_controller(reference_t1,depth_t1,e_t0,Kp,Kd): + # Calculates the control action at time t1 based on the current state and past state at time t0 + + e_t1 = reference_t1-depth_t1 + u_t = Kp * e_t1 + Kd * (e_t1 - e_t0) + + + return e_t1, u_t \ No newline at end of file diff --git a/uuv_mission/dynamic.py b/uuv_mission/dynamic.py index 16f8db13..06d09ff4 100644 --- a/uuv_mission/dynamic.py +++ b/uuv_mission/dynamic.py @@ -2,8 +2,9 @@ from dataclasses import dataclass import numpy as np import matplotlib.pyplot as plt -#from .terrain import generate_reference_and_limits +from .terrain import generate_reference_and_limits import csv +from .control import PD_controller class Submarine: def __init__(self): @@ -116,10 +117,16 @@ def simulate(self, mission: Mission, disturbances: np.ndarray) -> Trajectory: actions = np.zeros(T) self.plant.reset_state() + #initialising the initial error + e_0 = mission.reference[0]-self.plant.get_depth() + for t in range(T): positions[t] = self.plant.get_position() observation_t = self.plant.get_depth() - # Call your controller here + # Call your controller here, change x to be more helpful once type is determied + [e_0, u_t] = PD_controller(mission.reference[t],observation_t, e_0, 0.15, 0.6) + + self.plant.transition(actions[t], disturbances[t]) return Trajectory(positions) @@ -128,8 +135,9 @@ def simulate_with_random_disturbances(self, mission: Mission, variance: float = disturbances = np.random.normal(0, variance, len(mission.reference)) return self.simulate(mission, disturbances) -#Initialise the class mission with empty arrays +#Creating an instance of the class Mission with empty arrays Mission([],[],[]) Mission.from_csv("mission") #Now Mission.reference has a list of all the refernce values, Mission.cave_depth has all of the depths and Mission.cave_height has all the heights in arrays + From 2ee2af5a2076bf4228ca75ba56511f020c8c70c1 Mon Sep 17 00:00:00 2001 From: = <=> Date: Wed, 16 Oct 2024 11:50:13 +0100 Subject: [PATCH 4/7] Working controller commit --- uuv_mission/control.py | 4 ++-- uuv_mission/dynamic.py | 42 +++++++++++++++++++++++++++++++----------- 2 files changed, 33 insertions(+), 13 deletions(-) diff --git a/uuv_mission/control.py b/uuv_mission/control.py index 9413e06b..19b3ec57 100644 --- a/uuv_mission/control.py +++ b/uuv_mission/control.py @@ -5,8 +5,8 @@ def PD_controller(reference_t1,depth_t1,e_t0,Kp,Kd): # Calculates the control action at time t1 based on the current state and past state at time t0 - e_t1 = reference_t1-depth_t1 - u_t = Kp * e_t1 + Kd * (e_t1 - e_t0) + e_t1 = float(reference_t1)-float(depth_t1) + u_t = float(Kp) * float(e_t1) + float(Kd) * (float(e_t1) - float(e_t0)) return e_t1, u_t \ No newline at end of file diff --git a/uuv_mission/dynamic.py b/uuv_mission/dynamic.py index 06d09ff4..8e1ccd6c 100644 --- a/uuv_mission/dynamic.py +++ b/uuv_mission/dynamic.py @@ -2,9 +2,9 @@ from dataclasses import dataclass import numpy as np import matplotlib.pyplot as plt -from .terrain import generate_reference_and_limits +from terrain import generate_reference_and_limits import csv -from .control import PD_controller +from control import PD_controller class Submarine: def __init__(self): @@ -88,14 +88,21 @@ def from_csv(cls, file_name: str): # You are required to implement this method with open("data/%s.csv" % (file_name)) as file: for row in file: - Mission.reference.append(row.split(',')[0]) - Mission.cave_height.append(row.split(',')[1]) + ref = row.split(',')[0] + if ref != 'reference': + Mission.reference.append(float(ref)) + + height = row.split(',')[1] + if height != 'cave_height': + Mission.cave_height.append(float(height)) #Each row in the csv actually ends with /n, which we need to remove full_value = row.split(',')[2] shortened_value = full_value[:-1] - Mission.cave_depth.append(shortened_value) - + if shortened_value != 'cave_depth': + Mission.cave_depth.append(float(shortened_value)) + + return Mission @@ -103,9 +110,10 @@ def from_csv(cls, file_name: str): class ClosedLoop: - def __init__(self, plant: Submarine, controller): + #def __init__(self, plant: Submarine, controller): + def __init__(self, plant: Submarine): self.plant = plant - self.controller = controller + #self.controller = controller def simulate(self, mission: Mission, disturbances: np.ndarray) -> Trajectory: @@ -118,14 +126,16 @@ def simulate(self, mission: Mission, disturbances: np.ndarray) -> Trajectory: self.plant.reset_state() #initialising the initial error - e_0 = mission.reference[0]-self.plant.get_depth() + e_0 = float(mission.reference[0])-float(self.plant.get_depth()) for t in range(T): positions[t] = self.plant.get_position() observation_t = self.plant.get_depth() # Call your controller here, change x to be more helpful once type is determied - [e_0, u_t] = PD_controller(mission.reference[t],observation_t, e_0, 0.15, 0.6) - + Kp = 0.15 + Kd = 0.6 + [e_0, u_t] = PD_controller(mission.reference[t],observation_t, e_0, Kp, Kd) + actions[t] = u_t self.plant.transition(actions[t], disturbances[t]) @@ -141,3 +151,13 @@ def simulate_with_random_disturbances(self, mission: Mission, variance: float = Mission.from_csv("mission") #Now Mission.reference has a list of all the refernce values, Mission.cave_depth has all of the depths and Mission.cave_height has all the heights in arrays + +##Code from Jupyter +sub = Submarine() +# Instantiate your controller (depending on your implementation) +##closed_loop = ClosedLoop(sub, controller) +closed_loop = ClosedLoop(sub) +##mission = Mission.from_csv("path/to/file") # You must implement this method in the Mission class #Already Done + +trajectory = closed_loop.simulate_with_random_disturbances(Mission) +trajectory.plot_completed_mission(Mission) \ No newline at end of file From 2e9b05ae77817c33c97c9e7bfa1829a5f0f58065 Mon Sep 17 00:00:00 2001 From: = <=> Date: Wed, 16 Oct 2024 12:27:59 +0100 Subject: [PATCH 5/7] Edited Kp and Kd values and refined comments --- uuv_mission/dynamic.py | 19 ++++--------------- 1 file changed, 4 insertions(+), 15 deletions(-) diff --git a/uuv_mission/dynamic.py b/uuv_mission/dynamic.py index 8e1ccd6c..c29e43ae 100644 --- a/uuv_mission/dynamic.py +++ b/uuv_mission/dynamic.py @@ -85,7 +85,6 @@ def random_mission(cls, duration: int, scale: float): @classmethod def from_csv(cls, file_name: str): - # You are required to implement this method with open("data/%s.csv" % (file_name)) as file: for row in file: ref = row.split(',')[0] @@ -106,8 +105,6 @@ def from_csv(cls, file_name: str): return Mission - pass - class ClosedLoop: #def __init__(self, plant: Submarine, controller): @@ -131,9 +128,8 @@ def simulate(self, mission: Mission, disturbances: np.ndarray) -> Trajectory: for t in range(T): positions[t] = self.plant.get_position() observation_t = self.plant.get_depth() - # Call your controller here, change x to be more helpful once type is determied - Kp = 0.15 - Kd = 0.6 + Kp = 0.125 + Kd = 0.65 [e_0, u_t] = PD_controller(mission.reference[t],observation_t, e_0, Kp, Kd) actions[t] = u_t @@ -145,19 +141,12 @@ def simulate_with_random_disturbances(self, mission: Mission, variance: float = disturbances = np.random.normal(0, variance, len(mission.reference)) return self.simulate(mission, disturbances) -#Creating an instance of the class Mission with empty arrays +#Initialse and create an instance of the Mission class Mission([],[],[]) - Mission.from_csv("mission") -#Now Mission.reference has a list of all the refernce values, Mission.cave_depth has all of the depths and Mission.cave_height has all the heights in arrays - -##Code from Jupyter +#Initialising the submarine and modeling it's course sub = Submarine() -# Instantiate your controller (depending on your implementation) -##closed_loop = ClosedLoop(sub, controller) closed_loop = ClosedLoop(sub) -##mission = Mission.from_csv("path/to/file") # You must implement this method in the Mission class #Already Done - trajectory = closed_loop.simulate_with_random_disturbances(Mission) trajectory.plot_completed_mission(Mission) \ No newline at end of file From a1b6ab79d42c64a3c1bb6d8e6340d3c0e660cc12 Mon Sep 17 00:00:00 2001 From: = <=> Date: Wed, 16 Oct 2024 16:02:45 +0100 Subject: [PATCH 6/7] removing unneccesary imports --- uuv_mission/control.py | 3 --- 1 file changed, 3 deletions(-) diff --git a/uuv_mission/control.py b/uuv_mission/control.py index 19b3ec57..1bf6ca35 100644 --- a/uuv_mission/control.py +++ b/uuv_mission/control.py @@ -1,6 +1,3 @@ -import numpy as np -import matplotlib.pyplot as plt -import pandas as pd def PD_controller(reference_t1,depth_t1,e_t0,Kp,Kd): # Calculates the control action at time t1 based on the current state and past state at time t0 From ee342b54548c0655aae3680a8f970b668975b83d Mon Sep 17 00:00:00 2001 From: = <=> Date: Thu, 17 Oct 2024 15:42:59 +0100 Subject: [PATCH 7/7] Final Kp and Kd values --- uuv_mission/dynamic.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/uuv_mission/dynamic.py b/uuv_mission/dynamic.py index c29e43ae..656b568e 100644 --- a/uuv_mission/dynamic.py +++ b/uuv_mission/dynamic.py @@ -128,7 +128,7 @@ def simulate(self, mission: Mission, disturbances: np.ndarray) -> Trajectory: for t in range(T): positions[t] = self.plant.get_position() observation_t = self.plant.get_depth() - Kp = 0.125 + Kp = 0.1 Kd = 0.65 [e_0, u_t] = PD_controller(mission.reference[t],observation_t, e_0, Kp, Kd) actions[t] = u_t