Merge pull request #318 from personalrobotics/bugfix/ActiveDOFs

Enabled ActiveDOFs in several planners.

src/prpy/planning/chomp.py

@@ -33,11 +33,19 @@
 import logging
 import numpy
 import openravepy
-from ..util import SetTrajectoryTags
+from ..util import SetTrajectoryTags, GetLinearCollisionCheckPts
+from .exceptions import (
+    CollisionPlanningError,
+    SelfCollisionPlanningError
+)
 from base import (BasePlanner, PlanningError, UnsupportedPlanningError,
                   ClonedPlanningMethod, Tags)
+from openravepy import CollisionOptions, CollisionOptionsStateSaver
+from prpy.util import VanDerCorputSampleGenerator, SampleTimeGenerator
 import prpy.tsr
 
+SaveParameters = openravepy.KinBody.SaveParameters.LinkEnable
+
 logger = logging.getLogger(__name__)
 
 DistanceFieldKey = collections.namedtuple('DistanceFieldKey',
@@ -92,7 +100,11 @@ def sync(self, robot):
                         if other_body == body:
                             continue
                         other_bodies.append(other_body)
-                    other_savers = [openravepy.KinBodyStateSaver(other_body,openravepy.KinBody.SaveParameters.LinkEnable) for other_body in other_bodies]
+
+                    other_savers = [
+                        other_body.CreateKinBodyStateSaver(SaveParameters.LinkEnable)
+                        for other_body in other_bodies]
+
                     with contextlib.nested(*other_savers):
                         for other_body in other_bodies:
                             other_body.Enable(False)
@@ -167,13 +179,15 @@ def setupEnv(self, env):
             from orcdchomp import orcdchomp
             module = openravepy.RaveCreateModule(self.env, 'orcdchomp')
         except ImportError:
-            raise UnsupportedPlanningError('Unable to import orcdchomp.')
+            raise UnsupportedPlanningError(
+                'Unable to import "orcdchomp". Is or_cdchomp installed?')
         except openravepy.openrave_exception as e:
             raise UnsupportedPlanningError(
-                'Unable to create orcdchomp module: ' + str(e))
+                'Failed loading "orcdchomp" module: ' + str(e))
 
         if module is None:
-            raise UnsupportedPlanningError('Failed loading CHOMP module.')
+            raise UnsupportedPlanningError(
+                'Failed loading "orcdchomp" module. Is or_cdchomp installed?')
 
         # This is a hack to prevent leaking memory.
         class CHOMPBindings(object):
@@ -197,7 +211,8 @@ class CHOMPBindings(object):
 
         # Create a DistanceFieldManager to track which distance fields are
         # currently loaded.
-        self.distance_fields = DistanceFieldManager(self.module, require_cache=self.require_cache)
+        self.distance_fields = DistanceFieldManager(
+            self.module, require_cache=self.require_cache)
 
     def __str__(self):
         return 'CHOMP'
@@ -220,18 +235,12 @@ def OptimizeTrajectory(self, robot, traj, lambda_=100.0, n_iter=50,
             cspec.InsertDeltaTime(waypoint, .1)
             traj.Insert(i, waypoint, True)
 
-        try:
-            traj = self.module.runchomp(robot=robot, starttraj=traj,
-                lambda_=lambda_, n_iter=n_iter, **kw_args)
-        except Exception as e:
-            raise PlanningError(str(e))
-
-        SetTrajectoryTags(traj, {Tags.SMOOTH: True}, append=True)
-        return traj
+        return self._Plan(robot=robot, starttraj=traj, lambda_=lambda_,
+            n_iter=n_iter, **kw_args)
 
     @ClonedPlanningMethod
     def PlanToConfiguration(self, robot, goal, lambda_=100.0, n_iter=15,
-                            **kw_args):
+                            **kwargs):
         """
         Plan to a single configuration with single-goal CHOMP.
         @param robot
@@ -241,104 +250,39 @@ def PlanToConfiguration(self, robot, goal, lambda_=100.0, n_iter=15,
         """
         self.distance_fields.sync(robot)
 
+        return self._Plan(robot=robot, adofgoal=goal, lambda_=lambda_,
+            n_iter=n_iter, **kwargs)
+
+    def _Plan(self, robot, sampling_func=VanDerCorputSampleGenerator, **kwargs):
         try:
-            traj = self.module.runchomp(robot=robot, adofgoal=goal,
-                                        lambda_=lambda_, n_iter=n_iter,
-                                        releasegil=True, **kw_args)
+            # Disable collision checking since we will perform them below.
+            traj = self.module.runchomp(robot=robot, no_collision_check=True,
+                    releasegil=True, **kwargs)
         except Exception as e:
             raise PlanningError(str(e))
 
-        SetTrajectoryTags(traj, {Tags.SMOOTH: True}, append=True)
-        return traj
-
-    @ClonedPlanningMethod
-    def PlanToEndEffectorPose(self, robot, goal_pose, lambda_=100.0,
-                              n_iter=100, goal_tolerance=0.01, **kw_args):
-        """
-        Plan to a desired end-effector pose using GSCHOMP
-        @param robot
-        @param goal_pose desired end-effector pose
-        @param lambda_ step size
-        @param n_iter number of iterations
-        @param goal_tolerance tolerance in meters
-        @return traj
-        """
-        self.distance_fields.sync(robot)
+        # Strip the extra groups added by CHOMP and change the trajectory to be
+        # linearly interpolated, as required by GetLinearCollisionCheckPts.
+        cspec = robot.GetActiveConfigurationSpecification('linear')
+        openravepy.planningutils.ConvertTrajectorySpecification(traj, cspec)
 
-        # CHOMP only supports start sets. Instead, we plan backwards from the
-        # goal TSR to the starting configuration. Afterwards, we reverse the
-        # trajectory.
-        manipulator_index = robot.GetActiveManipulatorIndex()
-        goal_tsr = prpy.tsr.TSR(T0_w=goal_pose, manip=manipulator_index)
-        start_config = robot.GetActiveDOFValues()
+        # Collision check the trajectory at DOF resolution. We do this in
+        # Python, instead of using CHOMP's native support for this, so we have
+        # access to the CollisionReport object.
+        checks = GetLinearCollisionCheckPts(robot, traj, norm_order=2,
+            sampling_func=sampling_func)
 
-        try:
-            traj = self.module.runchomp(
-                robot=robot, adofgoal=start_config, start_tsr=goal_tsr,
-                lambda_=lambda_, n_iter=n_iter, goal_tolerance=goal_tolerance,
-                releasegil=True, **kw_args
-            )
-            traj = openravepy.planningutils.ReverseTrajectory(traj)
-        except RuntimeError as e:
-            raise PlanningError(str(e))
-        except openravepy.openrave_exception as e:
-            raise PlanningError(str(e))
+        with CollisionOptionsStateSaver(self.env.GetCollisionChecker(),
+                                        CollisionOptions.ActiveDOFs):
+            for t, q in checks:
+                robot.SetActiveDOFValues(q)
 
-        # Verify that CHOMP didn't converge to the wrong goal. This is a
-        # workaround for a bug in GSCHOMP where the constraint projection
-        # fails because of joint limits.
-        config_spec = traj.GetConfigurationSpecification()
-        last_waypoint = traj.GetWaypoint(traj.GetNumWaypoints() - 1)
-        final_config = config_spec.ExtractJointValues(
-                last_waypoint, robot, robot.GetActiveDOFIndices())
-        robot.SetActiveDOFValues(final_config)
-        final_pose = robot.GetActiveManipulator().GetEndEffectorTransform()
-
-        # TODO: Also check the orientation.
-        goal_distance = numpy.linalg.norm(final_pose[0:3, 3]
-                                         - goal_pose[0:3, 3])
-        if goal_distance > goal_tolerance:
-            raise PlanningError(
-                'CHOMP deviated from the goal pose by {0:f} meters.'.format(
-                    goal_distance))
+                report = openravepy.CollisionReport()
+                if self.env.CheckCollision(robot, report=report):
+                    raise CollisionPlanningError.FromReport(report)
+                elif robot.CheckSelfCollision(report=report):
+                    raise SelfCollisionPlanningError.FromReport(report)
 
         SetTrajectoryTags(traj, {Tags.SMOOTH: True}, append=True)
-        return traj
-
-    # JK - Disabling. This is not working reliably.
-    '''
-    @ClonedPlanningMethod
-    def PlanToTSR(self, robot, tsrchains, lambda_=100.0, n_iter=100,
-                  goal_tolerance=0.01, **kw_args):
-        """
-        Plan to a goal TSR.
-        @param robot
-        @param tsrchains A TSR chain with a single goal tsr
-        @return traj
-        """
-        self.distance_fields.sync(robot)
 
-        manipulator_index = robot.GetActiveManipulatorIndex()
-        start_config = robot.GetActiveDOFValues()
-        
-        if len(tsrchains) != 1:
-            raise UnsupportedPlanningError('CHOMP')
-        
-        tsrchain = tsrchains[0]
-        if not tsrchain.sample_goal or len(tsrchain.TSRs) > 1:
-            raise UnsupportedPlanningError(
-                'CHOMP only supports TSR chains that contain a single goal'
-                ' TSR.'
-            )
-        
-        try:
-            goal_tsr = tsrchain.TSRs[0]
-            traj = self.module.runchomp(
-                robot=robot, adofgoal=start_config, start_tsr=goal_tsr,
-                lambda_=lambda_, n_iter=n_iter, goal_tolerance=goal_tolerance,
-                releasegil=True, **kw_args
-            )
-            return openravepy.planningutils.ReverseTrajectory(traj)
-        except RuntimeError as e:
-            raise PlanningError(str(e))
-    '''
+        return traj

src/prpy/planning/ompl.py

@@ -34,7 +34,11 @@
 from ..util import CopyTrajectory, SetTrajectoryTags
 from base import (BasePlanner, PlanningError, UnsupportedPlanningError,
                   ClonedPlanningMethod, Tags)
-from openravepy import PlannerStatus
+from openravepy import (
+    CollisionOptions,
+    CollisionOptionsStateSaver,
+    PlannerStatus
+)
 from .cbirrt import SerializeTSRChain
 
 logger = logging.getLogger(__name__)
@@ -106,7 +110,10 @@ def _Plan(self, robot, goal=None, timeout=30., shortcut_timeout=5.,
                 self.planner.InitPlan(robot, params)
                 self.setup = True
 
-            status = self.planner.PlanPath(traj, releasegil=True)
+            with CollisionOptionsStateSaver(self.env.GetCollisionChecker(),
+                                            CollisionOptions.ActiveDOFs):
+                status = self.planner.PlanPath(traj, releasegil=True)
+
             if status not in [PlannerStatus.HasSolution,
                               PlannerStatus.InterruptedWithSolution]:
                 raise PlanningError('Planner returned with status {0:s}.'

src/prpy/planning/snap.py

@@ -36,6 +36,7 @@
     ClonedPlanningMethod,
     Tags
 )
+from openravepy import CollisionOptions, CollisionOptionsStateSaver
 
 
 class SnapPlanner(BasePlanner):
@@ -173,18 +174,20 @@ def _Snap(self, robot, goal, **kw_args):
                                             norm_order=2,
                                             sampling_func=vdc)
 
-        # Run constraint checks at DOF resolution:
-        for t, q in checks:
-            # Set the joint positions
-            # Note: the planner is using a cloned 'robot' object
-            robot.SetActiveDOFValues(q)
-
-            # Check for collisions
-            report = openravepy.CollisionReport()
-            if self.env.CheckCollision(robot, report=report):
-                raise CollisionPlanningError.FromReport(report)
-            elif robot.CheckSelfCollision(report=report):
-                raise SelfCollisionPlanningError.FromReport(report)
+        with CollisionOptionsStateSaver(self.env.GetCollisionChecker(),
+                                        CollisionOptions.ActiveDOFs):
+            # Run constraint checks at DOF resolution:
+            for t, q in checks:
+                # Set the joint positions
+                # Note: the planner is using a cloned 'robot' object
+                robot.SetActiveDOFValues(q)
+
+                # Check for collisions
+                report = openravepy.CollisionReport()
+                if self.env.CheckCollision(robot, report=report):
+                    raise CollisionPlanningError.FromReport(report)
+                elif robot.CheckSelfCollision(report=report):
+                    raise SelfCollisionPlanningError.FromReport(report)
 
         # Tag the return trajectory as smooth (in joint space).
         SetTrajectoryTags(traj, {Tags.SMOOTH: True}, append=True)

src/prpy/planning/vectorfield.py

@@ -36,6 +36,7 @@
 from .. import util
 from base import BasePlanner, PlanningError, ClonedPlanningMethod, Tags
 from enum import Enum
+from openravepy import CollisionOptions, CollisionOptionsStateSaver
 
 logger = logging.getLogger(__name__)
 
@@ -558,9 +559,11 @@ def fn_callback(t, q):
                                   rtol=1e-3)
         # Set function to be called at every successful integration step.
         integrator.set_solout(fn_callback)
-        # Initial conditions
         integrator.set_initial_value(y=robot.GetActiveDOFValues(), t=0.)
-        integrator.integrate(t=integration_time_interval)
+
+        with CollisionOptionsStateSaver(self.env.GetCollisionChecker(),
+                                        CollisionOptions.ActiveDOFs):
+            integrator.integrate(t=integration_time_interval)
 
         t_cache = nonlocals['t_cache']
         exception = nonlocals['exception']

src/prpy/planning/workspace.py

@@ -152,13 +152,21 @@ def PlanWorkspacePath(self, robot, traj, timelimit=5.0,
         @param timelimit timeout in seconds
         @return qtraj configuration space path
         """
-        from .exceptions import (TimeoutPlanningError,
-                                 CollisionPlanningError,
-                                 SelfCollisionPlanningError)
-        from openravepy import CollisionReport
+        from .exceptions import (
+            TimeoutPlanningError,
+            CollisionPlanningError,
+            SelfCollisionPlanningError
+        )
+        from openravepy import (
+            CollisionOptions,
+            CollisionOptionsStateSaver,
+            CollisionReport
+        )
+
         p = openravepy.KinBody.SaveParameters
 
-        with robot:
+        with robot, CollisionOptionsStateSaver(self.env.GetCollisionChecker(),
+                                               CollisionOptions.ActiveDOFs):
             manip = robot.GetActiveManipulator()
             robot.SetActiveDOFs(manip.GetArmIndices())