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.
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
costfunction 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_costfunction.