Non-quadratic slew rate line search

[bamos]

If I understand correctly, 206bd93 does the LQR step with the slew rate penalty in the quadratic cost approximation, but does the line search on the unmodified cost function without the slew rate penalty.

This is non-trivial to fix because we currently have a time-invariant non-quadratic cost function and no easy way to access the nominal control trajectory within it. If we did, the clean solution would be to write a new cost function with the slew rate penalty. Instead, perhaps the easiest way of fixing this would be to add the slew rate to the get_cost function.

[ivandariojr]

That is correct I forgot we were using the true cost for the line search.

[bamos]

I just added an error message for this case so nobody accidentally starts getting silent errors from this.