Merge pull request #149 from personalrobotics/bugfix/simplify_retimer

Bugfix: SimplifyTrajectory calls in retimer

Author: mkoval

src/prpy/base/robot.py

@@ -459,8 +459,7 @@ def ExecuteTrajectory(self, traj, defer=False, timeout=None, period=0.01, **kwar
 
         # Verify that the trajectory is timed by checking whether the first
         # waypoint has a valid deltatime value.
-        cspec = traj.GetConfigurationSpecification()
-        if cspec.ExtractDeltaTime(traj.GetWaypoint(0)) is None:
+        if not util.IsTimedTrajectory(traj):
             raise ValueError('Trajectory cannot be executed, it is not timed.')
 
         # Verify that the trajectory has non-zero duration.

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,10 @@ 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.
+        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,13 @@
 # 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 +59,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 +91,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 +105,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 +182,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 +195,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)))
 

src/prpy/util.py

@@ -735,3 +735,17 @@ def IsAtTrajectoryStart(robot, trajectory):
 
     # If all joints match, return True.
     return True
+
+
+def IsTimedTrajectory(trajectory):
+    """
+    Returns True if the trajectory is timed.
+
+    This function checks whether a trajectory has a valid `deltatime` group,
+    indicating that it is a timed trajectory.
+
+    @param trajectory: an OpenRAVE trajectory
+    @returns: True if the trajectory is timed, False otherwise
+    """
+    cspec = trajectory.GetConfigurationSpecification()
+    return cspec.ExtractDeltaTime(trajectory.GetWaypoint(0)) is not None