Merge pull request #188 from personalrobotics/bugfix/issue153

Made IsAtTrajectoryStart work for affine DOFs.

Author: mkoval

src/prpy/base/robot.py

@@ -457,6 +457,13 @@ def ExecuteTrajectory(self, traj, defer=False, timeout=None, period=0.01,
         if traj.GetNumWaypoints() <= 0:
             raise ValueError('Trajectory must contain at least one waypoint.')
 
+        # Check if this trajectory contains both affine and joint DOFs
+        cspec = traj.GetConfigurationSpecification()
+        needs_base = util.HasAffineDOFs(cspec)
+        needs_joints = util.HasJointDOFs(cspec)
+        if needs_base and needs_joints:
+            raise ValueError('Trajectories with affine and joint DOFs are not supported')
+
         # Check that the current configuration of the robot matches the
         # initial configuration specified by the trajectory.
         if not util.IsAtTrajectoryStart(self, traj):
@@ -485,9 +492,6 @@ def ExecuteTrajectory(self, traj, defer=False, timeout=None, period=0.01,
                           ' function that produced this trajectory to return a'
                           ' single-waypoint trajectory.', FutureWarning)
 
-        # TODO: Check if this trajectory contains the base.
-        needs_base = util.HasAffineDOFs(traj.GetConfigurationSpecification())
-
         self.GetController().SetPath(traj)
 
         active_manipulators = self.GetTrajectoryManipulators(traj)

src/prpy/tsr/tsr.py

@@ -36,26 +36,6 @@
 EPSILON = 0.001
 
 
-def wrap_to_interval(angles, lower=-pi):
-    """
-    Wraps an angle into a semi-closed interval of width 2*pi.
-
-    By default, this interval is `[-pi, pi)`.  However, the lower bound of the
-    interval can be specified to wrap to the interval `[lower, lower + 2*pi)`.
-
-    If `lower` is an array the same length as angles, the bounds will be
-    applied element-wise to each angle in `angles`.
-
-    See: http://stackoverflow.com/a/32266181
-
-    @param angles an angle or 1D array of angles to wrap
-    @type  angles float or numpy.array
-    @param lower optional lower bound on wrapping interval
-    @type  lower float or numpy.array
-    """
-    return (angles - lower) % (2*pi) + lower
-
-
 class TSR(object):
     """ A Task-Space-Region (TSR) represents a motion constraint. """
     def __init__(self, T0_w=None, Tw_e=None, Bw=None,
@@ -82,6 +62,7 @@ def __init__(self, T0_w=None, Tw_e=None, Bw=None,
         Bw_interval = Bw_cont[3:6, 1] - Bw_cont[3:6, 0]
         Bw_interval = numpy.minimum(Bw_interval, 2*pi)
 
+        from prpy.util import wrap_to_interval
         Bw_cont[3:6, 0] = wrap_to_interval(Bw_cont[3:6, 0])
         Bw_cont[3:6, 1] = Bw_cont[3:6, 0] + Bw_interval
 
@@ -200,6 +181,7 @@ def rpy_within_bounds(rpy, Bw):
         @return check a (3,) vector of True if within and False if outside
         """
         # Unwrap rpy to Bw_cont.
+        from prpy.util import wrap_to_interval
         rpy = wrap_to_interval(rpy, lower=Bw[:, 0])
 
         # Check bounds condition on RPY component.
@@ -398,6 +380,7 @@ def sample_xyzrpy(self, xyzrpy=NANBW):
                                 if numpy.isnan(x) else x
                                 for i, x in enumerate(xyzrpy)])
         # Unwrap rpy to [-pi, pi]
+        from prpy.util import wrap_to_interval
         Bw_sample[3:6] = wrap_to_interval(Bw_sample[3:6])
         return Bw_sample
 

src/prpy/util.py

@@ -96,17 +96,23 @@ def CreatePlannerParametersString(options, params=None,
 
     return lxml.etree.tostring(params_xml)
 
+def HasGroup(cspec, group_name):
+    try:
+        cspec.GetGroupFromName(group_name)
+        return True
+    except openravepy.openrave_exception:
+        return False
+
 def HasAffineDOFs(cspec):
-    def has_group(cspec, group_name):
-        try:
-            cspec.GetGroupFromName(group_name)
-            return True
-        except openravepy.openrave_exception:
-            return False
+    return (HasGroup(cspec, 'affine_transform')
+         or HasGroup(cspec, 'affine_velocities')
+         or HasGroup(cspec, 'affine_accelerations'))
 
-    return (has_group(cspec, 'affine_transform')
-         or has_group(cspec, 'affine_velocities')
-         or has_group(cspec, 'affine_accelerations'))
+def HasJointDOFs(cspec):
+    return (HasGroup(cspec, 'joint_values')
+            or HasGroup(cspec, 'joint_velocities')
+            or HasGroup(cspec, 'joint_accelerations')
+            or HasGroup(cspec, 'joint_torques'))
 
 def GetTrajectoryIndices(traj):
     try:
@@ -689,28 +695,58 @@ def IsAtTrajectoryStart(robot, trajectory):
     @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:
+    needs_base = HasAffineDOFs(cspec)
+    needs_joints = HasJointDOFs(cspec)
+    
+    if needs_base and needs_joints:
+        raise ValueError('Trajectories with affine and joint DOFs are not supported')
+
+    if trajectory.GetEnv() != robot.GetEnv():
+        raise ValueError('The environment attached to the trajectory does not match the environment attached to the robot')
+
+    if needs_base:
+        doft = openravepy.DOFAffine.X | openravepy.DOFAffine.Y | openravepy.DOFAffine.RotationAxis
+        current_pose = openravepy.RaveGetAffineDOFValuesFromTransform(robot.GetTransform(), doft)
+        start_transform = numpy.eye(4)
+        start_transform = cspec.ExtractTransform(start_transform, trajectory.GetWaypoint(0), robot)
+        traj_start = openravepy.RaveGetAffineDOFValuesFromTransform(start_transform, doft)
+
+        # Compare translation distance
+        trans_delta_value = abs(current_pose[:2] - traj_start[:2])
+        trans_resolution = robot.GetAffineTranslationResolution()[:2]
+        if trans_delta_value[0] > trans_resolution[0] or \
+           trans_delta_value[1] > trans_resolution[1]:
             return False
+        
+        # Compare rotation distance
+        rot_delta_value = abs(wrap_to_interval(current_pose[2] - traj_start[2]))
+        rot_resolution = robot.GetAffineRotationAxisResolution()[2] # Rotation about z?
+        if rot_delta_value > rot_resolution:
+            return False
+
+    else:
+        # Get used indices and starting configuration from trajectory.
+        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
@@ -1116,3 +1152,17 @@ def BodyPointsStateFromTraj(bodypoints, traj, time, derivatives=[0, 1, 2]):
     """
     return BodyPointsStatesFromTraj(bodypoints, traj, (time,), derivatives)[0]
 
+def wrap_to_interval(angles, lower=-numpy.pi):
+    """
+    Wraps an angle into a semi-closed interval of width 2*pi.
+    By default, this interval is `[-pi, pi)`.  However, the lower bound of the
+    interval can be specified to wrap to the interval `[lower, lower + 2*pi)`.
+    If `lower` is an array the same length as angles, the bounds will be
+    applied element-wise to each angle in `angles`.
+    See: http://stackoverflow.com/a/32266181
+    @param angles an angle or 1D array of angles to wrap
+    @type  angles float or numpy.array
+    @param lower optional lower bound on wrapping interval
+    @type  lower float or numpy.array
+    """
+    return (angles - lower) % (2*numpy.pi) + lower