Merge pull request #131 from personalrobotics/feature/herbpy_issue22

Feature/herbpy issue22

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,11 +92,11 @@ 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
+                return path
         else:
             raise NotImplementedError('DriveForward is not implemented')
 
@@ -116,11 +113,11 @@ 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
+                return path
         else:
             raise NotImplementedError('Rotate is not implemented')
 
@@ -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
@@ -186,45 +187,51 @@ def GetTrajectoryManipulators(self, traj):
     def PostProcessPath(self, path, defer=False, executor=None,
                         constrained=None, smooth=None, default_timelimit=0.5,
                         shortcut_options=None, smoothing_options=None,
-                        retiming_options=None, **kwargs):
+                        retiming_options=None, affine_retiming_options=None,
+                        **kwargs):
         """ Post-process a geometric path to prepare it for execution.
 
         This method post-processes a geometric path by (optionally) optimizing
-        it and timing it. Three different post-processing pipelines are used:
-
-        1. For constrained trajectories, we do not modify the geometric path
-           and retime the path to be time-optimal. This trajectory must stop
-           at every waypoint. The only exception is for...
-        2. For smooth trajectories, we attempt to fit a time-optimal smooth
+        it and timing it. Four different post-processing pipelines are used:
+
+        1. For base trajectories (i..e affine DOFs), we time the trajectory
+           using self.affine_retimer. This function does not currently support
+           trajectories that contain both regular and affine DOFs.
+        2. For constrained trajectories, we do not modify the geometric path
+           and retime the path to be time-optimal via self.retimer. This
+           trajectory must stop at every waypoint. The only exception is for...
+        3. For smooth trajectories, we attempt to fit a time-optimal smooth
            curve through the waypoints (not implemented). If this curve is
            not collision free, then we fall back on...
-        3. By default, we run a smoother that jointly times and smooths the
-           path. This algorithm can change the geometric path to optimize
-           runtime.
+        4. By default, we run a smoother that jointly times and smooths the
+           path via self.smoother. This algorithm can change the geometric path to
+           optimize runtime.
 
-        The behavior in (1) and (2) can be forced by passing constrained=True
+        The behavior in (2) and (3) can be forced by passing constrained=True
         or smooth=True. By default, the case is inferred by the tag(s) attached
         to the trajectory: (1) is triggered by the CONSTRAINED tag and (2) is
         tiggered by the SMOOTH tag.
 
         Options an be passed to each post-processing routine using the
-        shortcut-options, smoothing_options, and retiming_options **kwargs
-        dictionaries. If no "timelimit" is specified in any of these
-        dictionaries, it defaults to default_timelimit seconds.
+        affine_retiming_options, shortcut_options, smoothing_options, and
+        retiming_options **kwargs dictionaries. If no "timelimit" is specified
+        in any of these dictionaries, it defaults to default_timelimit seconds.
 
         @param path un-timed OpenRAVE trajectory
         @param defer return immediately with a future trajectory
         @param executor executor to use when defer = True
         @param constrained the path is constrained; do not change it
         @param smooth the path is smooth; attempt to execute it directly
         @param default_timelimit timelimit for all operations, if not set
-        @param shortcut_options kwargs to ShortcutPath for shortcutting
-        @param smoothing_options kwargs to RetimeTrajectory for smoothing
-        @param retiming_options kwargs to RetimeTrajectory for timing
+        @param shortcut_options kwargs to self.simplifier
+        @param smoothing_options kwargs to self.smoother
+        @param retiming_options kwargs to self.retimer
+        @param affine_retiming_options kwargs to self.affine_retimer
         @return trajectory ready for execution
         """
         from ..planning.base import Tags
         from ..util import GetTrajectoryTags, CopyTrajectory
+        from openravepy import DOFAffine
 
         # Default parameters.
         if shortcut_options is None:
@@ -233,10 +240,13 @@ def PostProcessPath(self, path, defer=False, executor=None,
             smoothing_options = dict()
         if retiming_options is None:
             retiming_options = dict()
+        if affine_retiming_options is None:
+            affine_retimer_options = dict()
 
         shortcut_options.setdefault('timelimit', default_timelimit)
         smoothing_options.setdefault('timelimit', default_timelimit)
         retiming_options.setdefault('timelimit', default_timelimit)
+        affine_retimer_options.setdefault('timelimit', default_timelimit)
 
         # Read default parameters from the trajectory's tags.
         tags = GetTrajectoryTags(path)
@@ -259,51 +269,75 @@ 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()))
 
+                # Extract active DOFs from teh trajectory and set them as active.
                 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.
 
-                # Directly compute a timing of smooth trajectories.
-                if smooth:
+                if prpy.util.HasAffineDOFs(cspec):
+                    affine_dofs = (DOFAffine.X | DOFAffine.Y | DOFAffine.RotationAxis)
+                    
+                    # Bug in OpenRAVE ExtractUsedIndices function makes 
+                    # dof_indices = affine_dofs. Temporary workaround for that bug.
+                    dof_indices = []
                     logger.warning(
-                        'Post-processing smooth paths is not supported.'
-                        ' Using the default post-processing logic; this may'
-                        ' significantly change the geometric path.'
+                        'Trajectory contains affine DOFs. Any regular DOFs'
+                        ' will be ignored.'
                     )
+                else:
+                    affine_dofs = 0
+
+                cloned_robot.SetActiveDOFs(dof_indices, affine_dofs)
+                logger.debug(
+                    'Setting robot "%s" DOFs %s (affine? %d) as active for'
+                    ' post-processing.',
+                    cloned_robot.GetName(), list(dof_indices), affine_dofs
+                )
 
-                # The trajectory is constrained. Retime it without changing the
-                # geometric path.
-                if constrained:
-                    logger.debug('Retiming a constrained path. The output'
-                                 ' trajectory will stop at every waypoint.')
-                    traj = self.retimer.RetimeTrajectory(
-                        cloned_robot, path, defer=False, **retiming_options)
+                if len(dof_indices) and affine_dofs:
+                    raise ValueError(
+                        'Trajectory contains both affine and regular DOFs.')
+                # Special case for timing affine-only trajectories.
+                elif affine_dofs:
+                    traj = self.affine_retimer.RetimeTrajectory(
+                        cloned_robot, path, defer=False, **affine_retimer_options)
                 else:
-                # The trajectory is not constrained, so we can shortcut it
-                # before execution.
-                    if self.simplifier is not None:
-                        logger.debug('Shortcutting an unconstrained path.')
-                        shortcut_path = self.simplifier.ShortcutPath(
-                            cloned_robot, path, defer=False, **shortcut_options)
+                    # Directly compute a timing of smooth trajectories.
+                    if smooth:
+                        logger.warning(
+                            'Post-processing smooth paths is not supported.'
+                            ' Using the default post-processing logic; this may'
+                            ' significantly change the geometric path.'
+                        )
+
+                    # The trajectory is constrained. Retime it without changing the
+                    # geometric path.
+                    if constrained:
+                        logger.debug('Retiming a constrained path. The output'
+                                     ' trajectory will stop at every waypoint.')
+                        traj = self.retimer.RetimeTrajectory(
+                            cloned_robot, path, defer=False, **retiming_options)
+                    # The trajectory is not constrained, so we can shortcut it
+                    # before execution.
                     else:
-                        logger.debug('Skipping shortcutting; no simplifier'
-                                     ' available.')
-                        shortcut_path = path
-
-                    logger.debug('Smoothing an unconstrained path.')
-                    traj = self.smoother.RetimeTrajectory(
-                        cloned_robot, shortcut_path, defer=False,
-                        **smoothing_options)
+                        if self.simplifier is not None:
+                            logger.debug('Shortcutting an unconstrained path.')
+                            shortcut_path = self.simplifier.ShortcutPath(
+                                cloned_robot, path, defer=False, **shortcut_options)
+                        else:
+                            logger.debug('Skipping shortcutting; no simplifier'
+                                         ' available.')
+                            shortcut_path = path
+
+                        logger.debug('Smoothing an unconstrained path.')
+                        traj = self.smoother.RetimeTrajectory(
+                            cloned_robot, shortcut_path, defer=False,
+                            **smoothing_options)
 
                 return CopyTrajectory(traj, env=self.GetEnv())
 

src/prpy/util.py

@@ -671,3 +671,25 @@ def GeodesicDistance(t1, t2, r=1.0):
     error = GeodesicError(t1, t2)
     error[3] = r*error[3]
     return numpy.linalg.norm(error)
+
+def FindCatkinResource(package, relative_path):
+    '''
+    Find a Catkin resource in the share directory or
+    the package source directory. Raises IOError
+    if resource is not found.
+    
+    @param relative_path Path relative to share or package 
+    source directory
+    @param package The package to search in
+    @return Absolute path to resource 
+    '''
+    from catkin.find_in_workspaces import find_in_workspaces
+    
+    paths = find_in_workspaces(project=package, search_dirs=['share'],
+                               path=relative_path, first_match_only=True)
+
+    if paths and len(paths) == 1:
+        return paths[0]
+    else:
+        raise IOError('Loading resource "{:s}" failed.'.format(
+                      relative_path))