Added separate LockedPlanningMethod and ClonedPlanningMethod calls.

This separates the @PlanningMethod decorator into two decorators:

- @LockedPlanningMethod locks the current robot environment and
  executes the wrapped method.
- @ClonedPlanningMethod clones the current robot environment and
  executes the wrapped method.

This distinction is useful for planning calls that are so fast that
they take less time than the cloning operation itself, such as
optimistic plans like `SnapPlanner` and fast mathematical solutions
such as `KunzCircularBlender`.

This commit refactors all existing planning calls except `SnapPlanner`
to use `@ClonedPlanningMethod` and updates the README to explain
the distinction.  `@PlanningMethod` is changed to inherit from
`@ClonedPlanningMethod` but print a warning if it is used.

Author: psigen

README.md

@@ -25,12 +25,16 @@ two subclasses:
 2. `prpy.planning.base.MetaPlanner`: combines the output of multiple motion
    planners, each of which is a `BasePlanner` or another `MetaPlanner`
 
-Each planner has one or more *planning methods*, annotated with the
-`@PlanningMethod` decorator, that look like ordinary functions. However, unlike
-an ordinary function, calling a planning method clones the robot's environment
+Each planner has one or more *planning methods*, annotated with either the
+`@LockedPlanningMethod` or `@ClonedPlanningMethod decorator, that look like
+ordinary functions. However, unlike an ordinary function, calling a planning
+method either locks the robot environment or clones the robot's environment
 into a *planning environment* associated with the planner. Planning occurs in
-the cloned environment to allow PrPy to run multiple planners in parallel and
-to paralellize planning and execution.
+the locked or cloned environment to allow PrPy to run multiple planners in
+parallel and to paralellize planning and execution.  In general, locked
+planning methods are used for calls that will terminate extremely quickly,
+while cloned planning methods are used for calls that might take a significant
+amount of time.
 
 For example, the following code will use OMPL to plan `robot`'s active DOFs
 from their current values to to the `goal_config` configuration:
@@ -40,10 +44,10 @@ planner = OMPLPlanner('RRTConnect')
 output_path = planner.PlanToConfiguration(robot, goal_config)
 ```
 
-First, `robot.GetEnv()` is cloned into the the `planner.env` planning
-environment. Next, planning occurs in the cloned environment. Finally, the
-output path is cloned back into `robot.GetEnv()` and is returned by the
-planner.
+As this is a `@ClonedPlanningMethod`, `robot.GetEnv()` is cloned into the
+the `planner.env` planning environment. Planning occurs within this cloned
+environment. Finally, the output path is cloned back into `robot.GetEnv()`
+and is returned by the planner.
 
 See the following sub-sections for more information about the built-in planners
 provided with PrPy, information about writing your own planner, and several
@@ -86,7 +90,7 @@ See the Python docstrings the above classes for more information.
 
 ### Common Planning Methods
 
-There is no formal list of `@PlanningMethod`s or their arguments. However, we
+There is no formal list of `@*PlanningMethod`s or their arguments. However, we
 have found these methods to be useful:
 
 - `PlanToConfiguration(robot, goal_config)`: plan the robot's active DOFs from
@@ -112,25 +116,25 @@ of these methods accept planner-specific keyword arguments.
 ### Writing a Custom Planner
 
 Implementing a custom planner requires extending the `BasePlanner` class and
-decorating one or more methods with the `@PlanningMethod` decorator. Extending
-the `BasePlanner` class constructs the planning environment `self.env` and
-allows PrPy to identify your planner as a base planner class, as opposed to a
-meta-planner. The `@PlanningMethod` decorator handles environment cloning and
-allows meta-planners to query the list of planning methods that the planner
-supports (e.g. to generate docstrings).
+decorating one or more methods with the `@LockedPlanningMethod` or
+`@ClonedPlanningMethod` decorator. Extending the `BasePlanner` class constructs
+the planning environment `self.env` and allows PrPy to identify your planner
+as a base planner class, as opposed to a meta-planner. The `@*PlanningMethod`
+decorators handle environment cloning or locking and allows meta-planners to
+query the list of planning methods that the planner supports (e.g. to generate
+docstrings).
 
 Please obey the following guidelines:
 
-- Assume that the cloned environment is locked during the entire call.
-- Subclass constructor **must** call `BasePlanner.__init__`.
-- A `@PlanningMethod` **must not** call another `@PlanningMethod`.
-- Each `@PlanningMethod` **must** accept the first argument `robot`, which is a
+- Assume that the environment is locked during the entire call.
+- Subclass constructors **must** call `BasePlanner.__init__`.
+- Each `@*PlanningMethod` **must** accept the first argument `robot`, which is a
   robot in the cloned environment.
-- Each `@PlanningMethod` **must** accept `**kwargs` to ignore arguments that
+- Each `@*PlanningMethod` **must** accept `**kwargs` to ignore arguments that
   are not supported by the planner.
-- Each `@PlanningMethod` **must** return a `Trajectory` which was created in
+- Each `@*PlanningMethod` **must** return a `Trajectory` which was created in
   the cloned environment.
-- When possible, use one of the defacto-standard `@PlanningMethod` names listed
+- When possible, use one of the defacto-standard `@*PlanningMethod` names listed
   below.
 - Raise a `PlanningError` to indicate an expected, but fatal, error (e.g.
   timeout with no collision-free path).

src/prpy/planning/base.py

@@ -91,10 +91,41 @@ def __init__(self, message, errors):
     # TODO: Print the inner exceptions.
 
 
-class PlanningMethod(object):
+class LockedPlanningMethod(object):
+    """
+    Decorate a planning method that locks the calling environment.
+    """
     def __init__(self, func):
         self.func = func
 
+    def __call__(self, instance, robot, *args, **kw_args):
+        with robot.GetEnv():
+            # Perform the actual planning operation.
+            traj = self.func(instance, robot, *args, **kw_args)
+
+            # Tag the trajectory with the planner and planning method
+            # used to generate it. We don't overwrite these tags if
+            # they already exist.
+            tags = GetTrajectoryTags(traj)
+            tags.setdefault(Tags.PLANNER, instance.__class__.__name__)
+            tags.setdefault(Tags.METHOD, self.func.__name__)
+            SetTrajectoryTags(traj, tags, append=False)
+
+            return traj
+
+    def __get__(self, instance, instancetype):
+        # Bind the self reference and use update_wrapper to propagate the
+        # function's metadata (e.g. name and docstring).
+        wrapper = functools.partial(self.__call__, instance)
+        functools.update_wrapper(wrapper, self.func)
+        wrapper.is_planning_method = True
+        return wrapper
+
+
+class ClonedPlanningMethod(LockedPlanningMethod):
+    """
+    Decorate a planning method that clones the calling environment.
+    """
     def __call__(self, instance, robot, *args, **kw_args):
         env = robot.GetEnv()
 
@@ -110,10 +141,11 @@ def __call__(self, instance, robot, *args, **kw_args):
             joint_values[1] = cloned_robot.GetActiveDOFValues()
 
             # Check for mismatches in the cloning and hackily reset them.
-            # (This is due to a possible bug in OpenRAVE environment cloning where in
-            # certain situations, the Active DOF ordering and values do not match the
-            # parent environment.  It seems to be exacerbated by multirotation joints,
-            # but the exact cause and repeatability is unclear at this point.)
+            # (This is due to a possible bug in OpenRAVE environment cloning
+            # where in certain situations, the Active DOF ordering and values
+            # do not match the parent environment.  It seems to be exacerbated
+            # by multirotation joints, but the exact cause and repeatability
+            # is unclear at this point.)
             if not numpy.array_equal(joint_indices[0], joint_indices[1]):
                 logger.warning("Cloned Active DOF index mismatch: %s != %s",
                                str(joint_indices[0]),
@@ -126,27 +158,16 @@ def __call__(self, instance, robot, *args, **kw_args):
                                str(joint_values[1]))
                 cloned_robot.SetActiveDOFValues(joint_values[0])
 
-            # Perform the actual planning operation.
-            planner_traj = self.func(instance, cloned_robot,
-                                     *args, **kw_args)
+            traj = super(ClonedPlanningMethod, self).__call__(
+                instance, cloned_robot, *args, **kw_args)
+            return CopyTrajectory(traj, env=env)
 
-            # Tag the trajectory with the planner and planning method
-            # used to generate it. We don't overwrite these tags if
-            # they already exist.
-            tags = GetTrajectoryTags(planner_traj)
-            tags.setdefault(Tags.PLANNER, instance.__class__.__name__)
-            tags.setdefault(Tags.METHOD, self.func.__name__)
-            SetTrajectoryTags(planner_traj, tags, append=False)
 
-            return CopyTrajectory(planner_traj, env=env)
-
-    def __get__(self, instance, instancetype):
-        # Bind the self reference and use update_wrapper to propagate the
-        # function's metadata (e.g. name and docstring).
-        wrapper = functools.partial(self.__call__, instance)
-        functools.update_wrapper(wrapper, self.func)
-        wrapper.is_planning_method = True
-        return wrapper
+class PlanningMethod(ClonedPlanningMethod):
+    def __init__(self, func):
+        logger.warn("Please explicitly declare a ClonedPlanningMethod "
+                    "instead of using PlanningMethod.")
+        super(ClonedPlanningMethod, self).__init__(func)
 
 
 class Planner(object):

src/prpy/planning/cbirrt.py

@@ -6,7 +6,7 @@
 #
 # Redistribution and use in source and binary forms, with or without
 # modification, are permitted provided that the following conditions are met:
-# 
+#
 # - Redistributions of source code must retain the above copyright notice, this
 #   list of conditions and the following disclaimer.
 # - Redistributions in binary form must reproduce the above copyright notice,
@@ -32,7 +32,7 @@
 import openravepy
 from ..util import SetTrajectoryTags
 from base import (BasePlanner, PlanningError, UnsupportedPlanningError,
-                  PlanningMethod, Tags)
+                  ClonedPlanningMethod, Tags)
 import prpy.kin
 import prpy.tsr
 
@@ -48,7 +48,7 @@ def __init__(self):
     def __str__(self):
         return 'CBiRRT'
 
-    @PlanningMethod
+    @ClonedPlanningMethod
     def PlanToConfigurations(self, robot, goals, **kw_args):
         """
         Plan to multiple goal configurations with CBiRRT. This adds each goal
@@ -61,7 +61,7 @@ def PlanToConfigurations(self, robot, goals, **kw_args):
         kw_args.setdefault('smoothingitrs', 0)
         return self.Plan(robot, jointgoals=goals, **kw_args)
 
-    @PlanningMethod
+    @ClonedPlanningMethod
     def PlanToConfiguration(self, robot, goal, **kw_args):
         """
         Plan to a single goal configuration with CBiRRT.
@@ -72,7 +72,7 @@ def PlanToConfiguration(self, robot, goal, **kw_args):
         kw_args.setdefault('smoothingitrs', 0)
         return self.Plan(robot, jointgoals=[goal], **kw_args)
 
-    @PlanningMethod
+    @ClonedPlanningMethod
     def PlanToEndEffectorPose(self, robot, goal_pose, **kw_args):
         """
         Plan to a desired end-effector pose.
@@ -90,7 +90,7 @@ def PlanToEndEffectorPose(self, robot, goal_pose, **kw_args):
 
         return self.Plan(robot, tsr_chains=[tsr_chain], **kw_args)
 
-    @PlanningMethod
+    @ClonedPlanningMethod
     def PlanToEndEffectorOffset(self, robot, direction, distance,
                                 smoothingitrs=100, **kw_args):
         """
@@ -153,7 +153,7 @@ def PlanToEndEffectorOffset(self, robot, direction, distance,
             **kw_args
         )
 
-    @PlanningMethod
+    @ClonedPlanningMethod
     def PlanToTSR(self, robot, tsr_chains, smoothingitrs=100, **kw_args):
         """
         Plan to a goal specified as a list of TSR chains. CBiRRT supports an

src/prpy/planning/chomp.py

@@ -32,10 +32,9 @@
 import logging
 import numpy
 import openravepy
-from .. import tsr
 from ..util import SetTrajectoryTags
 from base import (BasePlanner, PlanningError, UnsupportedPlanningError,
-                  PlanningMethod, Tags)
+                  ClonedPlanningMethod, Tags)
 import prpy.tsr
 
 logger = logging.getLogger(__name__)
@@ -192,7 +191,7 @@ def ComputeDistanceField(self, robot):
         logger.warning('ComputeDistanceField is deprecated. Distance fields are'
                        ' now implicity created by DistanceFieldManager.')
 
-    @PlanningMethod
+    @ClonedPlanningMethod
     def OptimizeTrajectory(self, robot, traj, lambda_=100.0, n_iter=50,
                            **kw_args):
         self.distance_fields.sync(robot)
@@ -215,7 +214,7 @@ def OptimizeTrajectory(self, robot, traj, lambda_=100.0, n_iter=50,
         SetTrajectoryTags(traj, {Tags.SMOOTH: True}, append=True)
         return traj
 
-    @PlanningMethod
+    @ClonedPlanningMethod
     def PlanToConfiguration(self, robot, goal, lambda_=100.0, n_iter=15,
                             **kw_args):
         """
@@ -237,7 +236,7 @@ def PlanToConfiguration(self, robot, goal, lambda_=100.0, n_iter=15,
         SetTrajectoryTags(traj, {Tags.SMOOTH: True}, append=True)
         return traj
 
-    @PlanningMethod
+    @ClonedPlanningMethod
     def PlanToEndEffectorPose(self, robot, goal_pose, lambda_=100.0,
                               n_iter=100, goal_tolerance=0.01, **kw_args):
         """
@@ -293,7 +292,7 @@ def PlanToEndEffectorPose(self, robot, goal_pose, lambda_=100.0,
 
     # JK - Disabling. This is not working reliably.
     '''
-    @PlanningMethod
+    @ClonedPlanningMethod
     def PlanToTSR(self, robot, tsrchains, lambda_=100.0, n_iter=100,
                   goal_tolerance=0.01, **kw_args):
         """

src/prpy/planning/ik.py

@@ -33,7 +33,7 @@
 from .. import ik_ranking
 from base import (BasePlanner,
                   PlanningError,
-                  PlanningMethod)
+                  ClonedPlanningMethod)
 
 logger = logging.getLogger(__name__)
 
@@ -46,7 +46,7 @@ def __init__(self, delegate_planner=None):
     def __str__(self):
         return 'IKPlanner'
 
-    @PlanningMethod
+    @ClonedPlanningMethod
     def PlanToIK(self, robot, goal_pose, ranker=ik_ranking.JointLimitAvoidance,
                  num_attempts=1, **kw_args):
         from openravepy import (IkFilterOptions,

src/prpy/planning/mac_smoother.py

@@ -30,7 +30,7 @@
 
 from ..util import (CopyTrajectory, SimplifyTrajectory, SetTrajectoryTags,
                     IsTimedTrajectory)
-from base import (BasePlanner, PlanningError, PlanningMethod,
+from base import (BasePlanner, PlanningError, ClonedPlanningMethod,
                   UnsupportedPlanningError, Tags)
 from openravepy import (Planner, PlannerStatus, RaveCreatePlanner,
                         openrave_exception)
@@ -54,7 +54,7 @@ def __init__(self):
                 ' installed and in your OPENRAVE_PLUGIN path?'
             )
 
-    @PlanningMethod
+    @ClonedPlanningMethod
     def RetimeTrajectory(self, robot, path):
         # Copy the input trajectory into the planning environment. This is
         # necessary for two reasons: (1) the input trajectory may be in another
@@ -72,8 +72,8 @@ def RetimeTrajectory(self, robot, path):
             params = Planner.PlannerParameters()
             self.blender.InitPlan(robot, params)
             status = self.blender.PlanPath(output_traj)
-            if status not in [ PlannerStatus.HasSolution,
-                               PlannerStatus.InterruptedWithSolution ]:
+            if status not in [PlannerStatus.HasSolution,
+                              PlannerStatus.InterruptedWithSolution]:
                 raise PlanningError('Blending trajectory failed.')
         except openrave_exception as e:
             raise PlanningError('Blending trajectory failed: ' + str(e))
@@ -84,12 +84,11 @@ def RetimeTrajectory(self, robot, path):
             params = Planner.PlannerParameters()
             self.retimer.InitPlan(robot, params)
             status = self.retimer.PlanPath(output_traj)
-            if status not in [ PlannerStatus.HasSolution,
-                               PlannerStatus.InterruptedWithSolution ]:
+            if status not in [PlannerStatus.HasSolution,
+                              PlannerStatus.InterruptedWithSolution]:
                 raise PlanningError('Timing trajectory failed.')
         except openrave_exception as e:
             raise PlanningError('Timing trajectory failed: ' + str(e))
 
         SetTrajectoryTags(output_traj, {Tags.SMOOTH: True}, append=True)
         return output_traj
-

src/prpy/planning/mk.py

@@ -30,8 +30,8 @@
 
 import logging, numpy, openravepy, time
 from ..util import SetTrajectoryTags
-from base import (BasePlanner, PlanningError, UnsupportedPlanningError,
-                  PlanningMethod, Tags)
+from base import (BasePlanner, PlanningError,
+                  ClonedPlanningMethod, Tags)
 
 logger = logging.getLogger(__name__)
 
@@ -100,7 +100,7 @@ def GetStraightVelocity(self, manip, velocity, initial_hand_pose, nullspace_fn,
 
         return numpy.dot(jacobian_pinv, pose_error) + numpy.dot(nullspace_projector, nullspace_goal)
 
-    @PlanningMethod
+    @ClonedPlanningMethod
     def PlanToEndEffectorOffset(self, robot, direction, distance, max_distance=None,
                                 nullspace=JointLimitAvoidance, timelimit=5.0, step_size=0.001,
                                 position_tolerance=0.01, angular_tolerance=0.15, **kw_args):

src/prpy/planning/named.py

@@ -28,7 +28,7 @@
 # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 # POSSIBILITY OF SUCH DAMAGE.
 import logging, numpy, openravepy
-from base import BasePlanner, PlanningError, UnsupportedPlanningError, PlanningMethod
+from base import BasePlanner, PlanningError, ClonedPlanningMethod
 
 class NamedPlanner(BasePlanner):
     def __init__(self, delegate_planner=None):
@@ -41,7 +41,7 @@ def __init__(self, delegate_planner=None):
     def __str__(self):
         return 'NamedPlanner'
 
-    @PlanningMethod
+    @ClonedPlanningMethod
     def PlanToNamedConfiguration(self, robot, name, **kw_args):
         """ Plan to a named configuration.