From 413255ea327586266edd7b3c5f5a7ef9504c4c14 Mon Sep 17 00:00:00 2001
From: akash-venkateshwaran
 <144670812+akash-venkateshwaran@users.noreply.github.com>
Date: Thu, 7 Dec 2023 00:36:54 -0800
Subject: [PATCH 01/25] Added normalising funcs to Distance Obj

---
 local_pathfinding/objectives.py | 36 ++++++++++++++++++++++++++++++++-
 1 file changed, 35 insertions(+), 1 deletion(-)

diff --git a/local_pathfinding/objectives.py b/local_pathfinding/objectives.py
index a53d8a4..9620048 100644
--- a/local_pathfinding/objectives.py
+++ b/local_pathfinding/objectives.py
@@ -3,6 +3,7 @@
 import math
 from enum import Enum, auto
 
+import numpy as np
 from ompl import base as ob
 
 import local_pathfinding.coord_systems as cs
@@ -64,7 +65,13 @@ class DistanceObjective(Objective):
             Only defined if the method is latlon.
     """
 
-    def __init__(self, space_information, method: DistanceMethod, reference=cs.LatLon(0, 0)):
+    def __init__(
+        self,
+        space_information,
+        method: DistanceMethod,
+        reference=cs.LatLon(0, 0),
+        num_samples: int = 10,
+    ):
         super().__init__(space_information)
         self.method = method
         if self.method == DistanceMethod.OMPL_PATH_LENGTH:
@@ -72,6 +79,10 @@ def __init__(self, space_information, method: DistanceMethod, reference=cs.LatLo
         elif self.method == DistanceMethod.LATLON:
             self.reference = reference
 
+        self.sampled_states = self._sample_states(si=space_information, num_samples=num_samples)
+        self.max_motionCost = 1
+        self.max_motionCost = self._find_maximum_motion_cost()
+
     def motionCost(self, s1: ob.SE2StateSpace, s2: ob.SE2StateSpace) -> ob.Cost:
         """Generates the distance between two points
 
@@ -137,6 +148,29 @@ def get_latlon_path_length_objective(s1: cs.XY, s2: cs.XY, reference: cs.LatLon)
 
         return distance_m
 
+    def _find_maximum_motion_cost(self):
+        max_cost = 0
+        n = len(self.sampled_states)
+        for i in range(n):
+            for j in range(i + 1, n):
+                cost = self.motionCost(self.sampled_states[i], self.sampled_states[j])
+                max_cost = max(max_cost, cost)
+
+        return max_cost
+
+    def _sample_states(self, si: ob.SpaceInformation, num_samples: int):
+        sampler = si.getStateSpace().allocDefaultStateSampler()
+
+        sampled_states = []
+
+        for _ in range(num_samples):
+            state = si.getStateSpace().allocState()
+            sampler.sampleUniform(state)
+            state_array = np.array([state[0], state[1]])
+            sampled_states.append(state_array)
+
+        return sampled_states
+
 
 class MinimumTurningObjective(Objective):
     """Generates a minimum turning objective function

From b187310bc96bff86ef6d53c72a6158fe03a457ce Mon Sep 17 00:00:00 2001
From: akash-venkateshwaran
 <144670812+akash-venkateshwaran@users.noreply.github.com>
Date: Thu, 7 Dec 2023 18:15:13 -0800
Subject: [PATCH 02/25] Corrected the code for SE2StateSpace()

---
 local_pathfinding/objectives.py | 15 +++++++--------
 1 file changed, 7 insertions(+), 8 deletions(-)

diff --git a/local_pathfinding/objectives.py b/local_pathfinding/objectives.py
index 9620048..9ec50ef 100644
--- a/local_pathfinding/objectives.py
+++ b/local_pathfinding/objectives.py
@@ -3,7 +3,6 @@
 import math
 from enum import Enum, auto
 
-import numpy as np
 from ompl import base as ob
 
 import local_pathfinding.coord_systems as cs
@@ -107,7 +106,7 @@ def motionCost(self, s1: ob.SE2StateSpace, s2: ob.SE2StateSpace) -> ob.Cost:
             )
             cost = ob.Cost(distance)
         elif self.method == DistanceMethod.OMPL_PATH_LENGTH:
-            cost = self.ompl_path_objective.motionCost(s1_xy, s2_xy)
+            cost = self.ompl_path_objective.motionCost(s1, s2)
         else:
             ValueError(f"Method {self.method} not supported")
         return cost
@@ -151,10 +150,11 @@ def get_latlon_path_length_objective(s1: cs.XY, s2: cs.XY, reference: cs.LatLon)
     def _find_maximum_motion_cost(self):
         max_cost = 0
         n = len(self.sampled_states)
+
         for i in range(n):
             for j in range(i + 1, n):
                 cost = self.motionCost(self.sampled_states[i], self.sampled_states[j])
-                max_cost = max(max_cost, cost)
+                max_cost = max(max_cost, cost.value())
 
         return max_cost
 
@@ -166,8 +166,7 @@ def _sample_states(self, si: ob.SpaceInformation, num_samples: int):
         for _ in range(num_samples):
             state = si.getStateSpace().allocState()
             sampler.sampleUniform(state)
-            state_array = np.array([state[0], state[1]])
-            sampled_states.append(state_array)
+            sampled_states.append(state)
 
         return sampled_states
 
@@ -418,10 +417,10 @@ def get_sailing_objective(
     space_information, simple_setup, heading_degrees: float, wind_direction_degrees: float
 ) -> ob.OptimizationObjective:
     objective = ob.MultiOptimizationObjective(si=space_information)
-    objective.addObjective(
-        objective=DistanceObjective(space_information, DistanceMethod.LATLON),
-        weight=1.0,
+    objective_1 = DistanceObjective(
+        space_information=space_information, method=DistanceMethod.LATLON, num_samples=20
     )
+    objective.addObjective(objective=objective_1, weight=1.0)
     objective.addObjective(
         objective=MinimumTurningObjective(
             space_information, simple_setup, heading_degrees, MinimumTurningMethod.GOAL_HEADING

From 29fbaef46458fe4fbe90a85af584936d1dd5deda Mon Sep 17 00:00:00 2001
From: akash-venkateshwaran
 <144670812+akash-venkateshwaran@users.noreply.github.com>
Date: Fri, 8 Dec 2023 10:08:14 -0800
Subject: [PATCH 03/25] Shifted the def of unc to parent class and inherited in
 all the objectives

---
 local_pathfinding/objectives.py | 114 +++++++++++++++++++-------------
 1 file changed, 69 insertions(+), 45 deletions(-)

diff --git a/local_pathfinding/objectives.py b/local_pathfinding/objectives.py
index 9ec50ef..0f423dc 100644
--- a/local_pathfinding/objectives.py
+++ b/local_pathfinding/objectives.py
@@ -52,6 +52,29 @@ def __init__(self, space_information):
     def motionCost(self, s1: ob.SE2StateSpace, s2: ob.SE2StateSpace) -> ob.Cost:
         raise NotImplementedError
 
+    def _find_maximum_motion_cost(self):
+        max_cost = 0
+        n = len(self.sampled_states)
+
+        for i in range(n):
+            for j in range(i + 1, n):
+                cost = self.motionCost(self.sampled_states[i], self.sampled_states[j])
+                max_cost = max(max_cost, cost.value())
+
+        return max_cost
+
+    def _sample_states(self, si: ob.SpaceInformation, num_samples: int):
+        sampler = si.getStateSpace().allocDefaultStateSampler()
+
+        sampled_states = []
+
+        for _ in range(num_samples):
+            state = si.getStateSpace().allocState()
+            sampler.sampleUniform(state)
+            sampled_states.append(state)
+
+        return sampled_states
+
 
 class DistanceObjective(Objective):
     """Generates a distance objective function
@@ -69,7 +92,7 @@ def __init__(
         space_information,
         method: DistanceMethod,
         reference=cs.LatLon(0, 0),
-        num_samples: int = 10,
+        num_samples: int = 100,
     ):
         super().__init__(space_information)
         self.method = method
@@ -79,7 +102,7 @@ def __init__(
             self.reference = reference
 
         self.sampled_states = self._sample_states(si=space_information, num_samples=num_samples)
-        self.max_motionCost = 1
+        self.max_motionCost = 1.0
         self.max_motionCost = self._find_maximum_motion_cost()
 
     def motionCost(self, s1: ob.SE2StateSpace, s2: ob.SE2StateSpace) -> ob.Cost:
@@ -99,17 +122,20 @@ def motionCost(self, s1: ob.SE2StateSpace, s2: ob.SE2StateSpace) -> ob.Cost:
         s2_xy = cs.XY(s2.getX(), s2.getY())
         if self.method == DistanceMethod.EUCLIDEAN:
             distance = DistanceObjective.get_euclidean_path_length_objective(s1_xy, s2_xy)
-            cost = ob.Cost(distance)
+            cost_value = distance / self.max_motionCost
         elif self.method == DistanceMethod.LATLON:
             distance = DistanceObjective.get_latlon_path_length_objective(
                 s1_xy, s2_xy, self.reference
             )
-            cost = ob.Cost(distance)
+            cost_value = distance / self.max_motionCost
         elif self.method == DistanceMethod.OMPL_PATH_LENGTH:
             cost = self.ompl_path_objective.motionCost(s1, s2)
+            cost_value = cost.value() / self.max_motionCost
         else:
-            ValueError(f"Method {self.method} not supported")
-        return cost
+            raise ValueError(f"Method {self.method} not supported")
+
+        normalized_cost = ob.Cost(cost_value)
+        return normalized_cost
 
     @staticmethod
     def get_euclidean_path_length_objective(s1: cs.XY, s2: cs.XY) -> float:
@@ -147,29 +173,6 @@ def get_latlon_path_length_objective(s1: cs.XY, s2: cs.XY, reference: cs.LatLon)
 
         return distance_m
 
-    def _find_maximum_motion_cost(self):
-        max_cost = 0
-        n = len(self.sampled_states)
-
-        for i in range(n):
-            for j in range(i + 1, n):
-                cost = self.motionCost(self.sampled_states[i], self.sampled_states[j])
-                max_cost = max(max_cost, cost.value())
-
-        return max_cost
-
-    def _sample_states(self, si: ob.SpaceInformation, num_samples: int):
-        sampler = si.getStateSpace().allocDefaultStateSampler()
-
-        sampled_states = []
-
-        for _ in range(num_samples):
-            state = si.getStateSpace().allocState()
-            sampler.sampleUniform(state)
-            sampled_states.append(state)
-
-        return sampled_states
-
 
 class MinimumTurningObjective(Objective):
     """Generates a minimum turning objective function
@@ -186,6 +189,7 @@ def __init__(
         simple_setup,
         heading_degrees: float,
         method: MinimumTurningMethod,
+        num_samples: int = 100,
     ):
         super().__init__(space_information)
         self.goal = cs.XY(
@@ -195,6 +199,10 @@ def __init__(
         self.heading = math.radians(heading_degrees)
         self.method = method
 
+        self.sampled_states = self._sample_states(si=space_information, num_samples=num_samples)
+        self.max_motionCost = 1.0
+        self.max_motionCost = self._find_maximum_motion_cost()
+
     def motionCost(self, s1: ob.SE2StateSpace, s2: ob.SE2StateSpace) -> ob.Cost:
         """Generates the turning cost between s1, s2, heading or the goal position
 
@@ -217,8 +225,11 @@ def motionCost(self, s1: ob.SE2StateSpace, s2: ob.SE2StateSpace) -> ob.Cost:
         elif self.method == MinimumTurningMethod.HEADING_PATH:
             angle = self.heading_path_turn_cost(s1_xy, s2_xy, self.heading)
         else:
-            ValueError(f"Method {self.method} not supported")
-        return ob.Cost(angle)
+            raise ValueError(f"Method {self.method} not supported")
+
+        normalized_angle = angle / self.max_motionCost
+        normalized_cost = ob.Cost(normalized_angle)
+        return normalized_cost
 
     @staticmethod
     def goal_heading_turn_cost(s1: cs.XY, goal: cs.XY, heading: float) -> float:
@@ -305,11 +316,20 @@ class WindObjective(Objective):
         wind_direction (float): The direction of the wind in radians (-pi, pi]
     """
 
-    def __init__(self, space_information, wind_direction_degrees: float):
+    def __init__(
+        self,
+        space_information,
+        wind_direction_degrees: float,
+        num_samples: int = 100,
+    ):
         super().__init__(space_information)
         assert -180 < wind_direction_degrees <= 180
         self.wind_direction = math.radians(wind_direction_degrees)
 
+        self.sampled_states = self._sample_states(si=space_information, num_samples=num_samples)
+        self.max_motionCost = 1.0
+        self.max_motionCost = self._find_maximum_motion_cost()
+
     def motionCost(self, s1: ob.SE2StateSpace, s2: ob.SE2StateSpace) -> ob.Cost:
         """Generates the cost associated with the upwind and downwind directions of the boat in
         relation to the wind.
@@ -319,11 +339,15 @@ def motionCost(self, s1: ob.SE2StateSpace, s2: ob.SE2StateSpace) -> ob.Cost:
             s2 (SE2StateInternal): The ending point of the local goal state
 
         Returns:
-            ob.Cost: The cost of going upwind or downwind
+            ob.Cost: The cost of going upwind or downwind normalized by max_motionCost
         """
         s1_xy = cs.XY(s1.getX(), s1.getY())
         s2_xy = cs.XY(s2.getX(), s2.getY())
-        return ob.Cost(WindObjective.wind_direction_cost(s1_xy, s2_xy, self.wind_direction))
+
+        wind_cost = WindObjective.wind_direction_cost(s1_xy, s2_xy, self.wind_direction)
+        normalized_wind_cost = wind_cost / self.max_motionCost
+        normalized_cost = ob.Cost(normalized_wind_cost)
+        return normalized_cost
 
     @staticmethod
     def wind_direction_cost(s1: cs.XY, s2: cs.XY, wind_direction: float) -> float:
@@ -418,17 +442,17 @@ def get_sailing_objective(
 ) -> ob.OptimizationObjective:
     objective = ob.MultiOptimizationObjective(si=space_information)
     objective_1 = DistanceObjective(
-        space_information=space_information, method=DistanceMethod.LATLON, num_samples=20
-    )
-    objective.addObjective(objective=objective_1, weight=1.0)
-    objective.addObjective(
-        objective=MinimumTurningObjective(
-            space_information, simple_setup, heading_degrees, MinimumTurningMethod.GOAL_HEADING
-        ),
-        weight=100.0,
+        space_information=space_information, method=DistanceMethod.LATLON, num_samples=100
     )
-    objective.addObjective(
-        objective=WindObjective(space_information, wind_direction_degrees), weight=1.0
+    objective_2 = MinimumTurningObjective(
+        space_information,
+        simple_setup,
+        heading_degrees,
+        MinimumTurningMethod.GOAL_HEADING,
+        num_samples=100,
     )
-
+    objective_3 = WindObjective(space_information, wind_direction_degrees, num_samples=100)
+    objective.addObjective(objective=objective_1, weight=0.33)
+    objective.addObjective(objective=objective_2, weight=0.33)
+    objective.addObjective(objective=objective_3, weight=0.34)
     return objective

From a61768ea810847ee3d670f6460e60b5ded1b9826 Mon Sep 17 00:00:00 2001
From: Patrick Creighton <pcreighton429@gmail.com>
Date: Fri, 8 Dec 2023 11:15:26 -0800
Subject: [PATCH 04/25] Add missing type hint

---
 local_pathfinding/objectives.py | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/local_pathfinding/objectives.py b/local_pathfinding/objectives.py
index 0f423dc..8e64037 100644
--- a/local_pathfinding/objectives.py
+++ b/local_pathfinding/objectives.py
@@ -91,7 +91,7 @@ def __init__(
         self,
         space_information,
         method: DistanceMethod,
-        reference=cs.LatLon(0, 0),
+        reference: cs.LatLon = cs.LatLon(0, 0),
         num_samples: int = 100,
     ):
         super().__init__(space_information)

From 4d1ef2874b29c96d2022a7eb7652f2fdeffad5df Mon Sep 17 00:00:00 2001
From: Patrick Creighton <pcreighton429@gmail.com>
Date: Fri, 8 Dec 2023 11:20:41 -0800
Subject: [PATCH 05/25] Remove _ prefix

---
 local_pathfinding/objectives.py | 16 ++++++++--------
 1 file changed, 8 insertions(+), 8 deletions(-)

diff --git a/local_pathfinding/objectives.py b/local_pathfinding/objectives.py
index 8e64037..9c281ca 100644
--- a/local_pathfinding/objectives.py
+++ b/local_pathfinding/objectives.py
@@ -52,7 +52,7 @@ def __init__(self, space_information):
     def motionCost(self, s1: ob.SE2StateSpace, s2: ob.SE2StateSpace) -> ob.Cost:
         raise NotImplementedError
 
-    def _find_maximum_motion_cost(self):
+    def find_maximum_motion_cost(self):
         max_cost = 0
         n = len(self.sampled_states)
 
@@ -63,7 +63,7 @@ def _find_maximum_motion_cost(self):
 
         return max_cost
 
-    def _sample_states(self, si: ob.SpaceInformation, num_samples: int):
+    def sample_states(self, si: ob.SpaceInformation, num_samples: int):
         sampler = si.getStateSpace().allocDefaultStateSampler()
 
         sampled_states = []
@@ -101,9 +101,9 @@ def __init__(
         elif self.method == DistanceMethod.LATLON:
             self.reference = reference
 
-        self.sampled_states = self._sample_states(si=space_information, num_samples=num_samples)
+        self.sampled_states = self.sample_states(si=space_information, num_samples=num_samples)
         self.max_motionCost = 1.0
-        self.max_motionCost = self._find_maximum_motion_cost()
+        self.max_motionCost = self.find_maximum_motion_cost()
 
     def motionCost(self, s1: ob.SE2StateSpace, s2: ob.SE2StateSpace) -> ob.Cost:
         """Generates the distance between two points
@@ -199,9 +199,9 @@ def __init__(
         self.heading = math.radians(heading_degrees)
         self.method = method
 
-        self.sampled_states = self._sample_states(si=space_information, num_samples=num_samples)
+        self.sampled_states = self.sample_states(si=space_information, num_samples=num_samples)
         self.max_motionCost = 1.0
-        self.max_motionCost = self._find_maximum_motion_cost()
+        self.max_motionCost = self.find_maximum_motion_cost()
 
     def motionCost(self, s1: ob.SE2StateSpace, s2: ob.SE2StateSpace) -> ob.Cost:
         """Generates the turning cost between s1, s2, heading or the goal position
@@ -326,9 +326,9 @@ def __init__(
         assert -180 < wind_direction_degrees <= 180
         self.wind_direction = math.radians(wind_direction_degrees)
 
-        self.sampled_states = self._sample_states(si=space_information, num_samples=num_samples)
+        self.sampled_states = self.sample_states(si=space_information, num_samples=num_samples)
         self.max_motionCost = 1.0
-        self.max_motionCost = self._find_maximum_motion_cost()
+        self.max_motionCost = self.find_maximum_motion_cost()
 
     def motionCost(self, s1: ob.SE2StateSpace, s2: ob.SE2StateSpace) -> ob.Cost:
         """Generates the cost associated with the upwind and downwind directions of the boat in

From 243d29a4acb6521f9e68f0d811fdc3fcd36c4c68 Mon Sep 17 00:00:00 2001
From: Patrick Creighton <pcreighton429@gmail.com>
Date: Fri, 8 Dec 2023 11:34:57 -0800
Subject: [PATCH 06/25] Refactor normalization

---
 local_pathfinding/objectives.py | 15 +++++----------
 1 file changed, 5 insertions(+), 10 deletions(-)

diff --git a/local_pathfinding/objectives.py b/local_pathfinding/objectives.py
index 9c281ca..1b4eaff 100644
--- a/local_pathfinding/objectives.py
+++ b/local_pathfinding/objectives.py
@@ -122,20 +122,17 @@ def motionCost(self, s1: ob.SE2StateSpace, s2: ob.SE2StateSpace) -> ob.Cost:
         s2_xy = cs.XY(s2.getX(), s2.getY())
         if self.method == DistanceMethod.EUCLIDEAN:
             distance = DistanceObjective.get_euclidean_path_length_objective(s1_xy, s2_xy)
-            cost_value = distance / self.max_motionCost
         elif self.method == DistanceMethod.LATLON:
             distance = DistanceObjective.get_latlon_path_length_objective(
                 s1_xy, s2_xy, self.reference
             )
-            cost_value = distance / self.max_motionCost
         elif self.method == DistanceMethod.OMPL_PATH_LENGTH:
-            cost = self.ompl_path_objective.motionCost(s1, s2)
-            cost_value = cost.value() / self.max_motionCost
+            distance = self.ompl_path_objective.motionCost(s1, s2).value()
         else:
             raise ValueError(f"Method {self.method} not supported")
 
-        normalized_cost = ob.Cost(cost_value)
-        return normalized_cost
+        normalized_distance = distance / self.max_motionCost
+        return ob.Cost(normalized_distance)
 
     @staticmethod
     def get_euclidean_path_length_objective(s1: cs.XY, s2: cs.XY) -> float:
@@ -228,8 +225,7 @@ def motionCost(self, s1: ob.SE2StateSpace, s2: ob.SE2StateSpace) -> ob.Cost:
             raise ValueError(f"Method {self.method} not supported")
 
         normalized_angle = angle / self.max_motionCost
-        normalized_cost = ob.Cost(normalized_angle)
-        return normalized_cost
+        return ob.Cost(normalized_angle)
 
     @staticmethod
     def goal_heading_turn_cost(s1: cs.XY, goal: cs.XY, heading: float) -> float:
@@ -346,8 +342,7 @@ def motionCost(self, s1: ob.SE2StateSpace, s2: ob.SE2StateSpace) -> ob.Cost:
 
         wind_cost = WindObjective.wind_direction_cost(s1_xy, s2_xy, self.wind_direction)
         normalized_wind_cost = wind_cost / self.max_motionCost
-        normalized_cost = ob.Cost(normalized_wind_cost)
-        return normalized_cost
+        return ob.Cost(normalized_wind_cost)
 
     @staticmethod
     def wind_direction_cost(s1: cs.XY, s2: cs.XY, wind_direction: float) -> float:

From 357f9422f7e45b42711c2dc03f8dccf708993958 Mon Sep 17 00:00:00 2001
From: Patrick Creighton <pcreighton429@gmail.com>
Date: Fri, 8 Dec 2023 11:48:52 -0800
Subject: [PATCH 07/25] sample and find max cost in base objective

---
 local_pathfinding/objectives.py | 23 +++++++++--------------
 1 file changed, 9 insertions(+), 14 deletions(-)

diff --git a/local_pathfinding/objectives.py b/local_pathfinding/objectives.py
index 1b4eaff..f2598aa 100644
--- a/local_pathfinding/objectives.py
+++ b/local_pathfinding/objectives.py
@@ -45,10 +45,14 @@ class Objective(ob.StateCostIntegralObjective):
             the space planning is done in.
     """
 
-    def __init__(self, space_information):
+    def __init__(self, space_information, num_samples: int):
         super().__init__(si=space_information, enableMotionCostInterpolation=True)
         self.space_information = space_information
 
+        self.sampled_states = self.sample_states(si=space_information, num_samples=num_samples)
+        self.max_motionCost = 1.0
+        self.max_motionCost = self.find_maximum_motion_cost()
+
     def motionCost(self, s1: ob.SE2StateSpace, s2: ob.SE2StateSpace) -> ob.Cost:
         raise NotImplementedError
 
@@ -94,16 +98,13 @@ def __init__(
         reference: cs.LatLon = cs.LatLon(0, 0),
         num_samples: int = 100,
     ):
-        super().__init__(space_information)
         self.method = method
         if self.method == DistanceMethod.OMPL_PATH_LENGTH:
-            self.ompl_path_objective = ob.PathLengthOptimizationObjective(self.space_information)
+            self.ompl_path_objective = ob.PathLengthOptimizationObjective(space_information)
         elif self.method == DistanceMethod.LATLON:
             self.reference = reference
 
-        self.sampled_states = self.sample_states(si=space_information, num_samples=num_samples)
-        self.max_motionCost = 1.0
-        self.max_motionCost = self.find_maximum_motion_cost()
+        super().__init__(space_information, num_samples)
 
     def motionCost(self, s1: ob.SE2StateSpace, s2: ob.SE2StateSpace) -> ob.Cost:
         """Generates the distance between two points
@@ -188,7 +189,6 @@ def __init__(
         method: MinimumTurningMethod,
         num_samples: int = 100,
     ):
-        super().__init__(space_information)
         self.goal = cs.XY(
             simple_setup.getGoal().getState().getX(), simple_setup.getGoal().getState().getY()
         )
@@ -196,9 +196,7 @@ def __init__(
         self.heading = math.radians(heading_degrees)
         self.method = method
 
-        self.sampled_states = self.sample_states(si=space_information, num_samples=num_samples)
-        self.max_motionCost = 1.0
-        self.max_motionCost = self.find_maximum_motion_cost()
+        super().__init__(space_information, num_samples)
 
     def motionCost(self, s1: ob.SE2StateSpace, s2: ob.SE2StateSpace) -> ob.Cost:
         """Generates the turning cost between s1, s2, heading or the goal position
@@ -318,13 +316,10 @@ def __init__(
         wind_direction_degrees: float,
         num_samples: int = 100,
     ):
-        super().__init__(space_information)
         assert -180 < wind_direction_degrees <= 180
         self.wind_direction = math.radians(wind_direction_degrees)
 
-        self.sampled_states = self.sample_states(si=space_information, num_samples=num_samples)
-        self.max_motionCost = 1.0
-        self.max_motionCost = self.find_maximum_motion_cost()
+        super().__init__(space_information, num_samples)
 
     def motionCost(self, s1: ob.SE2StateSpace, s2: ob.SE2StateSpace) -> ob.Cost:
         """Generates the cost associated with the upwind and downwind directions of the boat in

From 40617913cebdadd14f7e54dd08de253abcc69edd Mon Sep 17 00:00:00 2001
From: Patrick Creighton <pcreighton429@gmail.com>
Date: Fri, 8 Dec 2023 13:22:23 -0800
Subject: [PATCH 08/25] Code style and docstrings

---
 local_pathfinding/objectives.py | 47 +++++++++++++++++++++------------
 1 file changed, 30 insertions(+), 17 deletions(-)

diff --git a/local_pathfinding/objectives.py b/local_pathfinding/objectives.py
index f2598aa..c0db2c6 100644
--- a/local_pathfinding/objectives.py
+++ b/local_pathfinding/objectives.py
@@ -1,5 +1,6 @@
 """Our custom OMPL optimization objectives."""
 
+import itertools
 import math
 from enum import Enum, auto
 
@@ -43,37 +44,49 @@ class Objective(ob.StateCostIntegralObjective):
     Attributes:
         space_information (StateSpacePtr): Contains all the information about
             the space planning is done in.
+        max_motion_cost (float): The maximum motion cost between any two states in the state space.
     """
 
     def __init__(self, space_information, num_samples: int):
         super().__init__(si=space_information, enableMotionCostInterpolation=True)
         self.space_information = space_information
 
-        self.sampled_states = self.sample_states(si=space_information, num_samples=num_samples)
-        self.max_motionCost = 1.0
-        self.max_motionCost = self.find_maximum_motion_cost()
+        states = self.sample_states(num_samples)
+        self.max_motion_cost = 1.0
+        self.max_motion_cost = self.find_maximum_motion_cost(states)
 
     def motionCost(self, s1: ob.SE2StateSpace, s2: ob.SE2StateSpace) -> ob.Cost:
         raise NotImplementedError
 
-    def find_maximum_motion_cost(self):
-        max_cost = 0
-        n = len(self.sampled_states)
+    def find_maximum_motion_cost(self, states: list[ob.SE2StateSpace]) -> float:
+        """Finds the maximum motion cost between any two states in `states`.
 
-        for i in range(n):
-            for j in range(i + 1, n):
-                cost = self.motionCost(self.sampled_states[i], self.sampled_states[j])
-                max_cost = max(max_cost, cost.value())
+        Args:
+            states (list[ob.SE2StateSpace]): OMPL states.
+
+        Returns:
+            float: Maximum motion cost.
+        """
+        return max(
+            self.motionCost(s1, s2).value()
+            for s1, s2 in itertools.combinations(iterable=states, r=2)
+        )
 
-        return max_cost
+    def sample_states(self, num_samples: int) -> list[ob.SE2StateSpace]:
+        """Samples `num_samples` states from the state space.
 
-    def sample_states(self, si: ob.SpaceInformation, num_samples: int):
-        sampler = si.getStateSpace().allocDefaultStateSampler()
+        Args:
+            num_samples (int): Number of states to sample.
+
+        Returns:
+            list[ob.SE2StateSpace]: OMPL states.
+        """
+        sampler = self.space_information.getStateSpace().allocDefaultStateSampler()
 
         sampled_states = []
 
         for _ in range(num_samples):
-            state = si.getStateSpace().allocState()
+            state = self.space_information.getStateSpace().allocState()
             sampler.sampleUniform(state)
             sampled_states.append(state)
 
@@ -132,7 +145,7 @@ def motionCost(self, s1: ob.SE2StateSpace, s2: ob.SE2StateSpace) -> ob.Cost:
         else:
             raise ValueError(f"Method {self.method} not supported")
 
-        normalized_distance = distance / self.max_motionCost
+        normalized_distance = distance / self.max_motion_cost
         return ob.Cost(normalized_distance)
 
     @staticmethod
@@ -222,7 +235,7 @@ def motionCost(self, s1: ob.SE2StateSpace, s2: ob.SE2StateSpace) -> ob.Cost:
         else:
             raise ValueError(f"Method {self.method} not supported")
 
-        normalized_angle = angle / self.max_motionCost
+        normalized_angle = angle / self.max_motion_cost
         return ob.Cost(normalized_angle)
 
     @staticmethod
@@ -336,7 +349,7 @@ def motionCost(self, s1: ob.SE2StateSpace, s2: ob.SE2StateSpace) -> ob.Cost:
         s2_xy = cs.XY(s2.getX(), s2.getY())
 
         wind_cost = WindObjective.wind_direction_cost(s1_xy, s2_xy, self.wind_direction)
-        normalized_wind_cost = wind_cost / self.max_motionCost
+        normalized_wind_cost = wind_cost / self.max_motion_cost
         return ob.Cost(normalized_wind_cost)
 
     @staticmethod

From e2f0de72631b0ffa855fdfe34752b31e371150d5 Mon Sep 17 00:00:00 2001
From: Patrick Creighton <pcreighton429@gmail.com>
Date: Fri, 8 Dec 2023 13:22:56 -0800
Subject: [PATCH 09/25] Add tests

---
 test/test_objectives.py | 39 +++++++++++++++++++++++++++++++++------
 1 file changed, 33 insertions(+), 6 deletions(-)

diff --git a/test/test_objectives.py b/test/test_objectives.py
index b63c03a..e23912f 100644
--- a/test/test_objectives.py
+++ b/test/test_objectives.py
@@ -1,6 +1,9 @@
+# import itertools
 import math
 
 import pytest
+
+# from ompl import base as ob
 from rclpy.impl.rcutils_logger import RcutilsLogger
 
 import local_pathfinding.coord_systems as coord_systems
@@ -20,19 +23,43 @@
 
 
 @pytest.mark.parametrize(
-    "method",
+    "method,max_motion_cost",
     [
-        objectives.DistanceMethod.EUCLIDEAN,
-        objectives.DistanceMethod.LATLON,
-        objectives.DistanceMethod.OMPL_PATH_LENGTH,
+        (objectives.DistanceMethod.EUCLIDEAN, 2.5),
+        (objectives.DistanceMethod.LATLON, 2700),
+        (objectives.DistanceMethod.OMPL_PATH_LENGTH, 4.0),
     ],
 )
-def test_distance_objective(method: objectives.DistanceMethod):
+def test_distance_objective(method: objectives.DistanceMethod, max_motion_cost: float):
     distance_objective = objectives.DistanceObjective(
         PATH._simple_setup.getSpaceInformation(),
         method,
     )
-    assert distance_objective is not None
+
+    # test sample_states()
+    num_samples = 3
+    sampled_states = distance_objective.sample_states(num_samples)
+    assert len(sampled_states) == num_samples
+    # for state in sampled_states:
+    #     assert ompl_path.is_state_valid(state)
+
+    # test find_maximum_motion_cost()
+    # implicitly
+    assert distance_objective.max_motion_cost == pytest.approx(max_motion_cost, rel=1e0)
+    # explicitly for the easiest method to set up
+    # don't need to test for all methods since they each have their own tests
+    # if method == objectives.DistanceMethod.OMPL_PATH_LENGTH:
+    #     states = []
+    #     for xy in [(-0.5, 0.4), (0.1, 0.2), (0.3, -0.6)]:
+    #         state = ob.State(distance_objective.space_information)
+    #         state().setXY(*xy)
+    #         states.append(state)
+    #     assert type(states[0]) is type(sampled_states[0]), "states are not the correct type"
+    #     costs = [
+    #         distance_objective.ompl_path_objective.motionCost(s1(), s2()).value()
+    #         for s1, s2 in itertools.combinations(iterable=states, r=2)
+    #     ]
+    #     assert distance_objective.find_maximum_motion_cost(states) == pytest.approx(max(costs))
 
 
 @pytest.mark.parametrize(

From 6e1de986bb86345250c1478f055ca32a2c819823 Mon Sep 17 00:00:00 2001
From: Patrick Creighton <pcreighton429@gmail.com>
Date: Fri, 8 Dec 2023 13:56:48 -0800
Subject: [PATCH 10/25] Fix explicit test

---
 test/test_objectives.py | 32 +++++++++++++++++---------------
 1 file changed, 17 insertions(+), 15 deletions(-)

diff --git a/test/test_objectives.py b/test/test_objectives.py
index e23912f..6846156 100644
--- a/test/test_objectives.py
+++ b/test/test_objectives.py
@@ -1,9 +1,8 @@
-# import itertools
+import itertools
 import math
 
 import pytest
-
-# from ompl import base as ob
+from ompl import base as ob
 from rclpy.impl.rcutils_logger import RcutilsLogger
 
 import local_pathfinding.coord_systems as coord_systems
@@ -48,18 +47,21 @@ def test_distance_objective(method: objectives.DistanceMethod, max_motion_cost:
     assert distance_objective.max_motion_cost == pytest.approx(max_motion_cost, rel=1e0)
     # explicitly for the easiest method to set up
     # don't need to test for all methods since they each have their own tests
-    # if method == objectives.DistanceMethod.OMPL_PATH_LENGTH:
-    #     states = []
-    #     for xy in [(-0.5, 0.4), (0.1, 0.2), (0.3, -0.6)]:
-    #         state = ob.State(distance_objective.space_information)
-    #         state().setXY(*xy)
-    #         states.append(state)
-    #     assert type(states[0]) is type(sampled_states[0]), "states are not the correct type"
-    #     costs = [
-    #         distance_objective.ompl_path_objective.motionCost(s1(), s2()).value()
-    #         for s1, s2 in itertools.combinations(iterable=states, r=2)
-    #     ]
-    #     assert distance_objective.find_maximum_motion_cost(states) == pytest.approx(max(costs))
+    if method == objectives.DistanceMethod.OMPL_PATH_LENGTH:
+        states = []
+        space = PATH._simple_setup.getStateSpace()
+        for xy in [(-0.5, 0.4), (0.1, 0.2), (0.3, -0.6)]:
+            state = ob.State(space)
+            state().setXY(*xy)
+            states.append(state)
+        assert type(states[0]()) is type(sampled_states[0]), "states are not the correct type"
+        costs = [
+            distance_objective.ompl_path_objective.motionCost(s1(), s2()).value()
+            for s1, s2 in itertools.combinations(iterable=states, r=2)
+        ]
+        assert distance_objective.find_maximum_motion_cost([s() for s in states]) == pytest.approx(
+            max(costs) / distance_objective.max_motion_cost
+        )
 
 
 @pytest.mark.parametrize(

From 1db8278864799df25241af1af61012334c356fdd Mon Sep 17 00:00:00 2001
From: Patrick Creighton <pcreighton429@gmail.com>
Date: Fri, 8 Dec 2023 14:17:18 -0800
Subject: [PATCH 11/25] Cleanup

---
 local_pathfinding/objectives.py | 1 +
 test/test_objectives.py         | 7 +++----
 2 files changed, 4 insertions(+), 4 deletions(-)

diff --git a/local_pathfinding/objectives.py b/local_pathfinding/objectives.py
index c0db2c6..dc45535 100644
--- a/local_pathfinding/objectives.py
+++ b/local_pathfinding/objectives.py
@@ -40,6 +40,7 @@ class Objective(ob.StateCostIntegralObjective):
     - This class inherits from the OMPL class StateCostIntegralObjective:
         https://ompl.kavrakilab.org/classompl_1_1base_1_1StateCostIntegralObjective.html
     - Camelcase is used for functions that override OMPL functions, as that is their convention.
+    - Also computes the maximum motion cost for normalization purposes.
 
     Attributes:
         space_information (StateSpacePtr): Contains all the information about
diff --git a/test/test_objectives.py b/test/test_objectives.py
index 6846156..5b20199 100644
--- a/test/test_objectives.py
+++ b/test/test_objectives.py
@@ -54,13 +54,12 @@ def test_distance_objective(method: objectives.DistanceMethod, max_motion_cost:
             state = ob.State(space)
             state().setXY(*xy)
             states.append(state)
-        assert type(states[0]()) is type(sampled_states[0]), "states are not the correct type"
-        costs = [
+        max_cost = max(
             distance_objective.ompl_path_objective.motionCost(s1(), s2()).value()
             for s1, s2 in itertools.combinations(iterable=states, r=2)
-        ]
+        )
         assert distance_objective.find_maximum_motion_cost([s() for s in states]) == pytest.approx(
-            max(costs) / distance_objective.max_motion_cost
+            max_cost / distance_objective.max_motion_cost
         )
 
 

From f8e873cbda163a423e6b9c1acf0bb5813e7a2317 Mon Sep 17 00:00:00 2001
From: Patrick Creighton <pcreighton429@gmail.com>
Date: Fri, 8 Dec 2023 14:23:16 -0800
Subject: [PATCH 12/25] Formatting

---
 local_pathfinding/objectives.py | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/local_pathfinding/objectives.py b/local_pathfinding/objectives.py
index dc45535..0faaa5b 100644
--- a/local_pathfinding/objectives.py
+++ b/local_pathfinding/objectives.py
@@ -348,8 +348,8 @@ def motionCost(self, s1: ob.SE2StateSpace, s2: ob.SE2StateSpace) -> ob.Cost:
         """
         s1_xy = cs.XY(s1.getX(), s1.getY())
         s2_xy = cs.XY(s2.getX(), s2.getY())
-
         wind_cost = WindObjective.wind_direction_cost(s1_xy, s2_xy, self.wind_direction)
+
         normalized_wind_cost = wind_cost / self.max_motion_cost
         return ob.Cost(normalized_wind_cost)
 

From 7ab1f645495876d2e40e865d53acab1f9d5a44c2 Mon Sep 17 00:00:00 2001
From: Akash V <144670812+akash-venkateshwaran@users.noreply.github.com>
Date: Sun, 28 Jan 2024 12:33:53 -0800
Subject: [PATCH 13/25] Added unit tests for all three objs

---
 test/test_objectives.py | 66 +++++++++++++++++++++++++++++++++++++++++
 1 file changed, 66 insertions(+)

diff --git a/test/test_objectives.py b/test/test_objectives.py
index 5b20199..85f0e1e 100644
--- a/test/test_objectives.py
+++ b/test/test_objectives.py
@@ -62,6 +62,72 @@ def test_distance_objective(method: objectives.DistanceMethod, max_motion_cost:
             max_cost / distance_objective.max_motion_cost
         )
 
+    # test if the motionCost() is normalized between 0 and 1 for 10 random samples
+    states = distance_objective.sample_states(10)
+    for s1, s2 in itertools.combinations(iterable=states, r=2):
+        assert 0 <= distance_objective.motionCost(s1, s2).value() < 1
+
+
+@pytest.mark.parametrize(
+    "method,heading_degrees,max_motion_cost",
+    [
+        (objectives.MinimumTurningMethod.GOAL_HEADING, 60.0, 174.862),
+        (objectives.MinimumTurningMethod.GOAL_PATH, 60.0, 179.340),
+        (objectives.MinimumTurningMethod.HEADING_PATH, 60.0, 179.962),
+    ],
+)
+def test_minimumturning_objective(
+    method: objectives.MinimumTurningMethod, heading_degrees: float, max_motion_cost: float
+):
+    minimumturning_objective = objectives.MinimumTurningObjective(
+        PATH._simple_setup.getSpaceInformation(),
+        PATH._simple_setup,
+        heading_degrees,
+        method,
+    )
+
+    # test sample_states()
+    num_samples = 3
+    sampled_states = minimumturning_objective.sample_states(num_samples)
+    assert len(sampled_states) == num_samples
+    # for state in sampled_states:
+    #     assert ompl_path.is_state_valid(state)
+
+    assert minimumturning_objective.max_motion_cost == pytest.approx(max_motion_cost, rel=1e0)
+
+    # test if the motionCost() is normalized between 0 and 1 for 10 random samples
+    states = minimumturning_objective.sample_states(10)
+    for s1, s2 in itertools.combinations(iterable=states, r=2):
+        assert 0 <= minimumturning_objective.motionCost(s1, s2).value() < 1
+
+
+@pytest.mark.parametrize(
+    "wind_direction_deg,max_motion_cost",
+    [
+        (60.0, 7593.768),
+        (45.0, 7763.842),
+        (0.0, 7579.767),
+    ],
+)
+def test_wind_objective(wind_direction_deg: float, max_motion_cost: float):
+    wind_objective = objectives.WindObjective(
+        PATH._simple_setup.getSpaceInformation(), wind_direction_deg
+    )
+
+    # test sample_states()
+    num_samples = 3
+    sampled_states = wind_objective.sample_states(num_samples)
+    assert len(sampled_states) == num_samples
+    # for state in sampled_states:
+    #     assert ompl_path.is_state_valid(state)
+
+    assert wind_objective.max_motion_cost == pytest.approx(max_motion_cost, rel=1e0)
+
+    # test if the motionCost() is normalized between 0 and 1 for 10 random samples
+    states = wind_objective.sample_states(10)
+    for s1, s2 in itertools.combinations(iterable=states, r=2):
+        assert 0 <= wind_objective.motionCost(s1, s2).value() < 1
+
 
 @pytest.mark.parametrize(
     "cs1,cs2,expected",

From 8b5de1d3aaf45fb9d40f714fcf6bd0ac747c3ccb Mon Sep 17 00:00:00 2001
From: Patrick Creighton <pcreighton429@gmail.com>
Date: Sun, 28 Jan 2024 14:28:02 -0800
Subject: [PATCH 14/25] Hardcode num_samples in each objective function

---
 local_pathfinding/objectives.py | 31 ++++++++-----------------------
 1 file changed, 8 insertions(+), 23 deletions(-)

diff --git a/local_pathfinding/objectives.py b/local_pathfinding/objectives.py
index 8cc16e5..26475bb 100644
--- a/local_pathfinding/objectives.py
+++ b/local_pathfinding/objectives.py
@@ -111,7 +111,6 @@ def __init__(
         space_information,
         method: DistanceMethod,
         reference=HelperLatLon(latitude=0.0, longitude=0.0),
-        num_samples: int = 100,
     ):
         self.method = method
         if self.method == DistanceMethod.OMPL_PATH_LENGTH:
@@ -119,7 +118,7 @@ def __init__(
         elif self.method == DistanceMethod.LATLON:
             self.reference = reference
 
-        super().__init__(space_information, num_samples)
+        super().__init__(space_information, num_samples=100)
 
     def motionCost(self, s1: ob.SE2StateSpace, s2: ob.SE2StateSpace) -> ob.Cost:
         """Generates the distance between two points
@@ -197,12 +196,7 @@ class MinimumTurningObjective(Objective):
     """
 
     def __init__(
-        self,
-        space_information,
-        simple_setup,
-        heading_degrees: float,
-        method: MinimumTurningMethod,
-        num_samples: int = 100,
+        self, space_information, simple_setup, heading_degrees: float, method: MinimumTurningMethod
     ):
         self.goal = cs.XY(
             simple_setup.getGoal().getState().getX(), simple_setup.getGoal().getState().getY()
@@ -211,7 +205,7 @@ def __init__(
         self.heading = math.radians(heading_degrees)
         self.method = method
 
-        super().__init__(space_information, num_samples)
+        super().__init__(space_information, num_samples=100)
 
     def motionCost(self, s1: ob.SE2StateSpace, s2: ob.SE2StateSpace) -> ob.Cost:
         """Generates the turning cost between s1, s2, heading or the goal position
@@ -325,16 +319,11 @@ class WindObjective(Objective):
         wind_direction (float): The direction of the wind in radians (-pi, pi]
     """
 
-    def __init__(
-        self,
-        space_information,
-        wind_direction_degrees: float,
-        num_samples: int = 100,
-    ):
+    def __init__(self, space_information, wind_direction_degrees: float):
         assert -180 < wind_direction_degrees <= 180
         self.wind_direction = math.radians(wind_direction_degrees)
 
-        super().__init__(space_information, num_samples)
+        super().__init__(space_information, num_samples=100)
 
     def motionCost(self, s1: ob.SE2StateSpace, s2: ob.SE2StateSpace) -> ob.Cost:
         """Generates the cost associated with the upwind and downwind directions of the boat in
@@ -447,16 +436,12 @@ def get_sailing_objective(
 ) -> ob.OptimizationObjective:
     objective = ob.MultiOptimizationObjective(si=space_information)
     objective_1 = DistanceObjective(
-        space_information=space_information, method=DistanceMethod.LATLON, num_samples=100
+        space_information=space_information, method=DistanceMethod.LATLON
     )
     objective_2 = MinimumTurningObjective(
-        space_information,
-        simple_setup,
-        heading_degrees,
-        MinimumTurningMethod.GOAL_HEADING,
-        num_samples=100,
+        space_information, simple_setup, heading_degrees, MinimumTurningMethod.GOAL_HEADING
     )
-    objective_3 = WindObjective(space_information, wind_direction_degrees, num_samples=100)
+    objective_3 = WindObjective(space_information, wind_direction_degrees)
     objective.addObjective(objective=objective_1, weight=0.33)
     objective.addObjective(objective=objective_2, weight=0.33)
     objective.addObjective(objective=objective_3, weight=0.34)

From 4c2b81c00ab5ac6befe93610b87f7000430d4fb7 Mon Sep 17 00:00:00 2001
From: Patrick Creighton <pcreighton429@gmail.com>
Date: Sun, 28 Jan 2024 14:43:34 -0800
Subject: [PATCH 15/25] Update docstrings

---
 local_pathfinding/objectives.py | 6 +++---
 1 file changed, 3 insertions(+), 3 deletions(-)

diff --git a/local_pathfinding/objectives.py b/local_pathfinding/objectives.py
index 26475bb..f8281cc 100644
--- a/local_pathfinding/objectives.py
+++ b/local_pathfinding/objectives.py
@@ -128,7 +128,7 @@ def motionCost(self, s1: ob.SE2StateSpace, s2: ob.SE2StateSpace) -> ob.Cost:
             s2 (SE2StateInternal): The ending point of the local goal state
 
         Returns:
-            ob.Cost: The distance between two points object
+            ob.Cost: The normalized distance between two points
 
         Raises:
             ValueError: If the distance method is not supported
@@ -215,7 +215,7 @@ def motionCost(self, s1: ob.SE2StateSpace, s2: ob.SE2StateSpace) -> ob.Cost:
             s2 (SE2StateInternal): The ending point of the local goal state
 
         Returns:
-            ob.Cost: The minimum turning angle in degrees
+            ob.Cost: The normalized minimum turning angle in degrees
 
         Raises:
             ValueError: If the minimum turning method is not supported
@@ -334,7 +334,7 @@ def motionCost(self, s1: ob.SE2StateSpace, s2: ob.SE2StateSpace) -> ob.Cost:
             s2 (SE2StateInternal): The ending point of the local goal state
 
         Returns:
-            ob.Cost: The cost of going upwind or downwind normalized by max_motionCost
+            ob.Cost: The normalized cost of going upwind or downwind
         """
         s1_xy = cs.XY(s1.getX(), s1.getY())
         s2_xy = cs.XY(s2.getX(), s2.getY())

From a6a68ac645b0a8f200c023ce1c08af1560955d1a Mon Sep 17 00:00:00 2001
From: Patrick Creighton <pcreighton429@gmail.com>
Date: Sun, 28 Jan 2024 14:43:43 -0800
Subject: [PATCH 16/25] Reorder tests

---
 test/test_objectives.py | 127 +++++++++++++++++++---------------------
 1 file changed, 59 insertions(+), 68 deletions(-)

diff --git a/test/test_objectives.py b/test/test_objectives.py
index 46ab6a5..809e95a 100644
--- a/test/test_objectives.py
+++ b/test/test_objectives.py
@@ -22,6 +22,9 @@
 )
 
 
+""" Tests for distance objective """
+
+
 @pytest.mark.parametrize(
     "method,max_motion_cost",
     [
@@ -69,67 +72,6 @@ def test_distance_objective(method: objectives.DistanceMethod, max_motion_cost:
         assert 0 <= distance_objective.motionCost(s1, s2).value() < 1
 
 
-@pytest.mark.parametrize(
-    "method,heading_degrees,max_motion_cost",
-    [
-        (objectives.MinimumTurningMethod.GOAL_HEADING, 60.0, 174.862),
-        (objectives.MinimumTurningMethod.GOAL_PATH, 60.0, 179.340),
-        (objectives.MinimumTurningMethod.HEADING_PATH, 60.0, 179.962),
-    ],
-)
-def test_minimumturning_objective(
-    method: objectives.MinimumTurningMethod, heading_degrees: float, max_motion_cost: float
-):
-    minimumturning_objective = objectives.MinimumTurningObjective(
-        PATH._simple_setup.getSpaceInformation(),
-        PATH._simple_setup,
-        heading_degrees,
-        method,
-    )
-
-    # test sample_states()
-    num_samples = 3
-    sampled_states = minimumturning_objective.sample_states(num_samples)
-    assert len(sampled_states) == num_samples
-    # for state in sampled_states:
-    #     assert ompl_path.is_state_valid(state)
-
-    assert minimumturning_objective.max_motion_cost == pytest.approx(max_motion_cost, rel=1e0)
-
-    # test if the motionCost() is normalized between 0 and 1 for 10 random samples
-    states = minimumturning_objective.sample_states(10)
-    for s1, s2 in itertools.combinations(iterable=states, r=2):
-        assert 0 <= minimumturning_objective.motionCost(s1, s2).value() < 1
-
-
-@pytest.mark.parametrize(
-    "wind_direction_deg,max_motion_cost",
-    [
-        (60.0, 7593.768),
-        (45.0, 7763.842),
-        (0.0, 7579.767),
-    ],
-)
-def test_wind_objective(wind_direction_deg: float, max_motion_cost: float):
-    wind_objective = objectives.WindObjective(
-        PATH._simple_setup.getSpaceInformation(), wind_direction_deg
-    )
-
-    # test sample_states()
-    num_samples = 3
-    sampled_states = wind_objective.sample_states(num_samples)
-    assert len(sampled_states) == num_samples
-    # for state in sampled_states:
-    #     assert ompl_path.is_state_valid(state)
-
-    assert wind_objective.max_motion_cost == pytest.approx(max_motion_cost, rel=1e0)
-
-    # test if the motionCost() is normalized between 0 and 1 for 10 random samples
-    states = wind_objective.sample_states(10)
-    for s1, s2 in itertools.combinations(iterable=states, r=2):
-        assert 0 <= wind_objective.motionCost(s1, s2).value() < 1
-
-
 @pytest.mark.parametrize(
     "cs1,cs2,expected",
     [
@@ -173,22 +115,40 @@ def test_get_latlon_path_length_objective(rf: tuple, cs1: tuple, cs2: tuple):
     ) == pytest.approx(distance_m)
 
 
+""" Tests for minimum turning objective """
+
+
 @pytest.mark.parametrize(
-    "method",
+    "method,heading_degrees,max_motion_cost",
     [
-        objectives.MinimumTurningMethod.GOAL_HEADING,
-        objectives.MinimumTurningMethod.GOAL_PATH,
-        objectives.MinimumTurningMethod.HEADING_PATH,
+        (objectives.MinimumTurningMethod.GOAL_HEADING, 60.0, 174.862),
+        (objectives.MinimumTurningMethod.GOAL_PATH, 60.0, 179.340),
+        (objectives.MinimumTurningMethod.HEADING_PATH, 60.0, 179.962),
     ],
 )
-def test_minimum_turning_objective(method: objectives.MinimumTurningMethod):
+def test_minimum_turning_objective(
+    method: objectives.MinimumTurningMethod, heading_degrees: float, max_motion_cost: float
+):
     minimum_turning_objective = objectives.MinimumTurningObjective(
         PATH._simple_setup.getSpaceInformation(),
         PATH._simple_setup,
-        PATH.state.heading_direction,
+        heading_degrees,
         method,
     )
-    assert minimum_turning_objective is not None
+
+    # test sample_states()
+    num_samples = 3
+    sampled_states = minimum_turning_objective.sample_states(num_samples)
+    assert len(sampled_states) == num_samples
+    # for state in sampled_states:
+    #     assert ompl_path.is_state_valid(state)
+
+    assert minimum_turning_objective.max_motion_cost == pytest.approx(max_motion_cost, rel=1e0)
+
+    # test if the motionCost() is normalized between 0 and 1 for 10 random samples
+    states = minimum_turning_objective.sample_states(10)
+    for s1, s2 in itertools.combinations(iterable=states, r=2):
+        assert 0 <= minimum_turning_objective.motionCost(s1, s2).value() < 1
 
 
 @pytest.mark.parametrize(
@@ -241,6 +201,37 @@ def test_heading_path_turn_cost(cs1: tuple, cs2: tuple, heading_degrees: float,
     ) == pytest.approx(expected, abs=1e-3)
 
 
+""" Tests for wind objective """
+
+
+@pytest.mark.parametrize(
+    "wind_direction_deg,max_motion_cost",
+    [
+        (60.0, 7593.768),
+        (45.0, 7763.842),
+        (0.0, 7579.767),
+    ],
+)
+def test_wind_objective(wind_direction_deg: float, max_motion_cost: float):
+    wind_objective = objectives.WindObjective(
+        PATH._simple_setup.getSpaceInformation(), wind_direction_deg
+    )
+
+    # test sample_states()
+    num_samples = 3
+    sampled_states = wind_objective.sample_states(num_samples)
+    assert len(sampled_states) == num_samples
+    # for state in sampled_states:
+    #     assert ompl_path.is_state_valid(state)
+
+    assert wind_objective.max_motion_cost == pytest.approx(max_motion_cost, rel=1e0)
+
+    # test if the motionCost() is normalized between 0 and 1 for 10 random samples
+    states = wind_objective.sample_states(10)
+    for s1, s2 in itertools.combinations(iterable=states, r=2):
+        assert 0 <= wind_objective.motionCost(s1, s2).value() < 1
+
+
 @pytest.mark.parametrize(
     "cs1,cs2,wind_direction_deg,expected",
     [

From 4bddba03f1acdf2d9f12a2ff5b7676dc5599358b Mon Sep 17 00:00:00 2001
From: Patrick Creighton <pcreighton429@gmail.com>
Date: Sun, 28 Jan 2024 16:36:18 -0800
Subject: [PATCH 17/25] Remove redundant check

---
 test/test_objectives.py | 22 ++--------------------
 1 file changed, 2 insertions(+), 20 deletions(-)

diff --git a/test/test_objectives.py b/test/test_objectives.py
index 809e95a..2cff201 100644
--- a/test/test_objectives.py
+++ b/test/test_objectives.py
@@ -47,24 +47,7 @@ def test_distance_objective(method: objectives.DistanceMethod, max_motion_cost:
     #     assert ompl_path.is_state_valid(state)
 
     # test find_maximum_motion_cost()
-    # implicitly
     assert distance_objective.max_motion_cost == pytest.approx(max_motion_cost, rel=1e0)
-    # explicitly for the easiest method to set up
-    # don't need to test for all methods since they each have their own tests
-    if method == objectives.DistanceMethod.OMPL_PATH_LENGTH:
-        states = []
-        space = PATH._simple_setup.getStateSpace()
-        for xy in [(-0.5, 0.4), (0.1, 0.2), (0.3, -0.6)]:
-            state = ob.State(space)
-            state().setXY(*xy)
-            states.append(state)
-        max_cost = max(
-            distance_objective.ompl_path_objective.motionCost(s1(), s2()).value()
-            for s1, s2 in itertools.combinations(iterable=states, r=2)
-        )
-        assert distance_objective.find_maximum_motion_cost([s() for s in states]) == pytest.approx(
-            max_cost / distance_objective.max_motion_cost
-        )
 
     # test if the motionCost() is normalized between 0 and 1 for 10 random samples
     states = distance_objective.sample_states(10)
@@ -143,6 +126,7 @@ def test_minimum_turning_objective(
     # for state in sampled_states:
     #     assert ompl_path.is_state_valid(state)
 
+    # test find_maximum_motion_cost()
     assert minimum_turning_objective.max_motion_cost == pytest.approx(max_motion_cost, rel=1e0)
 
     # test if the motionCost() is normalized between 0 and 1 for 10 random samples
@@ -224,6 +208,7 @@ def test_wind_objective(wind_direction_deg: float, max_motion_cost: float):
     # for state in sampled_states:
     #     assert ompl_path.is_state_valid(state)
 
+    # test find_maximum_motion_cost()
     assert wind_objective.max_motion_cost == pytest.approx(max_motion_cost, rel=1e0)
 
     # test if the motionCost() is normalized between 0 and 1 for 10 random samples
@@ -278,9 +263,6 @@ def test_is_downwind(wind_direction_deg: float, heading_deg: float, expected: fl
     assert objectives.WindObjective.is_downwind(wind_direction, heading) == expected
 
 
-""" Tests for is_angle_between() """
-
-
 @pytest.mark.parametrize(
     "afir,amid,asec,expected",
     [

From a2f81cddd67954f137449a2ae5bca9bd7a2ea22d Mon Sep 17 00:00:00 2001
From: Patrick Creighton <pcreighton429@gmail.com>
Date: Sun, 28 Jan 2024 16:39:53 -0800
Subject: [PATCH 18/25] Fix lint error

---
 test/test_objectives.py | 1 -
 1 file changed, 1 deletion(-)

diff --git a/test/test_objectives.py b/test/test_objectives.py
index 2cff201..a71e4ee 100644
--- a/test/test_objectives.py
+++ b/test/test_objectives.py
@@ -3,7 +3,6 @@
 
 import pytest
 from custom_interfaces.msg import HelperLatLon
-from ompl import base as ob
 from rclpy.impl.rcutils_logger import RcutilsLogger
 
 import local_pathfinding.coord_systems as coord_systems

From cfc839a89ce863984e1a119325c6b474a8c106dd Mon Sep 17 00:00:00 2001
From: Akash V <144670812+akash-venkateshwaran@users.noreply.github.com>
Date: Sat, 3 Feb 2024 15:15:20 -0800
Subject: [PATCH 19/25] Added cap func for motionCost

---
 local_pathfinding/objectives.py | 8 ++++++++
 1 file changed, 8 insertions(+)

diff --git a/local_pathfinding/objectives.py b/local_pathfinding/objectives.py
index f8281cc..bda9421 100644
--- a/local_pathfinding/objectives.py
+++ b/local_pathfinding/objectives.py
@@ -94,6 +94,11 @@ def sample_states(self, num_samples: int) -> list[ob.SE2StateSpace]:
 
         return sampled_states
 
+    def capping_motionCost(self, value):
+        if value > 1:
+            value == 1
+        return value
+
 
 class DistanceObjective(Objective):
     """Generates a distance objective function
@@ -147,6 +152,7 @@ def motionCost(self, s1: ob.SE2StateSpace, s2: ob.SE2StateSpace) -> ob.Cost:
             raise ValueError(f"Method {self.method} not supported")
 
         normalized_distance = distance / self.max_motion_cost
+        normalized_distance = self.capping_motionCost(normalized_distance)
         return ob.Cost(normalized_distance)
 
     @staticmethod
@@ -232,6 +238,7 @@ def motionCost(self, s1: ob.SE2StateSpace, s2: ob.SE2StateSpace) -> ob.Cost:
             raise ValueError(f"Method {self.method} not supported")
 
         normalized_angle = angle / self.max_motion_cost
+        normalized_angle = self.capping_motionCost(normalized_angle)
         return ob.Cost(normalized_angle)
 
     @staticmethod
@@ -341,6 +348,7 @@ def motionCost(self, s1: ob.SE2StateSpace, s2: ob.SE2StateSpace) -> ob.Cost:
         wind_cost = WindObjective.wind_direction_cost(s1_xy, s2_xy, self.wind_direction)
 
         normalized_wind_cost = wind_cost / self.max_motion_cost
+        normalized_wind_cost = self.capping_motionCost(normalized_wind_cost)
         return ob.Cost(normalized_wind_cost)
 
     @staticmethod

From 2383c52d88b814d53605ae063c4b964a974ec998 Mon Sep 17 00:00:00 2001
From: Akash V <144670812+akash-venkateshwaran@users.noreply.github.com>
Date: Sat, 10 Feb 2024 12:05:26 -0800
Subject: [PATCH 20/25] Replaced capping with a single function named
 normalization

---
 local_pathfinding/objectives.py | 29 ++++++++++++++++-------------
 test/test_objectives.py         |  6 +++---
 2 files changed, 19 insertions(+), 16 deletions(-)

diff --git a/local_pathfinding/objectives.py b/local_pathfinding/objectives.py
index bda9421..f983260 100644
--- a/local_pathfinding/objectives.py
+++ b/local_pathfinding/objectives.py
@@ -94,10 +94,19 @@ def sample_states(self, num_samples: int) -> list[ob.SE2StateSpace]:
 
         return sampled_states
 
-    def capping_motionCost(self, value):
-        if value > 1:
-            value == 1
-        return value
+    def normalization(self, cost):
+        """Normalizes cost using max_motion_cost and caps it at 1
+
+        Args:
+            cost (float): motionCost value from an objective function
+        Returns:
+            float: normalized cost between 0 to 1
+        """
+        normalized_cost = cost / self.max_motion_cost
+        if normalized_cost > 1:
+            return 1
+        else:
+            return normalized_cost
 
 
 class DistanceObjective(Objective):
@@ -151,9 +160,7 @@ def motionCost(self, s1: ob.SE2StateSpace, s2: ob.SE2StateSpace) -> ob.Cost:
         else:
             raise ValueError(f"Method {self.method} not supported")
 
-        normalized_distance = distance / self.max_motion_cost
-        normalized_distance = self.capping_motionCost(normalized_distance)
-        return ob.Cost(normalized_distance)
+        return ob.Cost(self.normalization(distance))
 
     @staticmethod
     def get_euclidean_path_length_objective(s1: cs.XY, s2: cs.XY) -> float:
@@ -237,9 +244,7 @@ def motionCost(self, s1: ob.SE2StateSpace, s2: ob.SE2StateSpace) -> ob.Cost:
         else:
             raise ValueError(f"Method {self.method} not supported")
 
-        normalized_angle = angle / self.max_motion_cost
-        normalized_angle = self.capping_motionCost(normalized_angle)
-        return ob.Cost(normalized_angle)
+        return ob.Cost(self.normalization(angle))
 
     @staticmethod
     def goal_heading_turn_cost(s1: cs.XY, goal: cs.XY, heading: float) -> float:
@@ -347,9 +352,7 @@ def motionCost(self, s1: ob.SE2StateSpace, s2: ob.SE2StateSpace) -> ob.Cost:
         s2_xy = cs.XY(s2.getX(), s2.getY())
         wind_cost = WindObjective.wind_direction_cost(s1_xy, s2_xy, self.wind_direction)
 
-        normalized_wind_cost = wind_cost / self.max_motion_cost
-        normalized_wind_cost = self.capping_motionCost(normalized_wind_cost)
-        return ob.Cost(normalized_wind_cost)
+        return ob.Cost(self.normalization(wind_cost))
 
     @staticmethod
     def wind_direction_cost(s1: cs.XY, s2: cs.XY, wind_direction: float) -> float:
diff --git a/test/test_objectives.py b/test/test_objectives.py
index a71e4ee..e704d6c 100644
--- a/test/test_objectives.py
+++ b/test/test_objectives.py
@@ -51,7 +51,7 @@ def test_distance_objective(method: objectives.DistanceMethod, max_motion_cost:
     # test if the motionCost() is normalized between 0 and 1 for 10 random samples
     states = distance_objective.sample_states(10)
     for s1, s2 in itertools.combinations(iterable=states, r=2):
-        assert 0 <= distance_objective.motionCost(s1, s2).value() < 1
+        assert 0 <= distance_objective.motionCost(s1, s2).value() <= 1
 
 
 @pytest.mark.parametrize(
@@ -131,7 +131,7 @@ def test_minimum_turning_objective(
     # test if the motionCost() is normalized between 0 and 1 for 10 random samples
     states = minimum_turning_objective.sample_states(10)
     for s1, s2 in itertools.combinations(iterable=states, r=2):
-        assert 0 <= minimum_turning_objective.motionCost(s1, s2).value() < 1
+        assert 0 <= minimum_turning_objective.motionCost(s1, s2).value() <= 1
 
 
 @pytest.mark.parametrize(
@@ -213,7 +213,7 @@ def test_wind_objective(wind_direction_deg: float, max_motion_cost: float):
     # test if the motionCost() is normalized between 0 and 1 for 10 random samples
     states = wind_objective.sample_states(10)
     for s1, s2 in itertools.combinations(iterable=states, r=2):
-        assert 0 <= wind_objective.motionCost(s1, s2).value() < 1
+        assert 0 <= wind_objective.motionCost(s1, s2).value() <= 1
 
 
 @pytest.mark.parametrize(

From 70513fd3303988603d1a035b1ed21b54203ec9c1 Mon Sep 17 00:00:00 2001
From: Patrick Creighton <pcreighton429@gmail.com>
Date: Sat, 10 Feb 2024 15:09:55 -0800
Subject: [PATCH 21/25] Simplify normalization logic

---
 local_pathfinding/objectives.py | 13 +++++--------
 1 file changed, 5 insertions(+), 8 deletions(-)

diff --git a/local_pathfinding/objectives.py b/local_pathfinding/objectives.py
index f983260..c1cc60a 100644
--- a/local_pathfinding/objectives.py
+++ b/local_pathfinding/objectives.py
@@ -94,19 +94,16 @@ def sample_states(self, num_samples: int) -> list[ob.SE2StateSpace]:
 
         return sampled_states
 
-    def normalization(self, cost):
-        """Normalizes cost using max_motion_cost and caps it at 1
+    def normalization(self, cost: float) -> float:
+        """Normalizes cost using max_motion_cost and caps it at 1.
 
         Args:
-            cost (float): motionCost value from an objective function
+            cost (float): motionCost value from an objective function.
         Returns:
-            float: normalized cost between 0 to 1
+            float: normalized cost between 0 to 1.
         """
         normalized_cost = cost / self.max_motion_cost
-        if normalized_cost > 1:
-            return 1
-        else:
-            return normalized_cost
+        return min(normalized_cost, 1.0)
 
 
 class DistanceObjective(Objective):

From e1e4068b68994b1412c738710192c046ef59134a Mon Sep 17 00:00:00 2001
From: Patrick Creighton <pcreighton429@gmail.com>
Date: Sat, 10 Feb 2024 15:16:43 -0800
Subject: [PATCH 22/25] Add explanatory comment

---
 local_pathfinding/objectives.py | 1 +
 1 file changed, 1 insertion(+)

diff --git a/local_pathfinding/objectives.py b/local_pathfinding/objectives.py
index c1cc60a..eec180c 100644
--- a/local_pathfinding/objectives.py
+++ b/local_pathfinding/objectives.py
@@ -54,6 +54,7 @@ def __init__(self, space_information, num_samples: int):
         self.space_information = space_information
 
         states = self.sample_states(num_samples)
+        # initialize to 1 so that motionCost is not normalized when finding the maximum motion cost
         self.max_motion_cost = 1.0
         self.max_motion_cost = self.find_maximum_motion_cost(states)
 

From 4879bbc0abb9f73703bee3cd407aae952897493a Mon Sep 17 00:00:00 2001
From: Akash V <144670812+akash-venkateshwaran@users.noreply.github.com>
Date: Sat, 10 Feb 2024 20:24:59 -0800
Subject: [PATCH 23/25] wokring on SpeedObj

---
 local_pathfinding/objectives.py | 19 ++++++++++---------
 1 file changed, 10 insertions(+), 9 deletions(-)

diff --git a/local_pathfinding/objectives.py b/local_pathfinding/objectives.py
index ddfb887..7887f50 100644
--- a/local_pathfinding/objectives.py
+++ b/local_pathfinding/objectives.py
@@ -479,7 +479,6 @@ def __init__(
         wind_speed: float,
         method: SpeedObjectiveMethod,
     ):
-        super().__init__(space_information)
         assert -180 < wind_direction <= 180
         self.wind_direction = math.radians(wind_direction)
 
@@ -488,6 +487,7 @@ def __init__(
 
         self.wind_speed = wind_speed
         self.method = method
+        super().__init__(space_information, num_samples=100)
 
     def motionCost(self, s1: ob.SE2StateSpace, s2: ob.SE2StateSpace) -> ob.Cost:
         """Generates the cost associated with the speed of the boat.
@@ -508,7 +508,7 @@ def motionCost(self, s1: ob.SE2StateSpace, s2: ob.SE2StateSpace) -> ob.Cost:
         )
 
         if sailbot_speed == 0:
-            return ob.Cost(10000)
+            return ob.Cost(1.0)
 
         if self.method == SpeedObjectiveMethod.SAILBOT_TIME:
             distance = DistanceObjective.get_euclidean_path_length_objective(s1_xy, s2_xy)
@@ -522,7 +522,7 @@ def motionCost(self, s1: ob.SE2StateSpace, s2: ob.SE2StateSpace) -> ob.Cost:
             cost = ob.Cost(self.get_continuous_cost(sailbot_speed))
         else:
             ValueError(f"Method {self.method} not supported")
-        return cost
+        return ob.Cost(self.normalization(cost.value()))
 
     @staticmethod
     def get_sailbot_speed(heading: float, wind_direction: float, wind_speed: float) -> float:
@@ -631,14 +631,15 @@ def get_sailing_objective(
     objective_2 = MinimumTurningObjective(
         space_information, simple_setup, heading_degrees, MinimumTurningMethod.GOAL_HEADING
     )
+    objective_3 = WindObjective(space_information, wind_direction_degrees)
     objective_4 = SpeedObjective(
-        space_information,
-        heading_degrees,
-        wind_direction_degrees,
-        wind_speed,
-        SpeedObjectiveMethod.SAILBOT_TIME,
+        space_information=space_information,
+        heading_direction=heading_degrees,
+        wind_direction=wind_direction_degrees,
+        wind_speed=wind_speed,
+        method=SpeedObjectiveMethod.SAILBOT_TIME,
     )
-    objective_3 = WindObjective(space_information, wind_direction_degrees)
+
     objective.addObjective(objective=objective_1, weight=0.25)
     objective.addObjective(objective=objective_2, weight=0.25)
     objective.addObjective(objective=objective_3, weight=0.25)

From 86be2427d730b21a99274d437913425cff8b644a Mon Sep 17 00:00:00 2001
From: Patrick Creighton <pcreighton429@gmail.com>
Date: Sun, 11 Feb 2024 11:40:17 -0800
Subject: [PATCH 24/25] Fix test and cleanup

---
 local_pathfinding/objectives.py |  2 +-
 test/test_objectives.py         | 39 ++++++++++++---------------------
 2 files changed, 15 insertions(+), 26 deletions(-)

diff --git a/local_pathfinding/objectives.py b/local_pathfinding/objectives.py
index 7887f50..57ae977 100644
--- a/local_pathfinding/objectives.py
+++ b/local_pathfinding/objectives.py
@@ -497,7 +497,7 @@ def motionCost(self, s1: ob.SE2StateSpace, s2: ob.SE2StateSpace) -> ob.Cost:
             s2 (SE2StateInternal): The ending point of the local goal state
 
         Returns:
-            ob.Cost: The cost of going upwind or downwind
+            ob.Cost: The normalized cost of going upwind or downwind
         """
 
         s1_xy = cs.XY(s1.getX(), s1.getY())
diff --git a/test/test_objectives.py b/test/test_objectives.py
index 090aea4..7f94280 100644
--- a/test/test_objectives.py
+++ b/test/test_objectives.py
@@ -42,8 +42,6 @@ def test_distance_objective(method: objectives.DistanceMethod, max_motion_cost:
     num_samples = 3
     sampled_states = distance_objective.sample_states(num_samples)
     assert len(sampled_states) == num_samples
-    # for state in sampled_states:
-    #     assert ompl_path.is_state_valid(state)
 
     # test find_maximum_motion_cost()
     assert distance_objective.max_motion_cost == pytest.approx(max_motion_cost, rel=1e0)
@@ -122,8 +120,6 @@ def test_minimum_turning_objective(
     num_samples = 3
     sampled_states = minimum_turning_objective.sample_states(num_samples)
     assert len(sampled_states) == num_samples
-    # for state in sampled_states:
-    #     assert ompl_path.is_state_valid(state)
 
     # test find_maximum_motion_cost()
     assert minimum_turning_objective.max_motion_cost == pytest.approx(max_motion_cost, rel=1e0)
@@ -204,8 +200,6 @@ def test_wind_objective(wind_direction_deg: float, max_motion_cost: float):
     num_samples = 3
     sampled_states = wind_objective.sample_states(num_samples)
     assert len(sampled_states) == num_samples
-    # for state in sampled_states:
-    #     assert ompl_path.is_state_valid(state)
 
     # test find_maximum_motion_cost()
     assert wind_objective.max_motion_cost == pytest.approx(max_motion_cost, rel=1e0)
@@ -293,12 +287,9 @@ def test_angle_between(afir: float, amid: float, asec: float, expected: float):
 @pytest.mark.parametrize(
     "method,max_motion_cost",
     [
-        objectives.SpeedObjectiveMethod.SAILBOT_TIME,
-        1.0,
-        objectives.SpeedObjectiveMethod.SAILBOT_PIECEWISE,
-        1.0,
-        objectives.SpeedObjectiveMethod.SAILBOT_CONTINUOUS,
-        1.0,
+        (objectives.SpeedObjectiveMethod.SAILBOT_TIME, 1.0),
+        (objectives.SpeedObjectiveMethod.SAILBOT_PIECEWISE, 1.0),
+        (objectives.SpeedObjectiveMethod.SAILBOT_CONTINUOUS, 1.0),
     ],
 )
 def test_speed_objective(method: objectives.SpeedObjectiveMethod, max_motion_cost: float):
@@ -309,22 +300,20 @@ def test_speed_objective(method: objectives.SpeedObjectiveMethod, max_motion_cos
         PATH.state.wind_speed,
         method,
     )
-    assert speed_objective is not None
 
+    # test sample_states()
+    num_samples = 3
+    sampled_states = speed_objective.sample_states(num_samples)
+    assert len(sampled_states) == num_samples
 
-# num_samples = 3
-# sampled_states = speed_objective.sample_states(num_samples)
-# assert len(sampled_states) == num_samples
-# print("\n", num_samples)
-
-# # test find_maximum_motion_cost()
-# print(speed_objective.max_motion_cost)
-# assert speed_objective.max_motion_cost == pytest.approx(max_motion_cost, rel=1e0)
+    # test find_maximum_motion_cost()
+    print(speed_objective.max_motion_cost)
+    assert speed_objective.max_motion_cost == pytest.approx(max_motion_cost, rel=1e0)
 
-# # test if the motionCost() is normalized between 0 and 1 for 10 random samples
-# states = distance_objective.sample_states(10)
-# for s1, s2 in itertools.combinations(iterable=states, r=2):
-#     assert 0 <= distance_objective.motionCost(s1, s2).value() <= 1
+    # test if the motionCost() is normalized between 0 and 1 for 10 random samples
+    states = speed_objective.sample_states(10)
+    for s1, s2 in itertools.combinations(iterable=states, r=2):
+        assert 0 <= speed_objective.motionCost(s1, s2).value() <= 1
 
 
 @pytest.mark.parametrize(

From 727fc78470416f0a0a72f7b7202cba95a6547279 Mon Sep 17 00:00:00 2001
From: Jamen Kaye <jamen.kaye@gmail.com>
Date: Thu, 29 Feb 2024 22:39:20 -0800
Subject: [PATCH 25/25] dynamically compute heading s1 to s2

---
 local_pathfinding/objectives.py | 3 ++-
 1 file changed, 2 insertions(+), 1 deletion(-)

diff --git a/local_pathfinding/objectives.py b/local_pathfinding/objectives.py
index 57ae977..b644f84 100644
--- a/local_pathfinding/objectives.py
+++ b/local_pathfinding/objectives.py
@@ -502,9 +502,10 @@ def motionCost(self, s1: ob.SE2StateSpace, s2: ob.SE2StateSpace) -> ob.Cost:
 
         s1_xy = cs.XY(s1.getX(), s1.getY())
         s2_xy = cs.XY(s2.getX(), s2.getY())
+        heading_s1_to_s2 = math.atan2(s2_xy.x - s1_xy.x, s2_xy.y - s1_xy.y)
 
         sailbot_speed = self.get_sailbot_speed(
-            self.heading_direction, self.wind_direction, self.wind_speed
+            heading_s1_to_s2, self.wind_direction, self.wind_speed
         )
 
         if sailbot_speed == 0: