diff --git a/Project.toml b/Project.toml index c36402207..f95eea853 100644 --- a/Project.toml +++ b/Project.toml @@ -1,7 +1,7 @@ name = "ModelPredictiveControl" uuid = "61f9bdb8-6ae4-484a-811f-bbf86720c31c" -authors = ["Francis Gagnon"] version = "1.11.1" +authors = ["Francis Gagnon"] [deps] ControlSystemsBase = "aaaaaaaa-a6ca-5380-bf3e-84a91bcd477e" @@ -11,6 +11,7 @@ Ipopt = "b6b21f68-93f8-5de0-b562-5493be1d77c9" JuMP = "4076af6c-e467-56ae-b986-b466b2749572" LinearAlgebra = "37e2e46d-f89d-539d-b4ee-838fcccc9c8e" Logging = "56ddb016-857b-54e1-b83d-db4d58db5568" +MathOptInterface = "b8f27783-ece8-5eb3-8dc8-9495eed66fee" OSQP = "ab2f91bb-94b4-55e3-9ba0-7f65df51de79" PrecompileTools = "aea7be01-6a6a-4083-8856-8a6e6704d82a" ProgressLogging = "33c8b6b6-d38a-422a-b730-caa89a2f386c" @@ -31,6 +32,7 @@ Ipopt = "1" JuMP = "1.21" LinearAlgebra = "1.10" Logging = "1.10" +MathOptInterface = "1.46" OSQP = "0.8" Plots = "1" PrecompileTools = "1" diff --git a/docs/make.jl b/docs/make.jl index 426dee53f..1107afc69 100644 --- a/docs/make.jl +++ b/docs/make.jl @@ -10,6 +10,7 @@ links = InterLinks( "Julia" => "https://docs.julialang.org/en/v1/objects.inv", "ControlSystemsBase" => "https://juliacontrol.github.io/ControlSystems.jl/stable/objects.inv", "JuMP" => "https://jump.dev/JuMP.jl/stable/objects.inv", + "MathOptInterface" => "https://jump.dev/MathOptInterface.jl/stable/objects.inv", "DifferentiationInterface" => "https://juliadiff.org/DifferentiationInterface.jl/DifferentiationInterface/stable/objects.inv", "ForwardDiff" => "https://juliadiff.org/ForwardDiff.jl/stable/objects.inv", "LowLevelParticleFilters" => "https://baggepinnen.github.io/LowLevelParticleFilters.jl/stable/objects.inv", diff --git a/docs/src/internals/predictive_control.md b/docs/src/internals/predictive_control.md index 2a6480a86..994cd6628 100644 --- a/docs/src/internals/predictive_control.md +++ b/docs/src/internals/predictive_control.md @@ -24,8 +24,7 @@ ModelPredictiveControl.relaxterminal ModelPredictiveControl.init_quadprog ModelPredictiveControl.init_stochpred ModelPredictiveControl.init_matconstraint_mpc -ModelPredictiveControl.init_nonlincon! -ModelPredictiveControl.get_optim_functions(::NonLinMPC, ::ModelPredictiveControl.GenericModel) +ModelPredictiveControl.get_nonlinops(::NonLinMPC, ::ModelPredictiveControl.GenericModel) ``` ## Update Quadratic Optimization diff --git a/docs/src/internals/state_estim.md b/docs/src/internals/state_estim.md index fe0ebd6ad..caba31010 100644 --- a/docs/src/internals/state_estim.md +++ b/docs/src/internals/state_estim.md @@ -18,7 +18,7 @@ ModelPredictiveControl.relaxX̂ ModelPredictiveControl.relaxŴ ModelPredictiveControl.relaxV̂ ModelPredictiveControl.init_matconstraint_mhe -ModelPredictiveControl.get_optim_functions(::MovingHorizonEstimator, ::ModelPredictiveControl.GenericModel) +ModelPredictiveControl.get_nonlinops(::MovingHorizonEstimator, ::ModelPredictiveControl.GenericModel) ``` ## Augmented Model diff --git a/src/controller/construct.jl b/src/controller/construct.jl index b01bc6b94..b4ee851ee 100644 --- a/src/controller/construct.jl +++ b/src/controller/construct.jl @@ -439,12 +439,8 @@ function setconstraint!( JuMP.delete(optim, optim[:linconstraint]) JuMP.unregister(optim, :linconstraint) @constraint(optim, linconstraint, A*Z̃var .≤ b) - if JuMP.solver_name(optim) ≠ "Ipopt" - set_nonlincon!(mpc, model, transcription, optim) - else - g_oracle, geq_oracle = get_nonlinops(mpc, optim) - set_nonlincon_exp!(mpc, g_oracle, geq_oracle) - end + g_oracle, geq_oracle = get_nonlinops(mpc, optim) + set_nonlincon!(mpc, optim, g_oracle, geq_oracle) else i_b, i_g = init_matconstraint_mpc( model, transcription, nc, @@ -458,11 +454,11 @@ function setconstraint!( return mpc end -"By default, no nonlinear operators, return 4 nothing" -get_nonlinops(::PredictiveController, _ ) = (nothing, nothing, nothing, nothing) +"By default, no nonlinear operators, return 3 nothing" +get_nonlinops(::PredictiveController, _ ) = (nothing, nothing, nothing) "By default, no nonlinear constraints, return nothing." -set_nonlincon_exp!(::PredictiveController, _ , _ ) = nothing +set_nonlincon!(::PredictiveController, _ , _ , _ ) = nothing """ default_Hp(model::LinModel) diff --git a/src/controller/nonlinmpc.jl b/src/controller/nonlinmpc.jl index 3b66d373b..b1f60f5fa 100644 --- a/src/controller/nonlinmpc.jl +++ b/src/controller/nonlinmpc.jl @@ -540,196 +540,35 @@ function init_optimization!( JuMP.set_attribute(optim, "nlp_scaling_max_gradient", 10.0/C) end end - if JuMP.solver_name(optim) ≠ "Ipopt" - # everything with the splatting syntax: - J_func, ∇J_func!, g_funcs, ∇g_funcs!, geq_funcs, ∇geq_funcs! = get_optim_functions( - mpc, optim - ) - else - # constraints with vector nonlinear oracle, objective function with splatting: - g_oracle, geq_oracle, J_func, ∇J_func! = get_nonlinops(mpc, optim) - end - @operator(optim, J, nZ̃, J_func, ∇J_func!) - @objective(optim, Min, J(Z̃var...)) - if JuMP.solver_name(optim) ≠ "Ipopt" - init_nonlincon!(mpc, model, transcription, g_funcs, ∇g_funcs!, geq_funcs, ∇geq_funcs!) - set_nonlincon!(mpc, model, transcription, optim) - else - set_nonlincon_exp!(mpc, g_oracle, geq_oracle) - end + # constraints with vector nonlinear oracle, objective function with splatting: + g_oracle, geq_oracle, J_op = get_nonlinops(mpc, optim) + optim[:J_op] = J_op + @objective(optim, Min, J_op(Z̃var...)) + set_nonlincon!(mpc, optim, g_oracle, geq_oracle) return nothing end """ - get_optim_functions( - mpc::NonLinMPC, optim::JuMP.GenericModel - ) -> J_func, ∇J_func!, g_funcs, ∇g_funcs!, geq_funcs, ∇geq_funcs! + get_nonlinops(mpc::NonLinMPC, optim) -> g_oracle, geq_oracle, J_op -Return the functions for the nonlinear optimization of `mpc` [`NonLinMPC`](@ref) controller. +Return the operators for the nonlinear optimization of `mpc` [`NonLinMPC`](@ref) controller. -Return the nonlinear objective `J_func` function, and `∇J_func!`, to compute its gradient. -Also return vectors with the nonlinear inequality constraint functions `g_funcs`, and -`∇g_funcs!`, for the associated gradients. Lastly, also return vectors with the nonlinear -equality constraint functions `geq_funcs` and gradients `∇geq_funcs!`. - -This method is really intricate and I'm not proud of it. That's because of 3 elements: +Return `g_oracle` and `geq_oracle`, the inequality and equality [`VectorNonlinearOracle`](@extref MathOptInterface MathOptInterface.VectorNonlinearOracle) +for the two respective constraints. Note that `g_oracle` only includes the non-`Inf` +inequality constraints, thus it must be re-constructed if they change. Also return `J_op`, +the [`NonlinearOperator`](@extref JuMP NonlinearOperator) for the objective function, based +on the splatting syntax. This method is really intricate and 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. 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)) +- The splatting syntax for objective functions implies the 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. + worse knowing that the 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(mpc::NonLinMPC, ::JuMP.GenericModel{JNT}) where JNT<:Real - # ----------- common cache for Jfunc, gfuncs and geqfuncs ---------------------------- - model = mpc.estim.model - transcription = mpc.transcription - grad, jac = mpc.gradient, mpc.jacobian - nu, ny, nx̂, nϵ = model.nu, model.ny, mpc.estim.nx̂, mpc.nϵ - nk = get_nk(model, transcription) - Hp, Hc = mpc.Hp, mpc.Hc - ng, nc, neq = length(mpc.con.i_g), mpc.con.nc, mpc.con.neq - 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) - J::Vector{JNT} = zeros(JNT, 1) - ΔŨ::Vector{JNT} = zeros(JNT, nΔŨ) - x̂0end::Vector{JNT} = zeros(JNT, nx̂) - K0::Vector{JNT} = zeros(JNT, nK) - Ue::Vector{JNT}, Ŷe::Vector{JNT} = zeros(JNT, nUe), zeros(JNT, nŶe) - U0::Vector{JNT}, Ŷ0::Vector{JNT} = zeros(JNT, nU), zeros(JNT, nŶ) - Û0::Vector{JNT}, X̂0::Vector{JNT} = zeros(JNT, nU), zeros(JNT, nX̂) - 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_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!, grad, Z̃_∇J, ∇J_context...; strict) - ∇J = Vector{JNT}(undef, nZ̃) - function update_objective!(J, ∇J, Z̃_∇J, Z̃arg) - if isdifferent(Z̃arg, Z̃_∇J) - Z̃_∇J .= Z̃arg - J[], _ = value_and_gradient!(Jfunc!, ∇J, ∇J_prep, grad, Z̃_∇J, ∇J_context...) - end - end - function J_func(Z̃arg::Vararg{T, N}) where {N, T<:Real} - update_objective!(J, ∇J, Z̃_∇J, Z̃arg) - return J[]::T - end - ∇J_func! = if nZ̃ == 1 # univariate syntax (see JuMP.@operator doc): - function (Z̃arg) - update_objective!(J, ∇J, Z̃_∇J, Z̃arg) - return ∇J[begin] - end - 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 - end - end - # --------------------- 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 g - end - 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(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, jac, Z̃_∇g, ∇g_context...; strict) - mpc.con.i_g[1:end-nc] .= false - ∇g = init_diffmat(JNT, jac, ∇g_prep, nZ̃, ng) - function update_con!(g, ∇g, Z̃_∇g, Z̃arg) - if isdifferent(Z̃arg, Z̃_∇g) - Z̃_∇g .= Z̃arg - value_and_jacobian!(gfunc!, g, ∇g, ∇g_prep, jac, Z̃_∇g, ∇g_context...) - end - end - g_funcs = Vector{Function}(undef, ng) - for i in eachindex(g_funcs) - 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 - g_funcs[i] = gfunc_i - end - ∇g_funcs! = Vector{Function}(undef, ng) - for i in eachindex(∇g_funcs!) - ∇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] - end - 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, :] - end - end - ∇g_funcs![i] = ∇gfuncs_i! - end - # --------------------- equality constraint functions --------------------------------- - function geqfunc!(geq, Z̃, ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, K0, X̂0, gc, g) - update_predictions!(ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, K0, X̂0, gc, g, geq, mpc, Z̃) - return geq - end - 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, jac, Z̃_∇geq, ∇geq_context...; strict) - ∇geq = init_diffmat(JNT, jac, ∇geq_prep, nZ̃, neq) - function update_con_eq!(geq, ∇geq, Z̃_∇geq, Z̃arg) - if isdifferent(Z̃arg, Z̃_∇geq) - Z̃_∇geq .= Z̃arg - value_and_jacobian!(geqfunc!, geq, ∇geq, ∇geq_prep, jac, Z̃_∇geq, ∇geq_context...) - end - end - geq_funcs = Vector{Function}(undef, neq) - for i in eachindex(geq_funcs) - 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 - geq_funcs[i] = geqfunc_i - end - ∇geq_funcs! = Vector{Function}(undef, neq) - for i in eachindex(∇geq_funcs!) - # 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} - update_con_eq!(geq, ∇geq, Z̃_∇geq, Z̃arg) - return ∇geq_i .= @views ∇geq[i, :] - end - ∇geq_funcs![i] = ∇geqfuncs_i! - end - return J_func, ∇J_func!, g_funcs, ∇g_funcs!, geq_funcs, ∇geq_funcs! -end - -# TODO: move docstring of method above here an re-work it -function get_nonlinops(mpc::NonLinMPC, ::JuMP.GenericModel{JNT}) where JNT<:Real +function get_nonlinops(mpc::NonLinMPC, optim::JuMP.GenericModel{JNT}) where JNT<:Real # ----------- common cache for all functions ---------------------------------------- model = mpc.estim.model transcription = mpc.transcription @@ -785,7 +624,7 @@ function get_nonlinops(mpc::NonLinMPC, ::JuMP.GenericModel{JNT}) where JNT<:Real gi_min = fill(-myInf, ngi) gi_max = zeros(JNT, ngi) ∇gi_structure = init_diffstructure(∇gi) - g_oracle = Ipopt._VectorNonlinearOracle(; + g_oracle = MOI.VectorNonlinearOracle(; dimension = nZ̃, l = gi_min, u = gi_max, @@ -823,7 +662,7 @@ function get_nonlinops(mpc::NonLinMPC, ::JuMP.GenericModel{JNT}) where JNT<:Real end geq_min = geq_max = zeros(JNT, neq) ∇geq_structure = init_diffstructure(∇geq) - geq_oracle = Ipopt._VectorNonlinearOracle(; + geq_oracle = MOI.VectorNonlinearOracle(; dimension = nZ̃, l = geq_min, u = geq_max, @@ -865,10 +704,10 @@ function get_nonlinops(mpc::NonLinMPC, ::JuMP.GenericModel{JNT}) where JNT<:Real return ∇J_arg .= ∇J end end - return g_oracle, geq_oracle, J_func, ∇J_func! + J_op = JuMP.add_nonlinear_operator(optim, nZ̃, J_func, ∇J_func!, name=:J_op) + return g_oracle, geq_oracle, J_op end - """ update_predictions!( ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, K0, X̂0, gc, g, geq, @@ -895,19 +734,20 @@ function update_predictions!( end """ - set_nonlincon_exp!(mpc::NonLinMPC, g_oracle, geq_oracle) + set_nonlincon!(mpc::NonLinMPC, optim, g_oracle, geq_oracle) Set the nonlinear inequality and equality constraints for `NonLinMPC`, if any. """ -function set_nonlincon_exp!( - mpc::NonLinMPC, g_oracle, geq_oracle -) - optim = mpc.optim +function set_nonlincon!( + mpc::NonLinMPC, optim::JuMP.GenericModel{JNT}, g_oracle, geq_oracle +) where JNT<:Real Z̃var = optim[:Z̃var] nonlin_constraints = JuMP.all_constraints( - optim, JuMP.Vector{JuMP.VariableRef}, Ipopt._VectorNonlinearOracle + optim, JuMP.Vector{JuMP.VariableRef}, MOI.VectorNonlinearOracle{JNT} ) map(con_ref -> JuMP.delete(optim, con_ref), nonlin_constraints) + optim[:g_oracle] = g_oracle + optim[:geq_oracle] = geq_oracle any(mpc.con.i_g) && @constraint(optim, Z̃var in g_oracle) mpc.con.neq > 0 && @constraint(optim, Z̃var in geq_oracle) return nothing diff --git a/src/controller/transcription.jl b/src/controller/transcription.jl index a63f41afa..8593c61a0 100644 --- a/src/controller/transcription.jl +++ b/src/controller/transcription.jl @@ -697,242 +697,6 @@ function init_matconstraint_mpc( return i_b, i_g, A, Aeq, neq 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], ∇gfuncs![i_base+i]; name - ) - end - 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 - -""" - init_nonlincon!( - mpc::PredictiveController, model::NonLinModel, transcription::TranscriptionMethod, - gfuncs, ∇gfuncs!, - geqfuncs, ∇geqfuncs! - ) - -Init nonlinear constraints for [`NonLinModel`](@ref) and other [`TranscriptionMethod`](@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, ::TranscriptionMethod, - 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 - - -"By default, there is no nonlinear constraint, thus do nothing." -function set_nonlincon!( - ::PredictiveController, ::SimModel, ::TranscriptionMethod, ::JuMP.GenericModel, - ) - return nothing -end - -""" - set_nonlincon!(mpc::PredictiveController, ::LinModel, ::TranscriptionMethod, optim) - -Set the custom nonlinear inequality constraints for `LinModel`. -""" -function set_nonlincon!( - mpc::PredictiveController, ::LinModel, ::TranscriptionMethod, ::JuMP.GenericModel{JNT} -) where JNT<:Real - optim = mpc.optim - Z̃var = optim[:Z̃var] - con = mpc.con - nonlin_constraints = JuMP.all_constraints(optim, JuMP.NonlinearExpr, MOI.LessThan{JNT}) - map(con_ref -> JuMP.delete(optim, con_ref), nonlin_constraints) - for i in 1:con.nc - gfunc_i = optim[Symbol("g_c_$i")] - @constraint(optim, gfunc_i(Z̃var...) <= 0) - end - return nothing -end - -""" - set_nonlincon!(mpc::PredictiveController, ::NonLinModel, ::MultipleShooting, optim) - -Also set output prediction `Ŷ` constraints for `NonLinModel` and non-`SingleShooting`. -""" -function set_nonlincon!( - mpc::PredictiveController, ::NonLinModel, ::TranscriptionMethod, ::JuMP.GenericModel{JNT} -) where JNT<:Real - optim = mpc.optim - Z̃var = optim[:Z̃var] - con = mpc.con - 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.Y0min)) - gfunc_i = optim[Symbol("g_Y0min_$(i)")] - @constraint(optim, gfunc_i(Z̃var...) <= 0) - end - for i in findall(.!isinf.(con.Y0max)) - gfunc_i = optim[Symbol("g_Y0max_$(i)")] - @constraint(optim, gfunc_i(Z̃var...) <= 0) - end - for i in 1:con.nc - gfunc_i = optim[Symbol("g_c_$i")] - @constraint(optim, gfunc_i(Z̃var...) <= 0) - end - return nothing -end - -""" - set_nonlincon!(mpc::PredictiveController, ::NonLinModel, ::SingleShooting, optim) - -Also set output prediction `Ŷ` and terminal state `x̂end` constraint for `SingleShooting`. -""" -function set_nonlincon!( - mpc::PredictiveController, ::NonLinModel, ::SingleShooting, ::JuMP.GenericModel{JNT} -) where JNT<:Real - optim = mpc.optim - Z̃var = optim[:Z̃var] - con = mpc.con - 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.Y0min)) - gfunc_i = optim[Symbol("g_Y0min_$(i)")] - @constraint(optim, gfunc_i(Z̃var...) <= 0) - end - for i in findall(.!isinf.(con.Y0max)) - gfunc_i = optim[Symbol("g_Y0max_$(i)")] - @constraint(optim, gfunc_i(Z̃var...) <= 0) - end - 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 1:con.nc - gfunc_i = optim[Symbol("g_c_$i")] - @constraint(optim, gfunc_i(Z̃var...) <= 0) - end - return nothing -end - @doc raw""" linconstraint!(mpc::PredictiveController, model::LinModel) diff --git a/src/estimator/mhe/construct.jl b/src/estimator/mhe/construct.jl index e343d73d4..526b2ffd0 100644 --- a/src/estimator/mhe/construct.jl +++ b/src/estimator/mhe/construct.jl @@ -706,12 +706,8 @@ function setconstraint!( JuMP.delete(optim, optim[:linconstraint]) JuMP.unregister(optim, :linconstraint) @constraint(optim, linconstraint, A*Z̃var .≤ b) - if JuMP.solver_name(optim) ≠ "Ipopt" - set_nonlincon!(estim, model, optim) - else - g_oracle, = get_nonlinops(estim, optim) - set_nonlincon_exp!(estim, model, g_oracle) - end + g_oracle, = get_nonlinops(estim, optim) + set_nonlincon!(estim, model, optim, g_oracle) else i_b, i_g = init_matconstraint_mhe(model, i_x̃min, i_x̃max, i_X̂min, i_X̂max, i_Ŵmin, i_Ŵmax, i_V̂min, i_V̂max @@ -1283,170 +1279,34 @@ function init_optimization!( JuMP.set_attribute(optim, "nlp_scaling_max_gradient", 10.0/C) end end - if JuMP.solver_name(optim) ≠ "Ipopt" - # everything with the splatting syntax: - J_func, ∇J_func!, g_funcs, ∇g_funcs! = get_optim_functions(estim, optim) - else - # constraints with vector nonlinear oracle, objective function with splatting: - g_oracle, J_func, ∇J_func! = get_nonlinops(estim, optim) - end - @operator(optim, J, nZ̃, J_func, ∇J_func!) - @objective(optim, Min, J(Z̃var...)) - nV̂, nX̂ = estim.He*estim.nym, estim.He*estim.nx̂ - if JuMP.solver_name(optim) ≠ "Ipopt" - 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̃, g_funcs[i_base + i], ∇g_funcs![i_base + i]; name - ) - end - i_base = nX̂ - for i in eachindex(con.X̂0max) - name = Symbol("g_X̂0max_$i") - optim[name] = JuMP.add_nonlinear_operator( - optim, nZ̃, g_funcs[i_base + i], ∇g_funcs![i_base + i]; name - ) - end - 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̃, g_funcs[i_base + i], ∇g_funcs![i_base + i]; name - ) - end - 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̃, g_funcs[i_base + i], ∇g_funcs![i_base + i]; name - ) - end - end - set_nonlincon!(estim, model, optim) - else - set_nonlincon_exp!(estim, model, g_oracle) - end + # constraints with vector nonlinear oracle, objective function with splatting: + g_oracle, J_op = get_nonlinops(estim, optim) + optim[:J_op] = J_op + @objective(optim, Min, J_op(Z̃var...)) + set_nonlincon!(estim, model, optim, g_oracle) return nothing end - """ - get_optim_functions( - estim::MovingHorizonEstimator, optim::JuMP.GenericModel, - ) -> Jfunc, ∇Jfunc!, gfuncs, ∇gfuncs! + get_nonlinops(estim::MovingHorizonEstimator, optim) -> g_oracle, J_op -Return the functions for the nonlinear optimization of [`MovingHorizonEstimator`](@ref). +Return the operators 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 intricate and I'm not proud of it. That's because of 3 elements: +Return `g_oracle`, the [`VectorNonlinearOracle`](@extref MathOptInterface MathOptInterface.VectorNonlinearOracle) +for the ineqaulity constraints. Note that `g_oracle` only includes the non-`Inf` +inequality constraints, thus it must be re-constructed if they change. Also return `J_op`, +the [`NonlinearOperator`](@extref JuMP NonlinearOperator) for the objective function, based +on the splatting syntax. This method is really intricate and that's because of 2 elements: - 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!). -- 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)) +- The splatting syntax for objective functions implies the 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. - -Inspired from: [User-defined operators with vector outputs](@extref JuMP User-defined-operators-with-vector-outputs) + worse knowing that the automatic differentiation tools do not support splatting. """ -function get_optim_functions( - estim::MovingHorizonEstimator, ::JuMP.GenericModel{JNT}, -) where {JNT <: Real} - # ----------- 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̃) - strict = Val(true) - 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) - û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̃, 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̃) # NaN to force update_predictions! at first call - ∇J_context = ( - Cache(V̂), Cache(X̂0), Cache(û0), Cache(k0), Cache(ŷ0), - Cache(g), - Cache(x̄), - ) - # temporarily "fill" the estimation window for the preparation of the gradient: - estim.Nk[] = He - ∇J_prep = prepare_gradient(Jfunc!, grad, Z̃_∇J, ∇J_context...; strict) - estim.Nk[] = 0 - ∇J = Vector{JNT}(undef, nZ̃) - function update_objective!(J, ∇J, Z̃_∇J, Z̃arg) - if isdifferent(Z̃arg, Z̃_∇J) - Z̃_∇J .= Z̃arg - J[], _ = value_and_gradient!(Jfunc!, ∇J, ∇J_prep, grad, Z̃_∇J, ∇J_context...) - end - end - function J_func(Z̃arg::Vararg{T, N}) where {N, T<:Real} - update_objective!(J, ∇J, Z̃_∇J, Z̃arg) - return J[]::T - end - ∇J_func! = 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 - update_objective!(J, ∇J, Z̃_∇J, Z̃arg) - return ∇Jarg .= ∇J - end - # --------------------- inequality constraint functions ------------------------------- - 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̃) # 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, jac, Z̃_∇g, ∇g_context...; strict) - estim.con.i_g .= false - estim.Nk[] = 0 - ∇g = init_diffmat(JNT, jac, ∇g_prep, nZ̃, ng) - function update_con!(g, ∇g, Z̃_∇g, Z̃arg) - if isdifferent(Z̃arg, Z̃_∇g) - Z̃_∇g .= Z̃arg - value_and_jacobian!(gfunc!, g, ∇g, ∇g_prep, jac, Z̃_∇g, ∇g_context...) - end - end - g_funcs = Vector{Function}(undef, ng) - for i in eachindex(g_funcs) - 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 - g_funcs[i] = gfunc_i - end - ∇g_funcs! = Vector{Function}(undef, ng) - for i in eachindex(∇g_funcs!) - ∇g_funcs![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 - update_con!(g, ∇g, Z̃_∇g, Z̃arg) - return ∇g_i .= @views ∇g[i, :] - end - end - return J_func, ∇J_func!, g_funcs, ∇g_funcs! -end - -# TODO: move docstring of method above here an re-work it function get_nonlinops( - estim::MovingHorizonEstimator, ::JuMP.GenericModel{JNT} + estim::MovingHorizonEstimator, optim::JuMP.GenericModel{JNT} ) where JNT<:Real # ----------- common cache for Jfunc and gfuncs -------------------------------------- model, con = estim.model, estim.con @@ -1497,7 +1357,7 @@ function get_nonlinops( gi_min = fill(-myInf, ngi) gi_max = zeros(JNT, ngi) ∇gi_structure = init_diffstructure(∇gi) - g_oracle = Ipopt._VectorNonlinearOracle(; + g_oracle = MOI.VectorNonlinearOracle(; dimension = nZ̃, l = gi_min, u = gi_max, @@ -1531,65 +1391,33 @@ function get_nonlinops( update_objective!(J, ∇J, Z̃_∇J, Z̃_arg) return J[]::T end - ∇J_func! = if nZ̃ == 1 # univariate syntax (see JuMP.@operator doc): - function (Z̃_arg) - update_objective!(J, ∇J, Z̃_∇J, Z̃_arg) - return ∇J[] - end - else # multivariate syntax (see JuMP.@operator doc): - function (∇J_arg::AbstractVector{T}, Z̃_arg::Vararg{T, N}) where {N, T<:Real} - update_objective!(J, ∇J, Z̃_∇J, Z̃_arg) - return ∇J_arg .= ∇J - end - end - g_oracle, J_func, ∇J_func! -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) + function ∇J_func!(∇J_arg::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 + update_objective!(J, ∇J, Z̃_∇J, Z̃_arg) + return ∇J_arg .= ∇J end - return nothing + J_op = JuMP.add_nonlinear_operator(optim, nZ̃, J_func, ∇J_func!, name=:J_op) + g_oracle, J_op end "By default, there is no nonlinear constraint, thus do nothing." -set_nonlincon_exp!(::MovingHorizonEstimator, ::SimModel, _ ) = nothing +set_nonlincon!(::MovingHorizonEstimator, ::SimModel, _ , _ ) = nothing """ - set_nonlincon_exp!(estim::MovingHorizonEstimator, ::NonLinModel, g_oracle) + set_nonlincon!(estim::MovingHorizonEstimator, ::NonLinModel, optim, g_oracle) Set the nonlinear inequality constraints for `NonLinModel`, if any. """ -function set_nonlincon_exp!(estim::MovingHorizonEstimator, ::NonLinModel, g_oracle) - optim = estim.optim +function set_nonlincon!( + estim::MovingHorizonEstimator, ::NonLinModel, optim::JuMP.GenericModel{JNT}, g_oracle +) where JNT<:Real Z̃var = optim[:Z̃var] nonlin_constraints = JuMP.all_constraints( - optim, JuMP.Vector{JuMP.VariableRef}, Ipopt._VectorNonlinearOracle + optim, JuMP.Vector{JuMP.VariableRef}, MOI.VectorNonlinearOracle{JNT} ) map(con_ref -> JuMP.delete(optim, con_ref), nonlin_constraints) + optim[:g_oracle] = g_oracle any(estim.con.i_g) && @constraint(optim, Z̃var in g_oracle) return nothing end \ No newline at end of file