CBiRRT and OpenRAVERetimer now use CO_ActiveOnly

Author: Michael Koval

src/prpy/planning/cbirrt.py

@@ -186,6 +186,8 @@ def PlanToTSR(self, robot, tsr_chains, smoothingitrs=100, **kw_args):
     def Plan(self, robot, smoothingitrs=None, timelimit=None, allowlimadj=0,
              jointstarts=None, jointgoals=None, psample=None, tsr_chains=None,
              extra_args=None, **kw_args):
+        from openravepy import CollisionOptions, CollisionOptionsStateSaver
+
         self.env.LoadProblem(self.problem, robot.GetName())
 
         args = [ 'RunCBiRRT' ]
@@ -252,7 +254,10 @@ def Plan(self, robot, smoothingitrs=None, timelimit=None, allowlimadj=0,
         args += [ 'filename', traj_path ]
         args_str = ' '.join(args)
 
-        response = self.problem.SendCommand(args_str, True)
+        with CollisionOptionsStateSaver(self.env.GetCollisionChecker(),
+                                        CollisionOptions.ActiveDOFs):
+            response = self.problem.SendCommand(args_str, True)
+
         if not response.strip().startswith('1'):
             raise PlanningError('Unknown error: ' + response)
 
@@ -287,4 +292,4 @@ def serialize_dof_values(dof_values):
         return [ str(len(dof_values)), 
                  ' '.join([ str(x) for x in dof_values]) ]
 
-   
+   

src/prpy/planning/retimer.py

@@ -57,6 +57,7 @@ def __str__(self):
     def RetimeTrajectory(self, robot, path, options=None, **kw_args):
         from ..util import (CreatePlannerParametersString, CopyTrajectory,
                             SimplifyTrajectory, HasAffineDOFs)
+        from openravepy import CollisionOptions, CollisionOptionsStateSaver
         from copy import deepcopy
         from openravepy import Planner
 
@@ -94,10 +95,18 @@ def RetimeTrajectory(self, robot, path, options=None, **kw_args):
         # not perform this check internally (e.g. ParabolicTrajectoryRetimer).
         output_traj = SimplifyTrajectory(input_path, robot)
 
+        # Only collision check the active DOFs.
+        dof_indices, _ = cspec.ExtractUsedIndices(robot)
+        robot.SetActiveDOFs(dof_indices)
+
         # Compute the timing. This happens in-place.
         output_traj = SimplifyTrajectory(input_path, robot)
         self.planner.InitPlan(None, params_str)
-        status = self.planner.PlanPath(output_traj, releasegil=True)
+
+
+        with CollisionOptionsStateSaver(self.env.GetCollisionChecker(),
+                                        CollisionOptions.ActiveDOFs):
+            status = self.planner.PlanPath(output_traj, releasegil=True)
 
         if status not in [ PlannerStatus.HasSolution,
                            PlannerStatus.InterruptedWithSolution ]: