Ability to add both inputs and input_change cost to quad cost

[gacamilo]

I've noticed that when you use quad_stage_cost it is not possible to use both add_inputs and add_inputs_change for the same set of inputs which is commonly required in many MPC problems. It results in the error:

TypeError("Two different varying trajectory for the same states are not allowed.")

I got around that by modifying _check_options in modeling.py like so:

if check_if_has_duplicates(self.name_varying_trajectories) and references is not None:
            raise TypeError("Two different varying trajectory for the same states are not allowed.")

I enabled IPOPT output and tested on the problem I am trying to solve, the objective values do change when I use add_inputs and add or remove add_inputs_change, and when I run the controller for a long trajectory I can see a difference in the solutions, the addition of add_inputs_change does smooth the control actions.

I don't know if this change breaks anything else in the library or if it is the right solution, so I thought I'd bring it up here as it would be nice to enable both add_inputs and add_inputs_change. What are your thoughts on that? Am I breaking something else by modifying that line?

On a separate note, I have noticed a huge speed up when I pass "expand": True to _nlp_opts, it would be useful to have that as an optional argument on NMPC.setup().

p.s. This library is awesome!

[brunomorampc]

Hi @camilogonzalez97
thank a lot for your feedback the detailed report :).
You are right this should not happen. Nevertheless, I tried to add both add_inputs and add_inputs_change on a simple model I had and it worked for me. Could you send us a minimum non-working example?
Thanks!

Here is the code I run,

import casadi as ca
import numpy as np
from hilo_mpc import NMPC, Model, SimpleControlLoop
 
model = Model(plot_backend='bokeh')
# Constants
M = 5.

# States and algebraic variables
xx = model.set_dynamical_states(['x', 'vx', 'y', 'vy'])
model.set_measurements(['y_x', 'y_vx', 'y_y', 'y_vy'])
model.set_measurement_equations([xx[0], xx[1], xx[2], xx[3]])
x = xx[0]
vx = xx[1]
y = xx[2]
vy = xx[3]
# Inputs
F = model.set_inputs(['Fx', 'Fy'])
Fx = F[0]
Fy = F[1]
# ODEs
dd = ca.SX.sym('dd', 4)
dd[0] = vx
dd[1] = Fx / M
dd[2] = vy
dd[3] = Fy / M

model.set_equations(ode=dd)

# Initial conditions
x0 = [0, 0, 0, 0]
u0 = [0., 0.]

# Create model and run simulation
dt = 0.1
model.setup(dt=dt)

nmpc = NMPC(model)
nmpc.quad_stage_cost.add_states(names=['x', 'y'], weights=[10, 10], ref=[1, 1])
nmpc.quad_terminal_cost.add_states(names=['x', 'y'], weights=[10, 10], ref=[1,1])
nmpc.quad_stage_cost.add_inputs_change(names=['Fx'], weights=[10])
nmpc.quad_stage_cost.add_inputs(names=['Fx'], weights=[10])
nmpc.horizon = 10
nmpc.set_initial_guess(x_guess=x0, u_guess=u0)
nmpc.setup()

model.set_initial_conditions(x0=x0)

u = nmpc.optimize(x0)

[gacamilo]

Hey @brunomorampc,

It seems that this only happens when you have another quad cost with trajectory_tracking=True defined before add_inputs and add_inputs_change. Here is a short example (but the behaviour can also be triggered in the example you provided)

from hilo_mpc.library.models import cstr_schaffner_and_zeitz
from hilo_mpc import NMPC

if __name__ == "__main__":
    model = cstr_schaffner_and_zeitz()
    model.setup(dt=1)
    x0 = [0, 0]
    model.set_initial_conditions(x0)

    nmpc = NMPC(model)
    nmpc.horizon = 10
    nmpc.quad_stage_cost.add_states(names=["x_1", "x_2"], ref=[0, 0], weights=[10, 10])
    nmpc.quad_terminal_cost.add_states(names=["x_1", "x_2"], ref=[0, 0], weights=[10, 10])
    nmpc.quad_stage_cost.add_measurements(names=["y"],
                                            trajectory_tracking=True,
                                            weights=2)
    nmpc.quad_stage_cost.add_inputs(names=["u"], ref=[0], weights=[2])
    nmpc.quad_stage_cost.add_inputs_change(names=["u"], weights=[1]) # Will throw error

But if I change the order of the costs, it works.

from hilo_mpc.library.models import cstr_schaffner_and_zeitz
from hilo_mpc import NMPC

if __name__ == "__main__":
    model = cstr_schaffner_and_zeitz()
    model.setup(dt=1)
    x0 = [0, 0]
    model.set_initial_conditions(x0)

    nmpc = NMPC(model)
    nmpc.horizon = 10
    nmpc.quad_stage_cost.add_states(names=["x_1", "x_2"], ref=[0, 0], weights=[10, 10])
    nmpc.quad_terminal_cost.add_states(names=["x_1", "x_2"], ref=[0, 0], weights=[10, 10])
    nmpc.quad_stage_cost.add_inputs(names=["u"], ref=[0], weights=[2])
    nmpc.quad_stage_cost.add_inputs_change(names=["u"], weights=[1]) # Does not throw error
    nmpc.quad_stage_cost.add_measurements(names=["y"],
                                            trajectory_tracking=True,
                                            weights=2)

Btw, which version of Casadi do you use with the library? When I install with pip I get Casadi 3.6.3, and if I try to run the example that you sent, I get the error:

RuntimeError: Error in Function::Function for 'function' [SXFunction] at .../casadi/core/function.cpp:224:
.../casadi/core/function_internal.cpp:145: Error calling SXFunction::init for 'function':
.../casadi/core/sx_function.cpp:476: function::init: Initialization failed since variables [Fx_old] are free. These symbols occur in the output expressions but you forgot to declare these as inputs. Set option 'allow_free' to allow free variables.

I've encountered that before with other problems I am working on, the only integrators that work for me are cvodes, idas or discrete. To use collocation I have to modify _collocation() in modeling.py and add allow_free to the Casadi function line. I've tried the same for the rk integrators but it only works for some problems.

[brunomorampc]

Hi @camilogonzalez97 , sorry for the late answer. Yes, I am familiar with that error, I think we are using 3.4.5, the problem occourse with the last versions of casadi. We'll have to figure out why.

thanks for providing an example, I'll check it probably next week as I do not have my computer at the moment