Merge pull request #280 from personalrobotics/feature/more_planningmethods

Added separate LockedPlanningMethod and ClonedPlanningMethod calls.

Author: mkoval

README.md

@@ -25,12 +25,27 @@ 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
-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.
+Each planner has one or more *planning methods*, annotated with either the
+`@LockedPlanningMethod` or `@ClonedPlanningMethod decorator, that look like
+ordinary functions. Using these decorators makes other PrPy components
+aware that these methods exist and follow a particular specification that
+allows them to be composed with other PrPy objects automatically.  For
+example, `MetaPlanner`s will report that they can perform planning methods
+that their child motion planners have enumerated via `@PlanningMethod`
+decorators.
+
+`@PlanningMethod` decorators also make sure that calls to planning code are
+executed in a thread-safe manner.  In the case of `@LockedPlanningMethod`,
+this is enforced by locking the calling environment until the planning method
+has completed. In the case of `@ClonedPlanningMethod`, this is enforced by
+cloning the calling environment, and calling the wrapped method with references
+to the cloned environment.  The result of the method is then copied back to the
+calling environment.  `@ClonedPlanningMethod`s can be used to run multiple
+planners in parallel and to parallelize 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 +55,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 +101,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 +127,36 @@ 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 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).
+
+Each instance of a `BasePlanner`-derived class constructs a planning
+environment `self.env`.  This environment is uniquely associated with each
+instance of the planner and is what will be used in `@ClonedPlanningMethod`
+calls. Since this environment is persistent and unique, it can also be used
+as a place to cache data or pre-load plugins for planners that have heavyweight
+initialization steps.  However, because of this, each planning instance can
+only execute one `@ClonedPlanningMethod` at a time.  It can still execute
+arbitrary `@LockedPlanningMethod` calls, as long as they are referring to
+robots in different environments.
 
 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`.
+- Assume that the planning 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.
+  robot in the environment it should be using to perform planning.
 - 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
-  the cloned environment.
-- When possible, use one of the defacto-standard `@PlanningMethod` names listed
+  the same environment as the robot it was passed (e.g. `robot.GetEnv()`).
+- 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):