Removing need for ExecuteBasePath. Instead base planning now uses ExecutePath.

Author: jeking04

src/prpy/base/mobilebase.py

@@ -31,7 +31,6 @@
 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
@@ -51,8 +50,6 @@ 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())
@@ -95,9 +92,9 @@ def Forward(self, meters, execute=True, direction=None, **kw_args):
                 offset_pose[0:3, 3] = meters * direction
                 goal_pose = numpy.dot(start_pose, offset_pose)
 
-            traj = create_affine_trajectory(self.robot, [ start_pose, goal_pose ])
+            path = create_affine_trajectory(self.robot, [ start_pose, goal_pose ])
             if execute:
-                return self.ExecuteBasePath(traj, **kw_args)
+                return self.robot.ExecutePath(path, **kw_args)
             else:
                 return traj
         else:
@@ -116,9 +113,9 @@ def Rotate(self, angle_rad, execute=True, **kw_args):
             relative_pose = openravepy.matrixFromAxisAngle([ 0., 0., angle_rad ])
             goal_pose = numpy.dot(start_pose, relative_pose)
 
-            traj = create_affine_trajectory(self.robot, [ start_pose, goal_pose ])
+            path = create_affine_trajectory(self.robot, [ start_pose, goal_pose ])
             if execute:
-                return self.ExecuteBasePath(traj, **kw_args)
+                return self.robot.ExecutePath(path, **kw_args)
             else:
                 return traj
         else:
@@ -147,6 +144,7 @@ def DriveStraightUntilForce(self, direction, velocity=0.1, force_threshold=3.0,
             raise NotImplementedError('DriveStraightUntilForce is not implemented')
 
     def _BasePlanWrapper(self, planning_method, args, kw_args):
+
         robot = self.robot
         with Clone(robot.GetEnv()) as cloned_env:
             cloned_robot = cloned_env.Cloned(robot)
@@ -169,41 +167,7 @@ def _BasePlanWrapper(self, planning_method, args, kw_args):
 
             # Optionally execute the trajectory.
             if 'execute' not in kw_args or kw_args['execute']:
-                return self.ExecuteBasePath(traj, **kw_args)
+                return self.robot.ExecutePath(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

@@ -35,7 +35,7 @@
 from ..tsr.tsrlibrary import TSRLibrary
 from ..planning.base import Sequence 
 from ..planning.ompl import OMPLSimplifier
-from ..planning.retimer import HauserParabolicSmoother, ParabolicRetimer
+from ..planning.retimer import HauserParabolicSmoother, OpenRAVEAffineRetimer, ParabolicRetimer
 from ..planning.mac_smoother import MacSmoother
 
 logger = logging.getLogger('robot')
@@ -73,6 +73,7 @@ def __init__(self, robot_name=None):
             HauserParabolicSmoother(),
             self.retimer
         )
+        self.affine_retimer = OpenRAVEAffineRetimer()
 
     def __dir__(self):
         # We have to manually perform a lookup in InstanceDeduplicator because
@@ -259,19 +260,32 @@ def do_postprocess():
                 # in the trajectory as active.
                 env = path.GetEnv()
                 cspec = path.GetConfigurationSpecification()
+
                 used_bodies = cspec.ExtractUsedBodies(env)
                 if self not in used_bodies:
                     raise ValueError(
                         'Robot "{:s}" is not in the trajectory.'.format(
                             self.GetName()))
 
-                dof_indices, _ = cspec.ExtractUsedIndices(self)
-                cloned_robot.SetActiveDOFs(dof_indices)
-                logger.debug(
-                    'Setting robot "%s" DOFs %s as active for post-processing.',
-                    cloned_robot.GetName(), list(dof_indices))
-
-                # TODO: Handle a affine DOF trajectories for the base.
+                # Check for affine dofs in the path
+                has_affine_dofs = prpy.util.HasAffineDOFs(cspec)
+
+                if has_affine_dofs:
+                    cloned_robot.SetActiveDOFs(
+                        [],
+                        affine=(openravepy.DOFAffine.X |
+                                openravepy.DOFAffine.Y |
+                                openravepy.DOFAffine.RotationAxis)
+                        )
+                    logger.debug(
+                        ' Setting robot "%s" affine DOFs as active for post-processing.',
+                        cloned_robot.GetName())
+                else:
+                    dof_indices, _ = cspec.ExtractUsedIndices(self)
+                    cloned_robot.SetActiveDOFs(dof_indices)
+                    logger.debug(
+                        'Setting robot "%s" DOFs %s as active for post-processing.',
+                        cloned_robot.GetName(), list(dof_indices))
 
                 # Directly compute a timing of smooth trajectories.
                 if smooth:
@@ -286,7 +300,11 @@ def do_postprocess():
                 if constrained:
                     logger.debug('Retiming a constrained path. The output'
                                  ' trajectory will stop at every waypoint.')
-                    traj = self.retimer.RetimeTrajectory(
+                    if has_affine_dofs:
+                        retimer = self.affine_retimer
+                    else:
+                        retimer = self.retimer
+                    traj = retimer.RetimeTrajectory(
                         cloned_robot, path, defer=False, **retiming_options)
                 else:
                 # The trajectory is not constrained, so we can shortcut it
@@ -301,7 +319,11 @@ def do_postprocess():
                         shortcut_path = path
 
                     logger.debug('Smoothing an unconstrained path.')
-                    traj = self.smoother.RetimeTrajectory(
+                    if has_affine_dofs:
+                        smoother = self.affine_retimer
+                    else:
+                        smoother = self.smoother
+                    traj = smoother.RetimeTrajectory(
                         cloned_robot, shortcut_path, defer=False,
                         **smoothing_options)
 

src/prpy/planning/sbpl.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 base import BasePlanner, PlanningError, PlanningMethod, UnsupportedPlanningError
+from ..util import SetTrajectoryTags
+from base import BasePlanner, PlanningError, PlanningMethod, UnsupportedPlanningError, Tags
 import openravepy
 
 class SBPLPlanner(BasePlanner):
@@ -108,5 +109,6 @@ def PlanToBasePose(self, robot, goal_pose, timelimit=60.0, return_first=False, *
         if status not in [ PlannerStatus.HasSolution, PlannerStatus.InterruptedWithSolution ]:
             raise PlanningError('Planner returned with status {0:s}.'.format(str(status)))
 
+        SetTrajectoryTags(traj, {Tags.SMOOTH: True}, append=True)
         return traj