(feature) Adds OMPL geometric planner and validity checking in C++

Author: mkabtoul

src/kompass_core/third_party/ompl/config.py

@@ -106,7 +106,9 @@ def _convert_log_level(log_level: Union[ompl.util.LogLevel, str]) -> ompl.util.L
     raise ValueError(f"Invalid OMPL log level: {log_level}")
 
 
-def initializePlanners(log_level: Union[ompl.util.LogLevel, str] = ompl.util.LogLevel.LOG_ERROR):
+def initializePlanners(
+    log_level: Union[ompl.util.LogLevel, str] = ompl.util.LogLevel.LOG_ERROR,
+):
     """Initialize planner map, similar to ompl python bindings."""
     log_level = _convert_log_level(log_level)
     ompl.util.setLogLevel(log_level)

src/kompass_core/third_party/ompl/planner.py

@@ -3,14 +3,12 @@
 import numpy as np
 from attrs import asdict, define, field
 from ...utils.common import BaseAttrs, base_validators
-from ...utils.geometry import convert_to_plus_minus_pi
 
 import omplpy as ompl
-from ...models import Robot, RobotState
+from ...models import Robot, RobotGeometry
 from omplpy import base, geometric
+from kompass_cpp.planning import OMPL2DGeometricPlanner
 
-from ..fcl.collisions import FCL
-from ..fcl.config import FCLConfig
 from .config import create_config_class, initializePlanners, optimization_objectives
 
 
@@ -54,7 +52,6 @@ def __init__(
         use_fcl: bool = True,
         config: Optional[OMPLGeometricConfig] = None,
         config_file: Optional[str] = None,
-        map_3d: Optional[np.ndarray] = None,
     ):
         """
         Init Open Motion Planning Lib geometric handler
@@ -93,45 +90,29 @@ def __init__(
         self.available_planners = ompl.geometric.planners.getPlanners()
 
         # create a simple setup object
-        self.setup = geometric.SimpleSetup(self.state_space)
-
-        # Configure FCL
-        if use_fcl:
-            self.__init_fcl(map_3d)
-            is_state_valid = self._validity_checker_with_fcl
+        self._ompl_setup = geometric.SimpleSetup(self.state_space)
 
-        else:
-            is_state_valid = self._validity_checker
+        # Set the selected planner
+        self._set_planner()
 
-        # Add validity method to ompl setup without custom validity checkers
-        self.setup.setStateValidityChecker(is_state_valid)
+        # Setup Path Optimization Objective
+        optimization_objective = getattr(base, self._config.optimization_objective)(
+            self._ompl_setup.getSpaceInformation()
+        )
 
-        # Set the selected planner
-        self.set_planner()
+        self._ompl_setup.setOptimizationObjective(optimization_objective)
 
         if config_file:
             self.configure(config_file)
 
-    def __init_fcl(self, map_3d: Optional[np.ndarray] = None):
-        """
-        Setup FCL
-
-        :param map_3d: Map data
-        :type map_3d: np.ndarray
-        """
-        fcl_config = FCLConfig(
+        self._cpp_planner: Optional[OMPL2DGeometricPlanner] = OMPL2DGeometricPlanner(
+            robot_shape=RobotGeometry.Type.to_kompass_cpp_lib(
+                self._robot.geometry_type
+            ),
+            robot_dimensions=self._robot.geometry_params,
+            ompl_setup=self._ompl_setup,
             map_resolution=self._config.map_resolution,
-            robot_geometry_type=self._robot.geometry_type,
-            robot_geometry_params=self._robot.geometry_params.tolist(),
         )
-        if not hasattr(self, "fcl"):
-            # setup collision check
-            if map_3d is not None:
-                self.fcl = FCL(fcl_config, map_3d)
-            else:
-                self.fcl = FCL(fcl_config)
-        else:
-            self.fcl._setup_from_config()
 
     def configure(
         self,
@@ -170,59 +151,19 @@ def configure(
 
         planner_params.from_file(config_file, nested_root_name + "." + planner_name)
 
-        self.set_planner(planner_params, planner_id)
+        self._set_planner(planner_params, planner_id)
 
         self.start = False
 
-        # configure FCL if it is used
-        if self._use_fcl:
-            self.__init_fcl()
-
-    def clear(self):
-        """
-        Clears planning setup
-        """
-        self.setup.clear()
-
     @property
-    def path_cost(self) -> Optional[float]:
+    def path_cost(self) -> float:
         """
         Getter of solution path cost using the configured optimization objective
 
         :return: Path cost
         :rtype: float | None
         """
-        if self.solution:
-            optimization_objective = getattr(base, self._config.optimization_objective)(
-                self.setup.getSpaceInformation()
-            )
-            cost = self.solution.cost(optimization_objective)
-            return cost.value()
-        return None
-
-    def get_cost_using_objective(self, objective_key: str) -> Optional[float]:
-        """
-        Get solution cost using a specific objective
-        This is used to get the cost using an objective other than the configured objective
-        To get the cost using the default objective use self.path_cost directly
-
-        :param objective_key: _description_
-        :type objective_key: str
-        :raises KeyError: Unknown objective name
-        :return: _description_
-        :rtype: _type_
-        """
-        if not self.solution:
-            return None
-        if objective_key in optimization_objectives:
-            optimization_objective = getattr(base, objective_key)(
-                self.setup.getSpaceInformation()
-            )
-            cost = self.solution.cost(optimization_objective)
-            return cost.value()
-        raise KeyError(
-            f"Unknown optimization objective. Available optimization objectives are: {optimization_objectives.keys()}"
-        )
+        return self._cpp_planner.get_cost()
 
     def setup_problem(
         self,
@@ -236,7 +177,7 @@ def setup_problem(
         map_3d: Optional[np.ndarray] = None,
     ):
         """
-        Setup a new planning problem with a map, start and goal infromation
+        Setup a new planning problem with a map, start and goal information
 
         :param map_meta_data: Global map meta data as a dictionary
         :type map_meta_data: Dict
@@ -255,132 +196,16 @@ def setup_problem(
         :param map_3d: 3D array for map PointCloud data, defaults to None
         :type map_3d: np.ndarray | None, optional
         """
-        # Clear previous setup
-        self.setup.clear()
-
-        self.set_space_bounds(map_meta_data)
-
-        scoped_start_state = base.ScopedState(self.state_space)
-        start_state = scoped_start_state.get()
-        start_state.setX(start_x)
-        start_state.setY(start_y)
-
-        # SE2 takes angle in [-pi,+pi]
-        yaw = convert_to_plus_minus_pi(start_yaw)
-        start_state.setYaw(yaw)
-
-        scoped_goal_state = base.ScopedState(self.state_space)
-        goal_state = scoped_goal_state.get()
-        goal_state.setX(goal_x)
-        goal_state.setY(goal_y)
-
-        # SE2 takes angle in [-pi,+pi]
-        yaw = convert_to_plus_minus_pi(goal_yaw)
-        goal_state.setYaw(yaw)
-
-        self.setup.setStartAndGoalStates(
-            start=scoped_start_state, goal=scoped_goal_state
-        )
-
-        # Setup Path Optimization Objective
-        optimization_objective = getattr(base, self._config.optimization_objective)(
-            self.setup.getSpaceInformation()
-        )
-
-        self.setup.setOptimizationObjective(optimization_objective)
-
-        if self._use_fcl and map_3d is None:
-            raise ValueError(
-                "OMPL is started with collision check -> Map should be provided"
-            )
-        elif self._use_fcl:
-            # TODO: Add an option to update the map periodically or just at the start, or after a time interval
-            if not self.start:
-                self.fcl.set_map(map_3d)
-                self.start = True
-            self.fcl.update_state(
-                robot_state=RobotState(x=start_x, y=start_y, yaw=start_yaw)
-            )
-
-    def add_validity_check(self, name: str, validity_function: Callable) -> None:
-        """
-        Add method for state validity check during planning
-
-        :param name: Method key name
-        :type name: str
-        :param validity_function: Validity check method
-        :type validity_function: Callable
-
-        :raises TypeError: If validity check is not callable
-        :raises TypeError: If validity check method does not return a boolean
-        """
-        # Check that validity function is callable
-        if callable(validity_function) and callable(validity_function):
-            # Check if the function returns a boolean value
-            args = (None,) * validity_function.__code__.co_argcount
-            if isinstance(validity_function(*args), bool):
-                self._custom_validity_check[name] = validity_function
-            else:
-                raise TypeError("Validity check function needs to return a boolean")
-        else:
-            raise TypeError("Validity check function must be callable")
-
-    def remove_validity_check(self, name: str) -> bool:
-        """
-        Removes an added validity chec!= Nonek
-
-        :param name: Validity check name key
-        :type name: str
-
-        :raises ValueError: If given key does not correspond to an added validity check
-
-        :return: If validity check is removed
-        :rtype: bool
-        """
-        deleted_method = self._custom_validity_check.pop(name, None)
-        if deleted_method is not None:
-            return True
-        else:
-            raise ValueError(
-                f"Cannot remove validity check titled {name} as it does not exist"
-            )
-
-    def _validity_checker(self, state, **_) -> bool:
-        """
-        State validity checker method
-
-        :param state: Robot state
-        :type state: SE2State
-
-        :return: If state is valid
-        :rtype: bool
-        """
-        # Run bounds and state check
-        return self.setup.getSpaceInformation().satisfiesBounds(state)
-
-    def _validity_checker_with_fcl(self, state, **_) -> bool:
-        """
-        State validity checker method
-
-        :param state: Robot state
-        :type state: SE2State
-
-        :return: If state is valid
-        :rtype: bool
-        """
-        # Run bounds and state check
-        state_space_valid: bool = self.setup.getSpaceInformation().satisfiesBounds(
-            state
+        self._set_space_bounds(map_meta_data)
+        self._cpp_planner.setup_problem(
+            start_x=start_x,
+            start_y=start_y,
+            start_yaw=start_yaw,
+            goal_x=goal_x,
+            goal_y=goal_y,
+            goal_yaw=goal_yaw,
+            map_3d=map_3d,
         )
-        if self.fcl.got_map:
-            self.fcl.update_state(
-                RobotState(x=state.getX(), y=state.getY(), yaw=state.getYaw())
-            )
-            is_collision = self.fcl.check_collision()
-        else:
-            is_collision = False
-
-        return state_space_valid and not is_collision
 
     @property
     def planner_params(self) -> Optional[ompl.base.ParamSet]:
@@ -420,7 +245,7 @@ def planner_id(self) -> str:
         """
         return self._config.planner_id
 
-    def set_planner(
+    def _set_planner(
         self,
         planner_config: Optional[BaseAttrs] = None,
         planner_id: Optional[str] = None,
@@ -441,40 +266,32 @@ def set_planner(
             else:
                 self._config.planner_id = planner_id
 
-            self.planner = eval("%s(self.setup.getSpaceInformation())" % planner_id)
+            self.planner = eval(
+                "%s(self._ompl_setup.getSpaceInformation())" % planner_id
+            )
 
             if planner_config:
                 self.planner_params = planner_config
 
-            self.setup.setPlanner(self.planner)
+            self._ompl_setup.setPlanner(self.planner)
 
         except Exception:
             raise
 
-    def set_space_bounds(self, map_meta_data: Dict):
+    def _set_space_bounds(self, map_meta_data: Dict):
         """
         Set planning space bounds from map data
 
         :param map_meta_data: Map meta data
         :type map_meta_data: Dict
         """
-        # X-axis bounds
-        x_lower = map_meta_data["origin_x"]
-        x_upper = x_lower + map_meta_data["resolution"] * map_meta_data["width"]
-
-        # Y-axis bounds
-        y_lower = map_meta_data["origin_y"]
-        y_upper = y_lower + map_meta_data["resolution"] * map_meta_data["height"]
-
-        # set lower and upper bounds
-        bounds = base.RealVectorBounds(2)
-
-        bounds.setLow(index=0, value=x_lower)
-        bounds.setLow(index=1, value=y_lower)
-        bounds.setHigh(index=0, value=x_upper)
-        bounds.setHigh(index=1, value=y_upper)
-
-        self.state_space.setBounds(bounds)
+        self._cpp_planner.set_space_bounds_from_map(
+            origin_x=map_meta_data["origin_x"],
+            origin_y=map_meta_data["origin_y"],
+            width=map_meta_data["width"],
+            height=map_meta_data["height"],
+            resolution=map_meta_data["resolution"],
+        )
 
     def solve(self) -> ompl.geometric.PathGeometric:
         """
@@ -483,23 +300,9 @@ def solve(self) -> ompl.geometric.PathGeometric:
         :return: Solution (Path points) or None if no solution is found
         :rtype: ompl.geometric.PathGeometric
         """
-        solved = self.setup.solve(self._config.planning_timeout)
+        solved = self._cpp_planner.solve(self._config.planning_timeout)
 
         if solved:
-            self.solution = self.setup.getSolutionPath()
-
-            return self.solution
-        return None
-
-    def simplify_solution(self) -> ompl.geometric.PathGeometric:
-        """
-        Simplify the path
-
-        :return: Simplified path
-        :rtype: ompl.geometric.PathGeometric
-        """
-        if self.solution:
-            self.setup.simplifySolution(self._config.simplification_timeout)
-            self.solution = self.setup.getSolutionPath()
+            self.solution = self._cpp_planner.get_solution()
             return self.solution
         return None