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, https://github.com/locuslab/mpc.pytorch/commit/206bd939ec1479424221a62ed45ad26830849fcc 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.