Floating point precision in comparison of shortest_angular_distance_with_large_limits

[wxmerkt]

I am currently debugging issues for a robot with very large joint limits (>2pi for both left and right limits). In particular, when the current implementation of shortest_angular_distance_with_large_limits returns false.

I traced some of these cases down to:
(a) user input: when from is outside of left_limit --> This is already highlighted in the doxygen as a requirement for the function to work as expected.
(b) machine precision when comparing the clockwise/counter-clockwise options with <=.

I'd like to get your insights into how to best address:

For (a), would it be an option to clamp from to left/right limits as a first step inside the function if outside, or should this be on the user to enforce? This would resolve the current stated limitation for the function to work as expected as highlighted in the function documentation, but would change current existing behaviour. Some users, e.g. the ROS-Control boilerplate controllers, do not currently take this restriction into account and may be unaware of the incorrect function if from is outside the limits. (I will prepare a fix for the JointGroupPositionController).

For (b), this case happens e.g. when we are exactly on the boundary of the limits (clamped outside as a solution to (a)), and with the additional floating point operations i.e. adding the delta/delta_2pi, the conditions are now no longer met with <= (cf.

angles/angles/include/angles/angles.h

Lines 250 to 264 in fe974f2

	// 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;
	}

). One work-around I am currently using is to subtract/add an eps to the limits when clamping before passing the from/to as arguments.. We could use the machine precision epsilon and figure out the number of operations/expected deviation or select a small-enough-to-be-not-noticeable-in-practice value (1e-6, 1e-9). Would that be workable?

Alternatively, and perhaps the "wontfix" solution could be to advise to clamp the from and to arguments to something smaller by eps than the limits. This would of course resolve (b) entirely in user-code.

Here are a few example cases:

from __future__ import print_function
import angles

examples = [
    #[5.95171, 0.331613, 0.331613, 5.95157, 0.66309],  # issue: from value is outside right_limit --> (a), expected behaviour
    #[6.80644, 2, 0.523599, 5.75959, -4.80644],             # issue: from value is outside right_limit --> (a), expected behaviour
    #[9.09866, 0.523599, 0.523599, 5.75959, -2.29187], # issue: from value is outside right_limit --> (a), expected behaviour
    [5.75959, 0.523599, 0.523599, 5.75959, -2.29187], # issue: from is _at_ right_limit, to is _at_ left_limit --> (b), machine precision issue
]
for example in examples:
    print(example[:-1], "=", angles.shortest_angular_distance_with_large_limits(example[0], example[1], example[2], example[3]))

[francofusco]

For (a), would it be an option to clamp from to left/right limits as a first step inside the function if outside, or should this be on the user to enforce?

I would stick to the second option, since the function more or less answers the two questions

• Can I go from her to there while remaining in limits?
• If yes, what would the distance be?

As such, I would not make the function modify the starting point internally, but rather ask the user to do so.

---

Regarding issue (b), I see the point. Your suggestion:

We could use the machine precision epsilon and figure out the number of operations/expected deviation or select a small-enough-to-be-not-noticeable-in-practice value (1e-6, 1e-9). Would that be workable?

seem reasonable to me. And, considering that we are talking about rotations in radians, I believe 1e-9 to be effectively a "small-enough-to-be-not-noticeable-in-practice". However, I would not be able to tell how many "false positives" would be introduced. Perhaps it would be nice to perform a test that estimates them in function of the chosen eps? Or perhaps it does not matter that much and I am just overthinking it 😅

Alternatively, and perhaps the "wontfix" solution could be to advise to clamp the from and to arguments to something smaller by eps than the limits. This would of course resolve (b) entirely in user-code.

Just for completeness: I think only the to argument would need to be clamped, the from is used "as-is" in the comparisons.