locuslab / mpc.pytorch

A fast and differentiable model predictive control (MPC) solver for PyTorch.
https://locuslab.github.io/mpc.pytorch/
MIT License
872 stars 146 forks source link

Why is the last state from the trajectory removed? #37

Open traikodinev opened 2 years ago

traikodinev commented 2 years ago

In (LQRStep.lqr_forward) https://github.com/locuslab/mpc.pytorch/blob/master/mpc/lqr_step.py, we have the following:

if t < T-1:
    if isinstance(true_dynamics, mpc.LinDx):
        F, f = true_dynamics.F, true_dynamics.f
        new_xtp1 = util.bmv(F[t], new_xut)
        if f is not None and f.nelement() > 0:
            new_xtp1 += f[t]
    else:
        new_xtp1 = true_dynamics(
            Variable(new_xt), Variable(new_ut)).data

    new_x.append(new_xtp1)
    dx.append(new_xtp1 - x[t+1])

(line 218 onwards)

This effectively removes the last state from the trajectory. If there is a cost associated with it, however, doesn't that make the gradient computation incorrect? We won't have dx (or the final x) in the backward function of the same class.

Note that further down the same function, the cost is in fact updated:

if isinstance(true_cost, mpc.QuadCost):
    C, c = true_cost.C, true_cost.c
    obj = 0.5*util.bquad(new_xut, C[t]) + util.bdot(new_xut, c[t])
else:
    obj = true_cost(new_xut)

which means the terminal state contributes to the cost, but is dropped from the list of states used elsewhere.

bamos commented 2 years ago

Hmm, I don't remember why I implemented it like this and at a first glance it indeed sounds wrong to ignore the last state, especially if there is a terminal cost