From 3640db7d8a2d6829a337844146ccc40b1f0b9241 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Tue, 4 Mar 2025 12:45:00 -0500 Subject: [PATCH 01/14] added: save NL equality constraints in `mpc.optim` --- 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 30d79cfc2..b8660d4b8 100644 --- a/src/controller/nonlinmpc.jl +++ b/src/controller/nonlinmpc.jl @@ -667,7 +667,7 @@ function init_nonlincon!( Z̃var = optim[:Z̃var] for i in 1:con.neq name = Symbol("geq_$i") - geqfunc_i = JuMP.add_nonlinear_operator(optim, nZ̃, geqfuncs[i]; name) + geqfunc_i = optim[name] = JuMP.add_nonlinear_operator(optim, nZ̃, geqfuncs[i]; name) # set with @constrains here instead of set_nonlincon!, since the number of nonlinear # equality constraints is known and constant (±Inf are impossible): @constraint(optim, geqfunc_i(Z̃var...) == 0) From e19783e525a361ace9c1bce7f074a56ae49fe031 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Tue, 4 Mar 2025 16:42:37 -0500 Subject: [PATCH 02/14] added: `GradientBuffer` --- src/general.jl | 22 ++++++++++++++++++---- 1 file changed, 18 insertions(+), 4 deletions(-) diff --git a/src/general.jl b/src/general.jl index 61a20999f..68126e11f 100644 --- a/src/general.jl +++ b/src/general.jl @@ -9,14 +9,28 @@ const DEFAULT_EWT = 0.0 "Abstract type for all differentiation buffers." abstract type DifferentiationBuffer end -"Struct with both function and configuration for ForwardDiff differentiation." -struct JacobianBuffer{FT<:Function, CT<:ForwardDiff.JacobianConfig} <: DifferentiationBuffer +function Base.show(io::IO, buffer::DifferentiationBuffer) + return print(io, "DifferentiationBuffer with a $(typeof(buffer.config).name.name)") +end + +"Struct with both function and configuration for ForwardDiff gradient." +struct GradientBuffer{FT<:Function, CT<:ForwardDiff.GradientConfig} <: DifferentiationBuffer f!::FT config::CT end -function Base.show(io::IO, buffer::DifferentiationBuffer) - return print(io, "DifferentiationBuffer with a $(typeof(buffer.config).name.name)") +GradientBuffer(f!, x) = GradientBuffer(f!, ForwardDiff.GradientConfig(f!, x)) + +function gradient!( + g, buffer::GradientBuffer, x +) + return ForwardDiff.gradient!(g, buffer.f!, x, buffer.config) +end + +"Struct with both function and configuration for ForwardDiff Jacobian." +struct JacobianBuffer{FT<:Function, CT<:ForwardDiff.JacobianConfig} <: DifferentiationBuffer + f!::FT + config::CT end "Create a JacobianBuffer with function `f!`, output `y` and input `x`." From 9f4c3572d469830d663dc08c7b8dd776fb285279 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Wed, 5 Mar 2025 14:45:34 -0500 Subject: [PATCH 03/14] added: `NonLinMPC` refactor for custom objective gradient --- src/controller/nonlinmpc.jl | 148 ++++++++++++++++++++++++------------ src/general.jl | 6 +- 2 files changed, 104 insertions(+), 50 deletions(-) diff --git a/src/controller/nonlinmpc.jl b/src/controller/nonlinmpc.jl index b8660d4b8..70298ec69 100644 --- a/src/controller/nonlinmpc.jl +++ b/src/controller/nonlinmpc.jl @@ -505,8 +505,8 @@ function init_optimization!(mpc::NonLinMPC, model::SimModel, optim) JuMP.set_attribute(optim, "nlp_scaling_max_gradient", 10.0/C) end end - Jfunc, gfuncs, geqfuncs = get_optim_functions(mpc, optim) - @operator(optim, J, nZ̃, Jfunc) + Jfunc, ∇Jfunc!, gfuncs, geqfuncs, ∇geqfuncs! = get_optim_functions(mpc, optim) + @operator(optim, J, nZ̃, Jfunc, ∇Jfunc!) @objective(optim, Min, J(Z̃var...)) init_nonlincon!(mpc, model, transcription, gfuncs, geqfuncs) set_nonlincon!(mpc, model, optim) @@ -514,10 +514,10 @@ function init_optimization!(mpc::NonLinMPC, model::SimModel, optim) end """ - get_optim_functions(mpc::NonLinMPC, ::JuMP.GenericModel) -> Jfunc, gfuncs, geqfuncs + get_optim_functions(mpc::NonLinMPC, ::JuMP.GenericModel) -> Jfunc, ∇Jfunc!, gfuncs, geqfuncs -Get the objective `Jfunc` function, and constraint `gfuncs` and `geqfuncs` function vectors -for [`NonLinMPC`](@ref). +Get the objective `Jfunc` function and `∇Jfunc!` to compute its gradient, and constraint +`gfuncs` and `geqfuncs` function vectors for [`NonLinMPC`](@ref). Inspired from: [User-defined operators with vector outputs](https://jump.dev/JuMP.jl/stable/tutorials/nonlinear/tips_and_tricks/#User-defined-operators-with-vector-outputs) """ @@ -541,22 +541,25 @@ function get_optim_functions(mpc::NonLinMPC, ::JuMP.GenericModel{JNT}) where JNT gc_cache::DiffCache{Vector{JNT}, Vector{JNT}} = DiffCache(zeros(JNT, nc), Ncache) g_cache::DiffCache{Vector{JNT}, Vector{JNT}} = DiffCache(zeros(JNT, ng), Ncache) geq_cache::DiffCache{Vector{JNT}, Vector{JNT}} = DiffCache(zeros(JNT, neq), Ncache) - function update_simulations!(Z̃, Z̃tup::NTuple{N, T}) where {N, T<:Real} - if any(new !== old for (new, old) in zip(Z̃tup, Z̃)) # new Z̃tup, update predictions: - Z̃1 = Z̃tup[begin] - for i in eachindex(Z̃tup) - Z̃[i] = Z̃tup[i] # Z̃ .= Z̃tup seems to produce a type instability + function update_simulations!( + Z̃arg::Union{NTuple{N, T}, AbstractVector{T}}, Z̃cache + ) where {N, T<:Real} + if any(cache !== arg for (cache, arg) in zip(Z̃cache, Z̃arg)) # new Z̃, update: + for i in eachindex(Z̃cache) + # Z̃cache .= Z̃arg is type unstable with Z̃arg::NTuple{N, FowardDiff.Dual} + Z̃cache[i] = Z̃arg[i] end + Z̃ = Z̃cache ϵ = (nϵ ≠ 0) ? Z̃[end] : zero(T) # ϵ = 0 if nϵ == 0 (meaning no relaxation) - ΔŨ = get_tmp(ΔŨ_cache, Z̃1) - x̂0end = get_tmp(x̂0end_cache, Z̃1) - Ue, Ŷe = get_tmp(Ue_cache, Z̃1), get_tmp(Ŷe_cache, Z̃1) - U0, Ŷ0 = get_tmp(U0_cache, Z̃1), get_tmp(Ŷ0_cache, Z̃1) - X̂0, Û0 = get_tmp(X̂0_cache, Z̃1), get_tmp(Û0_cache, Z̃1) - gc, g = get_tmp(gc_cache, Z̃1), get_tmp(g_cache, Z̃1) - geq = get_tmp(geq_cache, Z̃1) + ΔŨ = get_tmp(ΔŨ_cache, T) + x̂0end = get_tmp(x̂0end_cache, T) + Ue, Ŷe = get_tmp(Ue_cache, T), get_tmp(Ŷe_cache, T) + U0, Ŷ0 = get_tmp(U0_cache, T), get_tmp(Ŷ0_cache, T) + X̂0, Û0 = get_tmp(X̂0_cache, T), get_tmp(Û0_cache, T) + gc, g = get_tmp(gc_cache, T), get_tmp(g_cache, T) + geq = get_tmp(geq_cache, T) U0 = getU0!(U0, mpc, Z̃) - ΔŨ = getΔŨ!(ΔŨ, mpc, mpc.transcription, Z̃) + ΔŨ = getΔŨ!(ΔŨ, mpc, transcription, Z̃) Ŷ0, x̂0end = predict!(Ŷ0, x̂0end, X̂0, Û0, mpc, model, transcription, U0, Z̃) Ue, Ŷe = extended_vectors!(Ue, Ŷe, mpc, U0, Ŷ0) gc = con_custom!(gc, mpc, Ue, Ŷe, ϵ) @@ -565,43 +568,94 @@ function get_optim_functions(mpc::NonLinMPC, ::JuMP.GenericModel{JNT}) where JNT end return nothing end - function Jfunc(Z̃tup::Vararg{T, N}) where {N, T<:Real} - Z̃1 = Z̃tup[begin] - Z̃ = get_tmp(Z̃_cache, Z̃1) - update_simulations!(Z̃, Z̃tup) - ΔŨ = get_tmp(ΔŨ_cache, Z̃1) - Ue, Ŷe = get_tmp(Ue_cache, Z̃1), get_tmp(Ŷe_cache, Z̃1) - U0, Ŷ0 = get_tmp(U0_cache, Z̃1), get_tmp(Ŷ0_cache, Z̃1) - return obj_nonlinprog!(Ŷ0, U0, mpc, model, Ue, Ŷe, ΔŨ)::T - end - function gfunc_i(i, Z̃tup::NTuple{N, T}) where {N, T<:Real} - Z̃1 = Z̃tup[begin] - Z̃ = get_tmp(Z̃_cache, Z̃1) - update_simulations!(Z̃, Z̃tup) - g = get_tmp(g_cache, Z̃1) - return g[i]::T + # force specialization using Vararg, see https://docs.julialang.org/en/v1/manual/performance-tips/#Be-aware-of-when-Julia-avoids-specializing + function Jfunc(Z̃arg::Vararg{T, N}) where {N, T<:Real} + return Jfunc(Z̃arg, get_tmp(Z̃_cache, T))::T end - gfuncs = Vector{Function}(undef, ng) - for i in 1:ng - # this is another syntax for anonymous function, allowing parameters T and N: - gfuncs[i] = function (Z̃tup::Vararg{T, N}) where {N, T<:Real} - return gfunc_i(i, Z̃tup) - end + # method with the additional cache argument: + function Jfunc(Z̃arg::Union{NTuple{N, T}, AbstractVector{T}}, Z̃cache) where {N, T<:Real} + update_simulations!(Z̃arg, Z̃cache) + ΔŨ = get_tmp(ΔŨ_cache, T) + Ue, Ŷe = get_tmp(Ue_cache, T), get_tmp(Ŷe_cache, T) + U0, Ŷ0 = get_tmp(U0_cache, T), get_tmp(Ŷ0_cache, T) + return obj_nonlinprog!(Ŷ0, U0, mpc, model, Ue, Ŷe, ΔŨ) end - function gfunceq_i(i, Z̃tup::NTuple{N, T}) where {N, T<:Real} - Z̃1 = Z̃tup[begin] - Z̃ = get_tmp(Z̃_cache, Z̃1) - update_simulations!(Z̃, Z̃tup) - geq = get_tmp(geq_cache, Z̃1) + Jfunc_vec(Z̃vec) = Jfunc(Z̃vec, get_tmp(Z̃_cache, Z̃vec[1])) + Z̃vec = Vector{JNT}(undef, nZ̃) + ∇Jbuffer = GradientBuffer(Jfunc_vec, Z̃vec) + function ∇Jfunc!(∇J, Z̃arg::Vararg{T, N}) where {N, T<:Real} + Z̃vec .= Z̃arg + gradient!(∇J, ∇Jbuffer, Z̃vec) + return nothing + end + + + function gfunceq_i(i, Z̃arg::NTuple{N, T}) where {N, T<:Real} + update_simulations!(Z̃arg, get_tmp(Z̃_cache, T)) + geq = get_tmp(geq_cache, T) return geq[i]::T end geqfuncs = Vector{Function}(undef, neq) for i in 1:neq - geqfuncs[i] = function (Z̃tup::Vararg{T, N}) where {N, T<:Real} - return gfunceq_i(i, Z̃tup) + geqfuncs[i] = function (Z̃arg::Vararg{T, N}) where {N, T<:Real} + return gfunceq_i(i, Z̃arg) + end + end + + + ∇geqfuncs! = nothing + #= + function geqfunc!(geq, Z̃) + update_simulations!(Z̃, get_tmp(Z̃_cache, T)) + geq = get_tmp(geq_cache, T) + return + end + =# + + #= + + + + ∇geq = Matrix{JNT}(undef, neq, nZ̃) # Jacobian of geq + function ∇geqfunc_vec!(∇geq, Z̃vec) + update_simulations!(Z̃arg, get_tmp(Z̃_cache, T)) + return nothing + end + + + + + + function ∇geqfuncs_i!(∇geq_i, i, Z̃arg::NTuple{N, T}) where {N, T<:Real} + Z̃arg_vec .= Z̃arg + ForwardDiff + + + end + + ∇geqfuncs! = Vector{Function}(undef, neq) + for i in 1:neq + ∇eqfuncs![i] = function (∇geq, Z̃arg::Vararg{T, N}) where {N, T<:Real} + return ∇geqfuncs_i!(∇geq, i, Z̃arg) + end + end +=# + + + # TODO:re-déplacer en haut: + function gfunc_i(i, Z̃arg::NTuple{N, T}) where {N, T<:Real} + update_simulations!(Z̃arg, get_tmp(Z̃_cache, T)) + g = get_tmp(g_cache, T) + return g[i]::T + end + gfuncs = Vector{Function}(undef, ng) + for i in 1:ng + # this is another syntax for anonymous function, allowing parameters T and N: + gfuncs[i] = function (Z̃arg::Vararg{T, N}) where {N, T<:Real} + return gfunc_i(i, Z̃arg) end end - return Jfunc, gfuncs, geqfuncs + return Jfunc, ∇Jfunc!, gfuncs, geqfuncs, ∇geqfuncs! end """ diff --git a/src/general.jl b/src/general.jl index 68126e11f..f46374129 100644 --- a/src/general.jl +++ b/src/general.jl @@ -15,16 +15,16 @@ end "Struct with both function and configuration for ForwardDiff gradient." struct GradientBuffer{FT<:Function, CT<:ForwardDiff.GradientConfig} <: DifferentiationBuffer - f!::FT + f::FT config::CT end -GradientBuffer(f!, x) = GradientBuffer(f!, ForwardDiff.GradientConfig(f!, x)) +GradientBuffer(f, x) = GradientBuffer(f, ForwardDiff.GradientConfig(f, x)) function gradient!( g, buffer::GradientBuffer, x ) - return ForwardDiff.gradient!(g, buffer.f!, x, buffer.config) + return ForwardDiff.gradient!(g, buffer.f, x, buffer.config) end "Struct with both function and configuration for ForwardDiff Jacobian." From 38531a86213100c37e8d3536ce970194c98a6a30 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Wed, 5 Mar 2025 16:19:47 -0500 Subject: [PATCH 04/14] added: similar refactoring for nonlinear equality constraints --- src/controller/nonlinmpc.jl | 125 +++++++++++++++--------------------- 1 file changed, 51 insertions(+), 74 deletions(-) diff --git a/src/controller/nonlinmpc.jl b/src/controller/nonlinmpc.jl index 70298ec69..e44563320 100644 --- a/src/controller/nonlinmpc.jl +++ b/src/controller/nonlinmpc.jl @@ -508,7 +508,7 @@ function init_optimization!(mpc::NonLinMPC, model::SimModel, optim) Jfunc, ∇Jfunc!, gfuncs, geqfuncs, ∇geqfuncs! = get_optim_functions(mpc, optim) @operator(optim, J, nZ̃, Jfunc, ∇Jfunc!) @objective(optim, Min, J(Z̃var...)) - init_nonlincon!(mpc, model, transcription, gfuncs, geqfuncs) + init_nonlincon!(mpc, model, transcription, gfuncs, geqfuncs, ∇geqfuncs!) set_nonlincon!(mpc, model, optim) return nothing end @@ -541,6 +541,7 @@ function get_optim_functions(mpc::NonLinMPC, ::JuMP.GenericModel{JNT}) where JNT gc_cache::DiffCache{Vector{JNT}, Vector{JNT}} = DiffCache(zeros(JNT, nc), Ncache) g_cache::DiffCache{Vector{JNT}, Vector{JNT}} = DiffCache(zeros(JNT, ng), Ncache) geq_cache::DiffCache{Vector{JNT}, Vector{JNT}} = DiffCache(zeros(JNT, neq), Ncache) + # --------------------- update simulation function ------------------------------------ function update_simulations!( Z̃arg::Union{NTuple{N, T}, AbstractVector{T}}, Z̃cache ) where {N, T<:Real} @@ -568,81 +569,30 @@ function get_optim_functions(mpc::NonLinMPC, ::JuMP.GenericModel{JNT}) where JNT end return nothing end - # force specialization using Vararg, see https://docs.julialang.org/en/v1/manual/performance-tips/#Be-aware-of-when-Julia-avoids-specializing - function Jfunc(Z̃arg::Vararg{T, N}) where {N, T<:Real} - return Jfunc(Z̃arg, get_tmp(Z̃_cache, T))::T - end - # method with the additional cache argument: - function Jfunc(Z̃arg::Union{NTuple{N, T}, AbstractVector{T}}, Z̃cache) where {N, T<:Real} - update_simulations!(Z̃arg, Z̃cache) + # --------------------- objective function -------------------------------------------- + function Jfunc(Z̃arg::Vararg{T, N}) where {N, T<:Real} # force specialization with Vararg, see https://docs.julialang.org/en/v1/manual/performance-tips/#Be-aware-of-when-Julia-avoids-specializing + update_simulations!(Z̃arg, get_tmp(Z̃_cache, T)) ΔŨ = get_tmp(ΔŨ_cache, T) Ue, Ŷe = get_tmp(Ue_cache, T), get_tmp(Ŷe_cache, T) U0, Ŷ0 = get_tmp(U0_cache, T), get_tmp(Ŷ0_cache, T) - return obj_nonlinprog!(Ŷ0, U0, mpc, model, Ue, Ŷe, ΔŨ) + return obj_nonlinprog!(Ŷ0, U0, mpc, model, Ue, Ŷe, ΔŨ)::T end - Jfunc_vec(Z̃vec) = Jfunc(Z̃vec, get_tmp(Z̃_cache, Z̃vec[1])) + function Jfunc_vec(Z̃arg::AbstractVector{T}) where T<:Real + update_simulations!(Z̃arg, get_tmp(Z̃_cache, T)) + ΔŨ = get_tmp(ΔŨ_cache, T) + Ue, Ŷe = get_tmp(Ue_cache, T), get_tmp(Ŷe_cache, T) + U0, Ŷ0 = get_tmp(U0_cache, T), get_tmp(Ŷ0_cache, T) + return obj_nonlinprog!(Ŷ0, U0, mpc, model, Ue, Ŷe, ΔŨ)::T + end Z̃vec = Vector{JNT}(undef, nZ̃) ∇Jbuffer = GradientBuffer(Jfunc_vec, Z̃vec) function ∇Jfunc!(∇J, Z̃arg::Vararg{T, N}) where {N, T<:Real} Z̃vec .= Z̃arg - gradient!(∇J, ∇Jbuffer, Z̃vec) - return nothing - end - - - function gfunceq_i(i, Z̃arg::NTuple{N, T}) where {N, T<:Real} - update_simulations!(Z̃arg, get_tmp(Z̃_cache, T)) - geq = get_tmp(geq_cache, T) - return geq[i]::T - end - geqfuncs = Vector{Function}(undef, neq) - for i in 1:neq - geqfuncs[i] = function (Z̃arg::Vararg{T, N}) where {N, T<:Real} - return gfunceq_i(i, Z̃arg) - end - end - - - ∇geqfuncs! = nothing - #= - function geqfunc!(geq, Z̃) - update_simulations!(Z̃, get_tmp(Z̃_cache, T)) - geq = get_tmp(geq_cache, T) - return - end - =# - - #= - - - - ∇geq = Matrix{JNT}(undef, neq, nZ̃) # Jacobian of geq - function ∇geqfunc_vec!(∇geq, Z̃vec) - update_simulations!(Z̃arg, get_tmp(Z̃_cache, T)) - return nothing - end - - - - - - function ∇geqfuncs_i!(∇geq_i, i, Z̃arg::NTuple{N, T}) where {N, T<:Real} - Z̃arg_vec .= Z̃arg - ForwardDiff - - + return gradient!(∇J, ∇Jbuffer, Z̃vec) end - - ∇geqfuncs! = Vector{Function}(undef, neq) - for i in 1:neq - ∇eqfuncs![i] = function (∇geq, Z̃arg::Vararg{T, N}) where {N, T<:Real} - return ∇geqfuncs_i!(∇geq, i, Z̃arg) - end - end -=# - - + # --------------------- inequality constraint functions ------------------------------- # TODO:re-déplacer en haut: + # TODO: modifier pour combiner en 1 seule fonction comme pour geqfuncs function gfunc_i(i, Z̃arg::NTuple{N, T}) where {N, T<:Real} update_simulations!(Z̃arg, get_tmp(Z̃_cache, T)) g = get_tmp(g_cache, T) @@ -655,12 +605,38 @@ function get_optim_functions(mpc::NonLinMPC, ::JuMP.GenericModel{JNT}) where JNT return gfunc_i(i, Z̃arg) end end + # --------------------- equality constraint functions --------------------------------- + geqfuncs = Vector{Function}(undef, neq) + for i in eachindex(geqfuncs) + func_i = function (Z̃arg::Vararg{T, N}) where {N, T<:Real} # see comment above + update_simulations!(Z̃arg, get_tmp(Z̃_cache, T)) + geq = get_tmp(geq_cache, T) + return geq[i]::T + end + geqfuncs[i] = func_i + end + function geqfunc_vec!(geq, Z̃vec::AbstractVector{T}) where T<:Real + update_simulations!(Z̃vec, get_tmp(Z̃_cache, T)) + geq .= get_tmp(geq_cache, T) + return geq + end + ∇geq = Matrix{JNT}(undef, neq, nZ̃) # Jacobian of geq + geqvec = Vector{JNT}(undef, neq) + ∇geqfuncs! = Vector{Function}(undef, neq) + for i in eachindex(∇geqfuncs!) + ∇geqfuncs![i] = function (∇geq_i, Z̃arg::Vararg{T, N}) where {N, T<:Real} + Z̃vec .= Z̃arg + ForwardDiff.jacobian!(∇geq, geqfunc_vec!, geqvec, Z̃vec) + ∇geq_i .= @views ∇geq[i, :] + return ∇geq_i + end + end return Jfunc, ∇Jfunc!, gfuncs, geqfuncs, ∇geqfuncs! end """ init_nonlincon!( - mpc::NonLinMPC, model::LinModel, ::TranscriptionMethod, gfuncs, geqfuncs + mpc::NonLinMPC, model::LinModel, ::TranscriptionMethod, gfuncs, geqfuncs, ∇geqfuncs! ) Init nonlinear constraints for [`LinModel`](@ref). @@ -669,7 +645,7 @@ The only nonlinear constraints are the custom inequality constraints `gc`. """ function init_nonlincon!( mpc::NonLinMPC, ::LinModel, ::TranscriptionMethod, - gfuncs::Vector{<:Function}, geqfuncs::Vector{<:Function} + gfuncs::Vector{<:Function}, _ , _ ) optim, con = mpc.optim, mpc.con nZ̃ = length(mpc.Z̃) @@ -685,7 +661,7 @@ end """ init_nonlincon!( - mpc::NonLinMPC, model::NonLinModel, ::MultipleShooting, gfuncs, geqfuncs + mpc::NonLinMPC, model::NonLinModel, ::MultipleShooting, gfuncs, geqfuncs, ∇geqfuncs! ) Init nonlinear constraints for [`NonLinModel`](@ref) and [`MultipleShooting`](@ref). @@ -695,7 +671,7 @@ constraints `gc` and all the nonlinear equality constraints `geq`. """ function init_nonlincon!( mpc::NonLinMPC, ::NonLinModel, ::MultipleShooting, - gfuncs::Vector{<:Function}, geqfuncs::Vector{<:Function} + gfuncs::Vector{<:Function}, geqfuncs::Vector{<:Function}, ∇geqfuncs!::Vector{<:Function} ) optim, con = mpc.optim, mpc.con ny, nx̂, Hp, nZ̃ = mpc.estim.model.ny, mpc.estim.nx̂, mpc.Hp, length(mpc.Z̃) @@ -719,9 +695,10 @@ function init_nonlincon!( end # --- nonlinear equality constraints --- Z̃var = optim[:Z̃var] - for i in 1:con.neq + for i in eachindex(geqfuncs) name = Symbol("geq_$i") - geqfunc_i = optim[name] = JuMP.add_nonlinear_operator(optim, nZ̃, geqfuncs[i]; name) + geqfunc_i = JuMP.add_nonlinear_operator(optim, nZ̃, geqfuncs[i], ∇geqfuncs![i]; name) + optim[name] = geqfunc_i # set with @constrains here instead of set_nonlincon!, since the number of nonlinear # equality constraints is known and constant (±Inf are impossible): @constraint(optim, geqfunc_i(Z̃var...) == 0) @@ -731,7 +708,7 @@ end """ init_nonlincon!( - mpc::NonLinMPC, model::NonLinModel, ::SingleShooting, gfuncs, geqfuncs + mpc::NonLinMPC, model::NonLinModel, ::SingleShooting, gfuncs, geqfuncs, ∇geqfuncs! ) Init nonlinear constraints for [`NonLinModel`](@ref) and [`SingleShooting`](@ref). @@ -741,7 +718,7 @@ prediction `Ŷ` bounds and the terminal state `x̂end` bounds. """ function init_nonlincon!( mpc::NonLinMPC, ::NonLinModel, ::SingleShooting, - gfuncs::Vector{<:Function}, geqfuncs::Vector{<:Function} + gfuncs::Vector{<:Function}, _ , _ ) optim, con = mpc.optim, mpc.con ny, nx̂, Hp, nZ̃ = mpc.estim.model.ny, mpc.estim.nx̂, mpc.Hp, length(mpc.Z̃) From 3d4f9173e754c42570a2e21c9e88ec06a21c59d7 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Wed, 5 Mar 2025 17:51:47 -0500 Subject: [PATCH 05/14] added: similar refactor for nonlinear inequality constraints --- src/controller/nonlinmpc.jl | 181 +++++++------------------------- src/controller/transcription.jl | 142 +++++++++++++++++++++++++ src/general.jl | 12 +-- 3 files changed, 186 insertions(+), 149 deletions(-) diff --git a/src/controller/nonlinmpc.jl b/src/controller/nonlinmpc.jl index e44563320..bb120cf82 100644 --- a/src/controller/nonlinmpc.jl +++ b/src/controller/nonlinmpc.jl @@ -505,10 +505,10 @@ function init_optimization!(mpc::NonLinMPC, model::SimModel, optim) JuMP.set_attribute(optim, "nlp_scaling_max_gradient", 10.0/C) end end - Jfunc, ∇Jfunc!, gfuncs, geqfuncs, ∇geqfuncs! = get_optim_functions(mpc, optim) + Jfunc, ∇Jfunc!, gfuncs, ∇gfuncs!, geqfuncs, ∇geqfuncs! = get_optim_functions(mpc, optim) @operator(optim, J, nZ̃, Jfunc, ∇Jfunc!) @objective(optim, Min, J(Z̃var...)) - init_nonlincon!(mpc, model, transcription, gfuncs, geqfuncs, ∇geqfuncs!) + init_nonlincon!(mpc, model, transcription, gfuncs, ∇gfuncs!, geqfuncs, ∇geqfuncs!) set_nonlincon!(mpc, model, optim) return nothing end @@ -569,8 +569,15 @@ function get_optim_functions(mpc::NonLinMPC, ::JuMP.GenericModel{JNT}) where JNT end return nothing end + # --------------------- cache for the AD functions ----------------------------------- + Z̃arg_vec = Vector{JNT}(undef, nZ̃) + g_vec = Vector{JNT}(undef, ng) + ∇g = Matrix{JNT}(undef, ng, nZ̃) # Jacobian of g + geq_vec = Vector{JNT}(undef, neq) + ∇geq = Matrix{JNT}(undef, neq, nZ̃) # Jacobian of geq # --------------------- objective function -------------------------------------------- - function Jfunc(Z̃arg::Vararg{T, N}) where {N, T<:Real} # force specialization with Vararg, see https://docs.julialang.org/en/v1/manual/performance-tips/#Be-aware-of-when-Julia-avoids-specializing + # force specialization with Vararg, see https://docs.julialang.org/en/v1/manual/performance-tips/#Be-aware-of-when-Julia-avoids-specializing + function Jfunc(Z̃arg::Vararg{T, N}) where {N, T<:Real} update_simulations!(Z̃arg, get_tmp(Z̃_cache, T)) ΔŨ = get_tmp(ΔŨ_cache, T) Ue, Ŷe = get_tmp(Ue_cache, T), get_tmp(Ŷe_cache, T) @@ -582,27 +589,36 @@ function get_optim_functions(mpc::NonLinMPC, ::JuMP.GenericModel{JNT}) where JNT ΔŨ = get_tmp(ΔŨ_cache, T) Ue, Ŷe = get_tmp(Ue_cache, T), get_tmp(Ŷe_cache, T) U0, Ŷ0 = get_tmp(U0_cache, T), get_tmp(Ŷ0_cache, T) - return obj_nonlinprog!(Ŷ0, U0, mpc, model, Ue, Ŷe, ΔŨ)::T + return obj_nonlinprog!(Ŷ0, U0, mpc, model, Ue, Ŷe, ΔŨ) end - Z̃vec = Vector{JNT}(undef, nZ̃) - ∇Jbuffer = GradientBuffer(Jfunc_vec, Z̃vec) + ∇J_buffer = GradientBuffer(Jfunc_vec, Z̃arg_vec) function ∇Jfunc!(∇J, Z̃arg::Vararg{T, N}) where {N, T<:Real} - Z̃vec .= Z̃arg - return gradient!(∇J, ∇Jbuffer, Z̃vec) + Z̃arg_vec .= Z̃arg + return gradient!(∇J, ∇J_buffer, Z̃arg_vec) end # --------------------- inequality constraint functions ------------------------------- - # TODO:re-déplacer en haut: - # TODO: modifier pour combiner en 1 seule fonction comme pour geqfuncs - function gfunc_i(i, Z̃arg::NTuple{N, T}) where {N, T<:Real} - update_simulations!(Z̃arg, get_tmp(Z̃_cache, T)) - g = get_tmp(g_cache, T) - return g[i]::T - end gfuncs = Vector{Function}(undef, ng) - for i in 1:ng - # this is another syntax for anonymous function, allowing parameters T and N: - gfuncs[i] = function (Z̃arg::Vararg{T, N}) where {N, T<:Real} - return gfunc_i(i, Z̃arg) + for i in eachindex(gfuncs) + func_i = function (Z̃arg::Vararg{T, N}) where {N, T<:Real} # see comment above + update_simulations!(Z̃arg, get_tmp(Z̃_cache, T)) + g = get_tmp(g_cache, T) + return g[i]::T + end + gfuncs[i] = func_i + end + function gfunc_vec!(g, Z̃vec::AbstractVector{T}) where T<:Real + update_simulations!(Z̃vec, get_tmp(Z̃_cache, T)) + g .= get_tmp(g_cache, T) + return g + end + ∇g_buffer = JacobianBuffer(gfunc_vec!, g_vec, Z̃arg_vec) + ∇gfuncs! = Vector{Function}(undef, ng) + for i in eachindex(∇gfuncs!) + ∇gfuncs![i] = function (∇g_i, Z̃arg::Vararg{T, N}) where {N, T<:Real} + Z̃arg_vec .= Z̃arg + jacobian!(∇g, ∇g_buffer, g_vec, Z̃arg_vec) + ∇g_i .= @views ∇g[i, :] + return ∇g_i end end # --------------------- equality constraint functions --------------------------------- @@ -620,136 +636,17 @@ function get_optim_functions(mpc::NonLinMPC, ::JuMP.GenericModel{JNT}) where JNT geq .= get_tmp(geq_cache, T) return geq end - ∇geq = Matrix{JNT}(undef, neq, nZ̃) # Jacobian of geq - geqvec = Vector{JNT}(undef, neq) + ∇geq_buffer = JacobianBuffer(geqfunc_vec!, geq_vec, Z̃arg_vec) ∇geqfuncs! = Vector{Function}(undef, neq) for i in eachindex(∇geqfuncs!) ∇geqfuncs![i] = function (∇geq_i, Z̃arg::Vararg{T, N}) where {N, T<:Real} - Z̃vec .= Z̃arg - ForwardDiff.jacobian!(∇geq, geqfunc_vec!, geqvec, Z̃vec) + Z̃arg_vec .= Z̃arg + jacobian!(∇geq, ∇geq_buffer, geq_vec, Z̃arg_vec) ∇geq_i .= @views ∇geq[i, :] return ∇geq_i end end - return Jfunc, ∇Jfunc!, gfuncs, geqfuncs, ∇geqfuncs! -end - -""" - init_nonlincon!( - mpc::NonLinMPC, model::LinModel, ::TranscriptionMethod, gfuncs, geqfuncs, ∇geqfuncs! - ) - -Init nonlinear constraints for [`LinModel`](@ref). - -The only nonlinear constraints are the custom inequality constraints `gc`. -""" -function init_nonlincon!( - mpc::NonLinMPC, ::LinModel, ::TranscriptionMethod, - gfuncs::Vector{<:Function}, _ , _ -) - optim, con = mpc.optim, mpc.con - nZ̃ = length(mpc.Z̃) - if length(con.i_g) ≠ 0 - i_base = 0 - for i in 1:con.nc - name = Symbol("g_c_$i") - optim[name] = JuMP.add_nonlinear_operator(optim, nZ̃, gfuncs[i_base+i]; name) - end - end - return nothing -end - -""" - init_nonlincon!( - mpc::NonLinMPC, model::NonLinModel, ::MultipleShooting, gfuncs, geqfuncs, ∇geqfuncs! - ) - -Init nonlinear constraints for [`NonLinModel`](@ref) and [`MultipleShooting`](@ref). - -The nonlinear constraints are the output prediction `Ŷ` bounds, the custom inequality -constraints `gc` and all the nonlinear equality constraints `geq`. -""" -function init_nonlincon!( - mpc::NonLinMPC, ::NonLinModel, ::MultipleShooting, - gfuncs::Vector{<:Function}, geqfuncs::Vector{<:Function}, ∇geqfuncs!::Vector{<:Function} -) - optim, con = mpc.optim, mpc.con - ny, nx̂, Hp, nZ̃ = mpc.estim.model.ny, mpc.estim.nx̂, mpc.Hp, length(mpc.Z̃) - # --- nonlinear inequality constraints --- - if length(con.i_g) ≠ 0 - i_base = 0 - for i in eachindex(con.Y0min) - name = Symbol("g_Y0min_$i") - optim[name] = JuMP.add_nonlinear_operator(optim, nZ̃, gfuncs[i_base+i]; name) - end - i_base = 1Hp*ny - for i in eachindex(con.Y0max) - name = Symbol("g_Y0max_$i") - optim[name] = JuMP.add_nonlinear_operator(optim, nZ̃, gfuncs[i_base+i]; name) - end - i_base = 2Hp*ny - for i in 1:con.nc - name = Symbol("g_c_$i") - optim[name] = JuMP.add_nonlinear_operator(optim, nZ̃, gfuncs[i_base+i]; name) - end - end - # --- nonlinear equality constraints --- - Z̃var = optim[:Z̃var] - for i in eachindex(geqfuncs) - name = Symbol("geq_$i") - geqfunc_i = JuMP.add_nonlinear_operator(optim, nZ̃, geqfuncs[i], ∇geqfuncs![i]; name) - optim[name] = geqfunc_i - # set with @constrains here instead of set_nonlincon!, since the number of nonlinear - # equality constraints is known and constant (±Inf are impossible): - @constraint(optim, geqfunc_i(Z̃var...) == 0) - end - return nothing -end - -""" - init_nonlincon!( - mpc::NonLinMPC, model::NonLinModel, ::SingleShooting, gfuncs, geqfuncs, ∇geqfuncs! - ) - -Init nonlinear constraints for [`NonLinModel`](@ref) and [`SingleShooting`](@ref). - -The nonlinear constraints are the custom inequality constraints `gc`, the output -prediction `Ŷ` bounds and the terminal state `x̂end` bounds. -""" -function init_nonlincon!( - mpc::NonLinMPC, ::NonLinModel, ::SingleShooting, - gfuncs::Vector{<:Function}, _ , _ -) - optim, con = mpc.optim, mpc.con - ny, nx̂, Hp, nZ̃ = mpc.estim.model.ny, mpc.estim.nx̂, mpc.Hp, length(mpc.Z̃) - if length(con.i_g) ≠ 0 - i_base = 0 - for i in eachindex(con.Y0min) - name = Symbol("g_Y0min_$i") - optim[name] = JuMP.add_nonlinear_operator(optim, nZ̃, gfuncs[i_base+i]; name) - end - i_base = 1Hp*ny - for i in eachindex(con.Y0max) - name = Symbol("g_Y0max_$i") - optim[name] = JuMP.add_nonlinear_operator(optim, nZ̃, gfuncs[i_base+i]; name) - end - i_base = 2Hp*ny - for i in eachindex(con.x̂0min) - name = Symbol("g_x̂0min_$i") - optim[name] = JuMP.add_nonlinear_operator(optim, nZ̃, gfuncs[i_base+i]; name) - end - i_base = 2Hp*ny + nx̂ - for i in eachindex(con.x̂0max) - name = Symbol("g_x̂0max_$i") - optim[name] = JuMP.add_nonlinear_operator(optim, nZ̃, gfuncs[i_base+i]; name) - end - i_base = 2Hp*ny + 2nx̂ - for i in 1:con.nc - name = Symbol("g_c_$i") - optim[name] = JuMP.add_nonlinear_operator(optim, nZ̃, gfuncs[i_base+i]; name) - end - end - return nothing + return Jfunc, ∇Jfunc!, gfuncs, ∇gfuncs!, geqfuncs, ∇geqfuncs! end """ diff --git a/src/controller/transcription.jl b/src/controller/transcription.jl index 72bb513dc..e84aae764 100644 --- a/src/controller/transcription.jl +++ b/src/controller/transcription.jl @@ -508,6 +508,148 @@ function init_defectmat( return Eŝ, Gŝ, Jŝ, Kŝ, Vŝ, Bŝ end +""" + init_nonlincon!( + mpc::PredictiveController, model::LinModel, transcription::TranscriptionMethod, + gfuncs , ∇gfuncs!, + geqfuncs, ∇geqfuncs! + ) + +Init nonlinear constraints for [`LinModel`](@ref) for all [`TranscriptionMethod`](@ref). + +The only nonlinear constraints are the custom inequality constraints `gc`. +""" +function init_nonlincon!( + mpc::PredictiveController, ::LinModel, ::TranscriptionMethod, gfuncs, ∇gfuncs!, _ , _ + ) + optim, con = mpc.optim, mpc.con + nZ̃ = length(mpc.Z̃) + if length(con.i_g) ≠ 0 + i_base = 0 + for i in 1:con.nc + name = Symbol("g_c_$i") + optim[name] = JuMP.add_nonlinear_operator( + optim, nZ̃, gfuncs[i_base+i]; name + ) + end + end + return nothing +end + +""" + init_nonlincon!( + mpc::PredictiveController, model::NonLinModel, transcription::MultipleShooting, + gfuncs, ∇gfuncs!, + geqfuncs, ∇geqfuncs! + ) + +Init nonlinear constraints for [`NonLinModel`](@ref) and [`MultipleShooting`](@ref). + +The nonlinear constraints are the output prediction `Ŷ` bounds, the custom inequality +constraints `gc` and all the nonlinear equality constraints `geq`. +""" +function init_nonlincon!( + mpc::PredictiveController, ::NonLinModel, ::MultipleShooting, + gfuncs, ∇gfuncs!, + geqfuncs, ∇geqfuncs! +) + optim, con = mpc.optim, mpc.con + ny, nx̂, Hp, nZ̃ = mpc.estim.model.ny, mpc.estim.nx̂, mpc.Hp, length(mpc.Z̃) + # --- nonlinear inequality constraints --- + if length(con.i_g) ≠ 0 + i_base = 0 + for i in eachindex(con.Y0min) + name = Symbol("g_Y0min_$i") + optim[name] = JuMP.add_nonlinear_operator( + optim, nZ̃, gfuncs[i_base+i], ∇gfuncs![i_base+i]; name + ) + end + i_base = 1Hp*ny + for i in eachindex(con.Y0max) + name = Symbol("g_Y0max_$i") + optim[name] = JuMP.add_nonlinear_operator( + optim, nZ̃, gfuncs[i_base+i], ∇gfuncs![i_base+i]; name + ) + end + i_base = 2Hp*ny + for i in 1:con.nc + name = Symbol("g_c_$i") + optim[name] = JuMP.add_nonlinear_operator( + optim, nZ̃, gfuncs[i_base+i], ∇gfuncs![i_base+i]; name + ) + end + end + # --- nonlinear equality constraints --- + Z̃var = optim[:Z̃var] + for i in eachindex(geqfuncs) + name = Symbol("geq_$i") + geqfunc_i = optim[name] = JuMP.add_nonlinear_operator( + optim, nZ̃, geqfuncs[i], ∇geqfuncs![i]; name + ) + # set with @constrains here instead of set_nonlincon!, since the number of nonlinear + # equality constraints is known and constant (±Inf are impossible): + @constraint(optim, geqfunc_i(Z̃var...) == 0) + end + return nothing +end + +""" + init_nonlincon!( + mpc::PredictiveController, model::NonLinModel, ::SingleShooting, + gfuncs, ∇gfuncs!, + geqfuncs, ∇geqfuncs! + ) + +Init nonlinear constraints for [`NonLinModel`](@ref) and [`SingleShooting`](@ref). + +The nonlinear constraints are the custom inequality constraints `gc`, the output +prediction `Ŷ` bounds and the terminal state `x̂end` bounds. +""" +function init_nonlincon!( + mpc::PredictiveController, ::NonLinModel, ::SingleShooting, gfuncs, ∇gfuncs!, _ , _ +) + optim, con = mpc.optim, mpc.con + ny, nx̂, Hp, nZ̃ = mpc.estim.model.ny, mpc.estim.nx̂, mpc.Hp, length(mpc.Z̃) + if length(con.i_g) ≠ 0 + i_base = 0 + for i in eachindex(con.Y0min) + name = Symbol("g_Y0min_$i") + optim[name] = JuMP.add_nonlinear_operator( + optim, nZ̃, gfuncs[i_base+i], ∇gfuncs![i_base+i]; name + ) + end + i_base = 1Hp*ny + for i in eachindex(con.Y0max) + name = Symbol("g_Y0max_$i") + optim[name] = JuMP.add_nonlinear_operator( + optim, nZ̃, gfuncs[i_base+i], ∇gfuncs![i_base+i]; name + ) + end + i_base = 2Hp*ny + for i in eachindex(con.x̂0min) + name = Symbol("g_x̂0min_$i") + optim[name] = JuMP.add_nonlinear_operator( + optim, nZ̃, gfuncs[i_base+i], ∇gfuncs![i_base+i]; name + ) + end + i_base = 2Hp*ny + nx̂ + for i in eachindex(con.x̂0max) + name = Symbol("g_x̂0max_$i") + optim[name] = JuMP.add_nonlinear_operator( + optim, nZ̃, gfuncs[i_base+i], ∇gfuncs![i_base+i]; name + ) + end + i_base = 2Hp*ny + 2nx̂ + for i in 1:con.nc + name = Symbol("g_c_$i") + optim[name] = JuMP.add_nonlinear_operator( + optim, nZ̃, gfuncs[i_base+i], ∇gfuncs![i_base+i]; name + ) + end + end + return nothing +end + @doc raw""" linconstrainteq!( mpc::PredictiveController, model::LinModel, transcription::MultipleShooting diff --git a/src/general.jl b/src/general.jl index f46374129..76eb86298 100644 --- a/src/general.jl +++ b/src/general.jl @@ -19,11 +19,11 @@ struct GradientBuffer{FT<:Function, CT<:ForwardDiff.GradientConfig} <: Different config::CT end +"Create a GradientBuffer with function `f` and input `x`." GradientBuffer(f, x) = GradientBuffer(f, ForwardDiff.GradientConfig(f, x)) -function gradient!( - g, buffer::GradientBuffer, x -) +"Compute in-place and return the gradient of `buffer.f` at `x`." +function gradient!(g, buffer::GradientBuffer, x) return ForwardDiff.gradient!(g, buffer.f, x, buffer.config) end @@ -33,13 +33,11 @@ struct JacobianBuffer{FT<:Function, CT<:ForwardDiff.JacobianConfig} <: Different config::CT end -"Create a JacobianBuffer with function `f!`, output `y` and input `x`." +"Create a JacobianBuffer with in-place function `f!`, output `y` and input `x`." JacobianBuffer(f!, y, x) = JacobianBuffer(f!, ForwardDiff.JacobianConfig(f!, y, x)) "Compute in-place and return the Jacobian matrix of `buffer.f!` at `x`." -function jacobian!( - A, buffer::JacobianBuffer, y, x -) +function jacobian!(A, buffer::JacobianBuffer, y, x) return ForwardDiff.jacobian!(A, buffer.f!, y, x, buffer.config) end From 9c1c4c057a7b8302de89aafaf37c45ed7ed85d2a Mon Sep 17 00:00:00 2001 From: franckgaga Date: Wed, 5 Mar 2025 20:51:37 -0500 Subject: [PATCH 06/14] debug: handle univariate syntax special case `JuMP.operator` expects a different signature for the gradient when the function is univariate. --- src/controller/nonlinmpc.jl | 54 +++++++++++++++++++++++++++---------- 1 file changed, 40 insertions(+), 14 deletions(-) diff --git a/src/controller/nonlinmpc.jl b/src/controller/nonlinmpc.jl index bb120cf82..509630531 100644 --- a/src/controller/nonlinmpc.jl +++ b/src/controller/nonlinmpc.jl @@ -571,6 +571,7 @@ function get_optim_functions(mpc::NonLinMPC, ::JuMP.GenericModel{JNT}) where JNT end # --------------------- cache for the AD functions ----------------------------------- Z̃arg_vec = Vector{JNT}(undef, nZ̃) + ∇J = Vector{JNT}(undef, nZ̃) # gradient of J g_vec = Vector{JNT}(undef, ng) ∇g = Matrix{JNT}(undef, ng, nZ̃) # Jacobian of g geq_vec = Vector{JNT}(undef, neq) @@ -591,10 +592,19 @@ function get_optim_functions(mpc::NonLinMPC, ::JuMP.GenericModel{JNT}) where JNT U0, Ŷ0 = get_tmp(U0_cache, T), get_tmp(Ŷ0_cache, T) return obj_nonlinprog!(Ŷ0, U0, mpc, model, Ue, Ŷe, ΔŨ) end - ∇J_buffer = GradientBuffer(Jfunc_vec, Z̃arg_vec) - function ∇Jfunc!(∇J, Z̃arg::Vararg{T, N}) where {N, T<:Real} - Z̃arg_vec .= Z̃arg - return gradient!(∇J, ∇J_buffer, Z̃arg_vec) + ∇J_buffer = GradientBuffer(Jfunc_vec, Z̃arg_vec) + ∇Jfunc! = if nZ̃ == 1 + function (Z̃arg::T) where T<:Real + Z̃arg_vec .= Z̃arg + gradient!(∇J, ∇J_buffer, Z̃arg_vec) + 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̃arg_vec .= Z̃arg + gradient!(∇J, ∇J_buffer, Z̃arg_vec) + return ∇J # multivariate syntax, see JuMP.@operator doc + end end # --------------------- inequality constraint functions ------------------------------- gfuncs = Vector{Function}(undef, ng) @@ -614,11 +624,19 @@ function get_optim_functions(mpc::NonLinMPC, ::JuMP.GenericModel{JNT}) where JNT ∇g_buffer = JacobianBuffer(gfunc_vec!, g_vec, Z̃arg_vec) ∇gfuncs! = Vector{Function}(undef, ng) for i in eachindex(∇gfuncs!) - ∇gfuncs![i] = function (∇g_i, Z̃arg::Vararg{T, N}) where {N, T<:Real} - Z̃arg_vec .= Z̃arg - jacobian!(∇g, ∇g_buffer, g_vec, Z̃arg_vec) - ∇g_i .= @views ∇g[i, :] - return ∇g_i + ∇gfuncs![i] = if nZ̃ == 1 + function (Z̃arg::T) where T<:Real + Z̃arg_vec .= Z̃arg + jacobian!(∇g, ∇g_buffer, g_vec, Z̃arg_vec) + return ∇g[i, begin] # univariate syntax, see JuMP.@operator doc + end + else + function (∇g_i, Z̃arg::Vararg{T, N}) where {N, T<:Real} + Z̃arg_vec .= Z̃arg + jacobian!(∇g, ∇g_buffer, g_vec, Z̃arg_vec) + ∇g_i .= @views ∇g[i, :] + return ∇g_i # multivariate syntax, see JuMP.@operator doc + end end end # --------------------- equality constraint functions --------------------------------- @@ -639,11 +657,19 @@ function get_optim_functions(mpc::NonLinMPC, ::JuMP.GenericModel{JNT}) where JNT ∇geq_buffer = JacobianBuffer(geqfunc_vec!, geq_vec, Z̃arg_vec) ∇geqfuncs! = Vector{Function}(undef, neq) for i in eachindex(∇geqfuncs!) - ∇geqfuncs![i] = function (∇geq_i, Z̃arg::Vararg{T, N}) where {N, T<:Real} - Z̃arg_vec .= Z̃arg - jacobian!(∇geq, ∇geq_buffer, geq_vec, Z̃arg_vec) - ∇geq_i .= @views ∇geq[i, :] - return ∇geq_i + ∇geqfuncs![i] = if nZ̃ == 1 + function (Z̃arg::T) where T<:Real + Z̃arg_vec .= Z̃arg + jacobian!(∇geq, ∇geq_buffer, geq_vec, Z̃arg_vec) + return ∇geq[i, begin] # univariate syntax, see JuMP.@operator doc + end + else + function (∇geq_i, Z̃arg::Vararg{T, N}) where {N, T<:Real} + Z̃arg_vec .= Z̃arg + jacobian!(∇geq, ∇geq_buffer, geq_vec, Z̃arg_vec) + ∇geq_i .= @views ∇geq[i, :] + return ∇geq_i # multivariate syntax, see JuMP.@operator doc + end end end return Jfunc, ∇Jfunc!, gfuncs, ∇gfuncs!, geqfuncs, ∇geqfuncs! From 4154ee88be30f7f29f1409ee6eaaae883128b5e1 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Thu, 6 Mar 2025 09:10:54 -0500 Subject: [PATCH 07/14] changed: remove the impossible univariate syntax for equality constraints --- src/controller/nonlinmpc.jl | 41 +++++++++++++++++++++++-------------- 1 file changed, 26 insertions(+), 15 deletions(-) diff --git a/src/controller/nonlinmpc.jl b/src/controller/nonlinmpc.jl index 509630531..828f821ca 100644 --- a/src/controller/nonlinmpc.jl +++ b/src/controller/nonlinmpc.jl @@ -514,10 +514,27 @@ function init_optimization!(mpc::NonLinMPC, model::SimModel, optim) end """ - get_optim_functions(mpc::NonLinMPC, ::JuMP.GenericModel) -> Jfunc, ∇Jfunc!, gfuncs, geqfuncs + get_optim_functions( + mpc::NonLinMPC, ::JuMP.GenericModel + ) -> Jfunc, ∇Jfunc!, gfuncs, ∇gfuncs!, geqfuncs, ∇geqfuncs! -Get the objective `Jfunc` function and `∇Jfunc!` to compute its gradient, and constraint -`gfuncs` and `geqfuncs` function vectors for [`NonLinMPC`](@ref). +Return the functions for the nonlinear optimization of `mpc` [`NonLinMPC`](@ref) controller. + +Return the nonlinear objective `Jfunc` function, and `∇Jfunc!`, to compute its gradient. +Also return vectors with the nonlinear inequality constraint functions `gfuncs`, and +`∇gfuncs!`, for the associated gradients. Lastly, also return vectors with the nonlinear +equality constraint functions `geqfuncs` and gradients `∇geqfuncs!`. + +This method is really indicated 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. +- The `JuMP` NLP syntax forces splatting for the decision variable, which implies use + of `Vararg{T,N}` (see the [performance tip](https://docs.julialang.org/en/v1/manual/performance-tips/#Be-aware-of-when-Julia-avoids-specializing)) + and memoization to avoid redundant computations. This is already complex, + but it's even worse knowing that AD tools for gradients 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](https://jump.dev/JuMP.jl/stable/tutorials/nonlinear/tips_and_tricks/#User-defined-operators-with-vector-outputs) """ @@ -577,7 +594,6 @@ function get_optim_functions(mpc::NonLinMPC, ::JuMP.GenericModel{JNT}) where JNT geq_vec = Vector{JNT}(undef, neq) ∇geq = Matrix{JNT}(undef, neq, nZ̃) # Jacobian of geq # --------------------- objective function -------------------------------------------- - # force specialization with Vararg, see https://docs.julialang.org/en/v1/manual/performance-tips/#Be-aware-of-when-Julia-avoids-specializing function Jfunc(Z̃arg::Vararg{T, N}) where {N, T<:Real} update_simulations!(Z̃arg, get_tmp(Z̃_cache, T)) ΔŨ = get_tmp(ΔŨ_cache, T) @@ -609,7 +625,7 @@ function get_optim_functions(mpc::NonLinMPC, ::JuMP.GenericModel{JNT}) where JNT # --------------------- inequality constraint functions ------------------------------- gfuncs = Vector{Function}(undef, ng) for i in eachindex(gfuncs) - func_i = function (Z̃arg::Vararg{T, N}) where {N, T<:Real} # see comment above + func_i = function (Z̃arg::Vararg{T, N}) where {N, T<:Real} update_simulations!(Z̃arg, get_tmp(Z̃_cache, T)) g = get_tmp(g_cache, T) return g[i]::T @@ -642,7 +658,7 @@ function get_optim_functions(mpc::NonLinMPC, ::JuMP.GenericModel{JNT}) where JNT # --------------------- equality constraint functions --------------------------------- geqfuncs = Vector{Function}(undef, neq) for i in eachindex(geqfuncs) - func_i = function (Z̃arg::Vararg{T, N}) where {N, T<:Real} # see comment above + func_i = function (Z̃arg::Vararg{T, N}) where {N, T<:Real} update_simulations!(Z̃arg, get_tmp(Z̃_cache, T)) geq = get_tmp(geq_cache, T) return geq[i]::T @@ -657,20 +673,15 @@ function get_optim_functions(mpc::NonLinMPC, ::JuMP.GenericModel{JNT}) where JNT ∇geq_buffer = JacobianBuffer(geqfunc_vec!, geq_vec, Z̃arg_vec) ∇geqfuncs! = Vector{Function}(undef, neq) for i in eachindex(∇geqfuncs!) - ∇geqfuncs![i] = if nZ̃ == 1 - function (Z̃arg::T) where T<:Real - Z̃arg_vec .= Z̃arg - jacobian!(∇geq, ∇geq_buffer, geq_vec, Z̃arg_vec) - return ∇geq[i, begin] # univariate syntax, see JuMP.@operator doc - end - else + # only multivariate syntax, univariate is impossible since nonlinear equality + # constraints imply MultipleShooting thus input AND state in Z̃: + ∇geqfuncs![i] = function (∇geq_i, Z̃arg::Vararg{T, N}) where {N, T<:Real} Z̃arg_vec .= Z̃arg jacobian!(∇geq, ∇geq_buffer, geq_vec, Z̃arg_vec) ∇geq_i .= @views ∇geq[i, :] - return ∇geq_i # multivariate syntax, see JuMP.@operator doc + return ∇geq_i end - end end return Jfunc, ∇Jfunc!, gfuncs, ∇gfuncs!, geqfuncs, ∇geqfuncs! end From 8ae06949702e27ba70e8b22ea7daf4bc74c61948 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Thu, 6 Mar 2025 11:08:29 -0500 Subject: [PATCH 08/14] test: improve coverage of `NonLinMPC` with `MultipleShooting` --- src/controller/nonlinmpc.jl | 17 +++++++++-------- test/3_test_predictive_control.jl | 4 +++- 2 files changed, 12 insertions(+), 9 deletions(-) diff --git a/src/controller/nonlinmpc.jl b/src/controller/nonlinmpc.jl index 828f821ca..abeb37ce9 100644 --- a/src/controller/nonlinmpc.jl +++ b/src/controller/nonlinmpc.jl @@ -531,8 +531,8 @@ This method is really indicated and I'm not proud of it. That's because of 3 ele and as efficient as possible. - The `JuMP` NLP syntax forces splatting for the decision variable, which implies use of `Vararg{T,N}` (see the [performance tip](https://docs.julialang.org/en/v1/manual/performance-tips/#Be-aware-of-when-Julia-avoids-specializing)) - and memoization to avoid redundant computations. This is already complex, - but it's even worse knowing that AD tools for gradients do not support splatting. + 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. @@ -546,6 +546,7 @@ function get_optim_functions(mpc::NonLinMPC, ::JuMP.GenericModel{JNT}) where JNT nΔŨ, nUe, nŶe = nu*Hc + nϵ, nU + nu, nŶ + ny Ncache = nZ̃ + 3 myNaN = convert(JNT, NaN) # fill Z̃ with NaNs to force update_simulations! at 1st call: + # ---------------------- differentiation cache --------------------------------------- Z̃_cache::DiffCache{Vector{JNT}, Vector{JNT}} = DiffCache(fill(myNaN, nZ̃), Ncache) ΔŨ_cache::DiffCache{Vector{JNT}, Vector{JNT}} = DiffCache(zeros(JNT, nΔŨ), Ncache) x̂0end_cache::DiffCache{Vector{JNT}, Vector{JNT}} = DiffCache(zeros(JNT, nx̂), Ncache) @@ -586,14 +587,14 @@ function get_optim_functions(mpc::NonLinMPC, ::JuMP.GenericModel{JNT}) where JNT end return nothing end - # --------------------- cache for the AD functions ----------------------------------- + # --------------------- normal cache for the AD functions ---------------------------- Z̃arg_vec = Vector{JNT}(undef, nZ̃) - ∇J = Vector{JNT}(undef, nZ̃) # gradient of J + ∇J = Vector{JNT}(undef, nZ̃) # gradient of objective J g_vec = Vector{JNT}(undef, ng) - ∇g = Matrix{JNT}(undef, ng, nZ̃) # Jacobian of g + ∇g = Matrix{JNT}(undef, ng, nZ̃) # Jacobian of inequality constraints g geq_vec = Vector{JNT}(undef, neq) - ∇geq = Matrix{JNT}(undef, neq, nZ̃) # Jacobian of geq - # --------------------- objective function -------------------------------------------- + ∇geq = Matrix{JNT}(undef, neq, nZ̃) # Jacobian of equality constraints geq + # --------------------- objective functions ------------------------------------------- function Jfunc(Z̃arg::Vararg{T, N}) where {N, T<:Real} update_simulations!(Z̃arg, get_tmp(Z̃_cache, T)) ΔŨ = get_tmp(ΔŨ_cache, T) @@ -674,7 +675,7 @@ function get_optim_functions(mpc::NonLinMPC, ::JuMP.GenericModel{JNT}) where JNT ∇geqfuncs! = Vector{Function}(undef, neq) for i in eachindex(∇geqfuncs!) # only multivariate syntax, univariate is impossible since nonlinear equality - # constraints imply MultipleShooting thus input AND state in Z̃: + # 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} Z̃arg_vec .= Z̃arg diff --git a/test/3_test_predictive_control.jl b/test/3_test_predictive_control.jl index 38983a835..1c265b57d 100644 --- a/test/3_test_predictive_control.jl +++ b/test/3_test_predictive_control.jl @@ -602,10 +602,12 @@ end LHS = zeros(1) nmpc15.con.gc!(LHS,[1,2],[3,4],[4,6],10,0.1) @test LHS ≈ [10*dot([1,2],[3,4])+sum([4,6])+0.1] - nmpc16 = NonLinMPC(nonlinmodel, Hp=10, transcription=MultipleShooting()) + gc! = (LHS,_,_,_,_,_)-> (LHS .= 0.0) # useless, only for coverage + nmpc16 = NonLinMPC(nonlinmodel, Hp=10, transcription=MultipleShooting(), nc=10, gc=gc!) @test nmpc16.transcription == MultipleShooting() @test length(nmpc16.Z̃) == nonlinmodel.nu*nmpc16.Hc + nmpc16.estim.nx̂*nmpc16.Hp + nmpc16.nϵ @test nmpc16.con.neq == nmpc16.estim.nx̂*nmpc16.Hp + @test nmpc16.con.nc == 10 nmpc17 = NonLinMPC(linmodel1, Hp=10, transcription=MultipleShooting()) @test nmpc17.transcription == MultipleShooting() @test length(nmpc17.Z̃) == linmodel1.nu*nmpc17.Hc + nmpc17.estim.nx̂*nmpc17.Hp + nmpc17.nϵ From 12269ee181714b1be68d7758c14d74c355e7ad3b Mon Sep 17 00:00:00 2001 From: franckgaga Date: Thu, 6 Mar 2025 12:26:16 -0500 Subject: [PATCH 09/14] added: huge performance improvement by caching Jacobians of the constraints MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit The splatting syntax of `JuMP` forces use to compute the Jacobians of the inequality and equality constraints as a multiple gradients (concatenated in the Jacobian matrices). This means that `jacobian!` function of the AD tools were called redundantly `ng` and `neq` times, for a specific decision vector value `Z̃`. This is wasteful. A caching mechanism was implemented to store the Jacobians of the constraints and reuse them when needed. The performance improvement is about 5-10x faster now on `NonLinMPC` with `NonLinModel`. --- src/controller/nonlinmpc.jl | 63 ++++++++++++++++++---------------- src/estimator/mhe/construct.jl | 1 + src/general.jl | 3 ++ 3 files changed, 38 insertions(+), 29 deletions(-) diff --git a/src/controller/nonlinmpc.jl b/src/controller/nonlinmpc.jl index abeb37ce9..8e0e4bdef 100644 --- a/src/controller/nonlinmpc.jl +++ b/src/controller/nonlinmpc.jl @@ -563,7 +563,7 @@ function get_optim_functions(mpc::NonLinMPC, ::JuMP.GenericModel{JNT}) where JNT function update_simulations!( Z̃arg::Union{NTuple{N, T}, AbstractVector{T}}, Z̃cache ) where {N, T<:Real} - if any(cache !== arg for (cache, arg) in zip(Z̃cache, Z̃arg)) # new Z̃, update: + if isdifferent(Z̃cache, Z̃arg) # new Z̃, update: for i in eachindex(Z̃cache) # Z̃cache .= Z̃arg is type unstable with Z̃arg::NTuple{N, FowardDiff.Dual} Z̃cache[i] = Z̃arg[i] @@ -587,13 +587,6 @@ function get_optim_functions(mpc::NonLinMPC, ::JuMP.GenericModel{JNT}) where JNT end return nothing end - # --------------------- normal cache for the AD functions ---------------------------- - Z̃arg_vec = Vector{JNT}(undef, nZ̃) - ∇J = Vector{JNT}(undef, nZ̃) # gradient of objective J - g_vec = Vector{JNT}(undef, ng) - ∇g = Matrix{JNT}(undef, ng, nZ̃) # Jacobian of inequality constraints g - geq_vec = Vector{JNT}(undef, neq) - ∇geq = Matrix{JNT}(undef, neq, nZ̃) # Jacobian of equality constraints geq # --------------------- objective functions ------------------------------------------- function Jfunc(Z̃arg::Vararg{T, N}) where {N, T<:Real} update_simulations!(Z̃arg, get_tmp(Z̃_cache, T)) @@ -608,18 +601,20 @@ function get_optim_functions(mpc::NonLinMPC, ::JuMP.GenericModel{JNT}) where JNT Ue, Ŷe = get_tmp(Ue_cache, T), get_tmp(Ŷe_cache, T) U0, Ŷ0 = get_tmp(U0_cache, T), get_tmp(Ŷ0_cache, T) return obj_nonlinprog!(Ŷ0, U0, mpc, model, Ue, Ŷe, ΔŨ) - end - ∇J_buffer = GradientBuffer(Jfunc_vec, Z̃arg_vec) + end + Z̃_∇J = fill(myNaN, nZ̃) + ∇J = Vector{JNT}(undef, nZ̃) # gradient of objective J + ∇J_buffer = GradientBuffer(Jfunc_vec, Z̃_∇J) ∇Jfunc! = if nZ̃ == 1 function (Z̃arg::T) where T<:Real - Z̃arg_vec .= Z̃arg - gradient!(∇J, ∇J_buffer, Z̃arg_vec) + Z̃_∇J .= Z̃arg + gradient!(∇J, ∇J_buffer, Z̃_∇J) 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̃arg_vec .= Z̃arg - gradient!(∇J, ∇J_buffer, Z̃arg_vec) + Z̃_∇J .= Z̃arg + gradient!(∇J, ∇J_buffer, Z̃_∇J) return ∇J # multivariate syntax, see JuMP.@operator doc end end @@ -638,21 +633,27 @@ function get_optim_functions(mpc::NonLinMPC, ::JuMP.GenericModel{JNT}) where JNT g .= get_tmp(g_cache, T) return g end - ∇g_buffer = JacobianBuffer(gfunc_vec!, g_vec, Z̃arg_vec) - ∇gfuncs! = Vector{Function}(undef, ng) + Z̃_∇g = fill(myNaN, nZ̃) + g_vec = Vector{JNT}(undef, ng) + ∇g = Matrix{JNT}(undef, ng, nZ̃) # Jacobian of inequality constraints g + ∇g_buffer = JacobianBuffer(gfunc_vec!, g_vec, Z̃_∇g) + ∇gfuncs! = Vector{Function}(undef, ng) for i in eachindex(∇gfuncs!) ∇gfuncs![i] = if nZ̃ == 1 function (Z̃arg::T) where T<:Real - Z̃arg_vec .= Z̃arg - jacobian!(∇g, ∇g_buffer, g_vec, Z̃arg_vec) - return ∇g[i, begin] # univariate syntax, see JuMP.@operator doc + if isdifferent(Z̃arg, Z̃_∇g) + Z̃_∇g .= Z̃arg + jacobian!(∇g, ∇g_buffer, g_vec, Z̃_∇g) + end + return ∇g[i, begin] # univariate syntax, see JuMP.@operator doc end else function (∇g_i, Z̃arg::Vararg{T, N}) where {N, T<:Real} - Z̃arg_vec .= Z̃arg - jacobian!(∇g, ∇g_buffer, g_vec, Z̃arg_vec) - ∇g_i .= @views ∇g[i, :] - return ∇g_i # multivariate syntax, see JuMP.@operator doc + if isdifferent(Z̃arg, Z̃_∇g) + Z̃_∇g .= Z̃arg + jacobian!(∇g, ∇g_buffer, g_vec, Z̃_∇g) + end + return ∇g_i .= @views ∇g[i, :] # multivariate syntax, see JuMP.@operator doc end end end @@ -671,17 +672,21 @@ function get_optim_functions(mpc::NonLinMPC, ::JuMP.GenericModel{JNT}) where JNT geq .= get_tmp(geq_cache, T) return geq end - ∇geq_buffer = JacobianBuffer(geqfunc_vec!, geq_vec, Z̃arg_vec) - ∇geqfuncs! = Vector{Function}(undef, neq) + Z̃_∇geq = fill(myNaN, nZ̃) # NaN to force update at 1st call + geq_vec = Vector{JNT}(undef, neq) + ∇geq = Matrix{JNT}(undef, neq, nZ̃) # Jacobian of equality constraints geq + ∇geq_buffer = JacobianBuffer(geqfunc_vec!, geq_vec, Z̃_∇geq) + ∇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} - Z̃arg_vec .= Z̃arg - jacobian!(∇geq, ∇geq_buffer, geq_vec, Z̃arg_vec) - ∇geq_i .= @views ∇geq[i, :] - return ∇geq_i + if isdifferent(Z̃arg, Z̃_∇geq) + Z̃_∇geq .= Z̃arg + jacobian!(∇geq, ∇geq_buffer, geq_vec, Z̃_∇geq) + end + return ∇geq_i .= @views ∇geq[i, :] end end return Jfunc, ∇Jfunc!, gfuncs, ∇gfuncs!, geqfuncs, ∇geqfuncs! diff --git a/src/estimator/mhe/construct.jl b/src/estimator/mhe/construct.jl index 2ebd8ff1c..4b7a68cde 100644 --- a/src/estimator/mhe/construct.jl +++ b/src/estimator/mhe/construct.jl @@ -1315,6 +1315,7 @@ function get_optim_functions( x̄_cache::DiffCache{Vector{JNT}, Vector{JNT}} = DiffCache(zeros(JNT, nx̂), Nc) û0_cache::DiffCache{Vector{JNT}, Vector{JNT}} = DiffCache(zeros(JNT, nu), Nc) ŷ0_cache::DiffCache{Vector{JNT}, Vector{JNT}} = DiffCache(zeros(JNT, nŷ), Nc) + # --------------------- update simulation function ------------------------------------ function update_simulations!(Z̃, Z̃tup::NTuple{N, T}) where {N, T <:Real} if any(new !== old for (new, old) in zip(Z̃tup, Z̃)) # new Z̃tup, update predictions: Z̃1 = Z̃tup[begin] diff --git a/src/general.jl b/src/general.jl index 76eb86298..700c3f1c1 100644 --- a/src/general.jl +++ b/src/general.jl @@ -91,6 +91,9 @@ function limit_solve_time(optim::GenericModel, Ts) end end +"Verify that x and y elements are different using `!==`." +isdifferent(x, y) = any(xi !== yi for (xi, yi) in zip(x, y)) + "Generate a block diagonal matrix repeating `n` times the matrix `A`." repeatdiag(A, n::Int) = kron(I(n), A) From f43ac3ce8585c38d478d64c8e99158d4a9de0c89 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Thu, 6 Mar 2025 15:41:35 -0500 Subject: [PATCH 10/14] added: same improvements in `MovingHorizonEstimator` with `NonLinModel` --- src/controller/nonlinmpc.jl | 6 +- src/estimator/mhe/construct.jl | 133 ++++++++++++++++++++++++--------- 2 files changed, 102 insertions(+), 37 deletions(-) diff --git a/src/controller/nonlinmpc.jl b/src/controller/nonlinmpc.jl index 8e0e4bdef..0c7cee9b9 100644 --- a/src/controller/nonlinmpc.jl +++ b/src/controller/nonlinmpc.jl @@ -515,7 +515,7 @@ end """ get_optim_functions( - mpc::NonLinMPC, ::JuMP.GenericModel + mpc::NonLinMPC, optim::JuMP.GenericModel ) -> Jfunc, ∇Jfunc!, gfuncs, ∇gfuncs!, geqfuncs, ∇geqfuncs! Return the functions for the nonlinear optimization of `mpc` [`NonLinMPC`](@ref) controller. @@ -563,7 +563,7 @@ function get_optim_functions(mpc::NonLinMPC, ::JuMP.GenericModel{JNT}) where JNT function update_simulations!( Z̃arg::Union{NTuple{N, T}, AbstractVector{T}}, Z̃cache ) where {N, T<:Real} - if isdifferent(Z̃cache, Z̃arg) # new Z̃, update: + if isdifferent(Z̃cache, Z̃arg) for i in eachindex(Z̃cache) # Z̃cache .= Z̃arg is type unstable with Z̃arg::NTuple{N, FowardDiff.Dual} Z̃cache[i] = Z̃arg[i] @@ -600,7 +600,7 @@ function get_optim_functions(mpc::NonLinMPC, ::JuMP.GenericModel{JNT}) where JNT ΔŨ = get_tmp(ΔŨ_cache, T) Ue, Ŷe = get_tmp(Ue_cache, T), get_tmp(Ŷe_cache, T) U0, Ŷ0 = get_tmp(U0_cache, T), get_tmp(Ŷ0_cache, T) - return obj_nonlinprog!(Ŷ0, U0, mpc, model, Ue, Ŷe, ΔŨ) + return obj_nonlinprog!(Ŷ0, U0, mpc, model, Ue, Ŷe, ΔŨ)::T end Z̃_∇J = fill(myNaN, nZ̃) ∇J = Vector{JNT}(undef, nZ̃) # gradient of objective J diff --git a/src/estimator/mhe/construct.jl b/src/estimator/mhe/construct.jl index 4b7a68cde..418e624de 100644 --- a/src/estimator/mhe/construct.jl +++ b/src/estimator/mhe/construct.jl @@ -1255,7 +1255,7 @@ function init_optimization!( JuMP.set_attribute(optim, "nlp_scaling_max_gradient", 10.0/C) end end - Jfunc, gfuncs = get_optim_functions(estim, optim) + Jfunc, ∇Jfunc!, gfuncs, ∇gfuncs! = get_optim_functions(estim, optim) @operator(optim, J, nZ̃, Jfunc) @objective(optim, Min, J(Z̃var...)) nV̂, nX̂ = estim.He*estim.nym, estim.He*estim.nx̂ @@ -1293,10 +1293,26 @@ end """ - get_optim_functions(estim::MovingHorizonEstimator, ::JuMP.GenericModel) -> Jfunc, gfuncs + get_optim_functions( + estim::MovingHorizonEstimator, optim::JuMP.GenericModel + ) -> Jfunc, ∇Jfunc!, gfuncs, ∇gfuncs! -Get the objective `Jfunc` function and constraint `gfuncs` function vector for -[`MovingHorizonEstimator`](@ref). +Return the functions for the nonlinear optimization of [`MovingHorizonEstimator`](@ref). + +Return the nonlinear objective `Jfunc` function, and `∇Jfunc!`, to compute its gradient. +Also return vectors with the nonlinear inequality constraint functions `gfuncs`, and +`∇gfuncs!`, for the associated gradients. + +This method is really indicated 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. +- The `JuMP` NLP syntax forces splatting for the decision variable, which implies use + of `Vararg{T,N}` (see the [performance tip](https://docs.julialang.org/en/v1/manual/performance-tips/#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](https://jump.dev/JuMP.jl/stable/tutorials/nonlinear/tips_and_tricks/#User-defined-operators-with-vector-outputs) """ @@ -1306,51 +1322,100 @@ function get_optim_functions( model, con = estim.model, estim.con nx̂, nym, nŷ, 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 + Ncache = nZ̃ + 3 myNaN = convert(JNT, NaN) # fill Z̃ with NaNs to force update_simulations! at 1st call: - Z̃_cache::DiffCache{Vector{JNT}, Vector{JNT}} = DiffCache(fill(myNaN, nZ̃), Nc) - V̂_cache::DiffCache{Vector{JNT}, Vector{JNT}} = DiffCache(zeros(JNT, nV̂), Nc) - g_cache::DiffCache{Vector{JNT}, Vector{JNT}} = DiffCache(zeros(JNT, ng), Nc) - X̂0_cache::DiffCache{Vector{JNT}, Vector{JNT}} = DiffCache(zeros(JNT, nX̂), Nc) - x̄_cache::DiffCache{Vector{JNT}, Vector{JNT}} = DiffCache(zeros(JNT, nx̂), Nc) - û0_cache::DiffCache{Vector{JNT}, Vector{JNT}} = DiffCache(zeros(JNT, nu), Nc) - ŷ0_cache::DiffCache{Vector{JNT}, Vector{JNT}} = DiffCache(zeros(JNT, nŷ), Nc) + # ---------------------- differentiation cache --------------------------------------- + Z̃_cache::DiffCache{Vector{JNT}, Vector{JNT}} = DiffCache(fill(myNaN, nZ̃), Ncache) + V̂_cache::DiffCache{Vector{JNT}, Vector{JNT}} = DiffCache(zeros(JNT, nV̂), Ncache) + g_cache::DiffCache{Vector{JNT}, Vector{JNT}} = DiffCache(zeros(JNT, ng), Ncache) + X̂0_cache::DiffCache{Vector{JNT}, Vector{JNT}} = DiffCache(zeros(JNT, nX̂), Ncache) + x̄_cache::DiffCache{Vector{JNT}, Vector{JNT}} = DiffCache(zeros(JNT, nx̂), Ncache) + û0_cache::DiffCache{Vector{JNT}, Vector{JNT}} = DiffCache(zeros(JNT, nu), Ncache) + ŷ0_cache::DiffCache{Vector{JNT}, Vector{JNT}} = DiffCache(zeros(JNT, nŷ), Ncache) # --------------------- update simulation function ------------------------------------ - function update_simulations!(Z̃, Z̃tup::NTuple{N, T}) where {N, T <:Real} - if any(new !== old for (new, old) in zip(Z̃tup, Z̃)) # new Z̃tup, update predictions: - Z̃1 = Z̃tup[begin] - for i in eachindex(Z̃tup) - Z̃[i] = Z̃tup[i] # Z̃ .= Z̃tup seems to produce a type instability + function update_simulations!( + Z̃arg::Union{NTuple{N, T}, AbstractVector{T}}, Z̃cache + ) where {N, T <:Real} + if isdifferent(Z̃cache, Z̃arg) + for i in eachindex(Z̃cache) + # Z̃cache .= Z̃arg is type unstable with Z̃arg::NTuple{N, FowardDiff.Dual} + Z̃cache[i] = Z̃arg[i] end + Z̃ = Z̃cache + ϵ = (nϵ ≠ 0) ? Z̃[begin] : zero(T) # ϵ = 0 if Cwt=Inf (meaning: no relaxation) V̂, X̂0 = get_tmp(V̂_cache, Z̃1), get_tmp(X̂0_cache, Z̃1) û0, ŷ0 = get_tmp(û0_cache, Z̃1), get_tmp(ŷ0_cache, Z̃1) g = get_tmp(g_cache, Z̃1) V̂, X̂0 = predict!(V̂, X̂0, û0, ŷ0, estim, model, 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 nothing end - function Jfunc(Z̃tup::Vararg{T, N}) where {N, T<:Real} - Z̃1 = Z̃tup[begin] - Z̃ = get_tmp(Z̃_cache, Z̃1) - update_simulations!(Z̃, Z̃tup) - x̄, V̂ = get_tmp(x̄_cache, Z̃1), get_tmp(V̂_cache, Z̃1) + # --------------------- objective functions ------------------------------------------- + function Jfunc(Z̃arg::Vararg{T, N}) where {N, T<:Real} + update_simulations!(Z̃arg, get_tmp(Z̃_cache, T)) + x̄, V̂ = get_tmp(x̄_cache, T), get_tmp(V̂_cache, T) return obj_nonlinprog!(x̄, estim, model, V̂, Z̃)::T end - function gfunc_i(i, Z̃tup::NTuple{N, T})::T where {N, T <:Real} - Z̃1 = Z̃tup[begin] - Z̃ = get_tmp(Z̃_cache, Z̃1) - update_simulations!(Z̃, Z̃tup) - g = get_tmp(g_cache, Z̃1) - return g[i] + function Jfunc_vec(Z̃arg::AbstractVector{T}) where T<:Real + update_simulations!(Z̃arg, get_tmp(Z̃_cache, T)) + x̄, V̂ = get_tmp(x̄_cache, T), get_tmp(V̂_cache, T) + return obj_nonlinprog!(x̄, estim, model, V̂, Z̃)::T end + Z̃_∇J = fill(myNaN, nZ̃) + ∇J = Vector{JNT}(undef, nZ̃) # gradient of objective J + ∇J_buffer = GradientBuffer(Jfunc_vec, Z̃_∇J) + ∇Jfunc! = if nZ̃ == 1 + function (Z̃arg::T) where T<:Real + Z̃_∇J .= Z̃arg + gradient!(∇J, ∇J_buffer, Z̃_∇J) + 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!(∇J, ∇J_buffer, Z̃_∇J) + return ∇J # multivariate syntax, see JuMP.@operator doc + end + end + # --------------------- inequality constraint functions ------------------------------- gfuncs = Vector{Function}(undef, ng) - for i in 1:ng - # this is another syntax for anonymous function, allowing parameters T and N: - gfuncs[i] = function (ΔŨtup::Vararg{T, N}) where {N, T<:Real} - return gfunc_i(i, ΔŨtup) + for i in eachindex(gfuncs) + func_i = function (Z̃arg::Vararg{T, N}) where {N, T<:Real} + update_simulations!(Z̃arg, get_tmp(Z̃_cache, T)) + g = get_tmp(g_cache, T) + return g[i]::T + end + gfuncs[i] = func_i + end + function gfunc_vec!(g, Z̃vec::AbstractVector{T}) where T<:Real + update_simulations!(Z̃vec, get_tmp(Z̃_cache, T)) + g .= get_tmp(g_cache, T) + return g + end + Z̃_∇g = fill(myNaN, nZ̃) + g_vec = Vector{JNT}(undef, ng) + ∇g = Matrix{JNT}(undef, ng, nZ̃) # Jacobian of inequality constraints g + ∇g_buffer = JacobianBuffer(gfunc_vec!, g_vec, Z̃_∇g) + ∇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!(∇g, ∇g_buffer, g_vec, Z̃_∇g) + end + 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!(∇g, ∇g_buffer, g_vec, Z̃_∇g) + end + return ∇g_i .= @views ∇g[i, :] # multivariate syntax, see JuMP.@operator doc + end end end - return Jfunc, gfuncs + return Jfunc, ∇Jfunc!, gfuncs, ∇gfuncs! end \ No newline at end of file From d658a68bccb0fc109cea4c7da6f1a593e6e8306c Mon Sep 17 00:00:00 2001 From: franckgaga Date: Thu, 6 Mar 2025 16:22:34 -0500 Subject: [PATCH 11/14] debug: MHE now works --- Project.toml | 2 +- src/estimator/mhe/construct.jl | 12 +++++++----- 2 files changed, 8 insertions(+), 6 deletions(-) diff --git a/Project.toml b/Project.toml index e278c898b..3aeb327cc 100644 --- a/Project.toml +++ b/Project.toml @@ -1,7 +1,7 @@ name = "ModelPredictiveControl" uuid = "61f9bdb8-6ae4-484a-811f-bbf86720c31c" authors = ["Francis Gagnon"] -version = "1.4.1" +version = "1.4.2" [deps] LinearAlgebra = "37e2e46d-f89d-539d-b4ee-838fcccc9c8e" diff --git a/src/estimator/mhe/construct.jl b/src/estimator/mhe/construct.jl index 418e624de..e2e624959 100644 --- a/src/estimator/mhe/construct.jl +++ b/src/estimator/mhe/construct.jl @@ -1343,9 +1343,9 @@ function get_optim_functions( end Z̃ = Z̃cache ϵ = (nϵ ≠ 0) ? Z̃[begin] : zero(T) # ϵ = 0 if Cwt=Inf (meaning: no relaxation) - V̂, X̂0 = get_tmp(V̂_cache, Z̃1), get_tmp(X̂0_cache, Z̃1) - û0, ŷ0 = get_tmp(û0_cache, Z̃1), get_tmp(ŷ0_cache, Z̃1) - g = get_tmp(g_cache, Z̃1) + V̂, X̂0 = get_tmp(V̂_cache, T), get_tmp(X̂0_cache, T) + û0, ŷ0 = get_tmp(û0_cache, T), get_tmp(ŷ0_cache, T) + g = get_tmp(g_cache, T) V̂, X̂0 = predict!(V̂, X̂0, û0, ŷ0, estim, model, Z̃) g = con_nonlinprog!(g, estim, model, X̂0, V̂, ϵ) end @@ -1353,12 +1353,14 @@ function get_optim_functions( end # --------------------- objective functions ------------------------------------------- function Jfunc(Z̃arg::Vararg{T, N}) where {N, T<:Real} - update_simulations!(Z̃arg, get_tmp(Z̃_cache, T)) + Z̃ = get_tmp(Z̃_cache, T) + update_simulations!(Z̃arg, Z̃) x̄, V̂ = get_tmp(x̄_cache, T), get_tmp(V̂_cache, T) return obj_nonlinprog!(x̄, estim, model, V̂, Z̃)::T end function Jfunc_vec(Z̃arg::AbstractVector{T}) where T<:Real - update_simulations!(Z̃arg, get_tmp(Z̃_cache, T)) + Z̃ = get_tmp(Z̃_cache, T) + update_simulations!(Z̃arg, Z̃) x̄, V̂ = get_tmp(x̄_cache, T), get_tmp(V̂_cache, T) return obj_nonlinprog!(x̄, estim, model, V̂, Z̃)::T end From 149cb159462605fa976b22d2612a7e0cc71f897d Mon Sep 17 00:00:00 2001 From: franckgaga Date: Thu, 6 Mar 2025 16:27:43 -0500 Subject: [PATCH 12/14] debug: actually use the new AD functions in MHE --- src/estimator/mhe/construct.jl | 17 +++++++++-------- 1 file changed, 9 insertions(+), 8 deletions(-) diff --git a/src/estimator/mhe/construct.jl b/src/estimator/mhe/construct.jl index e2e624959..5a71900b2 100644 --- a/src/estimator/mhe/construct.jl +++ b/src/estimator/mhe/construct.jl @@ -1256,35 +1256,36 @@ function init_optimization!( end end Jfunc, ∇Jfunc!, gfuncs, ∇gfuncs! = get_optim_functions(estim, optim) - @operator(optim, J, nZ̃, Jfunc) + @operator(optim, J, nZ̃, Jfunc, ∇Jfunc!) @objective(optim, Min, J(Z̃var...)) nV̂, nX̂ = estim.He*estim.nym, estim.He*estim.nx̂ if length(con.i_g) ≠ 0 + i_base = 0 for i in eachindex(con.X̂0min) name = Symbol("g_X̂0min_$i") optim[name] = JuMP.add_nonlinear_operator( - optim, nZ̃, gfuncs[i]; name + optim, nZ̃, gfuncs[i_base + i], ∇gfuncs![i_base + i]; name ) end - i_end_X̂min = nX̂ + i_base = nX̂ for i in eachindex(con.X̂0max) name = Symbol("g_X̂0max_$i") optim[name] = JuMP.add_nonlinear_operator( - optim, nZ̃, gfuncs[i_end_X̂min + i]; name + optim, nZ̃, gfuncs[i_base + i], ∇gfuncs![i_base + i]; name ) end - i_end_X̂max = 2*nX̂ + i_base = 2*nX̂ for i in eachindex(con.V̂min) name = Symbol("g_V̂min_$i") optim[name] = JuMP.add_nonlinear_operator( - optim, nZ̃, gfuncs[i_end_X̂max + i]; name + optim, nZ̃, gfuncs[i_base + i], ∇gfuncs![i_base + i]; name ) end - i_end_V̂min = 2*nX̂ + nV̂ + i_base = 2*nX̂ + nV̂ for i in eachindex(con.V̂max) name = Symbol("g_V̂max_$i") optim[name] = JuMP.add_nonlinear_operator( - optim, nZ̃, gfuncs[i_end_V̂min + i]; name + optim, nZ̃, gfuncs[i_base + i], ∇gfuncs![i_base + i]; name ) end end From 007fd1751c1c7f24b5f99b6dddbaef0517268a95 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Thu, 6 Mar 2025 16:34:36 -0500 Subject: [PATCH 13/14] debug: call `set_nonlincon!` at the end of MHE construction --- src/estimator/mhe/construct.jl | 61 +++++++++++++++++----------------- 1 file changed, 31 insertions(+), 30 deletions(-) diff --git a/src/estimator/mhe/construct.jl b/src/estimator/mhe/construct.jl index 5a71900b2..b59279f2b 100644 --- a/src/estimator/mhe/construct.jl +++ b/src/estimator/mhe/construct.jl @@ -718,36 +718,6 @@ function init_matconstraint_mhe(::SimModel{NT}, return i_b, i_g, A end -"By default, no nonlinear constraints in the MHE, thus return nothing." -set_nonlincon!(::MovingHorizonEstimator, ::SimModel, ::JuMP.GenericModel) = nothing - -"Set the nonlinear constraints on the output predictions `Ŷ` and terminal states `x̂end`." -function set_nonlincon!( - estim::MovingHorizonEstimator, ::NonLinModel, optim::JuMP.GenericModel{JNT} -) where JNT<:Real - optim, con = estim.optim, estim.con - Z̃var = optim[:Z̃var] - nonlin_constraints = JuMP.all_constraints(optim, JuMP.NonlinearExpr, MOI.LessThan{JNT}) - map(con_ref -> JuMP.delete(optim, con_ref), nonlin_constraints) - for i in findall(.!isinf.(con.X̂0min)) - gfunc_i = optim[Symbol("g_X̂0min_$(i)")] - @constraint(optim, gfunc_i(Z̃var...) <= 0) - end - for i in findall(.!isinf.(con.X̂0max)) - gfunc_i = optim[Symbol("g_X̂0max_$(i)")] - @constraint(optim, gfunc_i(Z̃var...) <= 0) - end - for i in findall(.!isinf.(con.V̂min)) - gfunc_i = optim[Symbol("g_V̂min_$(i)")] - JuMP.@constraint(optim, gfunc_i(Z̃var...) <= 0) - end - for i in findall(.!isinf.(con.V̂max)) - gfunc_i = optim[Symbol("g_V̂max_$(i)")] - JuMP.@constraint(optim, gfunc_i(Z̃var...) <= 0) - end - return nothing -end - """ init_defaultcon_mhe( model::SimModel, He, C, nx̂, nym, E, ex̄, Ex̂, Fx̂, Gx̂, Jx̂, Bx̂ @@ -1289,6 +1259,7 @@ function init_optimization!( ) end end + set_nonlincon!(estim, model, optim) return nothing end @@ -1421,4 +1392,34 @@ function get_optim_functions( end end return Jfunc, ∇Jfunc!, gfuncs, ∇gfuncs! +end + +"By default, no nonlinear constraints in the MHE, thus return nothing." +set_nonlincon!(::MovingHorizonEstimator, ::SimModel, ::JuMP.GenericModel) = nothing + +"Set the nonlinear constraints on the output predictions `Ŷ` and terminal states `x̂end`." +function set_nonlincon!( + estim::MovingHorizonEstimator, ::NonLinModel, optim::JuMP.GenericModel{JNT} +) where JNT<:Real + optim, con = estim.optim, estim.con + Z̃var = optim[:Z̃var] + nonlin_constraints = JuMP.all_constraints(optim, JuMP.NonlinearExpr, MOI.LessThan{JNT}) + map(con_ref -> JuMP.delete(optim, con_ref), nonlin_constraints) + for i in findall(.!isinf.(con.X̂0min)) + gfunc_i = optim[Symbol("g_X̂0min_$(i)")] + @constraint(optim, gfunc_i(Z̃var...) <= 0) + end + for i in findall(.!isinf.(con.X̂0max)) + gfunc_i = optim[Symbol("g_X̂0max_$(i)")] + @constraint(optim, gfunc_i(Z̃var...) <= 0) + end + for i in findall(.!isinf.(con.V̂min)) + gfunc_i = optim[Symbol("g_V̂min_$(i)")] + JuMP.@constraint(optim, gfunc_i(Z̃var...) <= 0) + end + for i in findall(.!isinf.(con.V̂max)) + gfunc_i = optim[Symbol("g_V̂max_$(i)")] + JuMP.@constraint(optim, gfunc_i(Z̃var...) <= 0) + end + return nothing end \ No newline at end of file From fc00d65ae712c1a100e92712bbc46807c0aa6d89 Mon Sep 17 00:00:00 2001 From: franckgaga Date: Thu, 6 Mar 2025 17:23:23 -0500 Subject: [PATCH 14/14] test: new `MovingHorizonEstimator` test to improve coverage of the univariate case --- test/2_test_state_estim.jl | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/test/2_test_state_estim.jl b/test/2_test_state_estim.jl index 3d83166fd..fa2eb408c 100644 --- a/test/2_test_state_estim.jl +++ b/test/2_test_state_estim.jl @@ -990,6 +990,11 @@ end info = getinfo(mhe5) @test info[:x̂] ≈ x̂ atol=1e-9 @test info[:Ŷ][end-1:end] ≈ [50, 30] atol=1e-9 + # for coverage of NLP functions, the univariate syntax of JuMP.@operator + mhe6 = MovingHorizonEstimator(nonlinmodel, He=1, Cwt=Inf) + setconstraint!(mhe6, v̂min=[-51,-52], v̂max=[53,54]) + x̂ = preparestate!(mhe6, [50, 30], [5]) + @test x̂ ≈ zeros(6) atol=1e-9 @test_nowarn ModelPredictiveControl.info2debugstr(info) end