Merge pull request #132 from personalrobotics/feature/PlanningExceptions

Added more fine-grained planning exceptions.

Author: mkoval

src/prpy/planning/base.py

@@ -34,6 +34,7 @@
 import openravepy
 from ..clone import Clone
 from ..util import CopyTrajectory, GetTrajectoryTags, SetTrajectoryTags
+from .exceptions import PlanningError, UnsupportedPlanningError
 
 logger = logging.getLogger('planning')
 
@@ -47,14 +48,6 @@ class Tags(object):
     POSTPROCESS_TIME = 'postprocess_time'
     EXECUTION_TIME = 'execution_time'
 
-class PlanningError(Exception):
-    pass
-
-
-class UnsupportedPlanningError(PlanningError):
-    pass
-
-
 class MetaPlanningError(PlanningError):
     def __init__(self, message, errors):
         PlanningError.__init__(self, message)

src/prpy/planning/exceptions.py

@@ -0,0 +1,51 @@
+
+class PlanningError(Exception):
+    pass
+
+
+class UnsupportedPlanningError(PlanningError):
+    pass
+
+
+class ConstraintViolationPlanningError(PlanningError):
+    pass
+
+
+class CollisionPlanningError(PlanningError):
+    def __init__(self, link1, link2, base_message='Detected collision'):
+        self.link1 = link1
+        self.link2 = link2
+
+        super(CollisionPlanningError, self).__init__(
+            '{:s}: {:s} x {:s}.'.format(
+                base_message,
+                self._get_link_str(link1),
+                self._get_link_str(link2)
+            )
+        )
+
+    @classmethod
+    def FromReport(cls, report):
+        return cls(report.plink1, report.plink2)
+
+    @staticmethod
+    def _get_link_str(link):
+        if link is not None:
+            return '<{:s}, {:s}>'.format(
+                link.GetParent().GetName(), link.GetName())
+        else:
+            return '<unknown>'
+
+
+class SelfCollisionPlanningError(CollisionPlanningError):
+    pass
+
+
+class TimeoutPlanningError(PlanningError):
+    def __init__(self, timelimit=None):
+        if timelimit is not None:
+            message = 'Exceeded {:.3f} s time limit.'.format(timelimit)
+        else:
+            message = 'Exceeded time limit.'
+
+        super(TimeoutPlanningError, self).__init__(message)

src/prpy/planning/vectorfield.py

@@ -160,15 +160,19 @@ def TerminateMove():
             Succeed if distance moved is larger than max_distance.
             Cache and continue if distance moved is larger than distance.
             '''
+            from .exceptions import ConstraintViolationPlanningError 
+
             Tnow = manip.GetEndEffectorTransform()
             error = util.GeodesicError(Tstart, Tnow)
             if numpy.fabs(error[3]) > angular_tolerance:
-                raise PlanningError('Deviated from orientation constraint.')
+                raise ConstraintViolationPlanningError(
+                    'Deviated from orientation constraint.')
             distance_moved = numpy.dot(error[0:3], direction)
             position_deviation = numpy.linalg.norm(error[0:3] -
                                                    distance_moved*direction)
             if position_deviation > position_tolerance:
-                raise PlanningError('Deviated from straight line constraint.')
+                raise ConstraintViolationPlanningError(
+                    'Deviated from straight line constraint.')
 
             if distance_moved > max_distance:
                 return Status.TERMINATE
@@ -198,6 +202,12 @@ def FollowVectorField(self, robot, fn_vectorfield, fn_terminate,
         @param kw_args keyword arguments to be passed to fn_vectorfield
         @return traj
         """
+        from .exceptions import (
+            CollisionPlanningError,
+            SelfCollisionPlanningError,
+            TimeoutPlanningError
+        )
+
         start_time = time.time()
 
         try:
@@ -219,28 +229,30 @@ def FollowVectorField(self, robot, fn_vectorfield, fn_terminate,
                               vlimits)
                 dt_step *= dt_multiplier
                 status = fn_terminate()
+                report = openravepy.CollisionReport()
+
                 while status != Status.TERMINATE:
                     # Check for a timeout.
                     current_time = time.time()
                     if (timelimit is not None and
                             current_time - start_time > timelimit):
-                        raise PlanningError('Reached time limit.')
+                        raise TimeoutPlanningError(timelimit)
 
                     dqout = fn_vectorfield()
                     numsteps = int(math.floor(max(
                         abs(dqout*dt_step/robot.GetActiveDOFResolutions())
                         )))
                     if numsteps == 0:
-                        raise PlanningError('Step size too small, '
-                                            'unable to progress')
+                        raise PlanningError('Step size too small,'
+                                            ' unable to progress')
                     dt = dt_step/numsteps
 
                     for step in xrange(numsteps):
                         # Check for collisions.
-                        if self.env.CheckCollision(robot):
-                            raise PlanningError('Encountered collision.')
-                        if robot.CheckSelfCollision():
-                            raise PlanningError('Encountered self-collision.')
+                        if self.env.CheckCollision(robot, report):
+                            raise CollisionPlanningError.FromReport(report)
+                        if robot.CheckSelfCollision(report):
+                            raise SelfCollisionPlanningError.FromReport(report)
 
                         status = fn_terminate()
                         if status == Status.CACHE_AND_CONTINUE:

src/prpy/planning/workspace.py

@@ -152,6 +152,7 @@ def PlanWorkspacePath(self, robot, traj, timelimit=5.0,
         @param timelimit timeout in seconds
         @return qtraj configuration space path
         """
+        from .exceptions import TimeoutPlanningError
 
         with robot:
             manip = robot.GetActiveManipulator()
@@ -179,7 +180,7 @@ def PlanWorkspacePath(self, robot, traj, timelimit=5.0,
                     current_time = time.time()
                     if (timelimit is not None and
                             current_time - start_time > timelimit):
-                        raise PlanningError('Reached time limit.')
+                        raise TimeoutPlanningError(timelimit)
 
                     # Hypothesize new configuration as closest IK to current
                     qcurr = robot.GetActiveDOFValues()  # Configuration at t.