Feature Request: Add debugging info to collision terms

[mpowelson]

A common failure mode is some link that is always in collision. Sometimes this can be difficult to find. It would be nice if you could enable some debug printouts that tell you what collisions there are at each step. A simplistic output could be something like below in the collision evaluators

if (super_verbose)
  std::cout << "links: " << res.link_names[0] << ", " << res.link_names[1] << "   distance: " << res.distance << std::endl;

[mpowelson]

I should note, that we should do it for both sets of collision terms, trajopt (to be deprecated soon?) and trajopt_ifopt!