added: test calling JE and gc functions in NonLinMPC constructor

franckgaga · franckgaga · commit 3d98d6d99efa · 2024-11-26T14:42:21.000-05:00
to guide the user with simple bugs
diff --git a/src/controller/execute.jl b/src/controller/execute.jl
@@ -120,8 +120,8 @@ function getinfo(mpc::PredictiveController{NT}) where NT<:Real
     Ȳ, Ū        = similar(mpc.Yop), similar(mpc.Uop)
     Ŷe, Ue      = Vector{NT}(undef, nŶe), Vector{NT}(undef, nUe)
     Ŷ0, x̂0end = predict!(Ŷ0, x̂0, x̂0next, u0, û0, mpc, model, mpc.ΔŨ)
-    Ŷe, Ue    = extended_predictions!(Ŷe, Ue, Ū, mpc, model, Ŷ0, mpc.ΔŨ)
-    J         = obj_nonlinprog!(Ȳ, Ū, mpc, model, Ŷe, Ue, mpc.ΔŨ)
+    Ue, Ŷe    = extended_predictions!(Ue, Ŷe, Ū, mpc, model, Ŷ0, mpc.ΔŨ)
+    J         = obj_nonlinprog!(Ȳ, Ū, mpc, model, Ue, Ŷe, mpc.ΔŨ)
     U  = Ū
     U .= @views Ue[1:end-model.nu]
     Ŷ  = Ȳ
@@ -362,28 +362,28 @@ function predict!(Ŷ0, x̂0, x̂0next, u0, û0, mpc::PredictiveController, mod
 end
 
 """
-    extended_predictions!(Ŷe, Ue, Ū, mpc, model, Ŷ0, ΔŨ) -> Ŷe, Ue
+    extended_predictions!(Ue, Ŷe, Ū, mpc, model, Ŷ0, ΔŨ) -> Ŷe, Ue
 
-Compute the extended predictions `Ŷe` and `Ue` for the nonlinear optimization.
+Compute the extended vectors `Ue` and `Ŷe` and  for the nonlinear optimization.
 
-The function mutates `Ŷe`, `Ue` and `Ū` in arguments, without assuming any initial values.
+The function mutates `Ue`, `Ŷe` and `Ū` in arguments, without assuming any initial values.
 """
-function extended_predictions!(Ŷe, Ue, Ū, mpc, model, Ŷ0, ΔŨ)
+function extended_predictions!(Ue, Ŷe, Ū, mpc, model, Ŷ0, ΔŨ)
     ny, nu = model.ny, model.nu
-    # --- extended output predictions Ŷe = [ŷ(k); Ŷ] ---
-    Ŷe[1:ny]     .= mpc.ŷ
-    Ŷe[ny+1:end] .= Ŷ0 .+ mpc.Yop
     # --- extended manipulated inputs Ue = [U; u(k+Hp-1)] ---
     U0 = Ū
     U0 .= mul!(U0, mpc.S̃, ΔŨ) .+ mpc.T_lastu0
     Ue[1:end-nu] .= U0 .+ mpc.Uop
     # u(k + Hp) = u(k + Hp - 1) since Δu(k+Hp) = 0 (because Hc ≤ Hp):
     Ue[end-nu+1:end] .= @views Ue[end-2nu+1:end-nu]
-    return Ŷe, Ue
+    # --- extended output predictions Ŷe = [ŷ(k); Ŷ] ---
+    Ŷe[1:ny]     .= mpc.ŷ
+    Ŷe[ny+1:end] .= Ŷ0 .+ mpc.Yop
+    return Ue, Ŷe
 end
 
 """
-    obj_nonlinprog!( _ , _ , mpc::PredictiveController, model::LinModel, Ŷe, Ue, ΔŨ)
+    obj_nonlinprog!( _ , _ , mpc::PredictiveController, model::LinModel, Ue, Ŷe, ΔŨ)
 
 Nonlinear programming objective function when `model` is a [`LinModel`](@ref).
 
@@ -392,23 +392,23 @@ also be called on any [`PredictiveController`](@ref)s to evaluate the objective
 at specific `Ue`, `Ŷe` and `ΔŨ`, values. It does not mutate any argument.
 """
 function obj_nonlinprog!(
-    _, _, mpc::PredictiveController, model::LinModel, Ŷe, Ue, ΔŨ::AbstractVector{NT}
+    _, _, mpc::PredictiveController, model::LinModel, Ue, Ŷe, ΔŨ::AbstractVector{NT}
 ) where NT <: Real
     JQP  = obj_quadprog(ΔŨ, mpc.H̃, mpc.q̃) + mpc.r[]
-    E_JE = obj_econ!(Ue, Ŷe, mpc, model)
+    E_JE = obj_econ(mpc, model, Ue, Ŷe)
     return JQP + E_JE
 end
 
 """
-    obj_nonlinprog!(Ȳ, Ū, mpc::PredictiveController, model::SimModel, Ŷe, Ue, ΔŨ)
+    obj_nonlinprog!(Ȳ, Ū, mpc::PredictiveController, model::SimModel, Ue, Ŷe, ΔŨ)
 
 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
 `Ȳ` and `Ū` arguments, without assuming any initial values (it recuperates the values in
 `Ŷe` and `Ue` arguments).
 """
 function obj_nonlinprog!(
-    Ȳ, Ū, mpc::PredictiveController, model::SimModel, Ŷe, Ue, ΔŨ::AbstractVector{NT}
+    Ȳ, Ū, mpc::PredictiveController, model::SimModel, Ue, Ŷe, ΔŨ::AbstractVector{NT}
 ) where NT<:Real
     nu, ny = model.nu, model.ny
     # --- output setpoint tracking term ---
@@ -426,12 +426,12 @@ function obj_nonlinprog!(
         JR̂u = 0.0
     end
     # --- economic term ---
-    E_JE = obj_econ!(Ue, Ŷe, mpc, model)
+    E_JE = obj_econ(mpc, model, Ue, Ŷe)
     return JR̂y + JΔŨ + JR̂u + E_JE
 end
 
 "By default, the economic term is zero."
-obj_econ!( _ , _ , ::PredictiveController, ::SimModel) = 0.0
+obj_econ(::PredictiveController, ::SimModel, _ , _ ) = 0.0
 
 @doc raw"""
     optim_objective!(mpc::PredictiveController) -> ΔŨ
diff --git a/src/controller/linmpc.jl b/src/controller/linmpc.jl
@@ -175,6 +175,7 @@ LinMPC controller with a sample time Ts = 4.0 s, OSQP optimizer, SteadyKalmanFil
     | ``H_p``              | prediction horizon (integer)                             | `()`             |
     | ``H_c``              | control horizon (integer)                                | `()`             |
     | ``\mathbf{ΔU}``      | manipulated input increments over ``H_c``                | `(nu*Hc,)`       |
+    | ``\mathbf{D̂}``       | predicted measured disturbances over ``H_p``             | `(nd*Hp,)`       |
     | ``\mathbf{Ŷ}``       | predicted outputs over ``H_p``                           | `(ny*Hp,)`       |
     | ``\mathbf{U}``       | manipulated inputs over ``H_p``                          | `(nu*Hp,)`       |
     | ``\mathbf{R̂_y}``     | predicted output setpoints over ``H_p``                  | `(ny*Hp,)`       |
diff --git a/src/controller/nonlinmpc.jl b/src/controller/nonlinmpc.jl
@@ -78,6 +78,7 @@ struct NonLinMPC{
         # dummy vals (updated just before optimization):
         d0, D̂0, D̂e = zeros(NT, nd), zeros(NT, nd*Hp), zeros(NT, nd + nd*Hp)
         Uop, Yop, Dop = repeat(model.uop, Hp), repeat(model.yop, Hp), repeat(model.dop, Hp)
+        test_custom_functions(NT, model, JE, gc!, nc, Uop, Yop, Dop, p)
         nΔŨ = size(Ẽ, 2)
         ΔŨ = zeros(NT, nΔŨ)
         buffer = PredictiveControllerBuffer{NT}(nu, ny, nd, Hp)
@@ -131,16 +132,15 @@ include the manipulated inputs, predicted outputs and measured disturbances, ext
     \mathbf{Ŷ_e} = \begin{bmatrix} \mathbf{ŷ}(k)   \\ \mathbf{Ŷ}            \end{bmatrix}  , \quad
     \mathbf{D̂_e} = \begin{bmatrix} \mathbf{d}(k)   \\ \mathbf{D̂}            \end{bmatrix}
 ```
-The vector ``\mathbf{D̂}`` comprises the measured disturbance predictions over ``H_p``. The
-argument ``\mathbf{p}`` is a custom parameter object of any type, but use a mutable one if
-you want to modify it later e.g.: a vector.
+The argument ``\mathbf{p}`` is a custom parameter object of any type, but use a mutable one
+if you want to modify it later e.g.: a vector. See [`LinMPC`](@ref) Extended Help for the
+definition of the other variables.
 
 !!! tip
     Replace any of the arguments of ``J_E`` and ``\mathbf{g_c}`` functions with `_` if not
     needed (see e.g. the default value of `JE` below).
 
-See [`LinMPC`](@ref) Extended Help for the definition of the other variables. This method
-uses the default state estimator :
+This method uses the default state estimator:
 
 - if `model` is a [`LinModel`](@ref), a [`SteadyKalmanFilter`](@ref) with default arguments;
 - else, an [`UnscentedKalmanFilter`](@ref) with default arguments. 
@@ -198,11 +198,17 @@ NonLinMPC controller with a sample time Ts = 10.0 s, Ipopt optimizer, UnscentedK
 
     The economic cost ``J_E`` and custom constraint ``\mathbf{g_c}`` functions receive the
     extended vectors ``\mathbf{U_e}`` (`nu*Hp+nu` elements), ``\mathbf{Ŷ_e}`` (`ny+ny*Hp`
-    elements) and  ``\mathbf{D̂_e}`` (`nd+nd*Hp` elements) as arguments. The last two time
-    steps in ``\mathbf{U_e}`` are forced to be equal, that is ``\mathbf{u}(k+H_p) =
-    \mathbf{u}(k+H_p-1)``, since ``H_c ≤ H_p`` implies that ``\mathbf{Δu}(k+H_p) =
-    \mathbf{0}``. If `LHS` represents the result of the left-hand side in the inequality 
-    ``\mathbf{g_c}(\mathbf{U_e}, \mathbf{Ŷ_e}, \mathbf{D̂_e}, \mathbf{p}, ϵ) ≤ \mathbf{0}``, 
+    elements) and  ``\mathbf{D̂_e}`` (`nd+nd*Hp` elements) as arguments. They all include the
+    values from ``k`` to ``k + H_p`` (inclusively). The custom constraint also receives the
+    slack ``ϵ`` (scalar), which is always zero if `Cwt=Inf`.
+    
+    More precisely, the last two time steps in ``\mathbf{U_e}`` are forced to be equal, i.e.
+    ``\mathbf{u}(k+H_p) = \mathbf{u}(k+H_p-1)``, since ``H_c ≤ H_p`` implies that
+    ``\mathbf{Δu}(k+H_p) = \mathbf{0}``. The vectors ``\mathbf{ŷ}(k)`` and ``\mathbf{d}(k)``
+    are the current state estimator output and measured disturbance, respectively, and 
+    ``\mathbf{Ŷ}`` and ``\mathbf{D̂}``, their respective predictions from ``k+1`` to ``k+H_p``. 
+    If `LHS` represents the result of the left-hand side in the inequality 
+    ``\mathbf{g_c}(\mathbf{U_e}, \mathbf{Ŷ_e}, \mathbf{D̂_e}, \mathbf{p}, ϵ) ≤ \mathbf{0}``,
     the function `gc` can be implemented in two possible ways:
     
     1. **Non-mutating function** (out-of-place): define it as `gc(Ue, Ŷe, D̂e, p, ϵ) -> LHS`.
@@ -388,25 +394,34 @@ function get_mutating_gc(NT, gc)
     return gc!
 end
 
-function test_custom_functions(JE, gc!, uop; Uop, dop, Dop, ΔŨ, p)
-    # TODO: contunue here (important to guide the user, sim! can be used on NonLinModel,
-    # but there is no similar function for the custom functions of NonLinMPC)
-    Ue = [Uop; uop]
-    D̂e = [dop; Dop]
-
-    Ŷ0, x̂0end  = predict!(Ȳ, x̂0, x̂0next, u0, û0, mpc, model, ΔŨ)
-    Ŷe, Ue     = extended_predictions!(Ŷe, Ue, Ū, mpc, model, Ŷ0, ΔŨ)
-    ϵ = (nϵ == 1) ? ΔŨ[end] : zero(JNT) # ϵ = 0 if nϵ == 0 (meaning no relaxation)
-    mpc.con.gc!(gc, Ue, Ŷe, mpc.D̂e, mpc.p, ϵ)
-    g = con_nonlinprog!(g, mpc, model, x̂0end, Ŷ0, gc, ϵ)
-    J = obj_nonlinprog!(Ȳ, Ū, mpc, model, Ŷe, Ue, ΔŨ)
-
-
-
-
-    Ŷ0, x̂0next =
-    Ŷ0, x̂0end = predict!(Ŷ0, x̂0, x̂0next, u0, û0, mpc, model, mpc.ΔŨ)
-    JE = JE(Uop, Uop, Dop, p)
+function test_custom_functions(NT, model::SimModel, JE, gc!, nc, Uop, Yop, Dop, p)
+    uop, dop, yop = model.uop, model.dop, model.yop
+    Ue, Ŷe, D̂e = [Uop; uop], [yop; Yop], [dop; Dop]
+    try 
+        JE(Ue, Ŷe, D̂e, p)
+    catch err
+        @warn(
+            """
+            Calling the JE function with Ue, Ŷe, D̂e arguments fixed at uop=$uop, 
+            yop=$yop, dop=$dop failed with the following stacktrace.
+            """, 
+            exception=(err, catch_backtrace())
+        )
+    end
+    ϵ, gc = 0, Vector{NT}(undef, nc) 
+    try
+        gc!(gc, Ue, Ŷe, D̂e, p, ϵ)
+    catch err
+        @warn(
+            """
+            Calling the gc function with Ue, Ŷe, D̂e, ϵ arguments fixed at uop=$uop,
+            yop=$yop, dop=$dop, ϵ=0 failed with the following stacktrace. Did you 
+            forget to set the keyword argument nc?
+            """, 
+            exception=(err, catch_backtrace())
+        )
+    end
+    return nothing
 end
 
 """
@@ -492,13 +507,13 @@ function get_optim_functions(mpc::NonLinMPC, ::JuMP.GenericModel{JNT}) where JNT
         Ȳ,  Ū      = get_tmp(Ȳ_cache, ΔŨ1),  get_tmp(Ū_cache, ΔŨ1)
         x̂0, x̂0next = get_tmp(x̂0_cache, ΔŨ1), get_tmp(x̂0next_cache, ΔŨ1)
         u0, û0     = get_tmp(u0_cache, ΔŨ1), get_tmp(û0_cache, ΔŨ1)
-        g,  gc     = get_tmp(g_cache, ΔŨ1),  get_tmp(gc_cache, ΔŨ1)
+        gc         = get_tmp(gc_cache, ΔŨ1)
         Ŷ0, x̂0end  = predict!(Ȳ, x̂0, x̂0next, u0, û0, mpc, model, ΔŨ)
-        Ŷe, Ue     = extended_predictions!(Ŷe, Ue, Ū, mpc, model, Ŷ0, ΔŨ)
-        ϵ = (nϵ == 1) ? ΔŨ[end] : zero(JNT) # ϵ = 0 if nϵ == 0 (meaning no relaxation)
+        Ue, Ŷe     = extended_predictions!(Ue, Ŷe, Ū, mpc, model, Ŷ0, ΔŨ)
+        ϵ = (nϵ ≠ 0) ? ΔŨ[end] : zero(T) # ϵ = 0 if nϵ == 0 (meaning no relaxation)
         mpc.con.gc!(gc, Ue, Ŷe, mpc.D̂e, mpc.p, ϵ)
         g = con_nonlinprog!(g, mpc, model, x̂0end, Ŷ0, gc, ϵ)
-        return obj_nonlinprog!(Ȳ, Ū, mpc, model, Ŷe, Ue, ΔŨ)::T
+        return obj_nonlinprog!(Ȳ, Ū, mpc, model, Ue, Ŷe, ΔŨ)::T
     end
     function gfunc_i(i, ΔŨtup::NTuple{N, T}) where {N, T<:Real}
         ΔŨ1 = ΔŨtup[begin]
@@ -511,10 +526,10 @@ function get_optim_functions(mpc::NonLinMPC, ::JuMP.GenericModel{JNT}) where JNT
             Ȳ,  Ū      = get_tmp(Ȳ_cache, ΔŨ1),  get_tmp(Ū_cache, ΔŨ1)
             x̂0, x̂0next = get_tmp(x̂0_cache, ΔŨ1), get_tmp(x̂0next_cache, ΔŨ1)
             u0, û0     = get_tmp(u0_cache, ΔŨ1), get_tmp(û0_cache, ΔŨ1)
-            g,  gc     = get_tmp(g_cache, ΔŨ1),  get_tmp(gc_cache, ΔŨ1)
+            gc         = get_tmp(gc_cache, ΔŨ1)
             Ŷ0, x̂0end  = predict!(Ȳ, x̂0, x̂0next, u0, û0, mpc, model, ΔŨ)
-            Ŷe, Ue     = extended_predictions!(Ŷe, Ue, Ū, mpc, model, Ŷ0, ΔŨ)
-            ϵ = (nϵ == 1) ? ΔŨ[end] : zero(JNT) # ϵ = 0 if nϵ == 0 (meaning no relaxation)
+            Ue, Ŷe     = extended_predictions!(Ue, Ŷe, Ū, mpc, model, Ŷ0, ΔŨ)
+            ϵ = (nϵ ≠ 0) ? ΔŨ[end] : zero(T) # ϵ = 0 if nϵ == 0 (meaning no relaxation)
             mpc.con.gc!(gc, Ue, Ŷe, mpc.D̂e, mpc.p, ϵ)
             g = con_nonlinprog!(g, mpc, model, x̂0end, Ŷ0, gc, ϵ)
         end
@@ -672,7 +687,7 @@ function con_nonlinprog!(g, mpc::NonLinMPC, ::SimModel, x̂0end, Ŷ0, gc, ϵ)
 end
 
 "Evaluate the economic term `E*JE` of the objective function for [`NonLinMPC`](@ref)."
-function obj_econ!(Ue, Ŷe, mpc::NonLinMPC, model::SimModel)
+function obj_econ(mpc::NonLinMPC, model::SimModel, Ue, Ŷe)
     E_JE = iszero(mpc.E) ? 0.0 : mpc.E*mpc.JE(Ue, Ŷe, mpc.D̂e, mpc.p)
     return E_JE
 end
diff --git a/src/estimator/mhe/construct.jl b/src/estimator/mhe/construct.jl
@@ -1298,7 +1298,7 @@ function get_optim_functions(
     estim::MovingHorizonEstimator, ::JuMP.GenericModel{JNT}
 ) where {JNT <: Real}
     model, con = estim.model, estim.con
-    nx̂, nym, nŷ, nu, He = estim.nx̂, estim.nym, model.ny, model.nu, estim.He
+    nx̂, nym, nŷ, nu, nϵ, He = estim.nx̂, estim.nym, model.ny, model.nu, estim.nϵ, estim.He
     nV̂, nX̂, ng, nZ̃ = He*nym, He*nx̂, length(con.i_g), length(estim.Z̃)
     Nc = nZ̃ + 3
     Z̃_cache::DiffCache{Vector{JNT}, Vector{JNT}}  = DiffCache(zeros(JNT, nZ̃), Nc)
@@ -1317,8 +1317,8 @@ function get_optim_functions(
         V̂,  X̂0 = get_tmp(V̂_cache, Z̃1),  get_tmp(X̂0_cache, Z̃1)
         û0, ŷ0 = get_tmp(û0_cache, Z̃1), get_tmp(ŷ0_cache, Z̃1)
         V̂,  X̂0 = predict!(V̂, X̂0, û0, ŷ0, estim, model, Z̃)
-        g = get_tmp(g_cache, Z̃1)
-        g = con_nonlinprog!(g, estim, model, X̂0, V̂, Z̃)
+        ϵ = (nϵ ≠ 0) ? Z̃[begin] : zero(T) # ϵ = 0 if Cwt=Inf (meaning: no relaxation)
+        g = con_nonlinprog!(g, estim, model, X̂0, V̂, ϵ)
         x̄ = get_tmp(x̄_cache, Z̃1)
         return obj_nonlinprog!(x̄, estim, model, V̂, Z̃)::T
     end
@@ -1332,7 +1332,8 @@ function get_optim_functions(
                 Z̃[i] = Z̃tup[i] # Z̃ .= Z̃tup seems to produce a type instability
             end
             V̂, X̂0 = predict!(V̂, X̂0, û0, ŷ0, estim, model, Z̃)
-            g = con_nonlinprog!(g, estim, model, X̂0, V̂, Z̃)
+            ϵ = (nϵ ≠ 0) ? Z̃[begin] : zero(T) # ϵ = 0 if Cwt=Inf (meaning: no relaxation)
+            g = con_nonlinprog!(g, estim, model, X̂0, V̂, ϵ)
         end
         return g[i]
     end
diff --git a/src/estimator/mhe/execute.jl b/src/estimator/mhe/execute.jl
@@ -541,14 +541,13 @@ function predict!(V̂, X̂0, û0, ŷ0, estim::MovingHorizonEstimator, model::S
 end
 
 """
-    con_nonlinprog!(g, estim::MovingHorizonEstimator, model::SimModel, X̂0, V̂, Z̃)
+    con_nonlinprog!(g, estim::MovingHorizonEstimator, model::SimModel, X̂0, V̂, ϵ)
 
 Nonlinear constrains for [`MovingHorizonEstimator`](@ref).
 """
-function con_nonlinprog!(g, estim::MovingHorizonEstimator, ::SimModel, X̂0, V̂, Z̃)
+function con_nonlinprog!(g, estim::MovingHorizonEstimator, ::SimModel, X̂0, V̂, ϵ)
     nX̂con, nX̂ = length(estim.con.X̂0min), estim.nx̂ *estim.Nk[]
     nV̂con, nV̂ = length(estim.con.V̂min),  estim.nym*estim.Nk[]
-    ϵ = estim.nϵ ≠ 0 ? Z̃[begin] : 0 # ϵ = 0 if Cwt=Inf (meaning: no relaxation)
     for i in eachindex(g)
         estim.con.i_g[i] || continue
         if i ≤ nX̂con
