Added extra logging to trajopt_sqp to match the output of trajopt_sco

[rjoomen]

[rjoomen]

I used this for direct comparisons. But maybe it's better to add the iteration counter logging from TrajOpt Ifopt to TrajOpt instead:

| ======================================================================================== |
|                                      ROS Industrial                                      |
|                              TrajOpt Ifopt Motion Planning                               |
| ======================================================================================== |
|                            Iteration (Box Size: 0.000437894)                             |
| ---------------------------------------------------------------------------------------- |
|        Overall: 23   |      Convexify: 7    |    Trust Region: 1   |        Penalty: 2   |
| ======================================================================================== |
|      merit |   oldexact |  new_exact | new_approx |    dapprox |     dexact |      ratio |
| ---------------------------------------------------------------------------------------- |

[Levi-Armstrong]

I used this for direct comparisons. But maybe it's better to add the iteration counter logging from TrajOpt Ifopt to TrajOpt instead:

Yea, I think that would be best.