Adding support for execution of base trajectories

Author: jeking04

src/prpy/base/mobilebase.py

@@ -31,6 +31,7 @@
 import copy, functools, numpy, openravepy
 from .. import bind
 from prpy.clone import Clone
+from ..planning.retimer import OpenRAVEAffineRetimer
 
 def create_affine_trajectory(robot, poses):
     doft = openravepy.DOFAffine.X | openravepy.DOFAffine.Y | openravepy.DOFAffine.RotationAxis
@@ -50,6 +51,8 @@ def __init__(self, sim, robot):
         self.simulated = sim
         self.robot = robot
 
+        self.retimer = OpenRAVEAffineRetimer()
+
     def __dir__(self):
         # Add planning methods to the tab-completion list.
         method_names = set(self.__dict__.keys())
@@ -167,6 +170,41 @@ def _BasePlanWrapper(self, planning_method, args, kw_args):
 
             # Optionally execute the trajectory.
             if 'execute' not in kw_args or kw_args['execute']:
-                return robot.ExecuteTrajectory(traj, **kw_args)
+                return self.ExecuteBasePath(traj, **kw_args)
             else:
                 return traj
+
+    def ExecuteBasePath(self, path, defer=False, **kwargs):
+
+        def do_execute(path, **kwargs):
+            robot = self.robot
+            with Clone(robot.GetEnv()) as cloned_env:
+
+                cloned_robot = cloned_env.Cloned(robot)
+                cloned_robot.SetActiveDOFs(
+                    [],
+                    affine=(openravepy.DOFAffine.X |
+                            openravepy.DOFAffine.Y |
+                            openravepy.DOFAffine.RotationAxis)
+                    )
+
+                cloned_timed_traj = self.retimer.RetimeTrajectory(cloned_robot, path, defer=False, **kwargs)
+
+                # Copy the trajectory back to the original environment.
+                from ..util import CopyTrajectory
+                timed_traj = CopyTrajectory(cloned_timed_traj, env=robot.GetEnv())
+
+            return robot.ExecuteTrajectory(timed_traj, defer=False, **kwargs)
+
+        if defer:
+            from trollius.executor import get_default_executor
+            from trollius.futures import wrap_future
+
+            executor = kwargs.get('executor') or get_default_executor()
+            return wrap_future(
+                executor.submit(do_execute,
+                    path, **kwargs
+                )
+            )
+        else:
+            return do_execute(path, **kwargs)

src/prpy/base/robot.py

@@ -166,7 +166,7 @@ def ExecutePath(self, path, simplify=True, smooth=True, defer=False,
         def do_execute(path, simplify, smooth, timeout, **kwargs):
 
             with Clone(self.GetEnv()) as cloned_env:
-                traj_dofs = prpy.util.GetTrajectoryIndices(path)
+                traj_dofs = util.GetTrajectoryIndices(path)
                 cloned_env.Cloned(self).SetActiveDOFs(traj_dofs)
                 cloned_robot = cloned_env.Cloned(self)
 
@@ -203,7 +203,7 @@ def ExecuteTrajectory(self, traj, defer=False, timeout=None, period=0.01, **kw_a
         # TODO: Check if this trajectory contains the base.
 
         logger.debug('Begin ExecuteTrajectory')
-        needs_base = False
+        needs_base = util.HasAffineDOFs(traj.GetConfigurationSpecification())
 
         self.GetController().SetPath(traj)
 
@@ -238,9 +238,7 @@ def do_poll():
 
             return trollius.async(do_poll())
         else:
-            import prpy.viz
-            with prpy.viz.RenderTrajectory(self, traj):
-                util.WaitForControllers(active_controllers, timeout=timeout)
+            util.WaitForControllers(active_controllers, timeout=timeout)
 
         return traj
 

src/prpy/planning/retimer.py

@@ -29,8 +29,8 @@
 # POSSIBILITY OF SUCH DAMAGE.
 
 import logging, numpy, openravepy, os, tempfile
-from ..util import CopyTrajectory, SimplifyTrajectory
-from base import BasePlanner, PlanningError, PlanningMethod
+from ..util import CopyTrajectory, SimplifyTrajectory, HasAffineDOFs
+from base import BasePlanner, PlanningError, PlanningMethod, UnsupportedPlanningError
 from openravepy import PlannerStatus 
 
 
@@ -44,6 +44,9 @@ def RetimeTrajectory(self, robot, path, **kw_args):
         RetimeTrajectory = openravepy.planningutils.RetimeTrajectory
 
         cspec = path.GetConfigurationSpecification()
+        if HasAffineDOFs(cspec):
+            raise UnsupportedPlanningError('OpenRAVERetimer does not support paths with affine DOFs')
+
         """
         group = cspec.GetGroupFromName('joint_values')
         if group.interpolation != 'linear':
@@ -83,3 +86,53 @@ def __init__(self):
 class ParabolicSmoother(OpenRAVERetimer):
     def __init__(self):
         super(ParabolicSmoother, self).__init__('HauserParabolicSmoother')
+
+class OpenRAVEAffineRetimer(BasePlanner):
+
+    def __init__(self,):
+        super(OpenRAVEAffineRetimer, self).__init__()
+
+    @PlanningMethod
+    def RetimeTrajectory(self, robot, path, **kw_args):
+
+        RetimeTrajectory = openravepy.planningutils.RetimeAffineTrajectory
+
+        cspec = path.GetConfigurationSpecification()
+
+        # Check for anything other than affine dofs in the traj
+        # TODO: Better way to do this?
+        affine_groups = ['affine_transform',
+                         'affine_velocities',
+                         'affine_accelerations']
+
+        for group in cspec.GetGroups():
+            found = False
+            for group_name in affine_groups:
+                if group_name in group.name: # group.name contains more stuff than just the name
+                    found = True
+                    break
+
+            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.
+        output_traj = CopyTrajectory(path, env=self.env)
+
+        # Compute the timing. This happens in-place.
+        max_velocities = [robot.GetAffineTranslationMaxVels()[0],
+                          robot.GetAffineTranslationMaxVels()[1],
+                          robot.GetAffineRotationAxisMaxVels()[2]]
+        max_accelerations = [3.*v for v in max_velocities]
+        status = RetimeTrajectory(output_traj, max_velocities, max_accelerations,
+                                  False)
+
+        if status not in [ PlannerStatus.HasSolution,
+                           PlannerStatus.InterruptedWithSolution ]:
+            raise PlanningError('Retimer returned with status {0:s}.'.format(
+                                str(status)))
+
+        return output_traj