joint_trajectory_controller - wraparoundOffset

[progtologist]

Perhaps I have misunderstood the usage of the wraparoundOffset(...) function, but I think the math is wrong in it. The simple version of the math is:

double dist = angles::shortest_angular_distance(previous, next);
if (std::abs(dist) - M_PI < 1e-9)
{
    dist = next > previous ? std::abs(dist) : -std::abs(dist);
}
offset = (previous + dist) - next;

So if one queries for a rotation starting from -90 degrees to 180 degrees, the offset would then be equal to -180.
Therefore the commanded "next" position would evaluate to:

next.position = next.position + offset;

0 degrees, which is not correct.

Am I missing something here?