Added proper checking for trajectory timing and length.

This adds several checks to the ExecuteTrajectory function:
1) there must be at least one waypoint.
2) the first waypoint must match the robot's current config
   (within joint DOF resolutions)
3) if there is only one waypoint, succeed after the first two checks.
4) if there are multiple waypoints, the duration must be nonzero.

Author: psigen

src/prpy/base/robot.py

@@ -29,8 +29,7 @@
 # POSSIBILITY OF SUCH DAMAGE.
 
 import functools, logging, openravepy, numpy
-import prpy.util
-from .. import bind, named_config, planning, util
+from .. import bind, named_config, exceptions, util
 from ..clone import Clone, Cloned
 from ..tsr.tsrlibrary import TSRLibrary
 from ..planning.base import Sequence, Tags
@@ -281,7 +280,7 @@ def do_postprocess():
                 # Extract active DOFs from teh trajectory and set them as active.
                 dof_indices, _ = cspec.ExtractUsedIndices(self)
 
-                if prpy.util.HasAffineDOFs(cspec):
+                if util.HasAffineDOFs(cspec):
                     affine_dofs = (DOFAffine.X | DOFAffine.Y | DOFAffine.RotationAxis)
 
                     # Bug in OpenRAVE ExtractUsedIndices function makes 
@@ -438,18 +437,33 @@ def ExecuteTrajectory(self, traj, defer=False, timeout=None, period=0.01, **kwar
         @param period poll rate, in seconds, for checking trajectory status
         @return trajectory executed on the robot
         """
+        # Don't execute trajectories that don't have at least one waypoint.
+        if traj.GetNumWaypoints() <= 0:
+            raise ValueError('Trajectory must contain at least one waypoint.')
+
+        # 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(
+                'Trajectory started from different configuration than robot.')
+
+        # If there was only one waypoint, at this point we are done!
+        if traj.GetNumWaypoints() == 1:
+            return traj
 
-        # TODO: Verify that the trajectory is timed.
-        # TODO: Check if this trajectory contains the base.
+        # Verify that the trajectory is timed.
+        if traj.GetDuration() <= 0.0:
+            raise ValueError('Attempted to execute untimed trajectory.')
 
+        # TODO: Check if this trajectory contains the base.
         needs_base = util.HasAffineDOFs(traj.GetConfigurationSpecification())
 
         self.GetController().SetPath(traj)
 
         active_manipulators = self.GetTrajectoryManipulators(traj)
         active_controllers = [
-            active_manipulator.controller \
-            for active_manipulator in active_manipulators \
+            active_manipulator.controller
+            for active_manipulator in active_manipulators
             if hasattr(active_manipulator, 'controller')
         ]
 
@@ -485,8 +499,8 @@ def do_poll():
             util.WaitForControllers(active_controllers, timeout=timeout)
             return traj
         else:
-            raise ValueError('Received unexpected value "{:s}" for defer.'.format(str(defer)))
-
+            raise ValueError('Received unexpected value "{:s}" for defer.'
+                             .format(str(defer)))
 
     def ViolatesVelocityLimits(self, traj):
         """

src/prpy/util.py

@@ -693,3 +693,44 @@ def FindCatkinResource(package, relative_path):
     else:
         raise IOError('Loading resource "{:s}" failed.'.format(
                       relative_path))
+
+
+def IsAtTrajectoryStart(robot, trajectory):
+    """
+    Check if robot's active DOFs match the start configuration of a trajectory.
+
+    Examines the current active DOF values of the specified robot and compares
+    these values to the first waypoint of the specified trajectory.  If every
+    DOF value differs by less than the DOF resolution of the specified
+    joint/axis, then the function returns True.  Otherwise, it returns False.
+
+    @param robot: the robot whose active DOFs will be checked
+    @param trajectory: the trajectory whose start configuration will be checked
+    @returns: True if the robot's active DOFs match the given trajectory
+              False if one or more active DOFs differ by DOF resolution
+    """
+    # Get current active configuration of robot.
+    dof_indices = robot.GetActiveDOFIndices()
+    dof_values = robot.GetActiveDOFValues()
+    dof_resolutions = robot.GetActiveDOFResolutions()
+
+    # Get starting configuration from trajectory.
+    cspec = trajectory.GetConfigurationSpecification()
+    start_values = cspec.ExtractJointValues(
+        trajectory.GetWaypoint(0), robot, dof_indices)
+
+    # Check deviation in each DOF, using OpenRAVE's SubtractValue function.
+    for i, dof_index in enumerate(dof_indices):
+
+        # Look up the Joint and Axis of the DOF from the robot.
+        joint = robot.GetJointFromDOFIndex(dof_index)
+        axis = dof_index - joint.GetDOFIndex()
+
+        # If any joint deviated too much, return False.
+        delta_value = abs(joint.SubtractValue(
+            start_values[i], dof_values[i], axis))
+        if delta_value > dof_resolutions[i]:
+            return False
+
+    # If all joints matched, return True.
+    return True