Merge pull request #146 from personalrobotics/bugfix/execute_trajectory

Added proper checking for trajectory timing and length.

Author: mkoval

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 
@@ -422,7 +421,7 @@ def ExecuteTrajectory(self, traj, defer=False, timeout=None, period=0.01, **kwar
         will be raised). If timeout is a float (including timeout = 0), this
         function will return None once the timeout has ellapsed, even if the
         trajectory is still being executed.
-        
+
         NOTE: We suggest that you either use timeout=None or defer=True. If
         trajectory execution times out, there is no way to tell whether
         execution was successful or not. Other values of timeout are only
@@ -438,18 +437,39 @@ 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:
+            if defer is True:
+                import trollius
+                future = trollius.Future()
+                future.set_result(traj)
+                return future
+            else:
+                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 +505,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,45 @@ def FindCatkinResource(package, relative_path):
     else:
         raise IOError('Loading resource "{:s}" failed.'.format(
                       relative_path))
+
+
+def IsAtTrajectoryStart(robot, trajectory):
+    """
+    Check if robot's DOFs match the start configuration of a trajectory.
+
+    This function examines the current DOF values of the specified robot and
+    compares these values to the first waypoint of the specified trajectory.
+    If every DOF value specified in the trajectory differs by less than the
+    DOF resolution of the specified joint/axis then it will return 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 used indices and starting configuration from trajectory.
+    cspec = trajectory.GetConfigurationSpecification()
+    dof_indices, _ = cspec.ExtractUsedIndices(robot)
+    traj_values = cspec.ExtractJointValues(
+        trajectory.GetWaypoint(0), robot, dof_indices)
+
+    # Get current configuration of robot for used indices.
+    with robot.GetEnv():
+        robot_values = robot.GetDOFValues(dof_indices)
+        dof_resolutions = robot.GetDOFResolutions(dof_indices)
+
+    # Check deviation in each DOF, using OpenRAVE's SubtractValue function.
+    dof_infos = zip(dof_indices, traj_values, robot_values, dof_resolutions)
+    for dof_index, traj_value, robot_value, dof_resolution in dof_infos:
+        # 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 deviates too much, return False.
+        delta_value = abs(joint.SubtractValue(traj_value, robot_value, axis))
+        if delta_value > dof_resolution:
+            return False
+
+    # If all joints match, return True.
+    return True