Changed retimers to only Simplify untimed trajectories.

Author: psigen

src/prpy/planning/mac_smoother.py

@@ -28,7 +28,8 @@
 # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 # POSSIBILITY OF SUCH DAMAGE.
 
-from ..util import CopyTrajectory, SimplifyTrajectory, SetTrajectoryTags
+from ..util import (CopyTrajectory, SimplifyTrajectory, SetTrajectoryTags,
+                    IsTimedTrajectory)
 from base import (BasePlanner, PlanningError, PlanningMethod,
                   UnsupportedPlanningError, Tags)
 from openravepy import (Planner, PlannerStatus, RaveCreatePlanner,
@@ -45,7 +46,7 @@ def __init__(self):
                 'Unable to create PrSplineMacBlender planner. Is or_pr_spline'
                 ' installed and in your OPENRAVE_PLUGIN path?'
             )
-            
+
         self.retimer = RaveCreatePlanner(self.env, 'PrSplineMacTimer')
         if self.retimer is None:
             raise UnsupportedPlanningError(
@@ -59,7 +60,11 @@ def RetimeTrajectory(self, robot, path):
         # necessary for two reasons: (1) the input trajectory may be in another
         # environment and/or (2) the retimer modifies the trajectory in-place.
         output_traj = CopyTrajectory(path, env=self.env)
-        output_traj = SimplifyTrajectory(output_traj, robot)
+
+        # Remove co-linear waypoints. Some of the default OpenRAVE retimers do
+        # not perform this check internally (e.g. ParabolicTrajectoryRetimer).
+        if not IsTimedTrajectory(output_traj):
+            output_traj = SimplifyTrajectory(output_traj, robot)
 
         # Blend the piecewise-linear input trajectory. The blender outputs a
         # collision-free path, consisting of piecewise-linear segments and

src/prpy/planning/retimer.py

@@ -28,10 +28,15 @@
 # ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 # POSSIBILITY OF SUCH DAMAGE.
 
-import logging, numpy, openravepy, os, tempfile
-from ..util import CopyTrajectory, SimplifyTrajectory, HasAffineDOFs
-from base import BasePlanner, PlanningError, PlanningMethod, UnsupportedPlanningError
-from openravepy import PlannerStatus
+import logging
+import openravepy
+from ..util import (CreatePlannerParametersString, CopyTrajectory,
+                    SimplifyTrajectory, HasAffineDOFs, IsTimedTrajectory)
+from base import (BasePlanner,
+                  PlanningError,
+                  PlanningMethod,
+                  UnsupportedPlanningError)
+from openravepy import PlannerStatus, Planner
 
 logger = logging.getLogger(__name__)
 
@@ -56,11 +61,8 @@ def __str__(self):
 
     @PlanningMethod
     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
 
         # Validate the input path.
         cspec = path.GetConfigurationSpecification()
@@ -91,11 +93,12 @@ def RetimeTrajectory(self, robot, path, options=None, **kw_args):
         # Copy the input trajectory into the planning environment. This is
         # necessary for two reasons: (1) the input trajectory may be in another
         # environment and/or (2) the retimer modifies the trajectory in-place.
-        input_path = CopyTrajectory(path, env=self.env)
+        output_traj = CopyTrajectory(path, env=self.env)
 
         # Remove co-linear waypoints. Some of the default OpenRAVE retimers do
         # not perform this check internally (e.g. ParabolicTrajectoryRetimer).
-        output_traj = SimplifyTrajectory(input_path, robot)
+        if not IsTimedTrajectory(output_traj):
+            output_traj = SimplifyTrajectory(output_traj, robot)
 
         # Only collision check the active DOFs.
         dof_indices, _ = cspec.ExtractUsedIndices(robot)
@@ -104,13 +107,12 @@ def RetimeTrajectory(self, robot, path, options=None, **kw_args):
         # Compute the timing. This happens in-place.
         self.planner.InitPlan(None, params_str)
 
-
         with CollisionOptionsStateSaver(self.env.GetCollisionChecker(),
                                         CollisionOptions.ActiveDOFs):
             status = self.planner.PlanPath(output_traj, releasegil=True)
 
-        if status not in [ PlannerStatus.HasSolution,
-                           PlannerStatus.InterruptedWithSolution ]:
+        if status not in [PlannerStatus.HasSolution,
+                          PlannerStatus.InterruptedWithSolution]:
             raise PlanningError(
                 'Retimer returned with status {:s}.'.format(str(status)))
 
@@ -182,8 +184,6 @@ def RetimeTrajectory(self, robot, path, **kw_args):
             if not found:
                 raise UnsupportedPlanningError('OpenRAVEAffineRetimer only supports untimed paths with affine DOFs. Found group: %s' % group.name)
 
-
-
         # Copy the input trajectory into the planning environment. This is
         # necessary for two reasons: (1) the input trajectory may be in another
         # environment and/or (2) the retimer modifies the trajectory in-place.
@@ -197,8 +197,8 @@ def RetimeTrajectory(self, robot, path, **kw_args):
         status = RetimeTrajectory(output_traj, max_velocities, max_accelerations,
                                   False)
 
-        if status not in [ PlannerStatus.HasSolution,
-                           PlannerStatus.InterruptedWithSolution ]:
+        if status not in [PlannerStatus.HasSolution,
+                          PlannerStatus.InterruptedWithSolution]:
             raise PlanningError('Retimer returned with status {0:s}.'.format(
                                 str(status)))