Merge branch 'master' of https://github.com/personalrobotics/prpy into bugfix/cloning_race

Conflicts:
	src/prpy/planning/base.py

Author: psigen

CHANGELOG.rst

@@ -2,6 +2,13 @@
 Changelog for package prpy
 ^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+1.1.0 (2015-06-01)
+------------------
+* Adding tags for capturing trajectory timing data
+* Update README.md
+  Added enum34 dependency instructions into README
+* Contributors: Jennifer King, Michael Koval, Stefanos Nikolaidis
+
 1.0.0 (2015-05-01)
 ------------------
 * Adding planner and planning_method and trajectory tag constants

package.xml

@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package>
   <name>prpy</name>
-  <version>1.0.0</version>
+  <version>1.1.0</version>
   <description>
     Python utilities used by the Personal Robotics Laboratory.
   </description>

src/prpy/base/robot.py

@@ -33,10 +33,11 @@
 from .. import bind, named_config, planning, util
 from ..clone import Clone, Cloned
 from ..tsr.tsrlibrary import TSRLibrary
-from ..planning.base import Sequence 
+from ..planning.base import Sequence, Tags
 from ..planning.ompl import OMPLSimplifier
 from ..planning.retimer import HauserParabolicSmoother, OpenRAVEAffineRetimer, ParabolicRetimer
 from ..planning.mac_smoother import MacSmoother
+from ..util import SetTrajectoryTags
 
 logger = logging.getLogger('robot')
 
@@ -377,6 +378,7 @@ def do_execute():
 
             with Timer() as timer:
                 traj = self.PostProcessPath(path, defer=False, **kwargs)
+            SetTrajectoryTags(traj, {Tags.POSTPROCESS_TIME: timer.get_duration()}, append=True)
 
             logger.info('Post-processing took %.3f seconds and produced a path'
                         ' with %d waypoints and a duration of %.3f seconds.',
@@ -385,7 +387,10 @@ def do_execute():
                 traj.GetDuration()
             )
 
-            return self.ExecuteTrajectory(traj, defer=False, **kwargs)
+            with Timer() as timer:
+                exec_traj = self.ExecuteTrajectory(traj, defer=False, **kwargs)
+            SetTrajectoryTags(exec_traj, {Tags.EXECUTION_TIME: timer.get_duration()}, append=True)
+            return exec_traj
 
         if defer is True:
             from trollius.executor import get_default_executor
@@ -541,7 +546,10 @@ def _PlanWrapper(self, planning_method, args, kw_args):
         config_spec = self.GetActiveConfigurationSpecification('linear')
 
         # Call the planner.
-        result = planning_method(self, *args, **kw_args)
+        from ..util import Timer
+        with Timer() as timer:
+            result = planning_method(self, *args, **kw_args)
+        SetTrajectoryTags(result, {Tags.PLAN_TIME: timer.get_duration()}, append=True)
 
         def postprocess_trajectory(traj):
             # Strip inactive DOFs from the trajectory.

src/prpy/planning/base.py

@@ -35,6 +35,7 @@
 import openravepy
 from ..clone import Clone
 from ..util import CopyTrajectory, GetTrajectoryTags, SetTrajectoryTags
+from .exceptions import PlanningError, UnsupportedPlanningError
 
 logger = logging.getLogger('planning')
 
@@ -44,15 +45,9 @@ class Tags(object):
     CONSTRAINED = 'constrained'
     PLANNER = 'planner'
     METHOD = 'planning_method'
-
-
-class PlanningError(Exception):
-    pass
-
-
-class UnsupportedPlanningError(PlanningError):
-    pass
-
+    PLAN_TIME = 'planning_time'
+    POSTPROCESS_TIME = 'postprocess_time'
+    EXECUTION_TIME = 'execution_time'
 
 class MetaPlanningError(PlanningError):
     def __init__(self, message, errors):

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

@@ -140,13 +140,20 @@ def __init__(self, **kwargs):
 
 
 class HauserParabolicSmoother(OpenRAVERetimer):
-    def __init__(self, **kwargs):
+    def __init__(self, do_blend=True, do_shortcut=True, blend_radius=0.5,
+                 blend_iterations=4, **kwargs):
         super(HauserParabolicSmoother, self).__init__(
                 'HauserParabolicSmoother', **kwargs)
 
+        self.default_options.update({
+            'do_blend': int(do_blend),
+            'do_shortcut': int(do_shortcut),
+            'blend_radius': float(blend_radius),
+            'blend_iterations': int(blend_iterations),
+        })
 
-class OpenRAVEAffineRetimer(BasePlanner):
 
+class OpenRAVEAffineRetimer(BasePlanner):
     def __init__(self,):
         super(OpenRAVEAffineRetimer, self).__init__()
 

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.