Normalizing Each Objective Class's motionCost()

local_pathfinding/objectives.py

@@ -1,5 +1,6 @@
 """Our custom OMPL optimization objectives."""
 
+import itertools
 import math
 from enum import Enum, auto
 
@@ -43,15 +44,54 @@ 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):
+    def __init__(self, space_information, num_samples: int):
         super().__init__(si=space_information, enableMotionCostInterpolation=True)
         self.space_information = space_information
 
+        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, states: list[ob.SE2StateSpace]) -> float:
+        """Finds the maximum motion cost between any two states in `states`.
+
+        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)
+        )
+
+    def sample_states(self, num_samples: int) -> list[ob.SE2StateSpace]:
+        """Samples `num_samples` states from the state space.
+
+        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 = self.space_information.getStateSpace().allocState()
+            sampler.sampleUniform(state)
+            sampled_states.append(state)
+
+        return sampled_states
+
 
 class DistanceObjective(Objective):
     """Generates a distance objective function
@@ -64,14 +104,21 @@ class DistanceObjective(Objective):
             Only defined if the method is latlon.
     """
 
-    def __init__(self, space_information, method: DistanceMethod, reference=cs.LatLon(0, 0)):
-        super().__init__(space_information)
+    def __init__(
+        self,
+        space_information,
+        method: DistanceMethod,
+        reference: cs.LatLon = cs.LatLon(0, 0),
+        num_samples: int = 100,
+    ):
         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
 
+        super().__init__(space_information, num_samples)
+
     def motionCost(self, s1: ob.SE2StateSpace, s2: ob.SE2StateSpace) -> ob.Cost:
         """Generates the distance between two points
 
@@ -89,17 +136,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 = ob.Cost(distance)
         elif self.method == DistanceMethod.LATLON:
             distance = DistanceObjective.get_latlon_path_length_objective(
                 s1_xy, s2_xy, self.reference
             )
-            cost = ob.Cost(distance)
         elif self.method == DistanceMethod.OMPL_PATH_LENGTH:
-            cost = self.ompl_path_objective.motionCost(s1_xy, s2_xy)
+            distance = self.ompl_path_objective.motionCost(s1, s2).value()
         else:
-            ValueError(f"Method {self.method} not supported")
-        return cost
+            raise ValueError(f"Method {self.method} not supported")
+
+        normalized_distance = distance / self.max_motion_cost
+        return ob.Cost(normalized_distance)
 
     @staticmethod
     def get_euclidean_path_length_objective(s1: cs.XY, s2: cs.XY) -> float:
@@ -153,15 +200,17 @@ def __init__(
         simple_setup,
         heading_degrees: float,
         method: MinimumTurningMethod,
+        num_samples: int = 100,
     ):
-        super().__init__(space_information)
         self.goal = cs.XY(
             simple_setup.getGoal().getState().getX(), simple_setup.getGoal().getState().getY()
         )
         assert -180 < heading_degrees <= 180
         self.heading = math.radians(heading_degrees)
         self.method = method
 
+        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
 
@@ -184,8 +233,10 @@ 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_motion_cost
+        return ob.Cost(normalized_angle)
 
     @staticmethod
     def goal_heading_turn_cost(s1: cs.XY, goal: cs.XY, heading: float) -> float:
@@ -272,11 +323,17 @@ class WindObjective(Objective):
         wind_direction (float): The direction of the wind in radians (-pi, pi]
     """
 
-    def __init__(self, space_information, wind_direction_degrees: float):
-        super().__init__(space_information)
+    def __init__(
+        self,
+        space_information,
+        wind_direction_degrees: float,
+        num_samples: int = 100,
+    ):
         assert -180 < wind_direction_degrees <= 180
         self.wind_direction = math.radians(wind_direction_degrees)
 
+        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
         relation to the wind.
@@ -286,11 +343,14 @@ 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_motion_cost
+        return ob.Cost(normalized_wind_cost)
 
     @staticmethod
     def wind_direction_cost(s1: cs.XY, s2: cs.XY, wind_direction: float) -> float:
@@ -384,18 +444,18 @@ 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=100
     )
-    objective.addObjective(
-        objective=MinimumTurningObjective(
-            space_information, simple_setup, heading_degrees, MinimumTurningMethod.GOAL_HEADING
-        ),
-        weight=100.0,
-    )
-    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

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(