Add pip install note for ubuntu 22.04+ to README

README.md

@@ -49,6 +49,11 @@ We depend on `ompl` with its Python bindings, which are not available on PyPI ye
 pip install https://github.com/ompl/ompl/releases/download/prerelease/ompl-1.6.0-cp310-cp310-manylinux_2_28_x86_64.whl
 ```
 
+For Ubuntu 22.04, use a different wheel:
+```
+pip install https://github.com/ompl/ompl/releases/download/prerelease/ompl-1.6.0-cp311-cp311-manylinux_2_28_x86_64.whl
+```
+
 ## Developer guide 🛠️
 See the [`airo-mono`](https://github.com/airo-ugent/airo-mono) developer guide.
 A very similar process and tools are used for this package.

airo_planner/interfaces.py

@@ -1,6 +1,33 @@
 import abc
 
-from airo_typing import HomogeneousMatrixType, JointConfigurationType, JointPathType
+from airo_typing import HomogeneousMatrixType, JointConfigurationType, JointPathType, Vector3DType
+
+
+# TODO: plan based on wheel configs as well or instead?
+class MobilePlatformPlanner(abc.ABC):
+    """Base class that defines an interface for mobile platform motion planners,
+    where the platform is represented as an unbounded planar joint.
+
+    The idea is that the custom settings for each motion planner are provided
+    through the constructor. After creation all motion planners can then be
+    used in the same way, e.g. for benchmarking.
+    """
+
+    @abc.abstractmethod
+    # TODO: Attitude2DType?
+    def plan_to_pose(self, start_pose: Vector3DType, goal_pose: Vector3DType) -> JointPathType:
+        """Plan a path from a start pose to a goal pose.
+
+        Args:
+            start_pose: The (x, y, theta) start pose.
+            goal_pose: The (x, y, theta) goal pose.
+
+        Returns:
+            A discretized path from the start pose to the goal pose.
+
+        Raises:
+            NoPathFoundError: If no path could be found between the start and
+            goal configuration."""
 
 
 class SingleArmPlanner(abc.ABC):
@@ -12,7 +39,7 @@ class SingleArmPlanner(abc.ABC):
     """
 
     def plan_to_joint_configuration(
-        self, start_configuration: JointConfigurationType, goal_configuration: JointConfigurationType
+            self, start_configuration: JointConfigurationType, goal_configuration: JointConfigurationType
     ) -> JointPathType:
         """Plan a path from a start configuration to a goal configuration.
 
@@ -35,7 +62,7 @@ def plan_to_joint_configuration(
         raise NotImplementedError("This planner has not implemented planning to joint configurations (yet).")
 
     def plan_to_tcp_pose(
-        self, start_configuration: JointConfigurationType, tcp_pose: HomogeneousMatrixType
+            self, start_configuration: JointConfigurationType, tcp_pose: HomogeneousMatrixType
     ) -> JointPathType:
         raise NotImplementedError("This planner has not implemented planning to TCP poses (yet).")
 
@@ -49,11 +76,11 @@ class DualArmPlanner(abc.ABC):
     """
 
     def plan_to_joint_configuration(
-        self,
-        start_configuration_left: JointConfigurationType,
-        start_configuration_right: JointConfigurationType,
-        goal_configuration_left: JointConfigurationType | None,
-        goal_configuration_right: JointConfigurationType | None,
+            self,
+            start_configuration_left: JointConfigurationType,
+            start_configuration_right: JointConfigurationType,
+            goal_configuration_left: JointConfigurationType | None,
+            goal_configuration_right: JointConfigurationType | None,
     ) -> JointPathType:
         """Plan a path from a start configurations to a goal configurations.
 
@@ -83,10 +110,10 @@ def plan_to_joint_configuration(
         raise NotImplementedError("This planner has not implemented planning to joint configurations (yet).")
 
     def plan_to_tcp_pose(
-        self,
-        start_configuration_left: JointConfigurationType,
-        start_configuration_right: JointConfigurationType,
-        tcp_pose_left: HomogeneousMatrixType | None,
-        tcp_pose_right: HomogeneousMatrixType | None,
+            self,
+            start_configuration_left: JointConfigurationType,
+            start_configuration_right: JointConfigurationType,
+            tcp_pose_left: HomogeneousMatrixType | None,
+            tcp_pose_right: HomogeneousMatrixType | None,
     ) -> JointPathType:
         raise NotImplementedError("This planner has not implemented planning to TCP poses (yet).")

airo_planner/ompl/mobile_platform_ompl_planner.py

@@ -0,0 +1,44 @@
+import numpy as np
+from airo_typing import Vector3DType, JointPathType, JointConfigurationCheckerType
+from loguru import logger
+
+from airo_planner import create_simple_setup, state_to_ompl, NoPathFoundError, solve_and_smooth_path
+from airo_planner.interfaces import MobilePlatformPlanner
+
+
+class MobilePlatformOmplPlanner(MobilePlatformPlanner):
+    """Utility class for mobile platform motion planning using OMPL.
+
+    The purpose of this class is to make working with OMPL easier. It basically
+    just handles the creation of OMPL objects and the conversion between numpy
+    arrays and OMPL states and paths. After creating an instance of this class,
+    you can also extract the SimpleSetup object and use it directly if you want.
+    This can be useful for benchmarking with the OMPL benchmarking tools.
+    """
+
+    def __init__(self, is_state_valid_fn: JointConfigurationCheckerType):
+        # TODO: JointConfigurationCheckerType is not ideal here, should be a new type.
+        self.is_state_valid_fn = is_state_valid_fn
+
+        joint_bounds = (
+            np.full(3, -np.inf),
+            np.full(3, np.inf),
+        )
+        self._simple_setup = create_simple_setup(self.is_state_valid_fn, joint_bounds)
+
+    def plan_to_pose(self, start_pose: Vector3DType, goal_pose: Vector3DType) -> JointPathType:
+        self._simple_setup.clear()
+
+        space = self._simple_setup.getStateSpace()
+        start_state = state_to_ompl(start_pose, space)
+        goal_state = state_to_ompl(goal_pose, space)
+        self._simple_setup.setStartAndGoalStates(start_state, goal_state)
+
+        path = solve_and_smooth_path(self._simple_setup)
+
+        if path is None:
+            raise NoPathFoundError(start_pose, goal_pose)
+
+        logger.success(f"Successfully found path (with {len(path)} waypoints).")
+
+        return path