Add collision callback to trajopt_sqp

[mpowelson]

Address this.
https://github.com/ros-industrial-consortium/trajopt_ros/blob/9360773d3dd672e5d698ba9535d60fbd338348e1/trajopt_optimizers/trajopt_sqp/src/callbacks/collision_plotter.cpp#L39

[mpowelson]

This should be relatively straightforward. See the plot functions in the old trajopt implementation. You can also look at how the other callbacks work in relation to their old trajopt implementation

https://github.com/ros-industrial-consortium/trajopt_ros/blob/9360773d3dd672e5d698ba9535d60fbd338348e1/trajopt/src/collision_terms.cpp#L985

[Srujan-D]

Hey! I'd like to work with this issue!

[mpowelson]

@Srujan-D If you need any help let us know. Thanks for working on this!

[gavanderhoorn]

@mpowelson: he was in a time-zone which made it difficult to wait on you guys.

@Srujan-D then asked to try and work on something else.

[mpowelson]

Ah. That's fine then.