changed: update_prediction! methods are now not nested

franckgaga · franckgaga · commit b28b7024cfae · 2025-03-18T12:41:21.000-04:00
It is no longer necessary with `DI.Constant` context.
diff --git a/src/controller/execute.jl b/src/controller/execute.jl
@@ -123,16 +123,16 @@ function getinfo(mpc::PredictiveController{NT}) where NT<:Real
     x̂0end = similar(mpc.estim.x̂0)
     Ue, Ŷe = Vector{NT}(undef, nUe), Vector{NT}(undef, nŶe)
     U0, Ŷ0 = similar(mpc.Uop), similar(mpc.Yop)
-    X̂0, Û0 = Vector{NT}(undef, nX̂0), Vector{NT}(undef, nÛ0)
+    Û0, X̂0 = Vector{NT}(undef, nÛ0), Vector{NT}(undef, nX̂0)
     U,  Ŷ  = similar(mpc.Uop), similar(mpc.Yop)
-    Ŷs = similar(mpc.Yop)
     U0 = getU0!(U0, mpc, Z̃)
-    ΔŨ = getΔŨ!(ΔŨ, mpc, mpc.transcription, Z̃)
+    ΔŨ = getΔŨ!(ΔŨ, mpc, transcription, Z̃)
     Ŷ0, x̂0end  = predict!(Ŷ0, x̂0end, X̂0, Û0, mpc, model, transcription, U0, Z̃)
     Ue, Ŷe = extended_vectors!(Ue, Ŷe, mpc, U0, Ŷ0)
     U .= U0 .+ mpc.Uop
     Ŷ .= Ŷ0 .+ mpc.Yop
     J = obj_nonlinprog!(Ŷ0, U0, mpc, model, Ue, Ŷe, ΔŨ)
+    Ŷs = similar(mpc.Yop)
     predictstoch!(Ŷs, mpc, mpc.estim)
     info[:ΔU]   = Z̃[1:mpc.Hc*model.nu]
     info[:ϵ]    = getϵ(mpc, Z̃)
@@ -370,6 +370,9 @@ function obj_nonlinprog!(
     return JR̂y + JΔŨ + JR̂u + E_JE
 end
 
+"No custom nonlinear constraints `gc` by default, return `gc` unchanged." 
+con_custom!(gc, ::PredictiveController, _ , _, _ ) = gc
+
 "By default, the economic term is zero."
 function obj_econ(::PredictiveController, ::SimModel, _ , ::AbstractVector{NT}) where NT
     return zero(NT)
diff --git a/src/controller/nonlinmpc.jl b/src/controller/nonlinmpc.jl
@@ -589,19 +589,6 @@ function get_optim_functions(
     grad_backend::AbstractADType,
     jac_backend ::AbstractADType
 ) where JNT<:Real
-    # ------ update simulation function (all args after `mpc` are mutated) ----------------
-    function update_simulations!(Z̃, mpc, ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, X̂0, gc, g, geq)
-        model, transcription = mpc.estim.model, mpc.transcription
-        U0 = getU0!(U0, mpc, Z̃)
-        ΔŨ = getΔŨ!(ΔŨ, mpc, transcription, Z̃)
-        Ŷ0, x̂0end  = predict!(Ŷ0, x̂0end, X̂0, Û0, mpc, model, transcription, U0, Z̃)
-        Ue, Ŷe = extended_vectors!(Ue, Ŷe, mpc, U0, Ŷ0)
-        ϵ = getϵ(mpc, Z̃)
-        gc  = con_custom!(gc, mpc, Ue, Ŷe, ϵ)
-        g   = con_nonlinprog!(g, mpc, model, transcription, x̂0end, Ŷ0, gc, ϵ)
-        geq = con_nonlinprogeq!(geq, X̂0, Û0, mpc, model, transcription, U0, Z̃)
-        return nothing
-    end
     # ----- common cache for Jfunc, gfuncs, geqfuncs called with floats -------------------
     model = mpc.estim.model
     nu, ny, nx̂, nϵ, Hp, Hc = model.nu, model.ny, mpc.estim.nx̂, mpc.nϵ, mpc.Hp, mpc.Hc
@@ -622,12 +609,12 @@ function get_optim_functions(
     function Jfunc(Z̃arg::Vararg{T, N}) where {N, T<:Real}
         if isdifferent(Z̃arg, Z̃)
             Z̃ .= Z̃arg
-            update_simulations!(Z̃, mpc, ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, X̂0, gc, g, geq)
+            update_predictions!(ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, X̂0, gc, g, geq, mpc, Z̃)
         end
         return obj_nonlinprog!(Ŷ0, U0, mpc, model, Ue, Ŷe, ΔŨ)::T
     end
     function Jfunc!(Z̃, mpc, ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, X̂0, gc, g, geq)
-        update_simulations!(Z̃, mpc, ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, X̂0, gc, g, geq)
+        update_predictions!(ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, X̂0, gc, g, geq, mpc, Z̃)
         return obj_nonlinprog!(Ŷ0, U0, mpc, model, Ue, Ŷe, ΔŨ)
     end
     Z̃_∇J = fill(myNaN, nZ̃) 
@@ -658,14 +645,14 @@ function get_optim_functions(
         gfunc_i = function (Z̃arg::Vararg{T, N}) where {N, T<:Real}
             if isdifferent(Z̃arg, Z̃)
                 Z̃ .= Z̃arg
-                update_simulations!(Z̃, mpc, ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, X̂0, gc, g, geq)
+                update_predictions!(ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, X̂0, gc, g, geq, mpc, Z̃)
             end
             return g[i]::T
         end
         gfuncs[i] = gfunc_i
     end
     function gfunc!(g, Z̃, mpc, ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, X̂0, gc, geq)
-        return update_simulations!(Z̃, mpc, ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, X̂0, gc, g, geq)
+        return update_predictions!(ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, X̂0, gc, g, geq, mpc, Z̃)
     end
     Z̃_∇g = fill(myNaN, nZ̃)
     ∇g_context = (
@@ -706,14 +693,14 @@ function get_optim_functions(
         geqfunc_i = function (Z̃arg::Vararg{T, N}) where {N, T<:Real}
             if isdifferent(Z̃arg, Z̃)
                 Z̃ .= Z̃arg
-                update_simulations!(Z̃, mpc, ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, X̂0, gc, g, geq)
+                update_predictions!(ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, X̂0, gc, g, geq, mpc, Z̃)
             end
             return geq[i]::T
         end
         geqfuncs[i] = geqfunc_i          
     end
     function geqfunc!(geq, Z̃, mpc, ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, X̂0, gc, g) 
-        return update_simulations!(Z̃, mpc, ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, X̂0, gc, g, geq)
+        return update_predictions!(ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, X̂0, gc, g, geq, mpc, Z̃)
     end
     Z̃_∇geq = fill(myNaN, nZ̃)
     ∇geq_context = (
@@ -743,6 +730,31 @@ function get_optim_functions(
     return Jfunc, ∇Jfunc!, gfuncs, ∇gfuncs!, geqfuncs, ∇geqfuncs!
 end
 
+"""
+    update_predictions!(
+        ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, X̂0, gc, g, geq, 
+        mpc::PredictiveController, Z̃
+    ) -> nothing
+
+Update in-place all vectors for the predictions of `mpc` controller at decision vector `Z̃`. 
+
+The method mutates all the arguments before the `mpc` argument.
+"""
+function update_predictions!(
+    ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, X̂0, gc, g, geq, mpc::PredictiveController, Z̃
+)
+    model, transcription = mpc.estim.model, mpc.transcription
+    U0 = getU0!(U0, mpc, Z̃)
+    ΔŨ = getΔŨ!(ΔŨ, mpc, transcription, Z̃)
+    Ŷ0, x̂0end  = predict!(Ŷ0, x̂0end, X̂0, Û0, mpc, model, transcription, U0, Z̃)
+    Ue, Ŷe = extended_vectors!(Ue, Ŷe, mpc, U0, Ŷ0)
+    ϵ = getϵ(mpc, Z̃)
+    gc  = con_custom!(gc, mpc, Ue, Ŷe, ϵ)
+    g   = con_nonlinprog!(g, mpc, model, transcription, x̂0end, Ŷ0, gc, ϵ)
+    geq = con_nonlinprogeq!(geq, X̂0, Û0, mpc, model, transcription, U0, Z̃)
+    return nothing
+end
+
 @doc raw"""
     con_custom!(gc, mpc::NonLinMPC, Ue, Ŷe, ϵ) -> gc
 
diff --git a/src/estimator/mhe/construct.jl b/src/estimator/mhe/construct.jl
@@ -1332,14 +1332,6 @@ function get_optim_functions(
     grad_backend::AbstractADType,
     jac_backend::AbstractADType
 ) where {JNT <: Real}
-    # -------- update simulation function (all args after `estim` are mutated) ------------
-    function update_simulations!(Z̃, estim, V̂, X̂0, û0, ŷ0, g)
-        model = estim.model
-        V̂, X̂0  = predict!(V̂, X̂0, û0, ŷ0, estim, model, Z̃)
-        ϵ = getϵ(estim, Z̃)
-        g = con_nonlinprog!(g, estim, model, X̂0, V̂, ϵ)
-        return nothing
-    end
     # ---------- common cache for Jfunc, gfuncs called with floats ------------------------
     model, con = estim.model, estim.con
     nx̂, nym, nŷ, nu, nϵ, He = estim.nx̂, estim.nym, model.ny, model.nu, estim.nϵ, estim.He
@@ -1355,12 +1347,12 @@ function get_optim_functions(
     function Jfunc(Z̃arg::Vararg{T, N}) where {N, T<:Real}
         if isdifferent(Z̃arg, Z̃)
             Z̃ .= Z̃arg
-            update_simulations!(Z̃, estim, V̂, X̂0, û0, ŷ0, g)
+            update_prediction!(V̂, X̂0, û0, ŷ0, g, estim, Z̃)
         end
         return obj_nonlinprog!(x̄, estim, model, V̂, Z̃)::T
     end
     function Jfunc!(Z̃, estim, V̂, X̂0, û0, ŷ0, g, x̄)
-        update_simulations!(Z̃, estim, V̂, X̂0, û0, ŷ0, g)
+        update_prediction!(V̂, X̂0, û0, ŷ0, g, estim, Z̃)
         return obj_nonlinprog!(x̄, estim, model, V̂, Z̃)
     end
     Z̃_∇J    = fill(myNaN, nZ̃) 
@@ -1392,14 +1384,14 @@ function get_optim_functions(
         gfunc_i = function (Z̃arg::Vararg{T, N}) where {N, T<:Real}
             if isdifferent(Z̃arg, Z̃)
                 Z̃ .= Z̃arg
-                update_simulations!(Z̃, estim, V̂, X̂0, û0, ŷ0, g)
+                update_prediction!(V̂, X̂0, û0, ŷ0, g, estim, Z̃)
             end
             return g[i]::T
         end
         gfuncs[i] = gfunc_i
     end
     function gfunc!(g, Z̃, estim, V̂, X̂0, û0, ŷ0)
-        return update_simulations!(Z̃, estim, V̂, X̂0, û0, ŷ0, g)
+        return update_prediction!(V̂, X̂0, û0, ŷ0, g, estim, Z̃)
     end
     Z̃_∇g     = fill(myNaN, nZ̃)
     ∇g_context = (
diff --git a/src/estimator/mhe/execute.jl b/src/estimator/mhe/execute.jl
@@ -591,6 +591,22 @@ function predict!(V̂, X̂0, û0, ŷ0, estim::MovingHorizonEstimator, model::S
     return V̂, X̂0
 end
 
+
+"""
+    update_predictions!(V̂, X̂0, û0, ŷ0, g, estim::MovingHorizonEstimator, Z̃)
+
+Update in-place the vectors for the predictions of `estim` estimator at decision vector `Z̃`.
+
+The method mutates all the arguments before `estim` argument.
+"""
+function update_prediction!(V̂, X̂0, û0, ŷ0, g, estim::MovingHorizonEstimator, Z̃)
+    model = estim.model
+    V̂, X̂0  = predict!(V̂, X̂0, û0, ŷ0, estim, model, Z̃)
+    ϵ = getϵ(estim, Z̃)
+    g = con_nonlinprog!(g, estim, model, X̂0, V̂, ϵ)
+    return nothing
+end
+
 """
     con_nonlinprog!(g, estim::MovingHorizonEstimator, model::SimModel, X̂0, V̂, ϵ)
 
