Moving wrap_to_interval from tsr code to prpy.util. Updating IsAtTrajectoryStart to use this function when comparing two orientations.

Author: jeking04

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

@@ -720,7 +720,7 @@ def IsAtTrajectoryStart(robot, trajectory):
             return False
 
         # Compare rotation distance
-        rot_delta_value = (current_pose[2] - traj_start[2] + numpy.pi) % (2.*numpy.pi) - numpy.pi
+        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
@@ -1152,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