From 9984e43d794e0ad15b412505242fdc4ac09f5f98 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Tue, 3 Mar 2026 16:23:37 -0500 Subject: [PATCH 1/3] 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. --- src/controller/execute.jl | 40 ++++++++++++++++++------------------- src/controller/legacy.jl | 2 +- src/controller/nonlinmpc.jl | 4 ++-- 3 files changed, 23 insertions(+), 23 deletions(-) diff --git a/src/controller/execute.jl b/src/controller/execute.jl index bb059ca4e..a5a3151e2 100644 --- a/src/controller/execute.jl +++ b/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,24 +387,7 @@ 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 @@ -412,7 +395,7 @@ function `dot(x, A, x)` is a performant way of calculating `x'*A*x`. This method `Ŷ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 diff --git a/src/controller/legacy.jl b/src/controller/legacy.jl index 2892154a8..51c1ff908 100644 --- a/src/controller/legacy.jl +++ b/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 = ( diff --git a/src/controller/nonlinmpc.jl b/src/controller/nonlinmpc.jl index 35d39e69a..0c27f783a 100644 --- a/src/controller/nonlinmpc.jl +++ b/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 = ( From 8e527364fd81107e540d401ac6f02ee7a814ed8e Mon Sep 17 00:00:00 2001 From: franckgaga Date: Wed, 4 Mar 2026 09:10:31 -0500 Subject: [PATCH 2/3] removed: specialized `obj_nonlinprog!` method --- src/controller/execute.jl | 26 ++++++-------------------- src/controller/nonlinmpc.jl | 4 ++-- 2 files changed, 8 insertions(+), 22 deletions(-) diff --git a/src/controller/execute.jl b/src/controller/execute.jl index a5a3151e2..18bfe370f 100644 --- a/src/controller/execute.jl +++ b/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, ΔŨ, Z̃) + J = obj_nonlinprog!(Ŷ0, U0, mpc, Ue, Ŷe, ΔŨ) Ŷs = similar(mpc.Yop) predictstoch!(Ŷs, mpc, mpc.estim) info[:ΔU] = Z̃[1:mpc.Hc*model.nu] @@ -387,15 +387,18 @@ end iszero_nc(mpc::PredictiveController) = (mpc.con.nc == 0) """ - obj_nonlinprog!(Ȳ, Ū, mpc::PredictiveController, model::SimModel, Ue, Ŷe, ΔŨ, Z̃) + obj_nonlinprog!(Ȳ, Ū, mpc::PredictiveController, Ue, Ŷe, ΔŨ) 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). + +Note that a specialized version on [`LinModel`](@ref) that uses the Hessian matrix `mpc.H̃` +is actually slower in the [`MultipleShooting`](@ref) case, so only one method is defined. """ function obj_nonlinprog!( - Ȳ, Ū, mpc::PredictiveController, model::SimModel, Ue, Ŷe, ΔŨ, ::AbstractVector{NT} + Ȳ, Ū, mpc::PredictiveController, Ue, Ŷe, ΔŨ::AbstractVector{NT} ) where NT<:Real nu, ny = model.nu, model.ny # --- output setpoint tracking term --- @@ -425,23 +428,6 @@ 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 diff --git a/src/controller/nonlinmpc.jl b/src/controller/nonlinmpc.jl index 0c27f783a..8050ff0c3 100644 --- a/src/controller/nonlinmpc.jl +++ b/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, ΔŨ, Z̃) + return obj_nonlinprog!(Ŷ0, U0, mpc, Ue, Ŷe, ΔŨ) 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, ΔŨ, Z̃) + return obj_nonlinprog!(Ŷ0, U0, mpc, Ue, Ŷe, ΔŨ) end Z̃_J = fill(myNaN, nZ̃) # NaN to force update at first call J_cache = ( From 3b0bd3e44b2e9944061db0206d096f736df564e9 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Wed, 4 Mar 2026 09:19:15 -0500 Subject: [PATCH 3/3] debug: extract `model` variable --- src/controller/execute.jl | 1 + 1 file changed, 1 insertion(+) diff --git a/src/controller/execute.jl b/src/controller/execute.jl index bdd792a52..d01e99de6 100644 --- a/src/controller/execute.jl +++ b/src/controller/execute.jl @@ -400,6 +400,7 @@ is actually slower in the [`MultipleShooting`](@ref) case, so only one method is function obj_nonlinprog!( Ȳ, Ū, mpc::PredictiveController, Ue, Ŷe, ΔŨ::AbstractVector{NT} ) where NT<:Real + model = mpc.estim.model nu, ny = model.nu, model.ny # --- output setpoint tracking term --- if mpc.weights.iszero_M_Hp[]