Future references for NLMPC

[Veng97]

The NLMPC doesn't seem to have a way of setting the future reference output. I guess something similar to the implementation in LMPC, that adds the ability to set a reference output, yr, and reference input, ur. Maybe we can add these two vectors directly in the definition of the ObjectiveFunction handle.

[nicolapiccinelli]

Hello @Veng97, in this case, if you give a look at the test test_vanderpol.cpp the setObjectiveFunction method provides the whole horizon for the input command and the state vector. So what you can do to have references is to capture some external matrices in your code and later use them in the lambda.

mpc::mat<TVAR(pred_hor + 1), TVAR(num_states)> x_r,
mpc::mat<TVAR(pred_hor + 1), TVAR(num_inputs)> u_r,

...

optsolver.setObjectiveFunction([&](
                                    mpc::mat<TVAR(pred_hor + 1), TVAR(num_states)> x,
                                    mpc::mat<TVAR(pred_hor + 1), TVAR(num_inputs)> u,
                                    double) {
    return (x_r.array() - x.array()).square().sum() + (u_r.array() - u.array()).square().sum();
});

What is really missing here is the possibility of directly penalizing the system's output vector, which can be definitely added.

[nicolapiccinelli]

@Veng97 I've added the output vector sequence in the lambda, once we completed your pr I will release a new version containing this

[nicolapiccinelli]

See release 0.2.0