From c53b86454cc6e6f72bc7ef6250df08f1b699c89a Mon Sep 17 00:00:00 2001 From: franckgaga Date: Fri, 28 Nov 2025 14:59:00 -0500 Subject: [PATCH 01/16] added: hide non-unicode keys in `info2debugstr` --- src/general.jl | 12 ++++++++++++ 1 file changed, 12 insertions(+) diff --git a/src/general.jl b/src/general.jl index ea814bcd6..9115ddab7 100644 --- a/src/general.jl +++ b/src/general.jl @@ -16,6 +16,14 @@ const ALL_COLORING_ORDERS = ( RandomOrder(StableRNG(0), 0) ) +const HIDDEN_GETINFO_KEYS_MHE = ( + :What, :xhatarr, :epsilon, :Xhat, :xhat, :Vhat, :Pbar, :xbar, :Yhat, :Yhatm +) + +const HIDDEN_GETINFO_KEYS_MPC = ( + :DeltaU, :epsilon, :Dhat, :yhat, :Yhat, :xhatend, :Yhats, :Rhaty, :Rhatu +) + "Termination status that means 'no solution available'." const ERROR_STATUSES = ( JuMP.INFEASIBLE, JuMP.DUAL_INFEASIBLE, JuMP.LOCALLY_INFEASIBLE, @@ -40,6 +48,10 @@ function info2debugstr(info) mystr = "Content of getinfo dictionary:\n" for (key, value) in info (key == :sol) && continue + if key in HIDDEN_GETINFO_KEYS_MHE || key in HIDDEN_GETINFO_KEYS_MPC + # skip the redundant non-Unicode keys + continue + end mystr *= " :$key => $value\n" end if haskey(info, :sol) From 982967f193230d84e58097a8089ea29863878fb1 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Fri, 28 Nov 2025 15:07:21 -0500 Subject: [PATCH 02/16] debug: correct treeview padding in `info2debugstr` --- src/general.jl | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/src/general.jl b/src/general.jl index 9115ddab7..1302b9750 100644 --- a/src/general.jl +++ b/src/general.jl @@ -56,8 +56,9 @@ function info2debugstr(info) end if haskey(info, :sol) split_sol = split(string(info[:sol]), "\n") - solstr = join((lpad(line, length(line) + 2) for line in split_sol), "\n", "") - mystr *= " :sol => \n"*solstr + # Add the treeview prefix to each line + solstr = join((" " * line for line in split_sol), "\n") + mystr *= " :sol => \n" * solstr * "\n" # Ensure a trailing newline end return mystr end From 4d9963731cabed419cda32bf8a4bea0e0cf26753 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Fri, 28 Nov 2025 15:09:09 -0500 Subject: [PATCH 03/16] added: deprecated field in hidden `getinfo` keys --- src/general.jl | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/general.jl b/src/general.jl index 1302b9750..73bd005b0 100644 --- a/src/general.jl +++ b/src/general.jl @@ -17,7 +17,7 @@ const ALL_COLORING_ORDERS = ( ) const HIDDEN_GETINFO_KEYS_MHE = ( - :What, :xhatarr, :epsilon, :Xhat, :xhat, :Vhat, :Pbar, :xbar, :Yhat, :Yhatm + :What, :xhatarr, :epsilon, :Xhat, :xhat, :Vhat, :Pbar, :xbar, :Yhat, :Yhatm, :ϵ ) const HIDDEN_GETINFO_KEYS_MPC = ( From ea4c42a0cbe595358e96ddad2879d4f2f844f8e1 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Fri, 28 Nov 2025 18:00:04 -0500 Subject: [PATCH 04/16] wip: adding derivatives to `getinfo(mpc::NonLinMPC)` --- src/ModelPredictiveControl.jl | 2 + src/controller/execute.jl | 13 ++++- src/controller/nonlinmpc.jl | 93 ++++++++++++++++++++++++++++++++++- 3 files changed, 105 insertions(+), 3 deletions(-) diff --git a/src/ModelPredictiveControl.jl b/src/ModelPredictiveControl.jl index efa05e3d9..84827076e 100644 --- a/src/ModelPredictiveControl.jl +++ b/src/ModelPredictiveControl.jl @@ -9,6 +9,8 @@ using RecipesBase using DifferentiationInterface: ADTypes.AbstractADType, AutoForwardDiff using DifferentiationInterface: AutoSparse, SecondOrder +using DifferentiationInterface: gradient, jacobian, hessian +using DifferentiationInterface: value_and_gradient, value_gradient_and_hessian using DifferentiationInterface: gradient!, value_and_gradient!, prepare_gradient using DifferentiationInterface: jacobian!, value_and_jacobian!, prepare_jacobian using DifferentiationInterface: hessian!, value_gradient_and_hessian!, prepare_hessian diff --git a/src/controller/execute.jl b/src/controller/execute.jl index 9a3c54eba..8f9e3a833 100644 --- a/src/controller/execute.jl +++ b/src/controller/execute.jl @@ -105,8 +105,17 @@ The function should be called after calling [`moveinput!`](@ref). It returns the - `:d` : current measured disturbance, ``\mathbf{d}(k)`` For [`LinMPC`](@ref) and [`NonLinMPC`](@ref), the field `:sol` also contains the optimizer -solution summary that can be printed. Lastly, the economical cost `:JE` and the custom -nonlinear constraints `:gc` values at the optimum are also available for [`NonLinMPC`](@ref). +solution summary that can be printed. Lastly, for [`NonLinMPC`](@ref), the following fields +are also available: + +- `:JE`: economic cost value at the optimum, ``J_E`` +- `:gc`: custom nonlinear constraints values at the optimum, ``\mathbf{g_c}`` +- `:∇J` or *`:nablaJ`* : gradient of the objective function, ``\mathbf{\nabla} J`` +- `:∇²J` or *`:nabla2J`* : Hessian of the objective function, ``\mathbf{\nabla^2}J`` +- `:∇g` or *`:nablag`* : Jacobian of the inequality constraint, ``\mathbf{\nabla g}`` +- `:∇²ℓg` or *`:nabla2lg`* : Hessian of the inequality Lagrangian, ``\mathbf{\nabla^2}\ell_{\mathbf{g}}`` +- `:∇geq` or *`:nablageq`* : Jacobian of the equality constraint, ``\mathbf{\nabla g_{eq}}`` +- `:∇²ℓgeq` or *`:nabla2lgeq`* : Hessian of the equality Lagrangian, ``\mathbf{\nabla^2}\ell_{\mathbf{g_{eq}}}`` # Examples ```jldoctest diff --git a/src/controller/nonlinmpc.jl b/src/controller/nonlinmpc.jl index 3a42e8b15..8485b7de7 100644 --- a/src/controller/nonlinmpc.jl +++ b/src/controller/nonlinmpc.jl @@ -526,7 +526,8 @@ end """ addinfo!(info, mpc::NonLinMPC) -> info -For [`NonLinMPC`](@ref), add `:sol` and the optimal economic cost `:JE`. +For [`NonLinMPC`](@ref), add `:sol`, the custom nonlinear objective `:JE`, the custom +constraint `:gc`, and the various derivatives. """ function addinfo!(info, mpc::NonLinMPC{NT}) where NT<:Real U, Ŷ, D̂, ŷ, d, ϵ = info[:U], info[:Ŷ], info[:D̂], info[:ŷ], info[:d], info[:ϵ] @@ -536,9 +537,99 @@ function addinfo!(info, mpc::NonLinMPC{NT}) where NT<:Real JE = mpc.JE(Ue, Ŷe, D̂e, mpc.p) LHS = Vector{NT}(undef, mpc.con.nc) mpc.con.gc!(LHS, Ue, Ŷe, D̂e, mpc.p, ϵ) + model, optim = mpc.estim.model, mpc.optim + transcription = mpc.transcription + nu, ny, nx̂, nϵ = model.nu, model.ny, mpc.estim.nx̂, mpc.nϵ + nk = get_nk(model, transcription) + Hp, Hc = mpc.Hp, mpc.Hc + ng = length(mpc.con.i_g) + nc, neq = mpc.con.nc, mpc.con.neq + nU, nŶ, nX̂, nK = mpc.Hp*nu, Hp*ny, Hp*nx̂, Hp*nk + nΔŨ, nUe, nŶe = nu*Hc + nϵ, nU + nu, nŶ + ny + ΔŨ = zeros(NT, nΔŨ) + x̂0end = zeros(NT, nx̂) + K0 = zeros(NT, nK) + Ue, Ŷe = zeros(NT, nUe), zeros(NT, nŶe) + U0, Ŷ0 = zeros(NT, nU), zeros(NT, nŶ) + Û0, X̂0 = zeros(NT, nU), zeros(NT, nX̂) + gc, g = zeros(NT, nc), zeros(NT, ng) + geq = zeros(NT, neq) + J_cache = ( + Cache(ΔŨ), Cache(x̂0end), Cache(Ue), Cache(Ŷe), Cache(U0), Cache(Ŷ0), + Cache(Û0), Cache(K0), Cache(X̂0), + Cache(gc), Cache(g), Cache(geq), + ) + 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, ΔŨ) + end + if !isnothing(mpc.hessian) + _, ∇J, ∇²J = value_gradient_and_hessian(J!, mpc.hessian, mpc.Z̃, J_cache...) + else + ∇J, ∇²J = gradient(J!, mpc.gradient, mpc.Z̃, J_cache...), nothing + end + JNT = typeof(mpc.optim).parameters[1] + nonlin_constraints = JuMP.all_constraints( + optim, JuMP.Vector{JuMP.VariableRef}, MOI.VectorNonlinearOracle{JNT} + ) + g_con, geq_func = nonlin_constraints + λ, λeq = JuMP.dual.(g_con), JuMP.dual.(geq_func) + display(λ) + display(λeq) + g_cache = ( + Cache(ΔŨ), Cache(x̂0end), Cache(Ue), Cache(Ŷe), Cache(U0), Cache(Ŷ0), + Cache(Û0), Cache(K0), Cache(X̂0), + Cache(gc), Cache(geq), + ) + function g!(g, Z̃, ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, K0, X̂0, gc, geq) + update_predictions!(ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, K0, X̂0, gc, g, geq, mpc, Z̃) + return nothing + end + ∇g = jacobian(g!, g, mpc.jacobian, mpc.Z̃, g_cache...) + #= + if !isnothing(mpc.hessian) + function ℓ_g(Z̃, λ, ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, K0, X̂0, gc, geq, g) + update_predictions!(ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, K0, X̂0, gc, g, geq, mpc, Z̃) + return dot(λ, g) + end + ∇²g_cache = ( + Cache(ΔŨ), Cache(x̂0end), Cache(Ue), Cache(Ŷe), Cache(U0), Cache(Ŷ0), + Cache(Û0), Cache(K0), Cache(X̂0), + Cache(gc), Cache(geq), Cache(g) + ) + ∇²ℓg = hessian(ℓ_g, mpc.hessian, mpc.Z̃, Constant(λ), ∇²g_cache...) + else + ∇²ℓg = nothing + end + =# ∇²ℓg = nothing #TODO: delete this line when enabling the above block + geq_cache = ( + Cache(ΔŨ), Cache(x̂0end), Cache(Ue), Cache(Ŷe), Cache(U0), Cache(Ŷ0), + Cache(Û0), Cache(K0), Cache(X̂0), + Cache(gc), Cache(g) + ) + function geq!(geq, Z̃, ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, K0, X̂0, gc, g) + update_predictions!(ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, K0, X̂0, gc, g, geq, mpc, Z̃) + return nothing + end + ∇geq = jacobian(geq!, geq, mpc.jacobian, mpc.Z̃, geq_cache...) + ∇²ℓgeq = nothing # TODO: implement later + info[:JE] = JE info[:gc] = LHS + info[:∇J] = ∇J + info[:∇²J] = ∇²J + info[:∇g] = ∇g + info[:∇²ℓg] = ∇²ℓg + info[:∇geq] = ∇geq + info[:∇²ℓgeq] = ∇²ℓgeq info[:sol] = JuMP.solution_summary(mpc.optim, verbose=true) + # --- non-Unicode fields --- + info[:nablaJ] = ∇J + info[:nabla2J] = ∇²J + info[:nablag] = ∇g + info[:nabla2lg] = ∇²ℓg + info[:nablageq] = ∇geq + info[:nabla2lgeq] = ∇²ℓgeq return info end From cacb2be558cec6ecdee6bdc1fae97b1c6060101e Mon Sep 17 00:00:00 2001 From: franckgaga Date: Sun, 30 Nov 2025 14:11:54 -0500 Subject: [PATCH 05/16] added: derivatives in `getinfo(::MovingHorizonEstimator)` --- src/controller/nonlinmpc.jl | 23 ++++++---- src/estimator/mhe/construct.jl | 4 +- src/estimator/mhe/execute.jl | 76 ++++++++++++++++++++++++++++++++++ 3 files changed, 92 insertions(+), 11 deletions(-) diff --git a/src/controller/nonlinmpc.jl b/src/controller/nonlinmpc.jl index 8485b7de7..d1dcb94a0 100644 --- a/src/controller/nonlinmpc.jl +++ b/src/controller/nonlinmpc.jl @@ -530,6 +530,7 @@ For [`NonLinMPC`](@ref), add `:sol`, the custom nonlinear objective `:JE`, the c constraint `:gc`, and the various derivatives. """ function addinfo!(info, mpc::NonLinMPC{NT}) where NT<:Real + # --- variables specific to NonLinMPC --- U, Ŷ, D̂, ŷ, d, ϵ = info[:U], info[:Ŷ], info[:D̂], info[:ŷ], info[:d], info[:ϵ] Ue = [U; U[(end - mpc.estim.model.nu + 1):end]] Ŷe = [ŷ; Ŷ] @@ -537,6 +538,10 @@ function addinfo!(info, mpc::NonLinMPC{NT}) where NT<:Real JE = mpc.JE(Ue, Ŷe, D̂e, mpc.p) LHS = Vector{NT}(undef, mpc.con.nc) mpc.con.gc!(LHS, Ue, Ŷe, D̂e, mpc.p, ϵ) + info[:JE] = JE + info[:gc] = LHS + info[:sol] = JuMP.solution_summary(mpc.optim, verbose=true) + # --- derivatives --- model, optim = mpc.estim.model, mpc.optim transcription = mpc.transcription nu, ny, nx̂, nϵ = model.nu, model.ny, mpc.estim.nx̂, mpc.nϵ @@ -568,15 +573,19 @@ function addinfo!(info, mpc::NonLinMPC{NT}) where NT<:Real else ∇J, ∇²J = gradient(J!, mpc.gradient, mpc.Z̃, J_cache...), nothing end - JNT = typeof(mpc.optim).parameters[1] + JNT = typeof(optim).parameters[1] nonlin_constraints = JuMP.all_constraints( optim, JuMP.Vector{JuMP.VariableRef}, MOI.VectorNonlinearOracle{JNT} ) - g_con, geq_func = nonlin_constraints - λ, λeq = JuMP.dual.(g_con), JuMP.dual.(geq_func) + g_con, geq_con = nonlin_constraints + display(g_con) + display(geq_con) + λ, λeq = JuMP.dual.(g_con), JuMP.dual.(geq_con) + println(JuMP.dual.(JuMP.FixBoundRef.(optim[:Z̃var]))) display(λ) display(λeq) - g_cache = ( + + ∇g_cache = ( Cache(ΔŨ), Cache(x̂0end), Cache(Ue), Cache(Ŷe), Cache(U0), Cache(Ŷ0), Cache(Û0), Cache(K0), Cache(X̂0), Cache(gc), Cache(geq), @@ -585,7 +594,7 @@ function addinfo!(info, mpc::NonLinMPC{NT}) where NT<:Real update_predictions!(ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, K0, X̂0, gc, g, geq, mpc, Z̃) return nothing end - ∇g = jacobian(g!, g, mpc.jacobian, mpc.Z̃, g_cache...) + ∇g = jacobian(g!, g, mpc.jacobian, mpc.Z̃, ∇g_cache...) #= if !isnothing(mpc.hessian) function ℓ_g(Z̃, λ, ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, K0, X̂0, gc, geq, g) @@ -614,15 +623,13 @@ function addinfo!(info, mpc::NonLinMPC{NT}) where NT<:Real ∇geq = jacobian(geq!, geq, mpc.jacobian, mpc.Z̃, geq_cache...) ∇²ℓgeq = nothing # TODO: implement later - info[:JE] = JE - info[:gc] = LHS + info[:∇J] = ∇J info[:∇²J] = ∇²J info[:∇g] = ∇g info[:∇²ℓg] = ∇²ℓg info[:∇geq] = ∇geq info[:∇²ℓgeq] = ∇²ℓgeq - info[:sol] = JuMP.solution_summary(mpc.optim, verbose=true) # --- non-Unicode fields --- info[:nablaJ] = ∇J info[:nabla2J] = ∇²J diff --git a/src/estimator/mhe/construct.jl b/src/estimator/mhe/construct.jl index 316529c3e..dcd43e162 100644 --- a/src/estimator/mhe/construct.jl +++ b/src/estimator/mhe/construct.jl @@ -1504,9 +1504,7 @@ function get_nonlincon_oracle( return dot(λi, gi) end Z̃_∇gi = fill(myNaN, nZ̃) # NaN to force update_predictions! at first call - ∇gi_cache = ( - Cache(V̂), Cache(X̂0), Cache(û0), Cache(k0), Cache(ŷ0), Cache(g) - ) + ∇gi_cache = (Cache(V̂), Cache(X̂0), Cache(û0), Cache(k0), Cache(ŷ0), Cache(g)) # temporarily "fill" the estimation window for the preparation of the gradient: estim.Nk[] = He ∇gi_prep = prepare_jacobian(gi!, gi, jac, Z̃_∇gi, ∇gi_cache...; strict) diff --git a/src/estimator/mhe/execute.jl b/src/estimator/mhe/execute.jl index 73a9d05ea..bcc13b99b 100644 --- a/src/estimator/mhe/execute.jl +++ b/src/estimator/mhe/execute.jl @@ -163,9 +163,85 @@ function getinfo(estim::MovingHorizonEstimator{NT}) where NT<:Real info[:Yhatm] = info[:Ŷm] # --- deprecated fields --- info[:ϵ] = info[:ε] + info = addinfo!(info, estim, model) return info end + +""" + addinfo!(info, estim::MovingHorizonEstimator, model::NonLinModel) + +For [`NonLinModel`](@ref), add the various derivatives. +""" +function addinfo!( + info, estim::MovingHorizonEstimator{NT}, model::NonLinModel +) where NT <:Real + # --- derivatives --- + optim, con = estim.optim, estim.con + nx̂, nym, nŷ, nu, nk = estim.nx̂, estim.nym, model.ny, model.nu, model.nk + He = estim.He + ng = length(con.i_g) + nV̂, nX̂, ng = He*nym, He*nx̂, length(con.i_g) + V̂, X̂0 = zeros(NT, nV̂), zeros(NT, nX̂) + k0 = zeros(NT, nk) + û0, ŷ0 = zeros(NT, nu), zeros(NT, nŷ) + g = zeros(NT, ng) + x̄ = zeros(NT, nx̂) + J_cache = ( + Cache(V̂), Cache(X̂0), Cache(û0), Cache(k0), Cache(ŷ0), + Cache(g), + Cache(x̄), + ) + function J!(Z̃, V̂, X̂0, û0, k0, ŷ0, g, x̄) + update_prediction!(V̂, X̂0, û0, k0, ŷ0, g, estim, Z̃) + return obj_nonlinprog!(x̄, estim, model, V̂, Z̃) + end + if !isnothing(estim.hessian) + _, ∇J, ∇²J = value_gradient_and_hessian(J!, estim.hessian, estim.Z̃, J_cache...) + else + ∇J, ∇²J = gradient(J!, estim.gradient, estim.Z̃, J_cache...), nothing + end + JNT = typeof(optim).parameters[1] + nonlin_constraints = JuMP.all_constraints( + optim, JuMP.Vector{JuMP.VariableRef}, MOI.VectorNonlinearOracle{JNT} + ) + g_con = nonlin_constraints[1] + λ = JuMP.dual.(g_con) + display(λ) + ∇g_cache = (Cache(V̂), Cache(X̂0), Cache(û0), Cache(k0), Cache(ŷ0)) + function g!(g, Z̃, V̂, X̂0, û0, k0, ŷ0) + update_prediction!(V̂, X̂0, û0, k0, ŷ0, g, estim, Z̃) + return nothing + end + ∇g = jacobian(g!, g, estim.jacobian, estim.Z̃, ∇g_cache...) + #= + if !isnothing(estim.hessian) + function ℓ_g(Z̃, λ, V̂, X̂0, û0, k0, ŷ0, g) + update_prediction!(V̂, X̂0, û0, k0, ŷ0, g, estim, Z̃) + return dot(λ, g) + end + ∇²g_cache = (Cache(V̂), Cache(X̂0), Cache(û0), Cache(k0), Cache(ŷ0), Cache(g)) + ∇²ℓg = hessian(ℓ_g, estim.hessian, estim.Z̃, Constant(λ), ∇²g_cache...) + else + ∇²ℓg = nothing + end + =# ∇²ℓg = nothing #TODO: delete this line when enabling the above block + + info[:∇J] = ∇J + info[:∇²J] = ∇²J + info[:∇g] = ∇g + info[:∇²ℓg] = ∇²ℓg + # --- non-Unicode fields --- + info[:nablaJ] = ∇J + info[:nabla2J] = ∇²J + info[:nablag] = ∇g + info[:nabla2lg] = ∇²ℓg + return info +end + +"Nothing to add in the `info` dict for [`LinModel`](@ref)." +addinfo!(info, ::MovingHorizonEstimator, ::LinModel) = info + """ getε(estim::MovingHorizonEstimator, Z̃) -> ε From 00341a11eeab49eb02fa1b39590c7a699648ad4d Mon Sep 17 00:00:00 2001 From: franckgaga Date: Sun, 30 Nov 2025 18:10:43 -0500 Subject: [PATCH 06/16] changed: storing nonlinear constraint instead of oracle in `optim` field --- src/controller/nonlinmpc.jl | 8 ++++---- src/estimator/mhe/construct.jl | 4 ++-- 2 files changed, 6 insertions(+), 6 deletions(-) diff --git a/src/controller/nonlinmpc.jl b/src/controller/nonlinmpc.jl index d1dcb94a0..50fb31504 100644 --- a/src/controller/nonlinmpc.jl +++ b/src/controller/nonlinmpc.jl @@ -1040,10 +1040,10 @@ function set_nonlincon!( optim, JuMP.Vector{JuMP.VariableRef}, MOI.VectorNonlinearOracle{JNT} ) map(con_ref -> JuMP.delete(optim, con_ref), nonlin_constraints) - optim[:g_oracle] = g_oracle - optim[:geq_oracle] = geq_oracle - any(mpc.con.i_g) && @constraint(optim, Z̃var in g_oracle) - mpc.con.neq > 0 && @constraint(optim, Z̃var in geq_oracle) + JuMP.unregister(optim, :nonlinconstraint) + JuMP.unregister(optim, :nonlinconstrainteq) + any(mpc.con.i_g) && @constraint(optim, nonlinconstraint, Z̃var in g_oracle) + mpc.con.neq > 0 && @constraint(optim, nonlinconstrainteq, Z̃var in geq_oracle) return nothing end diff --git a/src/estimator/mhe/construct.jl b/src/estimator/mhe/construct.jl index dcd43e162..f0d777f4b 100644 --- a/src/estimator/mhe/construct.jl +++ b/src/estimator/mhe/construct.jl @@ -1575,7 +1575,7 @@ function set_nonlincon!( optim, JuMP.Vector{JuMP.VariableRef}, MOI.VectorNonlinearOracle{JNT} ) map(con_ref -> JuMP.delete(optim, con_ref), nonlin_constraints) - optim[:g_oracle] = g_oracle - any(estim.con.i_g) && @constraint(optim, Z̃var in g_oracle) + JuMP.unregister(optim, :nonlinconstraint) + any(estim.con.i_g) && @constraint(optim, nonlinconstraint, Z̃var in g_oracle) return nothing end \ No newline at end of file From 4492c833cb679b02eac7e13be861e94868732dc7 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Sun, 30 Nov 2025 18:26:31 -0500 Subject: [PATCH 07/16] changed: using stored nonlinear constraints in `getinfo` --- src/controller/nonlinmpc.jl | 12 +++--------- src/estimator/mhe/execute.jl | 9 +++------ 2 files changed, 6 insertions(+), 15 deletions(-) diff --git a/src/controller/nonlinmpc.jl b/src/controller/nonlinmpc.jl index 50fb31504..d2eb37610 100644 --- a/src/controller/nonlinmpc.jl +++ b/src/controller/nonlinmpc.jl @@ -573,15 +573,9 @@ function addinfo!(info, mpc::NonLinMPC{NT}) where NT<:Real else ∇J, ∇²J = gradient(J!, mpc.gradient, mpc.Z̃, J_cache...), nothing end - JNT = typeof(optim).parameters[1] - nonlin_constraints = JuMP.all_constraints( - optim, JuMP.Vector{JuMP.VariableRef}, MOI.VectorNonlinearOracle{JNT} - ) - g_con, geq_con = nonlin_constraints - display(g_con) - display(geq_con) - λ, λeq = JuMP.dual.(g_con), JuMP.dual.(geq_con) - println(JuMP.dual.(JuMP.FixBoundRef.(optim[:Z̃var]))) + nonlincon = optim[:nonlinconstraint] + nonlinconeq = optim[:nonlinconstrainteq] + λ, λeq = JuMP.dual.(nonlincon), JuMP.dual.(nonlinconeq) display(λ) display(λeq) diff --git a/src/estimator/mhe/execute.jl b/src/estimator/mhe/execute.jl index bcc13b99b..cb968c1ee 100644 --- a/src/estimator/mhe/execute.jl +++ b/src/estimator/mhe/execute.jl @@ -201,13 +201,10 @@ function addinfo!( else ∇J, ∇²J = gradient(J!, estim.gradient, estim.Z̃, J_cache...), nothing end - JNT = typeof(optim).parameters[1] - nonlin_constraints = JuMP.all_constraints( - optim, JuMP.Vector{JuMP.VariableRef}, MOI.VectorNonlinearOracle{JNT} - ) - g_con = nonlin_constraints[1] - λ = JuMP.dual.(g_con) + nonlincon = optim[:nonlinconstraint] + λ = JuMP.dual.(nonlincon) display(λ) + ∇g_cache = (Cache(V̂), Cache(X̂0), Cache(û0), Cache(k0), Cache(ŷ0)) function g!(g, Z̃, V̂, X̂0, û0, k0, ŷ0) update_prediction!(V̂, X̂0, û0, k0, ŷ0, g, estim, Z̃) From 7700d4542f0af9f994ed059b0d6a617b18dfe6e5 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Sun, 30 Nov 2025 20:19:15 -0500 Subject: [PATCH 08/16] added: non-unicode keys of derivative not shown in debug log --- src/general.jl | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/src/general.jl b/src/general.jl index 73bd005b0..ab9c49199 100644 --- a/src/general.jl +++ b/src/general.jl @@ -17,11 +17,13 @@ const ALL_COLORING_ORDERS = ( ) const HIDDEN_GETINFO_KEYS_MHE = ( - :What, :xhatarr, :epsilon, :Xhat, :xhat, :Vhat, :Pbar, :xbar, :Yhat, :Yhatm, :ϵ + :What, :xhatarr, :epsilon, :Xhat, :xhat, :Vhat, :Pbar, :xbar, :Yhat, :Yhatm, :ϵ, + :nablaJ, :nabla2J, :nablag, :nabla2lg ) const HIDDEN_GETINFO_KEYS_MPC = ( - :DeltaU, :epsilon, :Dhat, :yhat, :Yhat, :xhatend, :Yhats, :Rhaty, :Rhatu + :DeltaU, :epsilon, :Dhat, :yhat, :Yhat, :xhatend, :Yhats, :Rhaty, :Rhatu, + :nablaJ, :nabla2J, :nablag, :nabla2lg, :nablageq, :nabla2lgeq ) "Termination status that means 'no solution available'." From ff2675f269f2395e64e59f336584a867c837820f Mon Sep 17 00:00:00 2001 From: franckgaga Date: Sun, 30 Nov 2025 20:25:22 -0500 Subject: [PATCH 09/16] debug: do not get duals if `isnothing(hessian)` --- src/controller/nonlinmpc.jl | 31 ++++++++++++++++++++----------- src/estimator/mhe/execute.jl | 12 ++++-------- 2 files changed, 24 insertions(+), 19 deletions(-) diff --git a/src/controller/nonlinmpc.jl b/src/controller/nonlinmpc.jl index d2eb37610..4b585dd1b 100644 --- a/src/controller/nonlinmpc.jl +++ b/src/controller/nonlinmpc.jl @@ -573,12 +573,6 @@ function addinfo!(info, mpc::NonLinMPC{NT}) where NT<:Real else ∇J, ∇²J = gradient(J!, mpc.gradient, mpc.Z̃, J_cache...), nothing end - nonlincon = optim[:nonlinconstraint] - nonlinconeq = optim[:nonlinconstrainteq] - λ, λeq = JuMP.dual.(nonlincon), JuMP.dual.(nonlinconeq) - display(λ) - display(λeq) - ∇g_cache = ( Cache(ΔŨ), Cache(x̂0end), Cache(Ue), Cache(Ŷe), Cache(U0), Cache(Ŷ0), Cache(Û0), Cache(K0), Cache(X̂0), @@ -589,7 +583,6 @@ function addinfo!(info, mpc::NonLinMPC{NT}) where NT<:Real return nothing end ∇g = jacobian(g!, g, mpc.jacobian, mpc.Z̃, ∇g_cache...) - #= if !isnothing(mpc.hessian) function ℓ_g(Z̃, λ, ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, K0, X̂0, gc, geq, g) update_predictions!(ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, K0, X̂0, gc, g, geq, mpc, Z̃) @@ -600,11 +593,13 @@ function addinfo!(info, mpc::NonLinMPC{NT}) where NT<:Real Cache(Û0), Cache(K0), Cache(X̂0), Cache(gc), Cache(geq), Cache(g) ) + nonlincon = optim[:nonlinconstraint] + λ = JuMP.dual.(nonlincon) # FIXME: does not work for now + λ = ones(NT, ng) ∇²ℓg = hessian(ℓ_g, mpc.hessian, mpc.Z̃, Constant(λ), ∇²g_cache...) else ∇²ℓg = nothing end - =# ∇²ℓg = nothing #TODO: delete this line when enabling the above block geq_cache = ( Cache(ΔŨ), Cache(x̂0end), Cache(Ue), Cache(Ŷe), Cache(U0), Cache(Ŷ0), Cache(Û0), Cache(K0), Cache(X̂0), @@ -615,9 +610,23 @@ function addinfo!(info, mpc::NonLinMPC{NT}) where NT<:Real return nothing end ∇geq = jacobian(geq!, geq, mpc.jacobian, mpc.Z̃, geq_cache...) - ∇²ℓgeq = nothing # TODO: implement later - - + if !isnothing(mpc.hessian) + ∇²geq_cache = ( + Cache(ΔŨ), Cache(x̂0end), Cache(Ue), Cache(Ŷe), Cache(U0), Cache(Ŷ0), + Cache(Û0), Cache(K0), Cache(X̂0), + Cache(gc), Cache(geq), Cache(g) + ) + function ℓ_geq(Z̃, λeq, ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, K0, X̂0, gc, geq, g) + update_predictions!(ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, K0, X̂0, gc, g, geq, mpc, Z̃) + return dot(λeq, geq) + end + nonlinconeq = optim[:nonlinconstrainteq] + λeq = JuMP.dual.(nonlinconeq) # FIXME: does not work for now + λeq = ones(NT, neq) + ∇²ℓgeq = hessian(ℓ_geq, mpc.hessian, mpc.Z̃, Constant(λeq), ∇²geq_cache...) + else + ∇²ℓgeq = nothing + end info[:∇J] = ∇J info[:∇²J] = ∇²J info[:∇g] = ∇g diff --git a/src/estimator/mhe/execute.jl b/src/estimator/mhe/execute.jl index cb968c1ee..3e80ff819 100644 --- a/src/estimator/mhe/execute.jl +++ b/src/estimator/mhe/execute.jl @@ -201,29 +201,25 @@ function addinfo!( else ∇J, ∇²J = gradient(J!, estim.gradient, estim.Z̃, J_cache...), nothing end - nonlincon = optim[:nonlinconstraint] - λ = JuMP.dual.(nonlincon) - display(λ) - ∇g_cache = (Cache(V̂), Cache(X̂0), Cache(û0), Cache(k0), Cache(ŷ0)) function g!(g, Z̃, V̂, X̂0, û0, k0, ŷ0) update_prediction!(V̂, X̂0, û0, k0, ŷ0, g, estim, Z̃) return nothing end ∇g = jacobian(g!, g, estim.jacobian, estim.Z̃, ∇g_cache...) - #= if !isnothing(estim.hessian) + ∇²g_cache = (Cache(V̂), Cache(X̂0), Cache(û0), Cache(k0), Cache(ŷ0), Cache(g)) function ℓ_g(Z̃, λ, V̂, X̂0, û0, k0, ŷ0, g) update_prediction!(V̂, X̂0, û0, k0, ŷ0, g, estim, Z̃) return dot(λ, g) end - ∇²g_cache = (Cache(V̂), Cache(X̂0), Cache(û0), Cache(k0), Cache(ŷ0), Cache(g)) + nonlincon = optim[:nonlinconstraint] + λ = JuMP.dual.(nonlincon) # FIXME: does not work for now + λ = ones(NT, ng) ∇²ℓg = hessian(ℓ_g, estim.hessian, estim.Z̃, Constant(λ), ∇²g_cache...) else ∇²ℓg = nothing end - =# ∇²ℓg = nothing #TODO: delete this line when enabling the above block - info[:∇J] = ∇J info[:∇²J] = ∇²J info[:∇g] = ∇g From 88043f21665d39abc9af09a9534b09c9d6a21af3 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Sun, 30 Nov 2025 20:36:45 -0500 Subject: [PATCH 10/16] wip: debug MHE `addinfo!` for the Hessian --- src/estimator/mhe/execute.jl | 1 - 1 file changed, 1 deletion(-) diff --git a/src/estimator/mhe/execute.jl b/src/estimator/mhe/execute.jl index 3e80ff819..f08379f9a 100644 --- a/src/estimator/mhe/execute.jl +++ b/src/estimator/mhe/execute.jl @@ -180,7 +180,6 @@ function addinfo!( optim, con = estim.optim, estim.con nx̂, nym, nŷ, nu, nk = estim.nx̂, estim.nym, model.ny, model.nu, model.nk He = estim.He - ng = length(con.i_g) nV̂, nX̂, ng = He*nym, He*nx̂, length(con.i_g) V̂, X̂0 = zeros(NT, nV̂), zeros(NT, nX̂) k0 = zeros(NT, nk) From d98d59e61699510e1dc86d61c9a9a18be51e939b Mon Sep 17 00:00:00 2001 From: franckgaga Date: Mon, 1 Dec 2025 12:35:01 -0500 Subject: [PATCH 11/16] debug: temporarily finite constraints in `addinfo!` --- src/controller/nonlinmpc.jl | 7 ++++++- src/estimator/mhe/execute.jl | 15 ++++++++++++++- 2 files changed, 20 insertions(+), 2 deletions(-) diff --git a/src/controller/nonlinmpc.jl b/src/controller/nonlinmpc.jl index 4b585dd1b..e89948c39 100644 --- a/src/controller/nonlinmpc.jl +++ b/src/controller/nonlinmpc.jl @@ -541,7 +541,7 @@ function addinfo!(info, mpc::NonLinMPC{NT}) where NT<:Real info[:JE] = JE info[:gc] = LHS info[:sol] = JuMP.solution_summary(mpc.optim, verbose=true) - # --- derivatives --- + # --- objective derivatives --- model, optim = mpc.estim.model, mpc.optim transcription = mpc.transcription nu, ny, nx̂, nϵ = model.nu, model.ny, mpc.estim.nx̂, mpc.nϵ @@ -573,6 +573,9 @@ function addinfo!(info, mpc::NonLinMPC{NT}) where NT<:Real else ∇J, ∇²J = gradient(J!, mpc.gradient, mpc.Z̃, J_cache...), nothing end + # --- inequality constraint derivatives --- + old_i_g = copy(mpc.con.i_g) + mpc.con.i_g .= 1 # temporarily set all constraints as finite so g is entirely computed ∇g_cache = ( Cache(ΔŨ), Cache(x̂0end), Cache(Ue), Cache(Ŷe), Cache(U0), Cache(Ŷ0), Cache(Û0), Cache(K0), Cache(X̂0), @@ -600,6 +603,8 @@ function addinfo!(info, mpc::NonLinMPC{NT}) where NT<:Real else ∇²ℓg = nothing end + mpc.con.i_g .= old_i_g # restore original finite/infinite constraint indices + # --- equality constraint derivatives --- geq_cache = ( Cache(ΔŨ), Cache(x̂0end), Cache(Ue), Cache(Ŷe), Cache(U0), Cache(Ŷ0), Cache(Û0), Cache(K0), Cache(X̂0), diff --git a/src/estimator/mhe/execute.jl b/src/estimator/mhe/execute.jl index f08379f9a..1d86b7fc5 100644 --- a/src/estimator/mhe/execute.jl +++ b/src/estimator/mhe/execute.jl @@ -94,6 +94,15 @@ following fields: - `:D` : measured disturbances over ``N_k``, ``\mathbf{D}`` - `:sol` : solution summary of the optimizer for printing +For [`NonLinModel`](@ref), it also includes the following derivative fields: + +- `:JE`: economic cost value at the optimum, ``J_E`` +- `:gc`: custom nonlinear constraints values at the optimum, ``\mathbf{g_c}`` +- `:∇J` or *`:nablaJ`* : gradient of the objective function, ``\mathbf{\nabla} J`` +- `:∇²J` or *`:nabla2J`* : Hessian of the objective function, ``\mathbf{\nabla^2}J`` +- `:∇g` or *`:nablag`* : Jacobian of the inequality constraint, ``\mathbf{\nabla g}`` +- `:∇²ℓg` or *`:nabla2lg`* : Hessian of the inequality Lagrangian, ``\mathbf{\nabla^2}\ell_{\mathbf{g}}`` + # Examples ```jldoctest julia> model = LinModel(ss(1.0, 1.0, 1.0, 0, 5.0)); @@ -176,7 +185,7 @@ For [`NonLinModel`](@ref), add the various derivatives. function addinfo!( info, estim::MovingHorizonEstimator{NT}, model::NonLinModel ) where NT <:Real - # --- derivatives --- + # --- objective derivatives --- optim, con = estim.optim, estim.con nx̂, nym, nŷ, nu, nk = estim.nx̂, estim.nym, model.ny, model.nu, model.nk He = estim.He @@ -200,6 +209,9 @@ function addinfo!( else ∇J, ∇²J = gradient(J!, estim.gradient, estim.Z̃, J_cache...), nothing end + # --- inequality constraint derivatives --- + old_i_g = copy(estim.con.i_g) + estim.con.i_g .= 1 # temporarily set all constraints as finite so g is entirely computed ∇g_cache = (Cache(V̂), Cache(X̂0), Cache(û0), Cache(k0), Cache(ŷ0)) function g!(g, Z̃, V̂, X̂0, û0, k0, ŷ0) update_prediction!(V̂, X̂0, û0, k0, ŷ0, g, estim, Z̃) @@ -219,6 +231,7 @@ function addinfo!( else ∇²ℓg = nothing end + estim.con.i_g .= old_i_g # restore original finite/infinite constraint indices info[:∇J] = ∇J info[:∇²J] = ∇²J info[:∇g] = ∇g From 512b596d3175cc93791618d7940fa878db0121a9 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Mon, 1 Dec 2025 12:44:30 -0500 Subject: [PATCH 12/16] bump --- Project.toml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Project.toml b/Project.toml index f7bd0ddd5..db8e65dd9 100644 --- a/Project.toml +++ b/Project.toml @@ -1,6 +1,6 @@ name = "ModelPredictiveControl" uuid = "61f9bdb8-6ae4-484a-811f-bbf86720c31c" -version = "1.13.3" +version = "1.14.0" authors = ["Francis Gagnon"] [deps] From d12d146cbaefe9f7b2cacd98fef0e228a62b6076 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Tue, 2 Dec 2025 14:24:34 -0500 Subject: [PATCH 13/16] debug: do not fetch Hessian Lagrangian without NL constraints --- src/controller/nonlinmpc.jl | 16 ++++++++-------- src/estimator/mhe/execute.jl | 2 +- 2 files changed, 9 insertions(+), 9 deletions(-) diff --git a/src/controller/nonlinmpc.jl b/src/controller/nonlinmpc.jl index e89948c39..65791d457 100644 --- a/src/controller/nonlinmpc.jl +++ b/src/controller/nonlinmpc.jl @@ -542,13 +542,13 @@ function addinfo!(info, mpc::NonLinMPC{NT}) where NT<:Real info[:gc] = LHS info[:sol] = JuMP.solution_summary(mpc.optim, verbose=true) # --- objective derivatives --- - model, optim = mpc.estim.model, mpc.optim + model, optim, con = mpc.estim.model, mpc.optim, mpc.con transcription = mpc.transcription nu, ny, nx̂, nϵ = model.nu, model.ny, mpc.estim.nx̂, mpc.nϵ nk = get_nk(model, transcription) Hp, Hc = mpc.Hp, mpc.Hc - ng = length(mpc.con.i_g) - nc, neq = mpc.con.nc, mpc.con.neq + ng = length(con.i_g) + nc, neq = con.nc, con.neq nU, nŶ, nX̂, nK = mpc.Hp*nu, Hp*ny, Hp*nx̂, Hp*nk nΔŨ, nUe, nŶe = nu*Hc + nϵ, nU + nu, nŶ + ny ΔŨ = zeros(NT, nΔŨ) @@ -574,8 +574,8 @@ function addinfo!(info, mpc::NonLinMPC{NT}) where NT<:Real ∇J, ∇²J = gradient(J!, mpc.gradient, mpc.Z̃, J_cache...), nothing end # --- inequality constraint derivatives --- - old_i_g = copy(mpc.con.i_g) - mpc.con.i_g .= 1 # temporarily set all constraints as finite so g is entirely computed + old_i_g = copy(con.i_g) + con.i_g .= 1 # temporarily set all constraints as finite so g is entirely computed ∇g_cache = ( Cache(ΔŨ), Cache(x̂0end), Cache(Ue), Cache(Ŷe), Cache(U0), Cache(Ŷ0), Cache(Û0), Cache(K0), Cache(X̂0), @@ -586,7 +586,7 @@ function addinfo!(info, mpc::NonLinMPC{NT}) where NT<:Real return nothing end ∇g = jacobian(g!, g, mpc.jacobian, mpc.Z̃, ∇g_cache...) - if !isnothing(mpc.hessian) + if !isnothing(mpc.hessian) && any(con.i_g) function ℓ_g(Z̃, λ, ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, K0, X̂0, gc, geq, g) update_predictions!(ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, K0, X̂0, gc, g, geq, mpc, Z̃) return dot(λ, g) @@ -603,7 +603,7 @@ function addinfo!(info, mpc::NonLinMPC{NT}) where NT<:Real else ∇²ℓg = nothing end - mpc.con.i_g .= old_i_g # restore original finite/infinite constraint indices + con.i_g .= old_i_g # restore original finite/infinite constraint indices # --- equality constraint derivatives --- geq_cache = ( Cache(ΔŨ), Cache(x̂0end), Cache(Ue), Cache(Ŷe), Cache(U0), Cache(Ŷ0), @@ -615,7 +615,7 @@ function addinfo!(info, mpc::NonLinMPC{NT}) where NT<:Real return nothing end ∇geq = jacobian(geq!, geq, mpc.jacobian, mpc.Z̃, geq_cache...) - if !isnothing(mpc.hessian) + if !isnothing(mpc.hessian) && con.neq > 0 ∇²geq_cache = ( Cache(ΔŨ), Cache(x̂0end), Cache(Ue), Cache(Ŷe), Cache(U0), Cache(Ŷ0), Cache(Û0), Cache(K0), Cache(X̂0), diff --git a/src/estimator/mhe/execute.jl b/src/estimator/mhe/execute.jl index 1d86b7fc5..ca41bbfd9 100644 --- a/src/estimator/mhe/execute.jl +++ b/src/estimator/mhe/execute.jl @@ -218,7 +218,7 @@ function addinfo!( return nothing end ∇g = jacobian(g!, g, estim.jacobian, estim.Z̃, ∇g_cache...) - if !isnothing(estim.hessian) + if !isnothing(estim.hessian) && any(con.i_g) ∇²g_cache = (Cache(V̂), Cache(X̂0), Cache(û0), Cache(k0), Cache(ŷ0), Cache(g)) function ℓ_g(Z̃, λ, V̂, X̂0, û0, k0, ŷ0, g) update_prediction!(V̂, X̂0, û0, k0, ŷ0, g, estim, Z̃) From e7bf9d37ea1e8662d8f16b57bdd67e7d77d9e1ce Mon Sep 17 00:00:00 2001 From: franckgaga Date: Tue, 2 Dec 2025 14:52:18 -0500 Subject: [PATCH 14/16] debug: idem --- src/controller/nonlinmpc.jl | 2 +- src/estimator/mhe/execute.jl | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/src/controller/nonlinmpc.jl b/src/controller/nonlinmpc.jl index 65791d457..f313d1c33 100644 --- a/src/controller/nonlinmpc.jl +++ b/src/controller/nonlinmpc.jl @@ -586,7 +586,7 @@ function addinfo!(info, mpc::NonLinMPC{NT}) where NT<:Real return nothing end ∇g = jacobian(g!, g, mpc.jacobian, mpc.Z̃, ∇g_cache...) - if !isnothing(mpc.hessian) && any(con.i_g) + if !isnothing(mpc.hessian) && any(old_i_g) function ℓ_g(Z̃, λ, ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, K0, X̂0, gc, geq, g) update_predictions!(ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, K0, X̂0, gc, g, geq, mpc, Z̃) return dot(λ, g) diff --git a/src/estimator/mhe/execute.jl b/src/estimator/mhe/execute.jl index ca41bbfd9..1d4803142 100644 --- a/src/estimator/mhe/execute.jl +++ b/src/estimator/mhe/execute.jl @@ -218,7 +218,7 @@ function addinfo!( return nothing end ∇g = jacobian(g!, g, estim.jacobian, estim.Z̃, ∇g_cache...) - if !isnothing(estim.hessian) && any(con.i_g) + if !isnothing(estim.hessian) && any(old_i_g) ∇²g_cache = (Cache(V̂), Cache(X̂0), Cache(û0), Cache(k0), Cache(ŷ0), Cache(g)) function ℓ_g(Z̃, λ, V̂, X̂0, û0, k0, ŷ0, g) update_prediction!(V̂, X̂0, û0, k0, ŷ0, g, estim, Z̃) From 36861617ecddd812f49a9bd8dd5ad4594de6905f Mon Sep 17 00:00:00 2001 From: franckgaga Date: Tue, 2 Dec 2025 16:47:52 -0500 Subject: [PATCH 15/16] added: `@warn` about random values in `getinfo!` The sparsity structure is still a useful information IMO, so it is worth it to output the matrices anyway with this warning --- src/controller/execute.jl | 3 +++ src/controller/nonlinmpc.jl | 12 ++++++++++-- src/estimator/mhe/execute.jl | 9 ++++++++- test/2_test_state_estim.jl | 1 + test/3_test_predictive_control.jl | 3 ++- 5 files changed, 24 insertions(+), 4 deletions(-) diff --git a/src/controller/execute.jl b/src/controller/execute.jl index 8f9e3a833..68cbde8e9 100644 --- a/src/controller/execute.jl +++ b/src/controller/execute.jl @@ -117,6 +117,9 @@ are also available: - `:∇geq` or *`:nablageq`* : Jacobian of the equality constraint, ``\mathbf{\nabla g_{eq}}`` - `:∇²ℓgeq` or *`:nabla2lgeq`* : Hessian of the equality Lagrangian, ``\mathbf{\nabla^2}\ell_{\mathbf{g_{eq}}}`` +Note that Hessian of Lagrangians are not fully supported yet. Their nonzero coefficients are +random values for now. + # Examples ```jldoctest julia> mpc = LinMPC(LinModel(tf(5, [2, 1]), 3), Nwt=[0], Hp=1, Hc=1); diff --git a/src/controller/nonlinmpc.jl b/src/controller/nonlinmpc.jl index f313d1c33..aebb63d64 100644 --- a/src/controller/nonlinmpc.jl +++ b/src/controller/nonlinmpc.jl @@ -586,7 +586,11 @@ function addinfo!(info, mpc::NonLinMPC{NT}) where NT<:Real return nothing end ∇g = jacobian(g!, g, mpc.jacobian, mpc.Z̃, ∇g_cache...) - if !isnothing(mpc.hessian) && any(old_i_g) + if !isnothing(mpc.hessian) && any(old_i_g) + @warn( + "Retrieving optimal Hessian of the Lagrangian is not fully supported yet.\n"* + "Its nonzero coefficients are random values for now.", maxlog=1 + ) function ℓ_g(Z̃, λ, ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, K0, X̂0, gc, geq, g) update_predictions!(ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, K0, X̂0, gc, g, geq, mpc, Z̃) return dot(λ, g) @@ -598,7 +602,7 @@ function addinfo!(info, mpc::NonLinMPC{NT}) where NT<:Real ) nonlincon = optim[:nonlinconstraint] λ = JuMP.dual.(nonlincon) # FIXME: does not work for now - λ = ones(NT, ng) + λ = rand(NT, ng) ∇²ℓg = hessian(ℓ_g, mpc.hessian, mpc.Z̃, Constant(λ), ∇²g_cache...) else ∇²ℓg = nothing @@ -616,6 +620,10 @@ function addinfo!(info, mpc::NonLinMPC{NT}) where NT<:Real end ∇geq = jacobian(geq!, geq, mpc.jacobian, mpc.Z̃, geq_cache...) if !isnothing(mpc.hessian) && con.neq > 0 + @warn( + "Retrieving optimal Hessian of the Lagrangian is not fully supported yet.\n"* + "Its nonzero coefficients are random values for now.", maxlog=1 + ) ∇²geq_cache = ( Cache(ΔŨ), Cache(x̂0end), Cache(Ue), Cache(Ŷe), Cache(U0), Cache(Ŷ0), Cache(Û0), Cache(K0), Cache(X̂0), diff --git a/src/estimator/mhe/execute.jl b/src/estimator/mhe/execute.jl index 1d4803142..76c9ac17b 100644 --- a/src/estimator/mhe/execute.jl +++ b/src/estimator/mhe/execute.jl @@ -103,6 +103,9 @@ For [`NonLinModel`](@ref), it also includes the following derivative fields: - `:∇g` or *`:nablag`* : Jacobian of the inequality constraint, ``\mathbf{\nabla g}`` - `:∇²ℓg` or *`:nabla2lg`* : Hessian of the inequality Lagrangian, ``\mathbf{\nabla^2}\ell_{\mathbf{g}}`` +Note that Hessian of Lagrangians are not fully supported yet. Their nonzero coefficients are +random values for now. + # Examples ```jldoctest julia> model = LinModel(ss(1.0, 1.0, 1.0, 0, 5.0)); @@ -218,7 +221,11 @@ function addinfo!( return nothing end ∇g = jacobian(g!, g, estim.jacobian, estim.Z̃, ∇g_cache...) - if !isnothing(estim.hessian) && any(old_i_g) + if !isnothing(estim.hessian) && any(old_i_g) + @warn( + "Retrieving optimal Hessian of the Lagrangian is not fully supported yet.\n"* + "Its nonzero coefficients are random values for now.", maxlog=1 + ) ∇²g_cache = (Cache(V̂), Cache(X̂0), Cache(û0), Cache(k0), Cache(ŷ0), Cache(g)) function ℓ_g(Z̃, λ, V̂, X̂0, û0, k0, ŷ0, g) update_prediction!(V̂, X̂0, û0, k0, ŷ0, g, estim, Z̃) diff --git a/test/2_test_state_estim.jl b/test/2_test_state_estim.jl index 892887889..55a904d7d 100644 --- a/test/2_test_state_estim.jl +++ b/test/2_test_state_estim.jl @@ -967,6 +967,7 @@ end @test info[:Ŷ][end-1:end] ≈ [50, 30] atol=1e-9 mhe1c = MovingHorizonEstimator(nonlinmodel, He=2, direct=false, hessian=true) + mhe1c = setconstraint!(mhe1c, v̂min = [-1000, -1000], v̂max = [1000, 1000]) # coverage of getinfo Hessian of Lagrangian preparestate!(mhe1c, [50, 30], [5]) x̂ = updatestate!(mhe1c, [10, 50], [50, 30], [5]) @test x̂ ≈ zeros(6) atol=1e-9 diff --git a/test/3_test_predictive_control.jl b/test/3_test_predictive_control.jl index db8ed3721..8b614f08a 100644 --- a/test/3_test_predictive_control.jl +++ b/test/3_test_predictive_control.jl @@ -814,7 +814,6 @@ end preparestate!(nmpc4, [0], [0]) u = moveinput!(nmpc4, [0], d, R̂u=fill(12, nmpc4.Hp)) @test u ≈ [12] atol=5e-2 - linmodel3 = LinModel{Float32}(0.5*ones(1,1), ones(1,1), ones(1,1), 0, 0, 3000.0) nmpc5 = NonLinMPC(nonlinmodel, Hp=1, Hc=1, Cwt=Inf, transcription=MultipleShooting()) nmpc5 = setconstraint!(nmpc5, ymin=[1]) f! = (ẋ,x,u,_,_) -> ẋ .= -0.001x .+ u @@ -830,6 +829,7 @@ end preparestate!(nmpc5_1, [0.0]) u = moveinput!(nmpc5_1, [1/0.001]) @test u ≈ [1.0] atol=5e-2 + linmodel3 = LinModel{Float32}(0.5*ones(1,1), ones(1,1), ones(1,1), 0, 0, 3000.0) nmpc6 = NonLinMPC(linmodel3, Hp=10) preparestate!(nmpc6, [0]) @test moveinput!(nmpc6, [0]) ≈ [0.0] atol=5e-2 @@ -849,6 +849,7 @@ end @test info[:Ŷ][end] ≈ 10 atol=5e-2 transcription = MultipleShooting(f_threads=true, h_threads=true) nmpc8t = NonLinMPC(nonlinmodel; Nwt=[0], Hp=100, Hc=1, transcription) + nmpc8t = setconstraint!(nmpc8t, ymax=[100], ymin=[-100]) # coverage of getinfo! Hessians of Lagrangian preparestate!(nmpc8t, [0], [0]) u = moveinput!(nmpc8t, [10], [0]) @test u ≈ [2] atol=5e-2 From 78886db579da4b8cdf9442a962fe82c72e5ac430 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Tue, 2 Dec 2025 17:52:28 -0500 Subject: [PATCH 16/16] test: improve coverage --- test/3_test_predictive_control.jl | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/test/3_test_predictive_control.jl b/test/3_test_predictive_control.jl index 8b614f08a..447cdee57 100644 --- a/test/3_test_predictive_control.jl +++ b/test/3_test_predictive_control.jl @@ -848,7 +848,7 @@ end @test info[:u] ≈ u @test info[:Ŷ][end] ≈ 10 atol=5e-2 transcription = MultipleShooting(f_threads=true, h_threads=true) - nmpc8t = NonLinMPC(nonlinmodel; Nwt=[0], Hp=100, Hc=1, transcription) + nmpc8t = NonLinMPC(nonlinmodel; Nwt=[0], Hp=100, Hc=1, transcription, hessian=true) nmpc8t = setconstraint!(nmpc8t, ymax=[100], ymin=[-100]) # coverage of getinfo! Hessians of Lagrangian preparestate!(nmpc8t, [0], [0]) u = moveinput!(nmpc8t, [10], [0])