use exceptions for runtime planning problems

airo_planner/__init__.py

@@ -3,6 +3,20 @@
 # but for building and many toolings, you still need to have __init__ files (at least in the root of the package).
 # e.g. if you remove this init file and try to build with pip install .
 # you won't be able to import the dummy module.
+from airo_planner.exceptions import (
+    AllGoalConfigurationsRemovedError,
+    GoalConfigurationOutOfBoundsError,
+    InvalidConfigurationError,
+    InvalidGoalConfigurationError,
+    InvalidStartConfigurationError,
+    JointOutOfBoundsError,
+    NoInverseKinematicsSolutionsError,
+    NoInverseKinematicsSolutionsWithinBoundsError,
+    NoPathFoundError,
+    NoValidInverseKinematicsSolutionsError,
+    PlannerError,
+    StartConfigurationOutOfBoundsError,
+)
 from airo_planner.joint_bounds import (
     JointBoundsType,
     concatenate_joint_bounds,
@@ -26,6 +40,7 @@
     solve_and_smooth_path,
 )
 
+
 from airo_planner.interfaces import DualArmPlanner, SingleArmPlanner  # isort:skip
 from airo_planner.ompl.single_arm_ompl_planner import SingleArmOmplPlanner  # isort:skip
 from airo_planner.ompl.dual_arm_ompl_planner import DualArmOmplPlanner  # isort:skip
@@ -52,4 +67,16 @@
     "rank_by_distance_to_desirable_configurations",
     "filter_with_distance_to_configurations",
     "stack_joints",
+    "PlannerError",
+    "NoPathFoundError",
+    "InvalidConfigurationError",
+    "InvalidStartConfigurationError",
+    "InvalidGoalConfigurationError",
+    "JointOutOfBoundsError",
+    "StartConfigurationOutOfBoundsError",
+    "GoalConfigurationOutOfBoundsError",
+    "NoInverseKinematicsSolutionsError",
+    "NoInverseKinematicsSolutionsWithinBoundsError",
+    "NoValidInverseKinematicsSolutionsError",
+    "AllGoalConfigurationsRemovedError",
 ]

airo_planner/exceptions.py

@@ -1,3 +1,8 @@
+from airo_typing import HomogeneousMatrixType, JointConfigurationType
+
+from airo_planner.joint_bounds import JointBoundsType
+
+
 class PlannerError(RuntimeError):
     """Base class for all things that can go wrong during planning.
     Mostly intended for problems that can happen, but are not per se
@@ -6,3 +11,150 @@ class PlannerError(RuntimeError):
     Note that not all planner problems need to be a subclass of this
     error, e.g. you can still use ValueError, AttributeError etc.
     """
+
+
+class NoPathFoundError(PlannerError):
+    """Raised when no path is found between configurations that are valid and
+    within bounds. This means either the underlying planning algorithm ran out
+    of time, is incomplete, or there is an obstacle in configuration space that
+    prevents connecting start and goal.
+
+    Note that this exception can also be raised with multiple goals to signal
+    that no path was found to any of the goals.
+    """
+
+    def __init__(
+        self,
+        start_configuration: JointConfigurationType,
+        goal_configurations: JointConfigurationType | list[JointConfigurationType] | None = None,
+        message: str | None = None,
+    ):
+        if message is None:
+            message_lines = [
+                "No path found between the start and goal configuration:",
+                f"Start configuration: {start_configuration}",
+            ]
+            if isinstance(goal_configurations, list):
+                message_lines.append("Goal configurations:")
+                for goal_configuration in goal_configurations:
+                    message_lines.append(f"  {goal_configuration}")
+            else:
+                message_lines.append(f"Goal configuration: {goal_configurations}")
+
+            message = "\n".join(message_lines)
+        super().__init__(message)
+
+
+class InvalidConfigurationError(PlannerError, ValueError):
+    """Base class for errors related to invalid configurations."""
+
+    def __init__(self, joint_configuration: JointConfigurationType, message: str | None = None):
+        if message is None:
+            message = f"The provided configuration is invalid: {joint_configuration}"
+        super().__init__(message)
+        self.joint_configuration = joint_configuration
+
+
+class InvalidStartConfigurationError(InvalidConfigurationError):
+    """Raised when the start configuration is invalid."""
+
+
+class InvalidGoalConfigurationError(InvalidConfigurationError):
+    """Raised when the goal configuration is invalid."""
+
+
+class JointOutOfBoundsError(PlannerError, ValueError):
+    """Raised when a joint configuration exceeds specified bounds."""
+
+    def __init__(
+        self, joint_configuration: JointConfigurationType, joint_bounds: JointBoundsType, message: str | None = None
+    ):
+        if message is None:
+            violations = self._find_violations(joint_configuration, joint_bounds)
+            message = self._generate_detailed_message(violations)
+
+        super().__init__(message)
+        self.joint_configuration = joint_configuration
+        self.joint_bounds = joint_bounds
+
+    def _find_violations(
+        self, joint_configuration: JointConfigurationType, joint_bounds: JointBoundsType
+    ) -> list[tuple[int, float, float, float]]:
+        violations = []
+        joints_lower, joints_upper = joint_bounds
+        for i, (value, lower, upper) in enumerate(zip(joint_configuration, joints_lower, joints_upper)):
+            if not (lower <= value <= upper):
+                violations.append((i, value, lower, upper))
+        return violations
+
+    def _generate_detailed_message(self, violations: list[tuple[int, float, float, float]]) -> str:
+        message_lines = ["Joint values exceed bounds:"]
+        for index, value, lower, upper in violations:
+            message_lines.append(f"  Joint {index}: Value {value} is outside the range [{lower}, {upper}]")
+        return "\n".join(message_lines)
+
+
+class StartConfigurationOutOfBoundsError(JointOutOfBoundsError):
+    """Raised when the start configuration is out of bounds."""
+
+
+class GoalConfigurationOutOfBoundsError(JointOutOfBoundsError):
+    """Raised when the goal configuration is out of bounds."""
+
+
+class NoInverseKinematicsSolutionsError(PlannerError):
+    """Raised when no IK solutions are found, can happen when the pose is
+    unreachable or the inverse kinematics function is incomplete."""
+
+    def __init__(self, tcp_pose: HomogeneousMatrixType, message: str | None = None):
+        if message is None:
+            message = f"No IK solutions found for the TCP pose:\n{tcp_pose}"
+        super().__init__(message)
+        self.tcp_pose = tcp_pose
+
+
+class NoInverseKinematicsSolutionsWithinBoundsError(PlannerError):
+    """Raised when all IK solutions are out of bounds."""
+
+    def __init__(
+        self, ik_solutions: list[JointConfigurationType], joint_bounds: JointBoundsType, message: str | None = None
+    ):
+        if message is None:
+            message_lines = [
+                "All inverse kinematics solutions are out of bounds:",
+                f"Lower bounds: {joint_bounds[0]}",
+                f"Upper bounds: {joint_bounds[1]}",
+            ]
+            message_lines.append("Solutions:")
+            for solution in ik_solutions:
+                message_lines.append(f"  {solution}")
+            message = "\n".join(message_lines)
+        super().__init__(message)
+        self.ik_solutions = ik_solutions
+
+
+class NoValidInverseKinematicsSolutionsError(PlannerError):
+    """Raised when no valid IK solutions are found."""
+
+    def __init__(self, ik_solutions: list[JointConfigurationType], message: str | None = None):
+        if message is None:
+            # message = f"All inverse kinematics solutions are invalid:\n{ik_solutions}"
+            message_lines = ["All inverse kinematics solutions are invalid:"]
+            for solution in ik_solutions:
+                message_lines.append(f"  {solution}")
+            message_lines.append("This can happen when your collision checking is not configured correctly.")
+            message = "\n".join(message_lines)
+        super().__init__(message)
+        self.ik_solutions = ik_solutions
+
+
+class AllGoalConfigurationsRemovedError(PlannerError):
+    """Raised when all IK solutions are removed by the goal configuration filter."""
+
+    def __init__(self, goal_configurations: list[JointConfigurationType], message: str | None = None):
+        if message is None:
+            message_lines = ["Valid goal configurations were found, but all were removed by the filter:"]
+            for goal_configuration in goal_configurations:
+                message_lines.append(f"  {goal_configuration}")
+            message = "\n".join(message_lines)
+        super().__init__(message)

airo_planner/interfaces.py

@@ -12,10 +12,8 @@ class SingleArmPlanner(abc.ABC):
     """
 
     def plan_to_joint_configuration(
-        self,
-        start_configuration: JointConfigurationType,
-        goal_configuration: JointConfigurationType,
-    ) -> JointPathType | None:
+        self, start_configuration: JointConfigurationType, goal_configuration: JointConfigurationType
+    ) -> JointPathType:
         """Plan a path from a start configuration to a goal configuration.
 
         Note that this path is not guarenteed to be dense, i.e. the distance
@@ -28,15 +26,17 @@ def plan_to_joint_configuration(
 
         Returns:
             A discretized path from the start configuration to the goal
-            configuration. If no solution could be found, then None is returned.
+            configuration.
+
+        Raises:
+            NoPathFoundError: If no path could be found between the start and
+            goal 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,
-    ) -> JointPathType | None:
+        self, start_configuration: JointConfigurationType, tcp_pose: HomogeneousMatrixType
+    ) -> JointPathType:
         raise NotImplementedError("This planner has not implemented planning to TCP poses (yet).")
 
 
@@ -54,7 +54,7 @@ def plan_to_joint_configuration(
         start_configuration_right: JointConfigurationType,
         goal_configuration_left: JointConfigurationType | None,
         goal_configuration_right: JointConfigurationType | None,
-    ) -> JointPathType | None:
+    ) -> JointPathType:
         """Plan a path from a start configurations to a goal configurations.
 
         The start cofinguration of the left and right arm must always be given.
@@ -75,6 +75,10 @@ def plan_to_joint_configuration(
             configuration. If the goal_configuration of an arm is None, then
             the start_configuration will simply be repeated in the path for that
             arm. If no solution could be found, then None is returned.
+
+        Raises:
+            NoPathFoundError: If no path could be found between the start and
+            goal configurations.
         """
         raise NotImplementedError("This planner has not implemented planning to joint configurations (yet).")
 
@@ -84,5 +88,5 @@ def plan_to_tcp_pose(
         start_configuration_right: JointConfigurationType,
         tcp_pose_left: HomogeneousMatrixType | None,
         tcp_pose_right: HomogeneousMatrixType | None,
-    ) -> JointPathType | None:
+    ) -> JointPathType:
         raise NotImplementedError("This planner has not implemented planning to TCP poses (yet).")

airo_planner/ompl/dual_arm_ompl_planner.py

@@ -10,6 +10,7 @@
 from airo_planner import (
     DualArmPlanner,
     JointBoundsType,
+    NoPathFoundError,
     SingleArmOmplPlanner,
     concatenate_joint_bounds,
     create_simple_setup,
@@ -121,7 +122,7 @@ def _plan_to_joint_configuration_dual_arm(
         start_configuration_right: JointConfigurationType,
         goal_configuration_left: JointConfigurationType,
         goal_configuration_right: JointConfigurationType,
-    ) -> JointPathType | None:
+    ) -> JointPathType:
 
         # Set start and goal
         simple_setup = self._simple_setup
@@ -132,14 +133,18 @@ def _plan_to_joint_configuration_dual_arm(
 
         # Run planning algorithm
         path = solve_and_smooth_path(simple_setup)
+
+        if path is None:
+            raise NoPathFoundError(start_configuration_left, goal_configuration_left)
+
         return path
 
     def _plan_to_joint_configuration_left_arm_only(
         self,
         start_configuration_left: JointConfigurationType,
         start_configuration_right: JointConfigurationType,
         goal_configuration_left: JointConfigurationType,
-    ) -> JointPathType | None:
+    ) -> JointPathType:
         # Set right goal to right start configuration
         self._set_start_and_goal_configurations(
             start_configuration_left, start_configuration_right, goal_configuration_left, start_configuration_right
@@ -162,7 +167,7 @@ def _plan_to_joint_configuration_right_arm_only(
         start_configuration_left: JointConfigurationType,
         start_configuration_right: JointConfigurationType,
         goal_configuration_right: JointConfigurationType,
-    ) -> JointPathType | None:
+    ) -> JointPathType:
         # Set left goal to left start configuration
         self._set_start_and_goal_configurations(
             start_configuration_left, start_configuration_right, start_configuration_left, goal_configuration_right
@@ -186,7 +191,7 @@ def plan_to_joint_configuration(
         start_configuration_right: JointConfigurationType,
         goal_configuration_left: JointConfigurationType | None,
         goal_configuration_right: JointConfigurationType | None,
-    ) -> JointPathType | None:
+    ) -> JointPathType:
         if goal_configuration_left is None and goal_configuration_right is None:
             raise ValueError("A goal configurations must be specified for at least one of the arms.")
 
@@ -216,18 +221,18 @@ def plan_to_tcp_pose(
         start_configuration_right: HomogeneousMatrixType,
         tcp_pose_left: HomogeneousMatrixType | None,
         tcp_pose_right: HomogeneousMatrixType | None,
-    ) -> JointPathType | None:
+    ) -> JointPathType:
         if tcp_pose_left is None and tcp_pose_right is None:
             raise ValueError("A goal TCP pose must be specified for at least one of the arms.")
-        return None
+        raise NoPathFoundError(start_configuration_left, start_configuration_right)  # this just temporary
 
     # def _plan_to_tcp_pose_left_arm_only(
     #     self,
     #     start_configuration_left: JointConfigurationType,
     #     start_configuration_right: JointConfigurationType,
     #     tcp_pose_left_in_base: HomogeneousMatrixType,
-    #     desirable_goal_configurations_left: List[JointConfigurationType] | None = None,
-    # ) -> List[Tuple[JointConfigurationType, JointConfigurationType]] | None:
+    #     desirable_goal_configurations_left: list[JointConfigurationType] | None = None,
+    # ) -> list[tuple[JointConfigurationType, JointConfigurationType]] | None:
     #     # Set right goal to right start configuration
     #     self._set_start_and_goal_configurations(
     #         start_configuration_left, start_configuration_right, start_configuration_left, start_configuration_right
@@ -248,8 +253,8 @@ def plan_to_tcp_pose(
     #     start_configuration_left: JointConfigurationType,
     #     start_configuration_right: JointConfigurationType,
     #     tcp_pose_right_in_base: HomogeneousMatrixType,
-    #     desirable_goal_configurations_right: List[JointConfigurationType] | None = None,
-    # ) -> List[Tuple[JointConfigurationType, JointConfigurationType]] | None:
+    #     desirable_goal_configurations_right: list[JointConfigurationType] | None = None,
+    # ) -> list[tuple[JointConfigurationType, JointConfigurationType]] | None:
     #     # Set left goal to left start configuration
     #     self._set_start_and_goal_configurations(
     #         start_configuration_left, start_configuration_right, start_configuration_left, start_configuration_right
@@ -271,7 +276,7 @@ def plan_to_tcp_pose(
     #     start_configuration_right: JointConfigurationType,
     #     tcp_pose_left_in_base: HomogeneousMatrixType,
     #     tcp_pose_right_in_base: HomogeneousMatrixType,
-    # ) -> List[Tuple[JointConfigurationType, JointConfigurationType]] | None:
+    # ) -> list[tuple[JointConfigurationType, JointConfigurationType]] | None:
     #     if self.inverse_kinematics_left_fn is None or self.inverse_kinematics_right_fn is None:
     #         logger.info(
     #             "Planning to left and right TCP poses attempted but inverse_kinematics_fn was not provided for both arms, returing None."
@@ -372,9 +377,9 @@ def plan_to_tcp_pose(
     #     start_configuration_right: JointConfigurationType,
     #     tcp_pose_left_in_base: HomogeneousMatrixType | None,
     #     tcp_pose_right_in_base: HomogeneousMatrixType | None,
-    #     desirable_goal_configurations_left: List[JointConfigurationType] | None = None,
-    #     desirable_goal_configurations_right: List[JointConfigurationType] | None = None,
-    # ) -> List[Tuple[JointConfigurationType, JointConfigurationType]] | None:
+    #     desirable_goal_configurations_left: list[JointConfigurationType] | None = None,
+    #     desirable_goal_configurations_right: list[JointConfigurationType] | None = None,
+    # ) -> list[tuple[JointConfigurationType, JointConfigurationType]] | None:
     #     if tcp_pose_left_in_base is None and tcp_pose_right_in_base is None:
     #         raise ValueError("A goal TCP pose must be specified for at least one of the arms.")