diff --git a/src/local_pathfinding/local_pathfinding/objectives.py b/src/local_pathfinding/local_pathfinding/objectives.py index c645716e9..716e2344c 100644 --- a/src/local_pathfinding/local_pathfinding/objectives.py +++ b/src/local_pathfinding/local_pathfinding/objectives.py @@ -1,5 +1,6 @@ """Our custom OMPL optimization objectives.""" +import itertools import math from enum import Enum, auto @@ -63,19 +64,74 @@ class Objective(pyompl.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 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.found_max_cost = False + # 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) + self.found_max_cost = True + + def motionCost(self, s1: pyompl.SE2StateSpace, s2: pyompl.SE2StateSpace) -> pyompl.Cost: raise NotImplementedError + def find_maximum_motion_cost(self, states: list[pyompl.SE2StateSpace]) -> float: + """Finds the maximum motion cost between any two states in `states`. + + Args: + states (list[pyompl.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[pyompl.SE2StateSpace]: + """Samples `num_samples` states from the state space. + + Args: + num_samples (int): Number of states to sample. + + Returns: + list[pyompl.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 + + 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. + Returns: + float: normalized cost between 0 to 1. + """ + normalized_cost = cost / self.max_motion_cost + return min(normalized_cost, 1.0) if self.found_max_cost else normalized_cost + class DistanceObjective(Objective): """Generates a distance objective function @@ -95,12 +151,9 @@ def __init__( method: DistanceMethod, reference=HelperLatLon(latitude=0.0, longitude=0.0), ): - super().__init__(space_information) self.method = method if self.method == DistanceMethod.OMPL_PATH_LENGTH: - self.ompl_path_objective = pyompl.PathLengthOptimizationObjective( - self.space_information - ) + self.ompl_path_objective = pyompl.PathLengthOptimizationObjective(self.space_information) elif self.method == DistanceMethod.LATLON: self.reference = reference @@ -128,10 +181,11 @@ def motionCost(self, s1: pyompl.SE2StateSpace, s2: pyompl.SE2StateSpace) -> pyom ) cost = pyompl.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") + + return pyompl.Cost(self.normalization(distance)) @staticmethod def get_euclidean_path_length_objective(s1: cs.XY, s2: cs.XY) -> float: @@ -180,13 +234,8 @@ class MinimumTurningObjective(Objective): """ def __init__( - self, - space_information, - simple_setup, - heading_degrees: float, - method: MinimumTurningMethod, + self, space_information, simple_setup, heading_degrees: float, method: MinimumTurningMethod ): - super().__init__(space_information) self.goal = cs.XY( simple_setup.getGoal().getState().getX(), simple_setup.getGoal().getState().getY() ) @@ -305,7 +354,6 @@ class WindObjective(Objective): """ def __init__(self, space_information, wind_direction_degrees: float): - super().__init__(space_information) assert -180 < wind_direction_degrees <= 180 self.wind_direction = math.radians(wind_direction_degrees) @@ -428,7 +476,6 @@ def __init__( wind_speed: float, method: SpeedObjectiveMethod, ): - super().__init__(space_information) assert -180 < wind_direction <= 180 self.wind_direction = math.radians(wind_direction) @@ -437,6 +484,9 @@ def __init__( self.wind_speed = wind_speed self.method = method + # sailbot time needs more samples to find max_motion_cost, even 2000 is not enough + num_samples = 2000 if self.method == SpeedObjectiveMethod.SAILBOT_TIME else 200 + super().__init__(space_information, num_samples=num_samples) def motionCost(self, s1: pyompl.SE2StateSpace, s2: pyompl.SE2StateSpace) -> pyompl.Cost: """Generates the cost associated with the speed of the boat. @@ -451,11 +501,10 @@ def motionCost(self, s1: pyompl.SE2StateSpace, s2: pyompl.SE2StateSpace) -> pyom s1_xy = cs.XY(s1.getX(), s1.getY()) s2_xy = cs.XY(s2.getX(), s2.getY()) - + heading_s1_to_s2 = 180 * math.atan2(s2_xy.x - s1_xy.x, s2_xy.y - s1_xy.y) / math.pi 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: return pyompl.Cost(10000) @@ -471,7 +520,7 @@ def motionCost(self, s1: pyompl.SE2StateSpace, s2: pyompl.SE2StateSpace) -> pyom cost = pyompl.Cost(self.get_continuous_cost(sailbot_speed)) else: ValueError(f"Method {self.method} not supported") - return cost + return pyompl.Cost(self.normalization(cost.value())) @staticmethod def get_sailbot_speed(heading: float, wind_direction: float, wind_speed: float) -> float: @@ -573,30 +622,25 @@ def get_sailing_objective( wind_direction_degrees: float, wind_speed: float, ) -> pyompl.OptimizationObjective: - objective = pyompl.MultiOptimizationObjective(si=space_information) objective.addObjective( objective=DistanceObjective(space_information, DistanceMethod.LATLON), weight=1.0, ) - objective.addObjective( - objective=MinimumTurningObjective( - space_information, simple_setup, heading_degrees, MinimumTurningMethod.GOAL_HEADING - ), - weight=100.0, + objective_2 = MinimumTurningObjective( + space_information, simple_setup, heading_degrees, MinimumTurningMethod.GOAL_HEADING ) - objective.addObjective( - objective=WindObjective(space_information, wind_direction_degrees), weight=1.0 - ) - objective.addObjective( - objective=SpeedObjective( - space_information, - heading_degrees, - wind_direction_degrees, - wind_speed, - SpeedObjectiveMethod.SAILBOT_TIME, - ), - weight=1.0, + objective_3 = WindObjective(space_information, wind_direction_degrees) + objective_4 = SpeedObjective( + space_information=space_information, + heading_direction=heading_degrees, + wind_direction=wind_direction_degrees, + wind_speed=wind_speed, + method=SpeedObjectiveMethod.SAILBOT_TIME, ) + objective.addObjective(objective=objective_1, weight=0.25) + objective.addObjective(objective=objective_2, weight=0.25) + objective.addObjective(objective=objective_3, weight=0.25) + objective.addObjective(objective=objective_4, weight=0.25) return objective diff --git a/src/local_pathfinding/test/test_objectives.py b/src/local_pathfinding/test/test_objectives.py index 6053a1e59..089b4f045 100644 --- a/src/local_pathfinding/test/test_objectives.py +++ b/src/local_pathfinding/test/test_objectives.py @@ -1,4 +1,5 @@ -"""import math +import itertools +import math import pytest from custom_interfaces.msg import GPS, AISShips, HelperLatLon, Path, WindSensor @@ -27,20 +28,35 @@ ) +""" Tests for distance objective """ + + @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 + + # test find_maximum_motion_cost() + assert distance_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 @pytest.mark.parametrize( @@ -86,22 +102,39 @@ 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 + + # 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 + 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( @@ -154,6 +187,36 @@ 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 + + # 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 + 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", [ @@ -225,23 +288,45 @@ def test_angle_between(afir: float, amid: float, asec: float, expected: float): ) +""" Tests for speed objective """ + + @pytest.mark.parametrize( - "method", + "method,heading,wind_direction,wind_speed,max_motion_cost", [ - objectives.SpeedObjectiveMethod.SAILBOT_TIME, - objectives.SpeedObjectiveMethod.SAILBOT_PIECEWISE, - objectives.SpeedObjectiveMethod.SAILBOT_CONTINUOUS, + (objectives.SpeedObjectiveMethod.SAILBOT_TIME, 5, 43, 28, 3672.594), + (objectives.SpeedObjectiveMethod.SAILBOT_PIECEWISE, -15, 67, 22, 50.0), + (objectives.SpeedObjectiveMethod.SAILBOT_CONTINUOUS, -15, 67, 22, 10000.0), ], ) -def test_speed_objective(method: objectives.SpeedObjectiveMethod): +def test_speed_objective( + method: objectives.SpeedObjectiveMethod, + heading, + wind_direction, + wind_speed, + max_motion_cost: float, +): speed_objective = objectives.SpeedObjective( PATH._simple_setup.getSpaceInformation(), - PATH.state.heading_direction, - PATH.state.wind_direction, - PATH.state.wind_speed, + heading, + wind_direction, + 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 + + # test find_maximum_motion_cost() + if method != objectives.SpeedObjectiveMethod.SAILBOT_TIME: + 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 = 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(