debug: specialized obj_nonlinprog! for LinModel in NonLinMPC

The two methods had different number of argument because of a refactor. It was always thus fallbacking to the one for `NonLinModel`. It works but it is less efficient.

Author: franckgaga

src/controller/execute.jl

@@ -155,7 +155,7 @@ function getinfo(mpc::PredictiveController{NT}) where NT<:Real
     U .= U0 .+ mpc.Uop
     Ŷ .= Ŷ0 .+ mpc.Yop
     D̂ .= mpc.D̂0 + mpc.Dop
-    J = obj_nonlinprog!(Ŷ0, U0, mpc, model, Ue, Ŷe, ΔŨ)
+    J = obj_nonlinprog!(Ŷ0, U0, mpc, model, Ue, Ŷe, ΔŨ, Z̃)
     Ŷs = similar(mpc.Yop)
     predictstoch!(Ŷs, mpc, mpc.estim)
     info[:ΔU]   = Z̃[1:mpc.Hc*model.nu]
@@ -387,32 +387,15 @@ end
 iszero_nc(mpc::PredictiveController) = (mpc.con.nc == 0)
 
 """
-    obj_nonlinprog!( _ , _ , mpc::PredictiveController, model::LinModel, Ue, Ŷe, _ , Z̃)
-
-Nonlinear programming objective function when `model` is a [`LinModel`](@ref).
-
-The method is called by the nonlinear optimizer of [`NonLinMPC`](@ref) controllers. It can
-also be called on any [`PredictiveController`](@ref)s to evaluate the objective function `J`
-at specific `Ue`, `Ŷe` and `Z̃`, values. It does not mutate any argument.
-"""
-function obj_nonlinprog!(
-    _, _, mpc::PredictiveController, model::LinModel, Ue, Ŷe, _ , Z̃::AbstractVector{NT}
-) where NT <: Real
-    JQP  = obj_quadprog(Z̃, mpc.H̃, mpc.q̃) + mpc.r[]
-    E_JE = obj_econ(mpc, model, Ue, Ŷe)
-    return JQP + E_JE
-end
-
-"""
-    obj_nonlinprog!(Ȳ, Ū, mpc::PredictiveController, model::SimModel, Ue, Ŷe, ΔŨ)
+    obj_nonlinprog!(Ȳ, Ū, mpc::PredictiveController, model::SimModel, Ue, Ŷe, ΔŨ, Z̃)
 
 Nonlinear programming objective method when `model` is not a [`LinModel`](@ref). The
 function `dot(x, A, x)` is a performant way of calculating `x'*A*x`. This method mutates
 `Ȳ` and `Ū` arguments, without assuming any initial values (it recuperates the values in
 `Ŷe` and `Ue` arguments).
 """
 function obj_nonlinprog!(
-    Ȳ, Ū, mpc::PredictiveController, model::SimModel, Ue, Ŷe, ΔŨ::AbstractVector{NT}
+    Ȳ, Ū, mpc::PredictiveController, model::SimModel, Ue, Ŷe, ΔŨ, ::AbstractVector{NT}
 ) where NT<:Real
     nu, ny = model.nu, model.ny
     # --- output setpoint tracking term ---
@@ -442,6 +425,23 @@ function obj_nonlinprog!(
     return JR̂y + JΔŨ + JR̂u + E_JE
 end
 
+"""
+    obj_nonlinprog!( _ , _ , mpc::PredictiveController, model::LinModel, Ue, Ŷe, ΔŨ, Z̃)
+
+Nonlinear programming objective function when `model` is a [`LinModel`](@ref).
+
+The method is called by the nonlinear optimizer of [`NonLinMPC`](@ref) controllers. It can
+also be called on any [`PredictiveController`](@ref)s to evaluate the objective function `J`
+at specific `Ue`, `Ŷe` and `Z̃`, values. It does not mutate any argument.
+"""
+function obj_nonlinprog!(
+    _, _, mpc::PredictiveController, model::LinModel, Ue, Ŷe, _ , Z̃::AbstractVector{NT}
+) where NT <: Real
+    JQP  = obj_quadprog(Z̃, mpc.H̃, mpc.q̃) + mpc.r[]
+    E_JE = obj_econ(mpc, model, Ue, Ŷe)
+    return JQP + E_JE
+end
+
 "No custom nonlinear constraints `gc` by default, return `gc` unchanged." 
 con_custom!(gc, ::PredictiveController, _ , _, _ ) = gc
 

src/controller/legacy.jl

@@ -32,7 +32,7 @@ function get_optim_functions(mpc::NonLinMPC, ::JuMP.GenericModel{JNT}) where JNT
     # ---------------------- objective function ------------------------------------------- 
     function Jfunc!(Z̃, ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, K0, X̂0, gc, g, geq)
         update_predictions!(ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, K0, X̂0, gc, g, geq, mpc, Z̃)
-        return obj_nonlinprog!(Ŷ0, U0, mpc, model, Ue, Ŷe, ΔŨ)
+        return obj_nonlinprog!(Ŷ0, U0, mpc, model, Ue, Ŷe, ΔŨ, Z̃)
     end
     Z̃_∇J = fill(myNaN, nZ̃)      # NaN to force update_predictions! at first call
     ∇J_context = (

src/controller/nonlinmpc.jl

@@ -585,7 +585,7 @@ function addinfo!(info, mpc::NonLinMPC{NT}) where NT<:Real
     )
     function J!(Z̃, ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, K0, X̂0, gc, g, geq)
         update_predictions!(ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, K0, X̂0, gc, g, geq, mpc, Z̃)
-        return obj_nonlinprog!(Ŷ0, U0, mpc, model, Ue, Ŷe, ΔŨ)
+        return obj_nonlinprog!(Ŷ0, U0, mpc, model, Ue, Ŷe, ΔŨ, Z̃)
     end
     if !isnothing(mpc.hessian)
         _, ∇J, ∇²J = value_gradient_and_hessian(J!, mpc.hessian, mpc.Z̃, J_cache...)
@@ -800,7 +800,7 @@ function get_nonlinobj_op(mpc::NonLinMPC, optim::JuMP.GenericModel{JNT}) where J
     geq::Vector{JNT}                 = zeros(JNT, neq)
     function J!(Z̃, ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, K0, X̂0, gc, g, geq)
         update_predictions!(ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, K0, X̂0, gc, g, geq, mpc, Z̃)
-        return obj_nonlinprog!(Ŷ0, U0, mpc, model, Ue, Ŷe, ΔŨ)
+        return obj_nonlinprog!(Ŷ0, U0, mpc, model, Ue, Ŷe, ΔŨ, Z̃)
     end
     Z̃_J = fill(myNaN, nZ̃)      # NaN to force update at first call
     J_cache = (