State error calculation not accounting for wrapping joint positions

[bponsler]

The following line in joint_trajectory_controller_impl.h is not properly handling wrapping joint angles:

state_error_.position[i] = desired_state_.position[i] - current_state_.position[i];

If the current joint position changes from -PI to PI this calculation will produce a large difference value which causes the joint to move very fast toward that new value.

I believe the following would properly account for this issue:

state_error_.position[i] = angles::shortest_angular_distance(current_state_.position[i] - desired_state_.position[i]);

[graiola]

Hi Brett,

Thanks for the detailed report! (#274)
Could I ask you to open a pull request with the code you've attached to this issue, feel free to add a test to our current test-suite. We should see how the rest of the tests behave.