From f6363d150f2d274e4d2d98dd1604bf93e5c140e2 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Mon, 28 Apr 2025 11:30:11 -0400 Subject: [PATCH 1/7] added: import `value_and_gradient!`/`jacobian!` of DI.jl --- src/ModelPredictiveControl.jl | 1 + src/controller/nonlinmpc.jl | 106 ++++++++++++++++++++-------------- 2 files changed, 65 insertions(+), 42 deletions(-) diff --git a/src/ModelPredictiveControl.jl b/src/ModelPredictiveControl.jl index 4bc201cd7..adefdfef2 100644 --- a/src/ModelPredictiveControl.jl +++ b/src/ModelPredictiveControl.jl @@ -9,6 +9,7 @@ using ProgressLogging using DifferentiationInterface: ADTypes.AbstractADType, AutoForwardDiff, AutoSparse using DifferentiationInterface: gradient!, jacobian!, prepare_gradient, prepare_jacobian +using DifferentiationInterface: value_and_gradient!, value_and_jacobian! using DifferentiationInterface: Constant, Cache using SparseConnectivityTracer: TracerSparsityDetector using SparseMatrixColorings: GreedyColoringAlgorithm, sparsity_pattern diff --git a/src/controller/nonlinmpc.jl b/src/controller/nonlinmpc.jl index 0794088da..fedfe16b7 100644 --- a/src/controller/nonlinmpc.jl +++ b/src/controller/nonlinmpc.jl @@ -584,8 +584,8 @@ function get_optim_functions(mpc::NonLinMPC, ::JuMP.GenericModel{JNT}) where JNT nZ̃, nU, nŶ, nX̂, nK = length(mpc.Z̃), Hp*nu, Hp*ny, Hp*nx̂, Hp*nk nΔŨ, nUe, nŶe = nu*Hc + nϵ, nU + nu, nŶ + ny strict = Val(true) - myNaN = convert(JNT, NaN) # NaN to force update_simulations! at first call: - Z̃ ::Vector{JNT} = fill(myNaN, nZ̃) + myNaN = convert(JNT, NaN) + J::Vector{JNT} = zeros(JNT, 1) ΔŨ::Vector{JNT} = zeros(JNT, nΔŨ) x̂0end::Vector{JNT} = zeros(JNT, nx̂) K0::Vector{JNT} = zeros(JNT, nK) @@ -595,18 +595,15 @@ function get_optim_functions(mpc::NonLinMPC, ::JuMP.GenericModel{JNT}) where JNT gc::Vector{JNT}, g::Vector{JNT} = zeros(JNT, nc), zeros(JNT, ng) geq::Vector{JNT} = zeros(JNT, neq) # ---------------------- objective function ------------------------------------------- - function Jfunc(Z̃arg::Vararg{T, N}) where {N, T<:Real} - if isdifferent(Z̃arg, Z̃) - Z̃ .= Z̃arg - update_predictions!(ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, K0, X̂0, gc, g, geq, mpc, Z̃) - end - return obj_nonlinprog!(Ŷ0, U0, mpc, model, Ue, Ŷe, ΔŨ)::T - end + + + 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, ΔŨ) end - Z̃_∇J = fill(myNaN, nZ̃) + + Z̃_∇J = fill(myNaN, nZ̃) # NaN to force update_simulations! at first call ∇J_context = ( Cache(ΔŨ), Cache(x̂0end), Cache(Ue), Cache(Ŷe), Cache(U0), Cache(Ŷ0), Cache(Û0), Cache(K0), Cache(X̂0), @@ -614,39 +611,48 @@ function get_optim_functions(mpc::NonLinMPC, ::JuMP.GenericModel{JNT}) where JNT ) ∇J_prep = prepare_gradient(Jfunc!, mpc.gradient, Z̃_∇J, ∇J_context...; strict) ∇J = Vector{JNT}(undef, nZ̃) + + function update_objective!(J, ∇J, Z̃, Z̃arg) + if isdifferent(Z̃arg, Z̃) + Z̃ .= Z̃arg + J[], _ = value_and_gradient!( + Jfunc!, ∇J, ∇J_prep, mpc.gradient, Z̃_∇J, ∇J_context... + ) + end + return nothing + end + + + function Jfunc(Z̃arg::Vararg{T, N}) where {N, T<:Real} + update_objective!(J, ∇J, Z̃_∇J, Z̃arg) + return J[] + end + + ∇Jfunc! = if nZ̃ == 1 function (Z̃arg) - Z̃_∇J .= Z̃arg - gradient!(Jfunc!, ∇J, ∇J_prep, mpc.gradient, Z̃_∇J, ∇J_context...) + update_objective!(J, ∇J, Z̃_∇J, Z̃arg) return ∇J[begin] # univariate syntax, see JuMP.@operator doc end else - function (∇J::AbstractVector{T}, Z̃arg::Vararg{T, N}) where {N, T<:Real} - Z̃_∇J .= Z̃arg - gradient!(Jfunc!, ∇J, ∇J_prep, mpc.gradient, Z̃_∇J, ∇J_context...) - return ∇J # multivariate syntax, see JuMP.@operator doc + function (∇Jarg::AbstractVector{T}, Z̃arg::Vararg{T, N}) where {N, T<:Real} + update_objective!(J, ∇J, Z̃_∇J, Z̃arg) + return ∇Jarg .= ∇J # multivariate syntax, see JuMP.@operator doc end end + + + ∇²Jfunc! = nothing + + # --------------------- inequality constraint functions ------------------------------- - gfuncs = Vector{Function}(undef, ng) - for i in eachindex(gfuncs) - gfunc_i = function (Z̃arg::Vararg{T, N}) where {N, T<:Real} - if isdifferent(Z̃arg, Z̃) - Z̃ .= Z̃arg - update_predictions!( - ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, K0, X̂0, gc, g, geq, mpc, Z̃ - ) - end - return g[i]::T - end - gfuncs[i] = gfunc_i - end + function gfunc!(g, Z̃, ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, K0, X̂0, gc, geq) - return update_predictions!( - ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, K0, X̂0, gc, g, geq, mpc, Z̃ - ) + update_predictions!(ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, K0, X̂0, gc, g, geq, mpc, Z̃) + return nothing end - Z̃_∇g = fill(myNaN, nZ̃) + + Z̃_∇g = fill(myNaN, nZ̃) # NaN to force update_simulations! at first call ∇g_context = ( Cache(ΔŨ), Cache(x̂0end), Cache(Ue), Cache(Ŷe), Cache(U0), Cache(Ŷ0), Cache(Û0), Cache(K0), Cache(X̂0), @@ -657,22 +663,38 @@ function get_optim_functions(mpc::NonLinMPC, ::JuMP.GenericModel{JNT}) where JNT ∇g_prep = prepare_jacobian(gfunc!, g, mpc.jacobian, Z̃_∇g, ∇g_context...; strict) mpc.con.i_g[1:end-nc] .= false ∇g = init_diffmat(JNT, mpc.jacobian, ∇g_prep, nZ̃, ng) + + + function update_con!(g, ∇g, Z̃, Z̃arg) + if isdifferent(Z̃arg, Z̃) + Z̃ .= Z̃arg + value_and_jacobian!( + gfunc!, g, ∇g, ∇g_prep, mpc.jacobian, Z̃, ∇g_context... + ) + end + return nothing + end + + gfuncs = Vector{Function}(undef, ng) + for i in eachindex(gfuncs) + gfunc_i = function (Z̃arg::Vararg{T, N}) where {N, T<:Real} + update_con!(g, ∇g, Z̃_∇g, Z̃arg) + return g[i] + end + gfuncs[i] = gfunc_i + end + + ∇gfuncs! = Vector{Function}(undef, ng) for i in eachindex(∇gfuncs!) ∇gfuncs_i! = if nZ̃ == 1 function (Z̃arg::T) where T<:Real - if isdifferent(Z̃arg, Z̃_∇g) - Z̃_∇g .= Z̃arg - jacobian!(gfunc!, g, ∇g, ∇g_prep, mpc.jacobian, Z̃_∇g, ∇g_context...) - end + update_con!(g, ∇g, Z̃_∇g, Z̃arg) return ∇g[i, begin] # univariate syntax, see JuMP.@operator doc end else function (∇g_i, Z̃arg::Vararg{T, N}) where {N, T<:Real} - if isdifferent(Z̃arg, Z̃_∇g) - Z̃_∇g .= Z̃arg - jacobian!(gfunc!, g, ∇g, ∇g_prep, mpc.jacobian, Z̃_∇g, ∇g_context...) - end + update_con!(g, ∇g, Z̃_∇g, Z̃arg) return ∇g_i .= @views ∇g[i, :] # multivariate syntax, see JuMP.@operator doc end end @@ -697,7 +719,7 @@ function get_optim_functions(mpc::NonLinMPC, ::JuMP.GenericModel{JNT}) where JNT ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, K0, X̂0, gc, g, geq, mpc, Z̃ ) end - Z̃_∇geq = fill(myNaN, nZ̃) + Z̃_∇geq = fill(myNaN, nZ̃) # NaN to force update_simulations! at first call ∇geq_context = ( Cache(ΔŨ), Cache(x̂0end), Cache(Ue), Cache(Ŷe), Cache(U0), Cache(Ŷ0), Cache(Û0), Cache(K0), Cache(X̂0), From 6d45049cd32df6f652baf997c0391f0a85327915 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Mon, 28 Apr 2025 12:15:42 -0400 Subject: [PATCH 2/7] added: `NonLinMP` now uses `value_and_jacobian!` of DI.jl --- src/controller/nonlinmpc.jl | 116 +++++++++++++----------------------- 1 file changed, 43 insertions(+), 73 deletions(-) diff --git a/src/controller/nonlinmpc.jl b/src/controller/nonlinmpc.jl index fedfe16b7..05de5e000 100644 --- a/src/controller/nonlinmpc.jl +++ b/src/controller/nonlinmpc.jl @@ -565,7 +565,8 @@ equality constraint functions `geqfuncs` and gradients `∇geqfuncs!`. This method is really intricate and I'm not proud of it. That's because of 3 elements: - These functions are used inside the nonlinear optimization, so they must be type-stable - and as efficient as possible. + and as efficient as possible. All the function outputs and derivatives are cached and + updated in-place if required to use the efficient [`value_and_jacobian!`](@extref DifferentiationInterface value_and_jacobian!)`. - The `JuMP` NLP syntax forces splatting for the decision variable, which implies use of `Vararg{T,N}` (see the [performance tip][@extref Julia Be-aware-of-when-Julia-avoids-specializing] ) and memoization to avoid redundant computations. This is already complex, but it's even @@ -578,6 +579,7 @@ Inspired from: [User-defined operators with vector outputs](@extref JuMP User-de function get_optim_functions(mpc::NonLinMPC, ::JuMP.GenericModel{JNT}) where JNT<:Real # ----- common cache for Jfunc, gfuncs, geqfuncs called with floats ------------------- model = mpc.estim.model + grad, jac = mpc.gradient, mpc.jacobian nu, ny, nx̂, nϵ, nk = model.nu, model.ny, mpc.estim.nx̂, mpc.nϵ, model.nk Hp, Hc = mpc.Hp, mpc.Hc ng, nc, neq = length(mpc.con.i_g), mpc.con.nc, mpc.con.neq @@ -595,150 +597,118 @@ function get_optim_functions(mpc::NonLinMPC, ::JuMP.GenericModel{JNT}) where JNT gc::Vector{JNT}, g::Vector{JNT} = zeros(JNT, nc), zeros(JNT, ng) geq::Vector{JNT} = zeros(JNT, neq) # ---------------------- 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, ΔŨ) end - - Z̃_∇J = fill(myNaN, nZ̃) # NaN to force update_simulations! at first call + Z̃_∇J = fill(myNaN, nZ̃) # NaN to force update_predictions! at first call ∇J_context = ( 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), ) - ∇J_prep = prepare_gradient(Jfunc!, mpc.gradient, Z̃_∇J, ∇J_context...; strict) + ∇J_prep = prepare_gradient(Jfunc!, grad, Z̃_∇J, ∇J_context...; strict) ∇J = Vector{JNT}(undef, nZ̃) - function update_objective!(J, ∇J, Z̃, Z̃arg) if isdifferent(Z̃arg, Z̃) Z̃ .= Z̃arg - J[], _ = value_and_gradient!( - Jfunc!, ∇J, ∇J_prep, mpc.gradient, Z̃_∇J, ∇J_context... - ) + J[], _ = value_and_gradient!(Jfunc!, ∇J, ∇J_prep, grad, Z̃_∇J, ∇J_context...) end - return nothing - end - - + end function Jfunc(Z̃arg::Vararg{T, N}) where {N, T<:Real} update_objective!(J, ∇J, Z̃_∇J, Z̃arg) - return J[] + return J[]::T end - - - ∇Jfunc! = if nZ̃ == 1 + ∇Jfunc! = if nZ̃ == 1 # univariate syntax (see JuMP.@operator doc): function (Z̃arg) update_objective!(J, ∇J, Z̃_∇J, Z̃arg) - return ∇J[begin] # univariate syntax, see JuMP.@operator doc + return ∇J[begin] end - else + else # multivariate syntax (see JuMP.@operator doc): function (∇Jarg::AbstractVector{T}, Z̃arg::Vararg{T, N}) where {N, T<:Real} update_objective!(J, ∇J, Z̃_∇J, Z̃arg) - return ∇Jarg .= ∇J # multivariate syntax, see JuMP.@operator doc + return ∇Jarg .= ∇J end end - - - ∇²Jfunc! = nothing - - # --------------------- inequality constraint functions ------------------------------- - function gfunc!(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 + return g end - - Z̃_∇g = fill(myNaN, nZ̃) # NaN to force update_simulations! at first call + Z̃_∇g = fill(myNaN, nZ̃) # NaN to force update_predictions! at first call ∇g_context = ( Cache(ΔŨ), Cache(x̂0end), Cache(Ue), Cache(Ŷe), Cache(U0), Cache(Ŷ0), - Cache(Û0), Cache(K0), Cache(X̂0), + Cache(Û0), Cache(K0), Cache(X̂0), Cache(gc), Cache(geq), ) # temporarily enable all the inequality constraints for sparsity detection: mpc.con.i_g[1:end-nc] .= true - ∇g_prep = prepare_jacobian(gfunc!, g, mpc.jacobian, Z̃_∇g, ∇g_context...; strict) + ∇g_prep = prepare_jacobian(gfunc!, g, jac, Z̃_∇g, ∇g_context...; strict) mpc.con.i_g[1:end-nc] .= false - ∇g = init_diffmat(JNT, mpc.jacobian, ∇g_prep, nZ̃, ng) - - + ∇g = init_diffmat(JNT, jac, ∇g_prep, nZ̃, ng) function update_con!(g, ∇g, Z̃, Z̃arg) if isdifferent(Z̃arg, Z̃) Z̃ .= Z̃arg - value_and_jacobian!( - gfunc!, g, ∇g, ∇g_prep, mpc.jacobian, Z̃, ∇g_context... - ) + value_and_jacobian!(gfunc!, g, ∇g, ∇g_prep, jac, Z̃, ∇g_context...) end - return nothing end - gfuncs = Vector{Function}(undef, ng) for i in eachindex(gfuncs) gfunc_i = function (Z̃arg::Vararg{T, N}) where {N, T<:Real} update_con!(g, ∇g, Z̃_∇g, Z̃arg) - return g[i] + return g[i]::T end gfuncs[i] = gfunc_i end - - ∇gfuncs! = Vector{Function}(undef, ng) for i in eachindex(∇gfuncs!) - ∇gfuncs_i! = if nZ̃ == 1 + ∇gfuncs_i! = if nZ̃ == 1 # univariate syntax (see JuMP.@operator doc): function (Z̃arg::T) where T<:Real update_con!(g, ∇g, Z̃_∇g, Z̃arg) - return ∇g[i, begin] # univariate syntax, see JuMP.@operator doc + return ∇g[i, begin] end - else + else # multivariate syntax (see JuMP.@operator doc): function (∇g_i, Z̃arg::Vararg{T, N}) where {N, T<:Real} update_con!(g, ∇g, Z̃_∇g, Z̃arg) - return ∇g_i .= @views ∇g[i, :] # multivariate syntax, see JuMP.@operator doc + return ∇g_i .= @views ∇g[i, :] end end ∇gfuncs![i] = ∇gfuncs_i! end # --------------------- equality constraint functions --------------------------------- - geqfuncs = Vector{Function}(undef, neq) - for i in eachindex(geqfuncs) - geqfunc_i = function (Z̃arg::Vararg{T, N}) where {N, T<:Real} - if isdifferent(Z̃arg, Z̃) - Z̃ .= Z̃arg - update_predictions!( - ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, K0, X̂0, gc, g, geq, mpc, Z̃ - ) - end - return geq[i]::T - end - geqfuncs[i] = geqfunc_i - end function geqfunc!(geq, Z̃, ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, K0, X̂0, gc, g) - return update_predictions!( - ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, K0, X̂0, gc, g, geq, mpc, Z̃ - ) + update_predictions!(ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, K0, X̂0, gc, g, geq, mpc, Z̃) + return geq end - Z̃_∇geq = fill(myNaN, nZ̃) # NaN to force update_simulations! at first call + Z̃_∇geq = fill(myNaN, nZ̃) # NaN to force update_predictions! at first call ∇geq_context = ( Cache(ΔŨ), Cache(x̂0end), Cache(Ue), Cache(Ŷe), Cache(U0), Cache(Ŷ0), Cache(Û0), Cache(K0), Cache(X̂0), Cache(gc), Cache(g) ) - ∇geq_prep = prepare_jacobian(geqfunc!, geq, mpc.jacobian, Z̃_∇geq, ∇geq_context...; strict) - ∇geq = init_diffmat(JNT, mpc.jacobian, ∇geq_prep, nZ̃, neq) + ∇geq_prep = prepare_jacobian(geqfunc!, geq, jac, Z̃_∇geq, ∇geq_context...; strict) + ∇geq = init_diffmat(JNT, jac, ∇geq_prep, nZ̃, neq) + function update_con_eq!(geq, ∇geq, Z̃, Z̃arg) + if isdifferent(Z̃arg, Z̃) + Z̃ .= Z̃arg + value_and_jacobian!(geqfunc!, geq, ∇geq, ∇geq_prep, jac, Z̃, ∇geq_context...) + end + end + geqfuncs = Vector{Function}(undef, neq) + for i in eachindex(geqfuncs) + geqfunc_i = function (Z̃arg::Vararg{T, N}) where {N, T<:Real} + update_con_eq!(geq, ∇geq, Z̃_∇geq, Z̃arg) + return geq[i]::T + end + geqfuncs[i] = geqfunc_i + end ∇geqfuncs! = Vector{Function}(undef, neq) for i in eachindex(∇geqfuncs!) # only multivariate syntax, univariate is impossible since nonlinear equality # constraints imply MultipleShooting, thus input increment ΔU and state X̂0 in Z̃: ∇geqfuncs_i! = function (∇geq_i, Z̃arg::Vararg{T, N}) where {N, T<:Real} - if isdifferent(Z̃arg, Z̃_∇geq) - Z̃_∇geq .= Z̃arg - jacobian!( - geqfunc!, geq, ∇geq, ∇geq_prep, mpc.jacobian, Z̃_∇geq, ∇geq_context... - ) - end + update_con_eq!(geq, ∇geq, Z̃_∇geq, Z̃arg) return ∇geq_i .= @views ∇geq[i, :] end ∇geqfuncs![i] = ∇geqfuncs_i! From ed7e71d6d29c943f1fde56e9fcf63acbd4f0969a Mon Sep 17 00:00:00 2001 From: franckgaga Date: Mon, 28 Apr 2025 12:25:34 -0400 Subject: [PATCH 3/7] doc: add `get_optim_functions` to internal doc --- docs/make.jl | 2 +- docs/src/internals/predictive_control.md | 1 + docs/src/internals/state_estim.md | 1 + 3 files changed, 3 insertions(+), 1 deletion(-) diff --git a/docs/make.jl b/docs/make.jl index de69e08d8..d3a9ef272 100644 --- a/docs/make.jl +++ b/docs/make.jl @@ -17,7 +17,7 @@ links = InterLinks( DocMeta.setdocmeta!( ModelPredictiveControl, :DocTestSetup, - :(using ModelPredictiveControl, ControlSystemsBase); + :(using ModelPredictiveControl, ControlSystemsBase, JuMP); recursive=true ) makedocs( diff --git a/docs/src/internals/predictive_control.md b/docs/src/internals/predictive_control.md index d15e9f953..97417ddd0 100644 --- a/docs/src/internals/predictive_control.md +++ b/docs/src/internals/predictive_control.md @@ -24,6 +24,7 @@ ModelPredictiveControl.init_quadprog ModelPredictiveControl.init_stochpred ModelPredictiveControl.init_matconstraint_mpc ModelPredictiveControl.init_nonlincon! +ModelPredictiveControl.get_optim_functions(::NonLinMPC, ::JuMP.GenericModel) ``` ## Update Quadratic Optimization diff --git a/docs/src/internals/state_estim.md b/docs/src/internals/state_estim.md index fe63f1e4c..2ea6881c0 100644 --- a/docs/src/internals/state_estim.md +++ b/docs/src/internals/state_estim.md @@ -25,6 +25,7 @@ ModelPredictiveControl.relaxX̂ ModelPredictiveControl.relaxŴ ModelPredictiveControl.relaxV̂ ModelPredictiveControl.init_matconstraint_mhe +ModelPredictiveControl.get_optim_functions(::MovingHorizonEstimator, ::JuMP.GenericModel) ``` ## Update Quadratic Optimization From b91368ec1866dcdc8f7308cbf5b00a99a9bf4972 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Mon, 28 Apr 2025 12:39:07 -0400 Subject: [PATCH 4/7] doc: correct ext ref --- src/controller/nonlinmpc.jl | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/controller/nonlinmpc.jl b/src/controller/nonlinmpc.jl index 05de5e000..db119c4c7 100644 --- a/src/controller/nonlinmpc.jl +++ b/src/controller/nonlinmpc.jl @@ -566,7 +566,7 @@ This method is really intricate and I'm not proud of it. That's because of 3 ele - These functions are used inside the nonlinear optimization, so they must be type-stable and as efficient as possible. All the function outputs and derivatives are cached and - updated in-place if required to use the efficient [`value_and_jacobian!`](@extref DifferentiationInterface value_and_jacobian!)`. + updated in-place if required to use the efficient [`value_and_jacobian!`](@extref DifferentiationInterface DifferentiationInterface.value_and_jacobian!)`. - The `JuMP` NLP syntax forces splatting for the decision variable, which implies use of `Vararg{T,N}` (see the [performance tip][@extref Julia Be-aware-of-when-Julia-avoids-specializing] ) and memoization to avoid redundant computations. This is already complex, but it's even From 50960cd4ec109dd4c18faf065337c63b97a3ede3 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Mon, 28 Apr 2025 13:29:41 -0400 Subject: [PATCH 5/7] doc: debug internal doc --- docs/make.jl | 2 +- docs/src/internals/predictive_control.md | 2 +- docs/src/internals/state_estim.md | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) diff --git a/docs/make.jl b/docs/make.jl index d3a9ef272..de69e08d8 100644 --- a/docs/make.jl +++ b/docs/make.jl @@ -17,7 +17,7 @@ links = InterLinks( DocMeta.setdocmeta!( ModelPredictiveControl, :DocTestSetup, - :(using ModelPredictiveControl, ControlSystemsBase, JuMP); + :(using ModelPredictiveControl, ControlSystemsBase); recursive=true ) makedocs( diff --git a/docs/src/internals/predictive_control.md b/docs/src/internals/predictive_control.md index 97417ddd0..51dee94c1 100644 --- a/docs/src/internals/predictive_control.md +++ b/docs/src/internals/predictive_control.md @@ -24,7 +24,7 @@ ModelPredictiveControl.init_quadprog ModelPredictiveControl.init_stochpred ModelPredictiveControl.init_matconstraint_mpc ModelPredictiveControl.init_nonlincon! -ModelPredictiveControl.get_optim_functions(::NonLinMPC, ::JuMP.GenericModel) +ModelPredictiveControl.get_optim_functions(::NonLinMPC, ::ModelPredictiveControl.GenericModel) ``` ## Update Quadratic Optimization diff --git a/docs/src/internals/state_estim.md b/docs/src/internals/state_estim.md index 2ea6881c0..749565a4c 100644 --- a/docs/src/internals/state_estim.md +++ b/docs/src/internals/state_estim.md @@ -25,7 +25,7 @@ ModelPredictiveControl.relaxX̂ ModelPredictiveControl.relaxŴ ModelPredictiveControl.relaxV̂ ModelPredictiveControl.init_matconstraint_mhe -ModelPredictiveControl.get_optim_functions(::MovingHorizonEstimator, ::JuMP.GenericModel) +ModelPredictiveControl.get_optim_functions(::MovingHorizonEstimator, ::ModelPredictiveControl.GenericModel) ``` ## Update Quadratic Optimization From f6eb80034333a7e618653e79d49a87a389ba2ccf Mon Sep 17 00:00:00 2001 From: franckgaga Date: Mon, 28 Apr 2025 14:23:10 -0400 Subject: [PATCH 6/7] added: `MovingHorizonEstimator` now uses `value_and_jacobian!` of DI.jl --- src/controller/nonlinmpc.jl | 4 +- src/estimator/mhe/construct.jl | 79 +++++++++++++++++----------------- 2 files changed, 42 insertions(+), 41 deletions(-) diff --git a/src/controller/nonlinmpc.jl b/src/controller/nonlinmpc.jl index db119c4c7..0fc281023 100644 --- a/src/controller/nonlinmpc.jl +++ b/src/controller/nonlinmpc.jl @@ -566,7 +566,7 @@ This method is really intricate and I'm not proud of it. That's because of 3 ele - These functions are used inside the nonlinear optimization, so they must be type-stable and as efficient as possible. All the function outputs and derivatives are cached and - updated in-place if required to use the efficient [`value_and_jacobian!`](@extref DifferentiationInterface DifferentiationInterface.value_and_jacobian!)`. + updated in-place if required to use the efficient [`value_and_jacobian!`](@extref DifferentiationInterface DifferentiationInterface.value_and_jacobian!). - The `JuMP` NLP syntax forces splatting for the decision variable, which implies use of `Vararg{T,N}` (see the [performance tip][@extref Julia Be-aware-of-when-Julia-avoids-specializing] ) and memoization to avoid redundant computations. This is already complex, but it's even @@ -577,7 +577,7 @@ This method is really intricate and I'm not proud of it. That's because of 3 ele Inspired from: [User-defined operators with vector outputs](@extref JuMP User-defined-operators-with-vector-outputs) """ function get_optim_functions(mpc::NonLinMPC, ::JuMP.GenericModel{JNT}) where JNT<:Real - # ----- common cache for Jfunc, gfuncs, geqfuncs called with floats ------------------- + # ----------- common cache for Jfunc, gfuncs and geqfuncs ---------------------------- model = mpc.estim.model grad, jac = mpc.gradient, mpc.jacobian nu, ny, nx̂, nϵ, nk = model.nu, model.ny, mpc.estim.nx̂, mpc.nϵ, model.nk diff --git a/src/estimator/mhe/construct.jl b/src/estimator/mhe/construct.jl index b85c05fbc..4c6ebb71c 100644 --- a/src/estimator/mhe/construct.jl +++ b/src/estimator/mhe/construct.jl @@ -1314,45 +1314,38 @@ Also return vectors with the nonlinear inequality constraint functions `gfuncs`, This method is really intricate and I'm not proud of it. That's because of 3 elements: - These functions are used inside the nonlinear optimization, so they must be type-stable - and as efficient as possible. + and as efficient as possible. All the function outputs and derivatives are cached and + updated in-place if required to use the efficient [`value_and_jacobian!`](@extref DifferentiationInterface DifferentiationInterface.value_and_jacobian!). - The `JuMP` NLP syntax forces splatting for the decision variable, which implies use of `Vararg{T,N}` (see the [performance tip](@extref Julia Be-aware-of-when-Julia-avoids-specializing)) and memoization to avoid redundant computations. This is already complex, but it's even worse knowing that most automatic differentiation tools do not support splatting. -- The signature of gradient and hessian functions is not the same for univariate (`nZ̃ == 1`) - and multivariate (`nZ̃ > 1`) operators in `JuMP`. Both must be defined. Inspired from: [User-defined operators with vector outputs](@extref JuMP User-defined-operators-with-vector-outputs) """ function get_optim_functions( estim::MovingHorizonEstimator, ::JuMP.GenericModel{JNT}, ) where {JNT <: Real} - # ---------- common cache for Jfunc, gfuncs called with floats ------------------------ + # ----------- common cache for Jfunc and gfuncs -------------------------------------- model, con = estim.model, estim.con + grad, jac = estim.gradient, estim.jacobian nx̂, nym, nŷ, nu, nϵ, nk = estim.nx̂, estim.nym, model.ny, model.nu, estim.nϵ, model.nk He = estim.He nV̂, nX̂, ng, nZ̃ = He*nym, He*nx̂, length(con.i_g), length(estim.Z̃) - myNaN = convert(JNT, NaN) # NaN to force update_simulations! at first call strict = Val(true) - Z̃::Vector{JNT} = fill(myNaN, nZ̃) + myNaN = convert(JNT, NaN) + J::Vector{JNT} = zeros(JNT, 1) V̂::Vector{JNT}, X̂0::Vector{JNT} = zeros(JNT, nV̂), zeros(JNT, nX̂) - k0::Vector{JNT} = zeros(JNT, nk) + k0::Vector{JNT} = zeros(JNT, nk) û0::Vector{JNT}, ŷ0::Vector{JNT} = zeros(JNT, nu), zeros(JNT, nŷ) g::Vector{JNT} = zeros(JNT, ng) x̄::Vector{JNT} = zeros(JNT, nx̂) # --------------------- objective functions ------------------------------------------- - function Jfunc(Z̃arg::Vararg{T, N}) where {N, T<:Real} - if isdifferent(Z̃arg, Z̃) - Z̃ .= Z̃arg - update_prediction!(V̂, X̂0, û0, k0, ŷ0, g, estim, Z̃) - end - return obj_nonlinprog!(x̄, estim, model, V̂, Z̃)::T - end function Jfunc!(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 - Z̃_∇J = fill(myNaN, nZ̃) + Z̃_∇J = fill(myNaN, nZ̃) # NaN to force update_predictions! at first call ∇J_context = ( Cache(V̂), Cache(X̂0), Cache(û0), Cache(k0), Cache(ŷ0), Cache(g), @@ -1360,51 +1353,59 @@ function get_optim_functions( ) # temporarily "fill" the estimation window for the preparation of the gradient: estim.Nk[] = He - ∇J_prep = prepare_gradient(Jfunc!, estim.gradient, Z̃_∇J, ∇J_context...; strict) + ∇J_prep = prepare_gradient(Jfunc!, grad, Z̃_∇J, ∇J_context...; strict) estim.Nk[] = 0 ∇J = Vector{JNT}(undef, nZ̃) - ∇Jfunc! = function (∇J::AbstractVector{T}, Z̃arg::Vararg{T, N}) where {N, T<:Real} + function update_objective!(J, ∇J, Z̃, Z̃arg) + if isdifferent(Z̃arg, Z̃) + Z̃ .= Z̃arg + J[], _ = value_and_gradient!(Jfunc!, ∇J, ∇J_prep, grad, Z̃_∇J, ∇J_context...) + end + end + function Jfunc(Z̃arg::Vararg{T, N}) where {N, T<:Real} + update_objective!(J, ∇J, Z̃_∇J, Z̃arg) + return J[]::T + end + ∇Jfunc! = function (∇Jarg::AbstractVector{T}, Z̃arg::Vararg{T, N}) where {N, T<:Real} # only the multivariate syntax of JuMP.@operator, univariate is impossible for MHE # since Z̃ comprises the arrival state estimate AND the estimated process noise - Z̃_∇J .= Z̃arg - gradient!(Jfunc!, ∇J, ∇J_prep, estim.gradient, Z̃_∇J, ∇J_context...) - return ∇J + update_objective!(J, ∇J, Z̃_∇J, Z̃arg) + return ∇Jarg .= ∇J end - # --------------------- inequality constraint functions ------------------------------- - gfuncs = Vector{Function}(undef, ng) - for i in eachindex(gfuncs) - gfunc_i = function (Z̃arg::Vararg{T, N}) where {N, T<:Real} - if isdifferent(Z̃arg, Z̃) - Z̃ .= Z̃arg - update_prediction!(V̂, X̂0, û0, k0, ŷ0, g, estim, Z̃) - end - return g[i]::T - end - gfuncs[i] = gfunc_i - end function gfunc!(g, Z̃, V̂, X̂0, û0, k0, ŷ0) return update_prediction!(V̂, X̂0, û0, k0, ŷ0, g, estim, Z̃) end - Z̃_∇g = fill(myNaN, nZ̃) + Z̃_∇g = fill(myNaN, nZ̃) # NaN to force update_predictions! at first call ∇g_context = ( Cache(V̂), Cache(X̂0), Cache(û0), Cache(k0), Cache(ŷ0), ) # temporarily enable all the inequality constraints for sparsity detection: estim.con.i_g .= true estim.Nk[] = He - ∇g_prep = prepare_jacobian(gfunc!, g, estim.jacobian, Z̃_∇g, ∇g_context...; strict) + ∇g_prep = prepare_jacobian(gfunc!, g, jac, Z̃_∇g, ∇g_context...; strict) estim.con.i_g .= false estim.Nk[] = 0 - ∇g = init_diffmat(JNT, estim.jacobian, ∇g_prep, nZ̃, ng) + ∇g = init_diffmat(JNT, jac, ∇g_prep, nZ̃, ng) + function update_con!(g, ∇g, Z̃, Z̃arg) + if isdifferent(Z̃arg, Z̃) + Z̃ .= Z̃arg + value_and_jacobian!(gfunc!, g, ∇g, ∇g_prep, jac, Z̃, ∇g_context...) + end + end + gfuncs = Vector{Function}(undef, ng) + for i in eachindex(gfuncs) + gfunc_i = function (Z̃arg::Vararg{T, N}) where {N, T<:Real} + update_con!(g, ∇g, Z̃_∇g, Z̃arg) + return g[i]::T + end + gfuncs[i] = gfunc_i + end ∇gfuncs! = Vector{Function}(undef, ng) for i in eachindex(∇gfuncs!) ∇gfuncs![i] = function (∇g_i, Z̃arg::Vararg{T, N}) where {N, T<:Real} # only the multivariate syntax of JuMP.@operator, see above for the explanation - if isdifferent(Z̃arg, Z̃_∇g) - Z̃_∇g .= Z̃arg - jacobian!(gfunc!, g, ∇g, ∇g_prep, estim.jacobian, Z̃_∇g, ∇g_context...) - end + update_con!(g, ∇g, Z̃_∇g, Z̃arg) return ∇g_i .= @views ∇g[i, :] end end From 282010a39a08bde27f5ca8ae47645b56973be142 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Mon, 28 Apr 2025 14:30:12 -0400 Subject: [PATCH 7/7] bump --- Project.toml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Project.toml b/Project.toml index 1144511d0..3c72934f2 100644 --- a/Project.toml +++ b/Project.toml @@ -1,7 +1,7 @@ name = "ModelPredictiveControl" uuid = "61f9bdb8-6ae4-484a-811f-bbf86720c31c" authors = ["Francis Gagnon"] -version = "1.5.3" +version = "1.6.0" [deps] ControlSystemsBase = "aaaaaaaa-a6ca-5380-bf3e-84a91bcd477e"