Added support for "large limits"

[francofusco]

This PR proposes a new function to deal with shortest angle calculations in presence of "large limits", i.e., when either left_limit or right_limit are outside the range [ -pi , pi ].

Motivating example

While using the effort_controllers/JointGroupPositionController, I experienced an issue similar to #2. In my specific case, a call to shortest_angular_distance_with_limits was performed with:

from = 0.999507
to = 1.0
left_limit = -62.831853 (-20*pi)
right_limit = 62.831853 (20*pi)

the function returned false, and the angle was evaluated to -6.282692. This is not really an error, since it is written in the documentation that limits are supposed to be within -pi and +pi and in addition the function returned false. However, the validity of the limits is not checked inside shortest_angular_distance_with_limits, and thus it's user's responsibility to make sure that such calls are not performed.

I however think that having shortest_angular_distance_with_limits to work with bounds in [ -pi , pi ] is a strong limitation. I am aware that having limits in a range such as [ -20*pi , 20*pi ] for a revolute joint is uncommon and that a continuous joint would solve the issue without posing many limitations. Nonetheless, I think it would be more robust to explicitly add support for "large limits".

Proposed solution

I implemented the function shortest_angular_distance_with_large_limits, which is supposed to work with bounds larger than pi.

The main difference with shortest_angular_distance_with_limits is that it does not allow "reversed bounds" such as [ 0.75*pi , -0.75*pi ]. Reverse bounds make sense in the unit circle where the rotation x+2*pi is considered exactly the same as x. However, I believe the same should not be true when considering larger bounds, and my function thus requires left_limit < right_limit.

The goal of the function is to return the smallest angle x such that fmod(from+x, 2*pi) equals fmod(to, 2*pi) also ensuring that left_limit <= from+x <= right_limit. Note that to does not need to be in the valid interval. As an example, shortest_angular_distance_with_limits(0, 0.5*pi, -2*pi, 2*pi, sa) will return true with sa=0.5*pi, while shortest_angular_distance_with_limits(0, 0.5*pi, -2*pi, 0.1*pi, sa) will return true with sa=-1.5*pi (since 0.5*pi is larger than right_limit).

I propose to let shortest_angular_distance_with_limits automatically call the new function whenever a bound is larger than pi (in magnitude). Since I know this might break back-compatibility in many ways, I am open to alternative solutions!

Minor changes

• Added few tests in angles/test/utest.cpp
• Added the pthread flag in angles/test/CMakeLists.txt - I had linker errors while building the tests, and followed what reported in gtest's wiki page

[tfoote]

Thanks for the contribution. Adding support for multiple turn joints makes sense for the use cases that need that. I think that keeping that semantically separate from the existing API is important though. Please see my inline notes.

[francofusco]

I followed the reviews and now the changes are limited to the new function and its related tests. I finally added the same function also in the python API for consistency and included the relevant tests.

[tfoote]

Thanks for the updates this looks good.

[francofusco]

Thanks to you for the review!