diff --git a/angles/include/angles/angles.h b/angles/include/angles/angles.h index df2b9ed..f66856e 100644 --- a/angles/include/angles/angles.h +++ b/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 * diff --git a/angles/src/angles/__init__.py b/angles/src/angles/__init__.py index b1172eb..c64f170 100644 --- a/angles/src/angles/__init__.py +++ b/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 diff --git a/angles/test/utest.cpp b/angles/test/utest.cpp index 4ce5f55..454fd85 100644 --- a/angles/test/utest.cpp +++ b/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; diff --git a/angles/test/utest.py b/angles/test/utest.py index 3a32f33..13b6a41 100644 --- a/angles/test/utest.py +++ b/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))