Add mobile platform planning and notebook

airo_planner/__init__.py

@@ -55,20 +55,23 @@
     convert_dual_arm_path_chooser_to_single_arm,
 )
 
-from airo_planner.interfaces import DualArmPlanner, SingleArmPlanner
+from airo_planner.interfaces import DualArmPlanner, SingleArmPlanner, MobilePlatformPlanner
 
 from airo_planner.ompl.single_arm_ompl_planner import SingleArmOmplPlanner
 from airo_planner.ompl.dual_arm_ompl_planner import DualArmOmplPlanner
+from airo_planner.ompl.mobile_platform_ompl_planner import MobilePlatformOmplPlanner
 
 
 __all__ = [
     "SingleArmPlanner",
     "DualArmPlanner",
+    "MobilePlatformPlanner",
     "MultipleGoalPlanner",
     "JointConfigurationsModifierType",
     "JointPathChooserType",
     "SingleArmOmplPlanner",
     "DualArmOmplPlanner",
+    "MobilePlatformOmplPlanner",
     "JointBoundsType",
     "is_within_bounds",
     "uniform_symmetric_joint_bounds",

airo_planner/ompl/mobile_platform_ompl_planner.py

@@ -17,12 +17,13 @@ class MobilePlatformOmplPlanner(MobilePlatformPlanner):
     """
 
     def __init__(self, is_state_valid_fn: JointConfigurationCheckerType):
-        # TODO: JointConfigurationCheckerType is not ideal here, should be a new type.
+        # TODO: JointConfigurationCheckerType is not ideal here, should be a new type or rename this type?
         self.is_state_valid_fn = is_state_valid_fn
 
+        # TODO: Allow user to set these bounds.
         joint_bounds = (
-            np.full(3, -np.inf),
-            np.full(3, np.inf),
+            np.array([-10.0, -10.0, -4 * np.pi]),
+            np.array([10.0, 10.0, 4 * np.pi])
         )
         self._simple_setup = create_simple_setup(self.is_state_valid_fn, joint_bounds)