Switched from ClonedPlanningMethod To LockedPlanningMethod #360

gilwoolee · 2016-09-30T19:37:03Z

This PR changes ClonedPlanningMethod to LockedPlanningMethod in SnapPlanner, GreedyIKPlanner, CBiRRT, IKPlanner, NamedPlanner, OMPLPlanner, OpenRavePlanner, VectorField, TSRPlanner.

Now each PlanTo.. method uses robot.env() instead of self.env(). robot.CreateRobotStateSaver(...) has been added to necessary blocks.

mkoval

This is a fantastic change! We saw that cloning the Environment in@ClonedPlanningMethod took a significant fraction of the total planning time in our ISER results, so switching to @LocedPlanningMethod should have significant performance benefits. Thank you @gilwoolee for taking the time to make this important, but unfortunately tedious, set of changes.

I would like to remove the RobotStateSaver from @LockedPlanningMethod because it provides a false sense of security: some planners (like CBiRRT) alter more than the robot's active DOFs. Once we remove that, we'll have to add a few extra RobotStateSavers to the individual planners.

I think I caught them all in my comments, but it would be great if someone could do another pass.

mkoval · 2016-09-30T19:41:24Z

src/prpy/planning/cbirrt.py

@@ -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)


CBiRRT seems to modify:

the robot's active DOF values

the robot's DOF limits

the robot's transformation

the DOF values of any mimic bodies specified in a TSRChain.

This should be wrapped in a RobotStateSaver that preserves the robot's LinkTransformations and a KinBodyStateSaver for each mimic body that preserves its LinkTransformations. I don't see an option in RobotStateSaverto restore DOF position limits, so we will need to do that ourselves.

mkoval · 2016-10-01T17:38:21Z

src/prpy/planning/ompl.py

-        self.planner.InitPlan(robot, params)
-        status = self.planner.PlanPath(output_path, releasegil=True)
+        planner.InitPlan(robot, params)
+        status = planner.PlanPath(output_path, releasegil=True)


This should be wrapped in a RobotStateSaver that saves robot's LinkTransformations.

mkoval · 2016-10-01T17:38:54Z

src/prpy/planning/ompl.py


        # 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)


This should be wrapped in a RobotStateSaver that saves robot's LinkTransformations.

mkoval · 2016-10-01T17:39:30Z

src/prpy/planning/openrave.py

                self.setup = True

-            status = self.planner.PlanPath(traj, releasegil=True)
+            status = planner.PlanPath(traj, releasegil=True)


This should be wrapped in a RobotStateSaver that saves robot's LinkTransformations.

mkoval · 2016-10-01T17:40:15Z

src/prpy/planning/snap.py

-        with self.robot_checker_factory(robot) as robot_checker:
+        with self.robot_checker_factory(robot) as robot_checker, \
+        robot.CreateRobotStateSaver(Robot.SaveParameters.ActiveDOF | 
+                                Robot.SaveParameters.LinkTransformation):


I don't think we need ActiveDOF here, since we're not changing robot's active DOF indices.

mkoval · 2016-10-01T17:40:57Z

src/prpy/planning/tsr.py

@@ -85,7 +86,7 @@ def PlanToIK(self, robot, goal_pose, **kw_args):
        return self.PlanToTSR(robot, [tsr_chain], **kw_args)
    """


It's not related to the pull request, but we really should delete this dead code.

mkoval · 2016-10-01T17:41:48Z

src/prpy/planning/vectorfield.py

-        with self.robot_checker_factory(robot) as robot_checker:
+        with self.robot_checker_factory(robot) as robot_checker, \
+            robot.CreateRobotStateSaver(Robot.SaveParameters.ActiveDOF |
+                            Robot.SaveParameters.LinkTransformation):


I don't think we need ActiveDOF here, since we do not alter the robot's active DOF indices.

mkoval · 2016-10-01T17:43:11Z

src/prpy/planning/workspace.py

            manip = robot.GetActiveManipulator()
            start_pose = manip.GetEndEffectorTransform()
-            traj = openravepy.RaveCreateTrajectory(self.env, '')
+            traj = openravepy.RaveCreateTrajectory(env, '')
            spec = openravepy.IkParameterization.\
                GetConfigurationSpecificationFromType(
                    openravepy.IkParameterizationType.Transform6D, 'linear')


You should wrap RetimeActiveTrajectory in a RobotStateSaver that preserves the robot's LinkTransformations, since the retimer may modify the robot's active DOF values.

mkoval · 2016-10-01T17:43:43Z

src/prpy/planning/workspace.py

@@ -121,7 +124,7 @@ def PlanToEndEffectorOffset(self, robot, direction, distance,
        with robot:
            manip = robot.GetActiveManipulator()
            start_pose = manip.GetEndEffectorTransform()
-            traj = openravepy.RaveCreateTrajectory(self.env, '')
+            traj = openravepy.RaveCreateTrajectory(env, '')


You should wrap RetimeActiveTrajectory in a RobotStateSaver that preserves the robot's LinkTransformations, since the retimer may modify the robot's active DOF values.

mkoval · 2016-10-11T20:58:32Z

src/prpy/planning/cbirrt.py

+            contextlib.nested(*mimicbody_savers):
+            response = problem.SendCommand(args_str, True)
+
+        robot.SetDOFLimits(upper, lower)


Put this in a finally block so it will run even if something raises an exception. Or, even better, write a simple context manager that takes advantage of this. The contextlib.contextmanager decorator could be useful here.

It would also be helpful to add a comment that CBiRRT adjusts the joint limits to include the robot's start configuration if allowlimadj is set.

mkoval · 2016-10-11T21:03:10Z

src/prpy/planning/ompl.py

-            self.planner.InitPlan(robot, params)
-            self.setup = True
+            planner.InitPlan(robot, params)
+            setup = True


The setup flag is no longer used anywhere.

mkoval · 2016-10-11T21:04:12Z

src/prpy/planning/ompl.py

+
+        if planner is None:
+            raise UnsupportedPlanningError(
+                'Unable to create OMPL_Simplifier planner.')


Nit: Modify this message use planner_name so this message can't get out of sync with the code.

mkoval · 2016-10-11T21:06:09Z

src/prpy/planning/openrave.py

+            planner = openravepy.RaveCreatePlanner(env, algorithm)
+        except openravepy.openrave_exception:
+            raise UnsupportedPlanningError('Unable to create {:s} module.'
+                                           .format(str(self)))


I believe RaveCreatePlanner returns None instead of raising an exception.

mkoval · 2016-10-11T21:06:36Z

src/prpy/planning/openrave.py

+        env = robot.GetEnv()
+
+        try:
+            planner = openravepy.RaveCreatePlanner(env, algorithm)


algorithm is not defined - this should be self.algorithm.

mkoval · 2016-10-11T21:19:50Z

Your changes look good! I added a few minor comments above. In addition, SnapPlanner.PlanToConfiguration still needs to be changed to a @LockedPlanningMethod.

We also need to give the same treatment to prpy.planning.retimer. This is where PrPy bindings for HauserParabolicSmoother live, so it's actually a pretty important file to change.

mkoval · 2016-11-02T10:42:39Z

Thanks @gilwoolee! These changes look great. I expect that planning will be quite a bit faster with these changes in place.

Swtiched from ClonedPlanningMethod To LockedPlanningMethod

a806ab5

gilwoolee assigned gilwoolee and mkoval and unassigned gilwoolee Sep 30, 2016

mkoval reviewed Oct 1, 2016

View reviewed changes

mkoval added the enhancement label Oct 1, 2016

mkoval changed the title ~~Swtiched from ClonedPlanningMethod To LockedPlanningMethod~~ Switched from ClonedPlanningMethod To LockedPlanningMethod Oct 1, 2016

gilwoolee added 3 commits October 10, 2016 12:48

Addressing Mike's comment.

19a33ed

Addressing Mike's comment on CBiRRT.

61aa833

Fixing indentations

7dafb8c

mkoval reviewed Oct 11, 2016

View reviewed changes

gilwoolee and others added 3 commits October 14, 2016 12:00

Adding OpenRAVEPlannerTest, addressing Mike's comments, some typos.

3c2c01b

Removing unused imports

f94e493

Switch BasePlanner -> Planner where possible.

13680f6

mkoval merged commit 630f31c into master Nov 2, 2016

mkoval deleted the feature/remove_cloning branch November 2, 2016 10:43

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Switched from ClonedPlanningMethod To LockedPlanningMethod #360

Switched from ClonedPlanningMethod To LockedPlanningMethod #360

gilwoolee commented Sep 30, 2016

mkoval left a comment

mkoval Sep 30, 2016 •

edited

Loading

mkoval Oct 1, 2016

mkoval Oct 1, 2016

mkoval Oct 1, 2016

mkoval Oct 1, 2016

mkoval Oct 1, 2016

mkoval Oct 1, 2016

mkoval Oct 1, 2016

mkoval Oct 1, 2016

mkoval Oct 11, 2016 •

edited

Loading

mkoval Oct 11, 2016

mkoval Oct 11, 2016

mkoval Oct 11, 2016

mkoval Oct 11, 2016

mkoval commented Oct 11, 2016 •

edited

Loading

mkoval commented Nov 2, 2016

		@@ -85,7 +86,7 @@ def PlanToIK(self, robot, goal_pose, **kw_args):
		return self.PlanToTSR(robot, [tsr_chain], **kw_args)
		"""

Switched from ClonedPlanningMethod To LockedPlanningMethod #360

Switched from ClonedPlanningMethod To LockedPlanningMethod #360

Conversation

gilwoolee commented Sep 30, 2016

mkoval left a comment

Choose a reason for hiding this comment

mkoval Sep 30, 2016 • edited Loading

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

mkoval Oct 11, 2016 • edited Loading

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

mkoval commented Oct 11, 2016 • edited Loading

mkoval commented Nov 2, 2016

mkoval Sep 30, 2016 •

edited

Loading

mkoval Oct 11, 2016 •

edited

Loading

mkoval commented Oct 11, 2016 •

edited

Loading