Rework wind cost function in ompl objectives

src/local_pathfinding/local_pathfinding/ompl_objectives.py

@@ -15,6 +15,8 @@
 ACCEPTABLE_COST_THRESHOLD = 0.85
 WIND_OBJECTIVE_WEIGHT = 0.85
 TIME_OBJECTIVE_WEIGHT = 0.15
+NO_GO_ZONE = math.pi / 4
+WIND_COST_SIN_EXPONENT = 80
 
 
 #               Estimated Boat Speeds (kmph) as function of True Wind Speed (kmph)
@@ -94,8 +96,14 @@ def motionCost(self, s1: ob.SE2StateSpace, s2: ob.SE2StateSpace) -> ob.Cost:
 
     @staticmethod
     def wind_direction_cost(s1: cs.XY, s2: cs.XY, tw_direction_rad: float) -> float:
-        """Returns a high cost when the path segment from s1 to s2 is pointing directly
-           (or close to directly) upwind or downwind.
+        """Computes a wind alignment cost based on the absolute angle θ between the segment
+        bearing and the true wind direction.
+
+        1) If θ ≤ NO_GO_ZONE or θ ≥ π − NO_GO_ZONE (i.e., within 45 degrees of directly upwind or
+        downwind), the cost is 1.0.
+        2) Otherwise, the cost is sin(2θ) ** WIND_COST_SIN_EXPONENT.
+
+        The cost is symmetric about both upwind (0) and downwind (π) and always lies in [0, 1].
 
         Args:
             s1 (cs.XY): The start point of the path segment
@@ -107,12 +115,13 @@ def wind_direction_cost(s1: cs.XY, s2: cs.XY, tw_direction_rad: float) -> float:
         """
         segment_true_bearing_rad = cs.get_path_segment_true_bearing(s1, s2, rad=True)
         tw_angle_rad = abs(wcs.get_true_wind_angle(segment_true_bearing_rad, tw_direction_rad))
-        cos_angle = math.cos(tw_angle_rad)
 
-        if cos_angle > 0:
-            return UPWIND_COST_MULTIPLIER * cos_angle
-        else:
-            return DOWNWIND_COST_MULTIPLIER * abs(cos_angle)
+        # NO-GO ZONE
+        if tw_angle_rad <= NO_GO_ZONE or tw_angle_rad >= math.pi - NO_GO_ZONE:
+            return 1.0
+
+        cost = math.sin(2*tw_angle_rad) ** WIND_COST_SIN_EXPONENT
+        return cost
 
 
 class TimeObjective(ob.OptimizationObjective):

src/local_pathfinding/test/test_ompl_objectives.py

@@ -3,60 +3,104 @@
 import pytest
 
 import local_pathfinding.coord_systems as cs
-import local_pathfinding.ompl_objectives as objectives
-from local_pathfinding.ompl_objectives import (
-    DOWNWIND_COST_MULTIPLIER,
-    UPWIND_COST_MULTIPLIER,
-)
+import local_pathfinding.ompl_objectives as ob
+from local_pathfinding.ompl_objectives import (NO_GO_ZONE, WIND_COST_SIN_EXPONENT)
 
 
 @pytest.mark.parametrize(
-    "cs1,cs2,wind_direction_rad,expected",
+    "s1, s2, tw_direction_rad, expected",
     [
-        # Moving directly into wind (upwind)
-        ((0, 0), (0, 1), 0.0, UPWIND_COST_MULTIPLIER * 1.0),
-        # Moving perpendicular to wind (crosswind)
-        ((0, 0), (1, 0), 0.0, 0.0),
-        # Moving directly with the wind (downwind)
-        ((0, 0), (0, -1), 0.0, DOWNWIND_COST_MULTIPLIER * 1.0),
-        # Moving at 45° off upwind
-        ((0, 0), (1, 1), 0.0, UPWIND_COST_MULTIPLIER * math.cos(math.radians(45))),
-        # Moving 30° off upwind
         (
-            (0, 0),
-            (math.sin(math.radians(30)), math.cos(math.radians(30))),
+            # Wind direction and boat heading north
+            cs.XY(0.0, 0.0),
+            cs.XY(0.0, 1.0),
             0.0,
-            UPWIND_COST_MULTIPLIER * math.cos(math.radians(30)),
+            1.0
+        ),
+        (
+            # Wind direction south and boat heading north
+            cs.XY(0.0, 0.0),
+            cs.XY(0.0, 1.0),
+            math.pi,
+            1.0
+        ),
+        (
+            # Boat heading north, wind at no-go zone at 45 degrees
+            cs.XY(0.0, 0.0),
+            cs.XY(0.0, 1.0),
+            NO_GO_ZONE,
+            1.0
+        ),
+        (
+            # Boat heading north, wind at no-go zone at -45 degrees
+            cs.XY(0.0, 0.0),
+            cs.XY(0.0, 1.0),
+            -NO_GO_ZONE,
+            1.0
+        ),
+        (
+            # Boat heading south, wind at no-go zone at 135 degrees
+            cs.XY(0.0, 0.0),
+            cs.XY(0.0, -1.0),
+            math.pi - NO_GO_ZONE,
+            1.0
+        ),
+        (
+            # Boat heading south, wind at no-go zone at -135 degrees
+            cs.XY(0.0, 0.0),
+            cs.XY(0.0, -1.0),
+            - math.pi + NO_GO_ZONE,
+            1.0
+        ),
+        (
+            # Boat heading east, wind just better than no-go zone
+            cs.XY(0.0, 0.0),
+            cs.XY(1.0, 0.0),
+            NO_GO_ZONE - 0.0001,
+            math.sin(2*(NO_GO_ZONE - 0.0001)) ** WIND_COST_SIN_EXPONENT
         ),
-        # Moving 135° off upwind (45° off downwind)
         (
-            (0, 0),
-            (math.sin(math.radians(135)), math.cos(math.radians(135))),
+            # Boat heading west, wind heading north
+            cs.XY(0.0, 0.0),
+            cs.XY(-1.0, 0.0),
             0.0,
-            DOWNWIND_COST_MULTIPLIER * abs(math.cos(math.radians(135))),
+            0.0
         ),
-        # Wind from 179°, boat moving North
         (
-            (0, 0),
-            (0, 1),
-            math.radians(179.0),
-            DOWNWIND_COST_MULTIPLIER * math.cos(math.radians(1)),
+            # Boat heading west, wind heading south
+            cs.XY(0.0, 0.0),
+            cs.XY(-1.0, 0.0),
+            math.pi,
+            0.0
         ),
-        # Wind from -179°, boat moving North
         (
-            (0, 0),
-            (0, 1),
-            math.radians(-179.0),
-            DOWNWIND_COST_MULTIPLIER * math.cos(math.radians(1)),
+            # Boat heading north, wind heading at 60 degrees
+            cs.XY(0.0, 0.0),
+            cs.XY(0.0, 1.0),
+            math.pi / 3,
+            math.sin(2*math.pi / 3) ** WIND_COST_SIN_EXPONENT
         ),
+        (
+            # Boat heading north, wind heading at -55 degrees (-0.959931 radians)
+            cs.XY(0.0, 0.0),
+            cs.XY(0.0, 1.0),
+            -0.959931,
+            math.sin(2*(-0.959931)) ** WIND_COST_SIN_EXPONENT
+        ),
+        (
+            # Boat heading north, wind heading at 50 degrees (0.872665 radians)
+            cs.XY(0.0, 0.0),
+            cs.XY(0.0, 1.0),
+            0.872665,
+            math.sin(2*0.872665) ** WIND_COST_SIN_EXPONENT
+        )
     ],
 )
-def test_wind_direction_cost(cs1: tuple, cs2: tuple, wind_direction_rad: float, expected: float):
-    s1 = cs.XY(*cs1)
-    s2 = cs.XY(*cs2)
-    assert objectives.WindObjective.wind_direction_cost(
-        s1, s2, wind_direction_rad
-    ) == pytest.approx(expected, abs=1e-3)
+def test_wind_direction_cost(
+    s1: cs.XY, s2: cs.XY, tw_direction_rad: float, expected: float
+):
+    result = ob.WindObjective.wind_direction_cost(s1, s2, tw_direction_rad)
+    assert result == pytest.approx(expected, abs=1e-12)
 
 
 @pytest.mark.parametrize(
@@ -84,7 +128,7 @@ def test_speed_cost(
 ):
     s1 = cs.XY(*cs1)
     s2 = cs.XY(*cs2)
-    assert objectives.TimeObjective.time_cost(
+    assert ob.TimeObjective.time_cost(
         s1, s2, wind_direction_rad, wind_speed_kmph
     ) == pytest.approx(expected, abs=0.1)
 
@@ -103,5 +147,5 @@ def test_speed_cost(
 def test_get_sailbot_speed(
     heading: float, wind_direction: float, wind_speed: float, expected: float
 ):
-    speed = objectives.TimeObjective.get_sailbot_speed(heading, wind_direction, wind_speed)
+    speed = ob.TimeObjective.get_sailbot_speed(heading, wind_direction, wind_speed)
     assert speed == pytest.approx(expected, abs=0.1)