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/interfaces.py

@@ -39,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.
 
@@ -62,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).")
 
@@ -76,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.
 
@@ -110,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

@@ -1,8 +1,8 @@
 import numpy as np
-from airo_typing import Vector3DType, JointPathType, JointConfigurationCheckerType
+from airo_typing import JointConfigurationCheckerType, JointPathType, Vector3DType
 from loguru import logger
 
-from airo_planner import create_simple_setup, state_to_ompl, NoPathFoundError, solve_and_smooth_path
+from airo_planner import NoPathFoundError, create_simple_setup, solve_and_smooth_path, state_to_ompl
 from airo_planner.interfaces import MobilePlatformPlanner
 
 
@@ -17,13 +17,11 @@ 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
 
-        joint_bounds = (
-            np.full(3, -np.inf),
-            np.full(3, np.inf),
-        )
+        # TODO: Allow user to set these bounds.
+        joint_bounds = (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)
 
     def plan_to_pose(self, start_pose: Vector3DType, goal_pose: Vector3DType) -> JointPathType: