Switched from ClonedPlanningMethod To LockedPlanningMethod

src/prpy/planning/adapters.py

@@ -1,7 +1,7 @@
 import numpy
 from openravepy import Environment
 from .base import (
-    ClonedPlanningMethod,
+    LockedPlanningMethod,
     Planner,
 )
 from ..kin import (
@@ -27,10 +27,8 @@ def __init__(self, delegate_planner):
         super(PlanToEndEffectorOffsetTSRAdapter, self).__init__()
 
         self.delegate_planner = delegate_planner
-        self.env = Environment()
 
-
-    @ClonedPlanningMethod
+    @LockedPlanningMethod
     def PlanToEndEffectorOffset(self, robot, direction, distance, **kwargs):
         """ Plan to a desired end-effector offset with fixed orientation.
 

src/prpy/planning/base.py

@@ -103,7 +103,7 @@ def __init__(self, func):
         self.func = func
 
     def __call__(self, instance, robot, *args, **kw_args):
-        with robot.GetEnv():
+        with robot.GetEnv(), robot.CreateRobotStateSaver():
             # Perform the actual planning operation.
             traj = self.func(instance, robot, *args, **kw_args)
 

src/prpy/planning/cbirrt.py

@@ -40,7 +40,7 @@
 from .adapters import PlanToEndEffectorOffsetTSRAdapter
 from .base import (
     BasePlanner,
-    ClonedPlanningMethod,
+    LockedPlanningMethod,
     PlanningError,
     Tags,
     UnsupportedPlanningError,
@@ -56,11 +56,6 @@ def __init__(self, robot_checker_factory=None, timelimit=5.):
         if robot_checker_factory is None:
             robot_checker_factory = DefaultRobotCollisionCheckerFactory
 
-        self.problem = openravepy.RaveCreateProblem(self.env, 'CBiRRT')
-
-        if self.problem is None:
-            raise UnsupportedPlanningError('Unable to create CBiRRT module.')
-
         self.robot_checker_factory = robot_checker_factory
         if isinstance(robot_checker_factory, SimpleRobotCollisionCheckerFactory):
             self._is_baked = False
@@ -73,7 +68,7 @@ def __init__(self, robot_checker_factory=None, timelimit=5.):
     def __str__(self):
         return 'CBiRRT'
 
-    @ClonedPlanningMethod
+    @LockedPlanningMethod
     def PlanToConfigurations(self, robot, goals, **kw_args):
         """
         Plan to multiple goal configurations with CBiRRT. This adds each goal
@@ -86,7 +81,7 @@ def PlanToConfigurations(self, robot, goals, **kw_args):
         kw_args.setdefault('smoothingitrs', 0)
         return self.Plan(robot, jointgoals=goals, **kw_args)
 
-    @ClonedPlanningMethod
+    @LockedPlanningMethod
     def PlanToConfiguration(self, robot, goal, **kw_args):
         """
         Plan to a single goal configuration with CBiRRT.
@@ -97,7 +92,7 @@ def PlanToConfiguration(self, robot, goal, **kw_args):
         kw_args.setdefault('smoothingitrs', 0)
         return self.Plan(robot, jointgoals=[goal], **kw_args)
 
-    @ClonedPlanningMethod
+    @LockedPlanningMethod
     def PlanToEndEffectorPose(self, robot, goal_pose, **kw_args):
         """
         Plan to a desired end-effector pose.
@@ -116,7 +111,7 @@ def PlanToEndEffectorPose(self, robot, goal_pose, **kw_args):
         return self.Plan(robot, tsr_chains=[tsr_chain], **kw_args)
 
 
-    @ClonedPlanningMethod
+    @LockedPlanningMethod
     def PlanToEndEffectorOffset(self, robot, direction, distance, **kw_args):
         """
         Plan to a desired end-effector offset.
@@ -132,7 +127,7 @@ def PlanToEndEffectorOffset(self, robot, direction, distance, **kw_args):
         return self.PlanToTSR(robot, tsr_chains=chains, **kw_args)
 
 
-    @ClonedPlanningMethod
+    @LockedPlanningMethod
     def PlanToTSR(self, robot, tsr_chains, smoothingitrs=100, **kw_args):
         """
         Plan to a goal specified as a list of TSR chains. CBiRRT supports an
@@ -177,14 +172,19 @@ def Plan(self, robot, smoothingitrs=None, timelimit=None, allowlimadj=0,
                              ' non-negative; got {:f}.'.format(timelimit))
 
         env = robot.GetEnv()
+        problem = openravepy.RaveCreateProblem(env, 'CBiRRT')
+
+        if problem is None:
+            raise UnsupportedPlanningError('Unable to create CBiRRT module.')
+
         is_endpoint_deterministic = True
         is_constrained = False
 
         # TODO We may need this work-around because CBiRRT doesn't like it
         # when an IK solver other than GeneralIK is loaded (e.g. nlopt_ik).
         # self.ClearIkSolver(robot.GetActiveManipulator())
 
-        env.LoadProblem(self.problem, robot.GetName())
+        env.LoadProblem(problem, robot.GetName())
 
         args =  ['RunCBiRRT']
         args += ['timelimit', str(timelimit)]
@@ -266,7 +266,7 @@ def Plan(self, robot, smoothingitrs=None, timelimit=None, allowlimadj=0,
         collision_checker = self.robot_checker_factory(robot)
         options = collision_checker.collision_options
         with CollisionOptionsStateSaver(env.GetCollisionChecker(), options):
-            response = self.problem.SendCommand(args_str, True)
+            response = problem.SendCommand(args_str, True)
 
         if not response.strip().startswith('1'):
             raise PlanningError('Unknown error: ' + response,

src/prpy/planning/ik.py

@@ -33,7 +33,7 @@
 from .. import ik_ranking
 from base import (BasePlanner,
                   PlanningError,
-                  ClonedPlanningMethod)
+                  LockedPlanningMethod)
 
 logger = logging.getLogger(__name__)
 
@@ -47,7 +47,7 @@ def __str__(self):
         return 'IKPlanner'
 
 
-    @ClonedPlanningMethod
+    @LockedPlanningMethod
     def PlanToIK(self, robot, pose, **kwargs):
         """
         Plan to a desired end effector pose with IKPlanner.
@@ -61,7 +61,7 @@ def PlanToIK(self, robot, pose, **kwargs):
         """
         return self._PlanToIK(robot, pose, **kwargs)
 
-    @ClonedPlanningMethod
+    @LockedPlanningMethod
     def PlanToEndEffectorPose(self, robot, pose, **kwargs):
         """
         Plan to a desired end effector pose with IKPlanner.

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, ClonedPlanningMethod
+from base import BasePlanner, PlanningError, LockedPlanningMethod
 
 class NamedPlanner(BasePlanner):
     def __init__(self, delegate_planner=None):
@@ -41,7 +41,7 @@ def __init__(self, delegate_planner=None):
     def __str__(self):
         return 'NamedPlanner'
 
-    @ClonedPlanningMethod
+    @LockedPlanningMethod
     def PlanToNamedConfiguration(self, robot, name, **kw_args):
         """ Plan to a named configuration.
 

src/prpy/planning/ompl.py

@@ -38,6 +38,7 @@
     PlannerStatus,
     RaveCreatePlanner,
     RaveCreateTrajectory,
+    Robot
 )
 from ..collision import (
     BakedRobotCollisionCheckerFactory,
@@ -55,7 +56,7 @@
 )
 from .base import (
     BasePlanner,
-    ClonedPlanningMethod,
+    LockedPlanningMethod,
     PlanningError,
     Tags,
     UnsupportedPlanningError,
@@ -109,18 +110,10 @@ def __init__(self, algorithm='RRTConnect', robot_checker_factory=None,
                 'or_ompl only supports Simple and'
                 ' BakedRobotCollisionCheckerFactory.')
 
-        planner_name = 'OMPL_{:s}'.format(algorithm)
-        self.planner = RaveCreatePlanner(self.env, planner_name)
-
-        if self.planner is None:
-            raise UnsupportedPlanningError(
-                'Unable to create "{:s}" planner. Is or_ompl installed?'
-                .format(planner_name))
-
     def __str__(self):
         return 'OMPL_{0:s}'.format(self.algorithm)
 
-    @ClonedPlanningMethod
+    @LockedPlanningMethod
     def PlanToConfiguration(self, robot, goal, **kw_args):
         """
         Plan to a configuration using the robot's active DOFs.
@@ -133,7 +126,7 @@ def PlanToConfiguration(self, robot, goal, **kw_args):
         """
         return self._Plan(robot, goal=goal, **kw_args)
 
-    @ClonedPlanningMethod
+    @LockedPlanningMethod
     def PlanToTSR(self, robot, tsrchains, **kw_args):
         """
         Plan to one or more goal TSR chains using the robot's active DOFs. This
@@ -166,6 +159,15 @@ def _Plan(self, robot, goal=None, tsrchains=None, timelimit=None,
 
         extraParams += '<time_limit>{:f}</time_limit>'.format(timelimit)
 
+        env = robot.GetEnv()
+        planner_name = 'OMPL_{:s}'.format(self.algorithm)
+        planner = RaveCreatePlanner(env, planner_name)
+
+        if planner is None:
+            raise UnsupportedPlanningError(
+                'Unable to create "{:s}" planner. Is or_ompl installed?'
+                .format(planner_name))
+
         # Handle other parameters that get passed directly to OMPL.
         if ompl_args is None:
             ompl_args = dict()
@@ -217,16 +219,16 @@ def _Plan(self, robot, goal=None, tsrchains=None, timelimit=None,
         params.SetExtraParameters(extraParams)
 
         if (not continue_planner) or (not self.setup):
-            self.planner.InitPlan(robot, params)
-            self.setup = True
+            planner.InitPlan(robot, params)
+            setup = True
 
         # Bypass the context manager since or_ompl does its own baking.
         env = robot.GetEnv()
         robot_checker = self.robot_checker_factory(robot)
         options = robot_checker.collision_options
         with CollisionOptionsStateSaver(env.GetCollisionChecker(), options):
             traj = RaveCreateTrajectory(env, 'GenericTrajectory')
-            status = self.planner.PlanPath(traj, releasegil=True)
+            status = planner.PlanPath(traj, releasegil=True)
 
         if status not in [PlannerStatus.HasSolution,
                           PlannerStatus.InterruptedWithSolution]:
@@ -327,7 +329,7 @@ def ComputeRange(self, robot):
         # a small value to cope with numerical precision issues inside OMPL.
         return multiplier * conservative_resolution - epsilon
 
-    @ClonedPlanningMethod
+    @LockedPlanningMethod
     def PlanToConfiguration(self, robot, goal, ompl_args=None, **kw_args):
         if ompl_args is None:
             ompl_args = dict()
@@ -336,7 +338,7 @@ def PlanToConfiguration(self, robot, goal, ompl_args=None, **kw_args):
 
         return self._Plan(robot, goal=goal, ompl_args=ompl_args, **kw_args)
 
-    @ClonedPlanningMethod
+    @LockedPlanningMethod
     def PlanToTSR(self, robot, tsrchains, ompl_args=None, **kw_args):
         if ompl_args is None:
             ompl_args = dict()
@@ -359,21 +361,23 @@ class OMPLSimplifier(BasePlanner):
     def __init__(self):
         super(OMPLSimplifier, self).__init__()
 
-        planner_name = 'OMPL_Simplifier'
-        self.planner = openravepy.RaveCreatePlanner(self.env, planner_name)
-
-        if self.planner is None:
-            raise UnsupportedPlanningError(
-                'Unable to create OMPL_Simplifier planner.')
-
     def __str__(self):
         return 'OMPL Simplifier'
 
-    @ClonedPlanningMethod
+    @LockedPlanningMethod
     def ShortcutPath(self, robot, path, timeout=1., **kwargs):
         # The planner operates in-place, so we need to copy the input path. We
         # also need to copy the trajectory into the planning environment.
-        output_path = CopyTrajectory(path, env=self.env)
+
+        planner_name = 'OMPL_Simplifier'
+        env = robot.GetEnv()
+        planner = openravepy.RaveCreatePlanner(env, planner_name)
+
+        if planner is None:
+            raise UnsupportedPlanningError(
+                'Unable to create OMPL_Simplifier planner.')
+
+        output_path = CopyTrajectory(path, env=env)
 
         extraParams = '<time_limit>{:f}</time_limit>'.format(timeout)
 
@@ -384,8 +388,8 @@ def ShortcutPath(self, robot, path, timeout=1., **kwargs):
         # TODO: It would be nice to call planningutils.SmoothTrajectory here,
         # but we can't because it passes a NULL robot to InitPlan. This is an
         # issue that needs to be fixed in or_ompl.
-        self.planner.InitPlan(robot, params)
-        status = self.planner.PlanPath(output_path, releasegil=True)
+        planner.InitPlan(robot, params)
+        status = planner.PlanPath(output_path, releasegil=True)
         if status not in [PlannerStatus.HasSolution,
                           PlannerStatus.InterruptedWithSolution]:
             raise PlanningError('Simplifier returned with status {0:s}.'