Added support for "large limits"

angles/include/angles/angles.h

@@ -195,6 +195,79 @@ namespace angles
   }
 
 
+  /*!
+   * \function
+   *
+   * \brief Returns the delta from `from_angle` to `to_angle`, making sure it does not violate limits specified by `left_limit` and `right_limit`.
+   * This function is similar to `shortest_angular_distance_with_limits()`, with the main difference that it accepts limits outside the `[-M_PI, M_PI]` range.
+   * Even if this is quite uncommon, one could indeed consider revolute joints with large rotation limits, e.g., in the range `[-2*M_PI, 2*M_PI]`.
+   *
+   * In this case, a strict requirement is to have `left_limit` smaller than `right_limit`.
+   * Note also that `from` must lie inside the valid range, while `to` does not need to.
+   * In fact, this function will evaluate the shortest (valid) angle `shortest_angle` so that `from+shortest_angle` equals `to` up to an integer multiple of `2*M_PI`.
+   * As an example, a call to `shortest_angular_distance_with_large_limits(0, 10.5*M_PI, -2*M_PI, 2*M_PI, shortest_angle)` will return `true`, with `shortest_angle=0.5*M_PI`.
+   * This is because `from` and `from+shortest_angle` are both inside the limits, and `fmod(to+shortest_angle, 2*M_PI)` equals `fmod(to, 2*M_PI)`.
+   * On the other hand, `shortest_angular_distance_with_large_limits(10.5*M_PI, 0, -2*M_PI, 2*M_PI, shortest_angle)` will return false, since `from` is not in the valid range.
+   * Finally, note that the call `shortest_angular_distance_with_large_limits(0, 10.5*M_PI, -2*M_PI, 0.1*M_PI, shortest_angle)` will also return `true`.
+   * However, `shortest_angle` in this case will be `-1.5*M_PI`.
+   *
+   * \return true if `left_limit < right_limit` and if "from" and "from+shortest_angle" positions are within the valid interval, false otherwise.
+   * \param from - "from" angle.
+   * \param to - "to" angle.
+   * \param left_limit - left limit of valid interval, must be smaller than right_limit.
+   * \param right_limit - right limit of valid interval, must be greater than left_limit.
+   * \param shortest_angle - result of the shortest angle calculation.
+   */
+  static inline bool shortest_angular_distance_with_large_limits(double from, double to, double left_limit, double right_limit, double &shortest_angle)
+  {
+    // Shortest steps in the two directions
+    double delta = shortest_angular_distance(from, to);
+    double delta_2pi = two_pi_complement(delta);
+
+    // "sort" distances so that delta is shorter than delta_2pi
+    if(std::fabs(delta) > std::fabs(delta_2pi))
+      std::swap(delta, delta_2pi);
+
+    if(left_limit > right_limit) {
+      // If limits are something like [PI/2 , -PI/2] it actually means that we
+      // want rotations to be in the interval [-PI,PI/2] U [PI/2,PI], ie, the
+      // half unit circle not containing the 0. This is already gracefully
+      // handled by shortest_angular_distance_with_limits, and therefore this
+      // function should not be called at all. However, if one has limits that
+      // are larger than PI, the same rationale behind shortest_angular_distance_with_limits
+      // does not hold, ie, M_PI+x should not be directly equal to -M_PI+x.
+      // In this case, the correct way of getting the shortest solution is to
+      // properly set the limits, eg, by saying that the interval is either
+      // [PI/2, 3*PI/2] or [-3*M_PI/2, -M_PI/2]. For this reason, here we
+      // return false by default.
+      shortest_angle = delta;
+      return false;
+    }
+
+    // Check in which direction we should turn (clockwise or counter-clockwise).
+
+    // start by trying with the shortest angle (delta).
+    double to2 = from + delta;
+    if(left_limit <= to2 && to2 <= right_limit) {
+      // we can move in this direction: return success if the "from" angle is inside limits
+      shortest_angle = delta;
+      return left_limit <= from && from <= right_limit;
+    }
+
+    // delta is not ok, try to move in the other direction (using its complement)
+    to2 = from + delta_2pi;
+    if(left_limit <= to2 && to2 <= right_limit) {
+      // we can move in this direction: return success if the "from" angle is inside limits
+      shortest_angle = delta_2pi;
+      return left_limit <= from && from <= right_limit;
+    }
+
+    // nothing works: we always go outside limits
+    shortest_angle = delta; // at least give some "coherent" result
+    return false;
+  }
+
+
   /*!
    * \function
    *

angles/src/angles/__init__.py

@@ -177,3 +177,62 @@ def shortest_angular_distance_with_limits(from_angle, to_angle, left_limit, righ
                 shortest_angle = delta_mod_2pi
         return False, shortest_angle
 
+def shortest_angular_distance_with_large_limits(from_angle, to_angle, left_limit, right_limit):
+    """ Returns the delta from `from_angle` to `to_angle`, making sure it does not violate limits specified by `left_limit` and `right_limit`.
+        This function is similar to `shortest_angular_distance_with_limits()`, with the main difference that it accepts limits outside the `[-M_PI, M_PI]` range.
+        Even if this is quite uncommon, one could indeed consider revolute joints with large rotation limits, e.g., in the range `[-2*M_PI, 2*M_PI]`.
+
+        In this case, a strict requirement is to have `left_limit` smaller than `right_limit`.
+        Note also that `from_angle` must lie inside the valid range, while `to_angle` does not need to.
+        In fact, this function will evaluate the shortest (valid) angle `shortest_angle` so that `from_angle+shortest_angle` equals `to_angle` up to an integer multiple of `2*M_PI`.
+        As an example, a call to `shortest_angular_distance_with_large_limits(0, 10.5*M_PI, -2*M_PI, 2*M_PI)` will return `true`, with `shortest_angle=0.5*M_PI`.
+        This is because `from_angle` and `from_angle+shortest_angle` are both inside the limits, and `fmod(to_angle+shortest_angle, 2*M_PI)` equals `fmod(to_angle, 2*M_PI)`.
+        On the other hand, `shortest_angular_distance_with_large_limits(10.5*M_PI, 0, -2*M_PI, 2*M_PI)` will return false, since `from_angle` is not in the valid range.
+        Finally, note that the call `shortest_angular_distance_with_large_limits(0, 10.5*M_PI, -2*M_PI, 0.1*M_PI)` will also return `true`.
+        However, `shortest_angle` in this case will be `-1.5*M_PI`.
+
+        \return valid_flag, shortest_angle - valid_flag will be true if `left_limit < right_limit` and if "from_angle" and "from_angle+shortest_angle" positions are within the valid interval, false otherwise.
+        \param left_limit - left limit of valid interval, must be smaller than right_limit.
+        \param right_limit - right limit of valid interval, must be greater than left_limit.
+    """
+    # Shortest steps in the two directions
+    delta = shortest_angular_distance(from_angle, to_angle)
+    delta_2pi = two_pi_complement(delta)
+
+    # "sort" distances so that delta is shorter than delta_2pi
+    if fabs(delta) > fabs(delta_2pi):
+        delta, delta_2pi = delta_2pi, delta
+
+    if left_limit > right_limit:
+        # If limits are something like [PI/2 , -PI/2] it actually means that we
+        # want rotations to be in the interval [-PI,PI/2] U [PI/2,PI], ie, the
+        # half unit circle not containing the 0. This is already gracefully
+        # handled by shortest_angular_distance_with_limits, and therefore this
+        # function should not be called at all. However, if one has limits that
+        # are larger than PI, the same rationale behind shortest_angular_distance_with_limits
+        # does not hold, ie, M_PI+x should not be directly equal to -M_PI+x.
+        # In this case, the correct way of getting the shortest solution is to
+        # properly set the limits, eg, by saying that the interval is either
+        # [PI/2, 3*PI/2] or [-3*M_PI/2, -M_PI/2]. For this reason, here we
+        # return false by default.
+        return False, delta
+
+
+    # Check in which direction we should turn (clockwise or counter-clockwise).
+
+    # start by trying with the shortest angle (delta).
+    to2 = from_angle + delta
+    if left_limit <= to2 and to2 <= right_limit:
+        # we can move in this direction: return success if the "from" angle is inside limits
+        valid_flag = left_limit <= from_angle and from_angle <= right_limit
+        return valid_flag, delta
+
+    # delta is not ok, try to move in the other direction (using its complement)
+    to2 = from_angle + delta_2pi
+    if left_limit <= to2 and to2 <= right_limit:
+        # we can move in this direction: return success if the "from" angle is inside limits
+        valid_flag = left_limit <= from_angle and from_angle <= right_limit
+        return valid_flag, delta_2pi
+
+    # nothing works: we always go outside limits
+    return False, delta

angles/test/utest.cpp

@@ -73,6 +73,41 @@ TEST(Angles, shortestDistanceWithLimits){
 
 }
 
+
+TEST(Angles, shortestDistanceWithLargeLimits)
+{
+  double shortest_angle;
+  bool result;
+
+  // 'delta' is valid
+  result = angles::shortest_angular_distance_with_large_limits(0, 10.5*M_PI, -2*M_PI, 2*M_PI, shortest_angle);
+  EXPECT_TRUE(result);
+  EXPECT_NEAR(shortest_angle, 0.5*M_PI, 1e-6);
+
+  // 'delta' is not valid, but 'delta_2pi' is
+  result = angles::shortest_angular_distance_with_large_limits(0, 10.5*M_PI, -2*M_PI, 0.1*M_PI, shortest_angle);
+  EXPECT_TRUE(result);
+  EXPECT_NEAR(shortest_angle, -1.5*M_PI, 1e-6);
+
+  // neither 'delta' nor 'delta_2pi' are valid
+  result = angles::shortest_angular_distance_with_large_limits(2*M_PI, M_PI, 2*M_PI-0.1, 2*M_PI+0.1, shortest_angle);
+  EXPECT_FALSE(result);
+
+  // start position outside limits
+  result = angles::shortest_angular_distance_with_large_limits(10.5*M_PI, 0, -2*M_PI, 2*M_PI, shortest_angle);
+  EXPECT_FALSE(result);
+
+  // invalid limits (lower > upper)
+  result = angles::shortest_angular_distance_with_large_limits(0, 0.1, 2*M_PI, -2*M_PI, shortest_angle);
+  EXPECT_FALSE(result);
+
+  // specific test case
+  result = angles::shortest_angular_distance_with_large_limits(0.999507, 1.0, -20*M_PI, 20*M_PI, shortest_angle);
+  EXPECT_TRUE(result);
+  EXPECT_NEAR(shortest_angle, 0.000493, 1e-6);
+}
+
+
 TEST(Angles, from_degrees)
 {
   double epsilon = 1e-9;

angles/test/utest.py

@@ -32,7 +32,7 @@
 #  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 #  POSSIBILITY OF SUCH DAMAGE.
 #********************************************************************/
-from angles import normalize_angle_positive, normalize_angle, shortest_angular_distance, two_pi_complement, shortest_angular_distance_with_limits
+from angles import normalize_angle_positive, normalize_angle, shortest_angular_distance, two_pi_complement, shortest_angular_distance_with_limits, shortest_angular_distance_with_large_limits
 from angles import _find_min_max_delta
 import sys
 import unittest
@@ -106,6 +106,34 @@ def test_shortestDistanceWithLimits(self):
         result, shortest_angle = shortest_angular_distance_with_limits(-pi, pi,-pi,pi)
         self.assertTrue(result)
         self.assertAlmostEqual(shortest_angle,0.0)
+
+    def test_shortestDistanceWithLargeLimits(self):
+        # 'delta' is valid
+        result, shortest_angle = shortest_angular_distance_with_large_limits(0, 10.5*pi, -2*pi, 2*pi)
+        self.assertTrue(result)
+        self.assertAlmostEqual(shortest_angle, 0.5*pi)
+
+        # 'delta' is not valid, but 'delta_2pi' is
+        result, shortest_angle = shortest_angular_distance_with_large_limits(0, 10.5*pi, -2*pi, 0.1*pi)
+        self.assertTrue(result)
+        self.assertAlmostEqual(shortest_angle, -1.5*pi)
+
+        # neither 'delta' nor 'delta_2pi' are valid
+        result, shortest_angle = shortest_angular_distance_with_large_limits(2*pi, pi, 2*pi-0.1, 2*pi+0.1)
+        self.assertFalse(result)
+
+        # start position outside limits
+        result, shortest_angle = shortest_angular_distance_with_large_limits(10.5*pi, 0, -2*pi, 2*pi)
+        self.assertFalse(result)
+
+        # invalid limits (lower > upper)
+        result, shortest_angle = shortest_angular_distance_with_large_limits(0, 0.1, 2*pi, -2*pi)
+        self.assertFalse(result)
+
+        # specific test case
+        result, shortest_angle = shortest_angular_distance_with_large_limits(0.999507, 1.0, -20*pi, 20*pi)
+        self.assertTrue(result)
+        self.assertAlmostEqual(shortest_angle, 0.000493)
 
     def test_normalize_angle_positive(self):
         self.assertAlmostEqual(0, normalize_angle_positive(0))