shortest_angular_distance_with_limits bug

[davetcoleman]

I believe I've found a bug in angles... if I call:

from = 1.04700021
to = 1.04700027
left_limit = 1.047
right_limit = -2.147

result = shortest_angular_distance_with_limits(from, to, left_limit, right_limit, shortest_angle)

I get

shortest_angle = -6.28318525
result = 0

Where the shortest_angle should be a small decimal number. So I guess its calculating the angle all the way around the unit circle, instead of the small sliver of distance between the two positions (my application is the effort-based position controller in ros_control).

[davetcoleman]

@hsu as we discussed

[tfoote]

Might be related:

From https://code.ros.org/trac/ros-pkg/ticket/3379

Lines 235 and 233 both test the same condition and set the same vlaue, yet they are an if/else if.

Are these supposed to be different?

Also the following conditions are not tested:

        230        else //to position is outside the limits
        231        {
        232          find_min_max_delta(to,left_limit,right_limit,min_delta_to,
-->F    233            if(fabs(min_delta_to) < fabs(max_delta_to))
        234              shortest_angle = std::max<double>(delta,delta_mod_2pi)
-->F    235            else if(fabs(min_delta_to) > fabs(max_delta_to))
        236              shortest_angle =  std::min<double>(delta,delta_mod_2pi
        237            else
        238            {
-->T    239              if (fabs(delta) < fabs(delta_mod_2pi))
        240                shortest_angle = delta;
        241              else
        242                shortest_angle = delta_mod_2pi;
  ...
        248      {
        249          find_min_max_delta(to,left_limit,right_limit,min_delta_to,
        250
TF      251            if(fabs(min_delta) < fabs(max_delta))
        252              shortest_angle = std::min<double>(delta,delta_mod_2pi)
-->T    253            else if (fabs(min_delta) > fabs(max_delta))
        254              shortest_angle =  std::max<double>(delta,delta_mod_2pi
        255            else
        256            {
-->     257              if (fabs(delta) < fabs(delta_mod_2pi))
        258                shortest_angle = delta;
        259              else
        260                shortest_angle = delta_mod_2pi;
}}} from http://pub1.willowgarage.com/coverage/ros-pkg-covtest-ubuntu-hardy-x86/report-31/c++/ba54c0234f6bbb45c4e534e539d6147c53330ffe.html

[adolfo-rt]

For the record, someone bumped into this one here: http://answers.ros.org/question/171071/jointpositioncontroller-state-erroneous-error/

[davetcoleman]

The infamous bug continues...

[AtsushiSakai]

I think this issue has been already fixed.

double from = 1.04700021;
double to = 1.04700027;
double  left_limit = 1.047;
double  right_limit = -2.147;

double shortest_angle;
bool result = angles::shortest_angular_distance_with_limits(from, to, left_limit, right_limit, shortest_angle);
cout<<"shortest_angle is "<<shortest_angle<<endl;
cout<<"result is "<<result<<endl;

then, I get

shortest_angle is 6e-08
result is 1

[tfoote]

Thanks, yes. I believe this was resolved by fa86236