Merge pull request #64 from personalrobotics/feature/SmoothingRefactor

Trajectory execution refactor

Author: jeking04

README.md

@@ -303,6 +303,38 @@ resolve multiple objects in one statement:
 ```
 
 
+## Concurrent Execution
+
+PrPy has native support for [futures](http://en.wikipedia.org/wiki/Futures_and_promises) and
+[coroutines](http://en.wikipedia.org/wiki/Coroutine) to simplify concurrent programming. A
+_future_ encapsulates the execution of a long-running task. We use the concurrency primitives
+provided by the [`trollius` module](http://trollius.readthedocs.org/en/latest/using.html),
+which is a Python 2 backport of the [`asyncio` module](https://docs.python.org/3/library/asyncio.html)
+from Python 3.
+
+We can use these primitives to parallelize planning and execution:
+
+```python
+@coroutine
+def do_plan(robot):
+    # Plan to goal1 and start executing the trajectory.
+    path1 = yield From(robot.PlanToEndEffectorPose(goal1, execute=False))
+    exec1_future = robot.ExecutePath(path1)
+    
+    # Plan from goal1 to goal2.
+    robot.SetDOFValues(GetLastWaypoint(path1))
+    path2 = yield From(robot.PlanToEndEffectorPope(goal2, execute=False))
+    
+    # Wait for path1 to finish executing, then execute path2.
+    exec1 = yield From(exec1_future)
+    exec2 = yield From(robot.ExecutePath(path2))
+    
+    raise Return(path1, path2)
+
+loop = trollius.get_event_loop()
+path = loop.run_until_complete(do_plan(robot))
+```
+
 ## Method Binding
 
 Finally, PrPy offers helper functions for binding custom methods on (i.e.

src/prpy/base/manipulator.py

@@ -153,7 +153,7 @@ def _PlanWrapper(self, planning_method, args, kw_args):
                                                     args, cloned_args)
 
             # Strip inactive DOFs from the trajectory.
-            config_spec = cloned_robot.GetActiveConfigurationSpecification()
+            config_spec = cloned_robot.GetActiveConfigurationSpecification('linear')
             openravepy.planningutils.ConvertTrajectorySpecification(
                 cloned_traj, config_spec
             )
@@ -164,6 +164,6 @@ def _PlanWrapper(self, planning_method, args, kw_args):
 
         # Optionally execute the trajectory.
         if 'execute' not in kw_args or kw_args['execute']:
-            return self.GetRobot().ExecuteTrajectory(traj, **kw_args)
+            return self.GetRobot().ExecutePath(traj, **kw_args)
         else:
             return traj

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)