Log exception string when traj update fails.
Handle singularity at pi when computing wraparound.

- angles::shortest_angular_distance() has a singularity at pi, that can yield
  unexpected results when computing wraparound values. This is now taken into
  account.

- Update unit tests.
Remove unused variable.
Extend controller rostest.

- Pretty much all of the controller code is exercised, except for tolerance
  checking.