Merge branch 'block_sorting' of https://github.com/personalrobotics/prpy into block_sorting

Author: Shushman Choudhury

.gitignore

@@ -1 +1,7 @@
 *.pyc
+
+# Generated by nosetest
+*.noseids
+
+# Generated by CBiRRT
+**/cmovetraj.txt

catkin.cmake

@@ -5,6 +5,5 @@ catkin_package()
 catkin_python_setup()
 
 if (CATKIN_ENABLE_TESTING)
-    #catkin_add_nosetests(tests/test_PlanningPipeline.py)
-    #catkin_add_nosetests(tests/test_DistanceFieldManager.py)
+    catkin_add_nosetests(tests)
 endif()

src/prpy/base/mico.py

@@ -45,8 +45,12 @@ def __init__(self, sim,
         with env:
             dof_indices = self.GetIndices()
             accel_limits = robot.GetDOFAccelerationLimits()
-            accel_limits[dof_indices[0:3]] = 1.2
-            accel_limits[dof_indices[3:6]] = 1.0
+            accel_limits[dof_indices[0]] = 1.45
+            accel_limits[dof_indices[1]] = 1.56
+            accel_limits[dof_indices[2]] = 1.56
+            accel_limits[dof_indices[3]] = 1.5
+            accel_limits[dof_indices[4]] = 1.48
+            accel_limits[dof_indices[5]] = 1.49
             robot.SetDOFAccelerationLimits(accel_limits)
 
         # Load or_nlopt_ik as the IK solver. Unfortunately, IKFast doesn't work

src/prpy/base/robot.py

@@ -215,8 +215,8 @@ def PostProcessPath(self, path, defer=False, executor=None,
            curve through the waypoints (not implemented). If this curve is
            not collision free, then we fall back on...
         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.
+           path via self.smoother. This algorithm can change the geometric path
+           to optimize runtime.
 
         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
@@ -421,8 +421,8 @@ def do_execute():
         else:
             raise ValueError('Received unexpected value "{:s}" for defer.'.format(str(defer)))
 
-
-    def ExecuteTrajectory(self, traj, defer=False, timeout=None, period=0.01, **kwargs):
+    def ExecuteTrajectory(self, traj, defer=False, timeout=None, period=0.01,
+                          **kwargs):
         """ Executes a time trajectory on the robot.
 
         This function directly executes a timed OpenRAVE trajectory on the
@@ -460,7 +460,7 @@ def ExecuteTrajectory(self, traj, defer=False, timeout=None, period=0.01, **kwar
         # Check that the current configuration of the robot matches the
         # initial configuration specified by the trajectory.
         if not util.IsAtTrajectoryStart(self, traj):
-            raise exceptions.TrajectoryAborted(
+            raise exceptions.TrajectoryNotExecutable(
                 'Trajectory started from different configuration than robot.')
 
         # If there was only one waypoint, at this point we are done!

src/prpy/exceptions.py

@@ -3,26 +3,50 @@ class PrPyException(Exception):
     Generic PrPy exception.
     """
 
-class TrajectoryAborted(PrPyException):
+
+class TrajectoryException(PrPyException):
+    """
+    Trajectory failed to execute.
+    """
+
+
+class TrajectoryNotExecutable(TrajectoryException):
+    """
+    Trajectory could not begin execution.
+
+    This exception typically indicates that some precondition of trajectory
+    execution was violated, such as the robot starting at a different
+    configuration, the trajectory not being in the correct format.
+
+    This exception indicates that the trajectory was not even attempted due to
+    one of these conditions.
+    """
+
+
+class TrajectoryAborted(TrajectoryException):
     """
     Trajectory was aborted.
     """
 
-class TrajectoryStalled(PrPyException):
+
+class TrajectoryStalled(TrajectoryException):
     """
     Trajectory stalled.
     """
 
+
 class SynchronizationException(PrPyException):
     """
     Controller synchronization failed.
     """
 
+
 class SerializationException(PrPyException):
     """
     Serialization failed.
     """
 
+
 class UnsupportedTypeSerializationException(SerializationException):
     """
     Serialization failed due to an unknown type.
@@ -35,6 +59,7 @@ def __init__(self, value):
             'Serializing type "{:s}.{:s}" is not supported.'.format(
                 self.type.__module__, self.type.__name__))
 
+
 class UnsupportedTypeDeserializationException(SerializationException):
     """
     Deserialization failed due to an unknown type.

src/prpy/planning/base.py

@@ -41,12 +41,44 @@
 
 class Tags(object):
     SMOOTH = 'smooth'
+    """
+    The `SMOOTH` tag means waypoints are close enough together that we can
+    approximate derivatives at the waypoints using divided differences. i.e.
+    we can safely fit a spline without collision checking.
+    """
+
     CONSTRAINED = 'constrained'
+    """
+    The `CONSTRAINED` tag means that the geometric path described by these
+    waypoints respects a constraint.  This means that the path cannot be
+    geometrically altered arbitrarily, at the risk of violating the original
+    constraint, it should only be changed in timing.
+    """
+
     PLANNER = 'planner'
+    """
+    The name of the planner used to generate a trajectory.
+    """
+
     METHOD = 'planning_method'
+    """
+    The type of planning call used to generate a trajectory.
+    """
+
     PLAN_TIME = 'planning_time'
+    """
+    The amount of time that was spent by a planner finding a solution.
+    """
+
     POSTPROCESS_TIME = 'postprocess_time'
+    """
+    The amount of time that was spent modifying the trajectory for execution.
+    """
+
     EXECUTION_TIME = 'execution_time'
+    """
+    The amount of time that was spent actually running a trajectory.
+    """
 
 
 class MetaPlanningError(PlanningError):

src/prpy/planning/chomp.py

@@ -74,7 +74,7 @@ def sync(self, robot):
                 # existing distance field if there is a key mismatch.
                 cached_state = self.cache.get(body.GetName(), None)
                 if cached_state is not None and cached_state != current_state:
-                    logger.debug('Clearing distance field for "{:s}".', body_name)
+                    logger.debug('Clearing distance field for "%s".', body_name)
                     self.module.removefield(body)
                     cached_state = None
 

src/prpy/planning/snap.py

@@ -118,14 +118,17 @@ def _Snap(self, robot, goal, **kw_args):
         if check != 0:
             raise PlanningError('Straight line trajectory is not valid.')
 
-        # Create a trajectory that starts at our current configuration.
-        cspec = robot.GetActiveConfigurationSpecification()
+        # Create a two-point trajectory that starts at our current
+        # configuration and takes us to the goal.
         traj = openravepy.RaveCreateTrajectory(self.env, '')
-        traj.Init(cspec)
+        cspec = robot.GetActiveConfigurationSpecification('linear')
+        active_indices = robot.GetActiveDOFIndices()
 
         start_waypoint = numpy.zeros(cspec.GetDOF())
         cspec.InsertJointValues(start_waypoint, start, robot,
                                 active_indices, False)
+
+        traj.Init(cspec)
         traj.Insert(0, start_waypoint.ravel())
 
         # Make the trajectory end at the goal configuration, as long as it

src/prpy/planning/vectorfield.py

@@ -360,5 +360,12 @@ def fn_callback(t, q):
         output_path.Insert(output_path.GetNumWaypoints(),
             path.Sample(t_cache), cspec)
 
+        # Flag this trajectory as constrained.
+        util.SetTrajectoryTags(
+            output_path, {
+                Tags.CONSTRAINED: 'true',
+                Tags.SMOOTH: 'true'
+            }, append=True
+        )
         return output_path
 

src/prpy/planning/workspace.py

@@ -162,7 +162,7 @@ def PlanWorkspacePath(self, robot, traj, timelimit=5.0,
 
             # Create a new trajectory starting at current robot location.
             qtraj = openravepy.RaveCreateTrajectory(self.env, '')
-            qtraj.Init(manip.GetArmConfigurationSpecification())
+            qtraj.Init(manip.GetArmConfigurationSpecification('linear'))
             qtraj.Insert(0, robot.GetActiveDOFValues())
 
             # Initial search for workspace path timing: one huge step.