diff --git a/Project.toml b/Project.toml index f95eea853..4a369fbba 100644 --- a/Project.toml +++ b/Project.toml @@ -1,6 +1,6 @@ name = "ModelPredictiveControl" uuid = "61f9bdb8-6ae4-484a-811f-bbf86720c31c" -version = "1.11.1" +version = "1.12.0" authors = ["Francis Gagnon"] [deps] diff --git a/src/controller/construct.jl b/src/controller/construct.jl index b4ee851ee..63aed9b0e 100644 --- a/src/controller/construct.jl +++ b/src/controller/construct.jl @@ -439,8 +439,7 @@ function setconstraint!( JuMP.delete(optim, optim[:linconstraint]) JuMP.unregister(optim, :linconstraint) @constraint(optim, linconstraint, A*Z̃var .≤ b) - g_oracle, geq_oracle = get_nonlinops(mpc, optim) - set_nonlincon!(mpc, optim, g_oracle, geq_oracle) + reset_nonlincon!(mpc) else i_b, i_g = init_matconstraint_mpc( model, transcription, nc, @@ -454,11 +453,8 @@ function setconstraint!( return mpc end -"By default, no nonlinear operators, return 3 nothing" -get_nonlinops(::PredictiveController, _ ) = (nothing, nothing, nothing) - "By default, no nonlinear constraints, return nothing." -set_nonlincon!(::PredictiveController, _ , _ , _ ) = nothing +reset_nonlincon!(::PredictiveController) = nothing """ default_Hp(model::LinModel) diff --git a/src/controller/legacy.jl b/src/controller/legacy.jl new file mode 100644 index 000000000..2892154a8 --- /dev/null +++ b/src/controller/legacy.jl @@ -0,0 +1,389 @@ +# TODO: Deprecated constraint splatting syntax (legacy), delete get_optim_functions later. + +""" + get_optim_functions(mpc::NonLinMPC, optim) + +Get the legacy nonlinear optimization functions for MPC (all based on the splatting syntax). + +See [`get_nonlinops`](@ref) for additional details. +""" +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: Deprecated constraint splatting syntax (legacy), delete init_nonlincon_leg! later. + +""" + init_nonlincon_leg!( + 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_leg!( + 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_leg!( + 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_leg!( + 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_leg!( + 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_leg!( + 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 + +# TODO: Deprecated constraint splatting syntax (legacy), delete set_nonlincon_leg! later. + +"By default, there is no nonlinear constraint, thus do nothing." +function set_nonlincon_leg!( + ::PredictiveController, ::SimModel, ::TranscriptionMethod, ::JuMP.GenericModel, + ) + return nothing +end + +""" + set_nonlincon_leg!(mpc::PredictiveController, ::LinModel, ::TranscriptionMethod, optim) + +Set the custom nonlinear inequality constraints for `LinModel`. +""" +function set_nonlincon_leg!( + 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_leg!(mpc::PredictiveController, ::NonLinModel, ::MultipleShooting, optim) + +Also set output prediction `Ŷ` constraints for `NonLinModel` and non-`SingleShooting`. +""" +function set_nonlincon_leg!( + 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_leg!(mpc::PredictiveController, ::NonLinModel, ::SingleShooting, optim) + +Also set output prediction `Ŷ` and terminal state `x̂end` constraint for `SingleShooting`. +""" +function set_nonlincon_leg!( + 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 \ No newline at end of file diff --git a/src/controller/nonlinmpc.jl b/src/controller/nonlinmpc.jl index b1f60f5fa..66473eebb 100644 --- a/src/controller/nonlinmpc.jl +++ b/src/controller/nonlinmpc.jl @@ -28,6 +28,7 @@ struct NonLinMPC{ con::ControllerConstraint{NT, GCfunc} gradient::GB jacobian::JB + oracle::Bool Z̃::Vector{NT} ŷ::Vector{NT} Hp::Int @@ -67,7 +68,7 @@ struct NonLinMPC{ estim::SE, Hp, Hc, nb, weights::CW, JE::JEfunc, gc!::GCfunc, nc, p::PT, transcription::TM, optim::JM, - gradient::GB, jacobian::JB + gradient::GB, jacobian::JB, oracle ) where { NT<:Real, SE<:StateEstimator, @@ -117,7 +118,7 @@ struct NonLinMPC{ buffer = PredictiveControllerBuffer(estim, transcription, Hp, Hc, nϵ) mpc = new{NT, SE, CW, TM, JM, GB, JB, PT, JEfunc, GCfunc}( estim, transcription, optim, con, - gradient, jacobian, + gradient, jacobian, oracle, Z̃, ŷ, Hp, Hc, nϵ, nb, weights, @@ -217,6 +218,8 @@ This controller allocates memory at each time step for the optimization. function, see [`DifferentiationInterface` doc](@extref DifferentiationInterface List). - `jacobian=default_jacobian(transcription)` : an `AbstractADType` backend for the Jacobian of the nonlinear constraints, see `gradient` above for the options (default in Extended Help). +- `oracle=JuMP.solver_name(optim)=="Ipopt"`: use the efficient [`VectorNonlinearOracle`](@extref MathOptInterface MathOptInterface.VectorNonlinearOracle) + for the nonlinear constraints (not supported by most optimizers for now). - additional keyword arguments are passed to [`UnscentedKalmanFilter`](@ref) constructor (or [`SteadyKalmanFilter`](@ref), for [`LinModel`](@ref)). @@ -311,13 +314,14 @@ function NonLinMPC( optim::JuMP.GenericModel = JuMP.Model(DEFAULT_NONLINMPC_OPTIMIZER, add_bridges=false), gradient::AbstractADType = DEFAULT_NONLINMPC_GRADIENT, jacobian::AbstractADType = default_jacobian(transcription), + oracle::Bool = JuMP.solver_name(optim)=="Ipopt", kwargs... ) estim = default_estimator(model; kwargs...) return NonLinMPC( estim; Hp, Hc, Mwt, Nwt, Lwt, Cwt, Ewt, JE, gc, nc, p, M_Hp, N_Hc, L_Hp, - transcription, optim, gradient, jacobian + transcription, optim, gradient, jacobian, oracle ) end @@ -375,6 +379,7 @@ function NonLinMPC( optim::JuMP.GenericModel = JuMP.Model(DEFAULT_NONLINMPC_OPTIMIZER, add_bridges=false), gradient::AbstractADType = DEFAULT_NONLINMPC_GRADIENT, jacobian::AbstractADType = default_jacobian(transcription), + oracle::Bool = JuMP.solver_name(optim)=="Ipopt" ) where { NT<:Real, SE<:StateEstimator{NT} @@ -390,7 +395,8 @@ function NonLinMPC( gc! = get_mutating_gc(NT, gc) weights = ControllerWeights(estim.model, Hp, Hc, M_Hp, N_Hc, L_Hp, Cwt, Ewt) return NonLinMPC{NT}( - estim, Hp, Hc, nb, weights, JE, gc!, nc, p, transcription, optim, gradient, jacobian + estim, Hp, Hc, nb, weights, JE, gc!, nc, p, + transcription, optim, gradient, jacobian, oracle ) end @@ -540,14 +546,41 @@ function init_optimization!( JuMP.set_attribute(optim, "nlp_scaling_max_gradient", 10.0/C) end 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 + if mpc.oracle + g_oracle, geq_oracle, J_op = get_nonlinops(mpc, optim) + optim[:J_op] = J_op + else + J_func, ∇J_func!, g_funcs, ∇g_funcs!, geq_funcs, ∇geq_funcs! = get_optim_functions( + mpc, optim + ) + @operator(optim, J_op, nZ̃, J_func, ∇J_func!) + end @objective(optim, Min, J_op(Z̃var...)) - set_nonlincon!(mpc, optim, g_oracle, geq_oracle) + if mpc.oracle + set_nonlincon!(mpc, optim, g_oracle, geq_oracle) + else + init_nonlincon_leg!( + mpc, model, transcription, g_funcs, ∇g_funcs!, geq_funcs, ∇geq_funcs! + ) + set_nonlincon_leg!(mpc, model, transcription, optim) + end return nothing end +""" + reset_nonlincon!(mpc::NonLinMPC) + +Re-construct nonlinear constraints and add them to `mpc.optim`. +""" +function reset_nonlincon!(mpc::NonLinMPC) + if mpc.oracle + g_oracle, geq_oracle = get_nonlinops(mpc, mpc.optim) + set_nonlincon!(mpc, mpc.optim, g_oracle, geq_oracle) + else + set_nonlincon_leg!(mpc, mpc.estim.model, mpc.transcription, mpc.optim) + end +end + """ get_nonlinops(mpc::NonLinMPC, optim) -> g_oracle, geq_oracle, J_op diff --git a/src/estimator/mhe.jl b/src/estimator/mhe.jl index b03414823..008a490e4 100644 --- a/src/estimator/mhe.jl +++ b/src/estimator/mhe.jl @@ -1,5 +1,6 @@ include("mhe/construct.jl") include("mhe/execute.jl") +include("mhe/legacy.jl") "Print optimizer and other information for `MovingHorizonEstimator`." function print_details(io::IO, estim::MovingHorizonEstimator) diff --git a/src/estimator/mhe/construct.jl b/src/estimator/mhe/construct.jl index 526b2ffd0..e8f63e4b5 100644 --- a/src/estimator/mhe/construct.jl +++ b/src/estimator/mhe/construct.jl @@ -61,6 +61,7 @@ struct MovingHorizonEstimator{ con::EstimatorConstraint{NT} gradient::GB jacobian::JB + oracle::Bool covestim::CE Z̃::Vector{NT} lastu0::Vector{NT} @@ -112,7 +113,7 @@ struct MovingHorizonEstimator{ function MovingHorizonEstimator{NT}( model::SM, He, i_ym, nint_u, nint_ym, cov::KC, Cwt, - optim::JM, gradient::GB, jacobian::JB, covestim::CE; + optim::JM, gradient::GB, jacobian::JB, oracle, covestim::CE; direct=true ) where { NT<:Real, @@ -161,7 +162,7 @@ struct MovingHorizonEstimator{ model, cov, optim, con, - gradient, jacobian, + gradient, jacobian, oracle, covestim, Z̃, lastu0, x̂op, f̂op, x̂0, He, nε, @@ -267,6 +268,8 @@ transcription for now. function when `model` is not a [`LinModel`](@ref), see [`DifferentiationInterface` doc](@extref DifferentiationInterface List). - `jacobian=AutoForwardDiff()` : an `AbstractADType` backend for the Jacobian of the constraints when `model` is not a [`LinModel`](@ref), see `gradient` above for the options. +- `oracle=JuMP.solver_name(optim)=="Ipopt"`: use the efficient [`VectorNonlinearOracle`](@extref MathOptInterface MathOptInterface.VectorNonlinearOracle) + for the nonlinear constraints (not supported by most optimizers for now). - `direct=true`: construct with a direct transmission from ``\mathbf{y^m}`` (a.k.a. current estimator, in opposition to the delayed/predictor form). @@ -386,6 +389,7 @@ function MovingHorizonEstimator( optim::JM = default_optim_mhe(model), gradient::AbstractADType = DEFAULT_NONLINMHE_GRADIENT, jacobian::AbstractADType = DEFAULT_NONLINMHE_JACOBIAN, + oracle::Bool = JuMP.solver_name(optim)=="Ipopt", direct = true, σP_0 = sigmaP_0, σQ = sigmaQ, @@ -401,7 +405,8 @@ function MovingHorizonEstimator( R̂ = Diagonal([σR;].^2) isnothing(He) && throw(ArgumentError("Estimation horizon He must be explicitly specified")) return MovingHorizonEstimator( - model, He, i_ym, nint_u, nint_ym, P̂_0, Q̂, R̂, Cwt; direct, optim, gradient, jacobian + model, He, i_ym, nint_u, nint_ym, P̂_0, Q̂, R̂, Cwt; + direct, optim, gradient, jacobian, oracle ) end @@ -414,6 +419,7 @@ default_optim_mhe(::SimModel) = JuMP.Model(DEFAULT_NONLINMHE_OPTIMIZER, add_brid optim=default_optim_mhe(model), gradient=AutoForwardDiff(), jacobian=AutoForwardDiff(), + oracle=JuMP.solver_name(optim)=="Ipopt", direct=true, covestim=default_covestim_mhe(model, i_ym, nint_u, nint_ym, P̂_0, Q̂, R̂; direct) ) @@ -434,6 +440,7 @@ function MovingHorizonEstimator( optim::JM = default_optim_mhe(model), gradient::AbstractADType = DEFAULT_NONLINMHE_GRADIENT, jacobian::AbstractADType = DEFAULT_NONLINMHE_JACOBIAN, + oracle::Bool = JuMP.solver_name(optim)=="Ipopt", direct = true, covestim::CE = default_covestim_mhe(model, i_ym, nint_u, nint_ym, P̂_0, Q̂, R̂; direct) ) where {NT<:Real, SM<:SimModel{NT}, JM<:JuMP.GenericModel, CE<:StateEstimator{NT}} @@ -443,12 +450,11 @@ function MovingHorizonEstimator( return MovingHorizonEstimator{NT}( model, He, i_ym, nint_u, nint_ym, cov, Cwt, - optim, gradient, jacobian, covestim; + optim, gradient, jacobian, oracle, covestim; direct ) end - function default_covestim_mhe(model::LinModel, i_ym, nint_u, nint_ym, P̂_0, Q̂, R̂; direct) return KalmanFilter(model, i_ym, nint_u, nint_ym, P̂_0, Q̂, R̂; direct) end @@ -706,8 +712,7 @@ function setconstraint!( JuMP.delete(optim, optim[:linconstraint]) JuMP.unregister(optim, :linconstraint) @constraint(optim, linconstraint, A*Z̃var .≤ b) - g_oracle, = get_nonlinops(estim, optim) - set_nonlincon!(estim, model, optim, g_oracle) + reset_nonlincon!(estim, model) 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 @@ -719,6 +724,24 @@ function setconstraint!( return estim end +"By default, no nonlinear constraints, return nothing." +reset_nonlincon!(::MovingHorizonEstimator, ::SimModel) = nothing + +""" + reset_nonlincon!(estim::MovingHorizonEstimator, model::NonLinModel) + +Re-construct nonlinear constraints and add them to `estim.optim`. +""" +function reset_nonlincon!(estim::MovingHorizonEstimator, model::NonLinModel) + optim = estim.optim + if estim.oracle + g_oracle, = get_nonlinops(estim, optim) + set_nonlincon!(estim, model, optim, g_oracle) + else + set_nonlincon_leg!(estim, model, optim) + end +end + @doc raw""" init_matconstraint_mhe(model::LinModel, i_x̃min, i_x̃max, i_X̂min, i_X̂max, i_Ŵmin, i_Ŵmax, i_V̂min, i_V̂max, args... @@ -1280,10 +1303,20 @@ function init_optimization!( end end # constraints with vector nonlinear oracle, objective function with splatting: - g_oracle, J_op = get_nonlinops(estim, optim) - optim[:J_op] = J_op + if estim.oracle + g_oracle, J_op = get_nonlinops(estim, optim) + optim[:J_op] = J_op + else + J_func, ∇J_func!, g_funcs, ∇g_funcs! = get_optim_functions(estim, optim) + @operator(optim, J_op, nZ̃, J_func, ∇J_func!) + end @objective(optim, Min, J_op(Z̃var...)) - set_nonlincon!(estim, model, optim, g_oracle) + if estim.oracle + set_nonlincon!(estim, model, optim, g_oracle) + else + init_nonlincon_leg!(estim, g_funcs, ∇g_funcs!) + set_nonlincon_leg!(estim, model, optim) + end return nothing end diff --git a/src/estimator/mhe/legacy.jl b/src/estimator/mhe/legacy.jl new file mode 100644 index 000000000..31041867c --- /dev/null +++ b/src/estimator/mhe/legacy.jl @@ -0,0 +1,168 @@ +# TODO: Deprecated constraint splatting syntax (legacy), delete get_optim_functions later. + +""" + get_optim_functions(estim::MovingHorizonEstimator, optim) + +Get the legacy nonlinear optimization functions for MHE (all based on the splatting syntax). + +See [`get_nonlinops`](@ref) for additional details. +""" +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: Deprecated constraint splatting syntax (legacy), delete init_nonlincon_leg! later. + +function init_nonlincon_leg!(estim::MovingHorizonEstimator, g_funcs, ∇g_funcs!) + optim, con = estim.optim, estim.con + nV̂, nX̂ = estim.He*estim.nym, estim.He*estim.nx̂ + nZ̃ = length(estim.Z̃) + 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 + return nothing +end + +# TODO: Deprecated constraint splatting syntax (legacy), delete set_nonlincon_leg! later. + +"By default, no nonlinear constraints in the MHE, thus return nothing." +set_nonlincon_leg!(::MovingHorizonEstimator, ::SimModel, ::JuMP.GenericModel) = nothing + +"Set the nonlinear constraints on the output predictions `Ŷ` and terminal states `x̂end`." +function set_nonlincon_leg!( + 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 diff --git a/src/predictive_control.jl b/src/predictive_control.jl index 3579d8402..33ea5e1d5 100644 --- a/src/predictive_control.jl +++ b/src/predictive_control.jl @@ -25,6 +25,7 @@ include("controller/execute.jl") include("controller/explicitmpc.jl") include("controller/linmpc.jl") include("controller/nonlinmpc.jl") +include("controller/legacy.jl") function Base.show(io::IO, mpc::PredictiveController) estim, model = mpc.estim, mpc.estim.model diff --git a/test/2_test_state_estim.jl b/test/2_test_state_estim.jl index d6a85e961..6dcb3bc0e 100644 --- a/test/2_test_state_estim.jl +++ b/test/2_test_state_estim.jl @@ -1190,6 +1190,13 @@ end setconstraint!(mhe3, C_v̂min=0.03(11:18), C_v̂max=0.04(11:18)) @test all((mhe3.con.C_v̂min, mhe3.con.C_v̂max) .≈ (0.03(11:18), 0.04(11:18))) + # TODO: delete these tests when the deprecated legacy splatting syntax will be. + mhe4 = MovingHorizonEstimator(nonlinmodel, He=4, nint_ym=0, Cwt=1e3, oracle=false) + setconstraint!(mhe3, C_x̂min=0.01(1:10), C_x̂max=0.02(1:10)) + @test all((mhe3.con.C_x̂min, mhe3.con.C_x̂max) .≈ (0.01(3:10), 0.02(3:10))) + setconstraint!(mhe3, C_v̂min=0.03(11:18), C_v̂max=0.04(11:18)) + @test all((mhe3.con.C_v̂min, mhe3.con.C_v̂max) .≈ (0.03(11:18), 0.04(11:18))) + @test_throws ArgumentError setconstraint!(mhe2, x̂min=[-1]) @test_throws ArgumentError setconstraint!(mhe2, x̂max=[+1]) @test_throws ArgumentError setconstraint!(mhe2, ŵmin=[-1]) @@ -1325,6 +1332,54 @@ end x̂ = updatestate!(mhe2, [10, 50], [50, 30]) info = getinfo(mhe2) @test info[:V̂] ≈ [-1,-1] atol=5e-2 + + # TODO: delete these tests when the deprecated legacy splatting syntax will be. + mhe3 = MovingHorizonEstimator(nonlinmodel, He=1, nint_ym=0, oracle=false) + + setconstraint!(mhe3, x̂min=[-100,-100], x̂max=[100,100]) + setconstraint!(mhe3, ŵmin=[-100,-100], ŵmax=[100,100]) + setconstraint!(mhe3, v̂min=[-100,-100], v̂max=[100,100]) + + setconstraint!(mhe3, x̂min=[1,1], x̂max=[100,100]) + preparestate!(mhe3, [50, 30]) + x̂ = updatestate!(mhe3, [10, 50], [50, 30]) + @test x̂ ≈ [1, 1] atol=5e-2 + + setconstraint!(mhe3, x̂min=[-100,-100], x̂max=[-1,-1]) + preparestate!(mhe3, [50, 30]) + x̂ = updatestate!(mhe3, [10, 50], [50, 30]) + @test x̂ ≈ [-1, -1] atol=5e-2 + + setconstraint!(mhe3, x̂min=[-100,-100], x̂max=[100,100]) + setconstraint!(mhe3, ŵmin=[-100,-100], ŵmax=[100,100]) + setconstraint!(mhe3, v̂min=[-100,-100], v̂max=[100,100]) + + setconstraint!(mhe3, ŵmin=[1,1], ŵmax=[100,100]) + preparestate!(mhe3, [50, 30]) + x̂ = updatestate!(mhe3, [10, 50], [50, 30]) + @test mhe3.Ŵ ≈ [1,1] atol=5e-2 + + setconstraint!(mhe3, ŵmin=[-100,-100], ŵmax=[-1,-1]) + preparestate!(mhe3, [50, 30]) + x̂ = updatestate!(mhe3, [10, 50], [50, 30]) + @test mhe3.Ŵ ≈ [-1,-1] atol=5e-2 + + setconstraint!(mhe3, x̂min=[-100,-100], x̂max=[100,100]) + setconstraint!(mhe3, ŵmin=[-100,-100], ŵmax=[100,100]) + setconstraint!(mhe3, v̂min=[-100,-100], v̂max=[100,100]) + + setconstraint!(mhe3, v̂min=[1,1], v̂max=[100,100]) + preparestate!(mhe3, [50, 30]) + x̂ = updatestate!(mhe3, [10, 50], [50, 30]) + info = getinfo(mhe3) + @test info[:V̂] ≈ [1,1] atol=5e-2 + + setconstraint!(mhe3, v̂min=[-100,-100], v̂max=[-1,-1]) + preparestate!(mhe3, [50, 30]) + x̂ = updatestate!(mhe3, [10, 50], [50, 30]) + info = getinfo(mhe3) + @test info[:V̂] ≈ [-1,-1] atol=5e-2 + end @testitem "MovingHorizonEstimator set model" setup=[SetupMPCtests] begin diff --git a/test/3_test_predictive_control.jl b/test/3_test_predictive_control.jl index 29840bc3e..a19a41745 100644 --- a/test/3_test_predictive_control.jl +++ b/test/3_test_predictive_control.jl @@ -1033,6 +1033,37 @@ end ) @test all((nmpc.con.c_x̂min, nmpc.con.c_x̂max) .≈ ([0.21,0.22,0.23,0.24,0.25,0.26], [0.31,0.32,0.33,0.34,0.35,0.36])) + + # TODO: delete these tests when the deprecated legacy splatting syntax will be. + nmpc_lin_leg = NonLinMPC(linmodel1, Hp=1, Hc=1, oracle=false) + nmpc_ms_leg = NonLinMPC(nonlinmodel, Hp=1, Hc=1, oracle=false, transcription=MultipleShooting()) + nmpc_leg = NonLinMPC(nonlinmodel, Hp=1, Hc=1, oracle=false) + + setconstraint!(nmpc_leg, umin=[-5, -9.9], umax=[100,99]) + @test all((nmpc_leg.con.U0min, nmpc_leg.con.U0max) .≈ ([-5, -9.9], [100,99])) + setconstraint!(nmpc_leg, Δumin=[-5,-10], Δumax=[6,11]) + @test all((nmpc_leg.con.ΔŨmin, nmpc_leg.con.ΔŨmax) .≈ ([-5,-10,0], [6,11,Inf])) + setconstraint!(nmpc_leg, ymin=[-6, -11],ymax=[55, 35]) + @test all((nmpc_leg.con.Y0min, nmpc_leg.con.Y0max) .≈ ([-6,-11], [55,35])) + setconstraint!(nmpc_leg, x̂min=[-21,-22,-23,-24,-25,-26], x̂max=[21,22,23,24,25,26]) + @test all((nmpc_leg.con.x̂0min, nmpc_leg.con.x̂0max) .≈ + ([-21,-22,-23,-24,-25,-26], [21,22,23,24,25,26])) + + setconstraint!(nmpc_leg, c_umin=[0.01,0.02], c_umax=[0.03,0.04]) + @test all((-nmpc_leg.con.A_Umin[:, end], -nmpc_leg.con.A_Umax[:, end]) .≈ + ([0.01,0.02], [0.03,0.04])) + setconstraint!(nmpc_leg, c_Δumin=[0.05,0.06], c_Δumax=[0.07,0.08]) + @test all((-nmpc_leg.con.A_ΔŨmin[1:end-1, end], -nmpc_leg.con.A_ΔŨmax[1:end-1, end]) .≈ + ([0.05,0.06], [0.07,0.08])) + setconstraint!(nmpc_leg, c_ymin=[1.00,1.01], c_ymax=[1.02,1.03]) + @test all((-nmpc_leg.con.A_Ymin, -nmpc_leg.con.A_Ymax) .≈ (zeros(0,3), zeros(0,3))) + @test all((nmpc_leg.con.C_ymin, nmpc_leg.con.C_ymax) .≈ ([1.00,1.01], [1.02,1.03])) + setconstraint!(nmpc_leg, + c_x̂min=[0.21,0.22,0.23,0.24,0.25,0.26], + c_x̂max=[0.31,0.32,0.33,0.34,0.35,0.36] + ) + @test all((nmpc_leg.con.c_x̂min, nmpc_leg.con.c_x̂max) .≈ + ([0.21,0.22,0.23,0.24,0.25,0.26], [0.31,0.32,0.33,0.34,0.35,0.36])) nmpc_ms = NonLinMPC(nonlinmodel, Hp=1, Hc=1, transcription=MultipleShooting()) @@ -1201,6 +1232,76 @@ end @test all(isapprox.(info[:Ŷ], 3.14; atol=1e-1)) @test all(isapprox.(info[:gc][Hp+1:end], 0.0; atol=1e-1)) + # TODO: delete these tests when the deprecated legacy splatting syntax will be. + + nmpc_leg = NonLinMPC(nonlinmodel; Hp, Hc=5, gc, nc=2Hp, p=[0; 0], oracle=false) + JuMP.set_attribute(nmpc_leg.optim, "constr_viol_tol", 1e-3) + + setconstraint!(nmpc_leg, x̂min=[-1e6,-Inf], x̂max=[+1e6,+Inf]) + setconstraint!(nmpc_leg, umin=[-1e6], umax=[+1e6]) + setconstraint!(nmpc_leg, Δumin=[-15], Δumax=[15]) + setconstraint!(nmpc_leg, ymin=[-100], ymax=[100]) + preparestate!(nmpc_leg, [0]) + + setconstraint!(nmpc_leg, umin=[-3], umax=[4]) + moveinput!(nmpc_leg, [-100]) + info = getinfo(nmpc_leg) + @test all(isapprox.(info[:U], -3; atol=1e-1)) + moveinput!(nmpc_leg, [100]) + info = getinfo(nmpc_leg) + @test all(isapprox.(info[:U], 4; atol=1e-1)) + setconstraint!(nmpc_leg, umin=[-1e6], umax=[+1e6]) + + setconstraint!(nmpc_leg, Δumin=[-1.5], Δumax=[1.25]) + moveinput!(nmpc_leg, [-100]) + info = getinfo(nmpc_leg) + @test all(isapprox.(info[:ΔU], -1.5; atol=1e-1)) + moveinput!(nmpc_leg, [100]) + info = getinfo(nmpc_leg) + @test all(isapprox.(info[:ΔU], 1.25; atol=1e-1)) + setconstraint!(nmpc_leg, Δumin=[-1e6], Δumax=[+1e6]) + + setconstraint!(nmpc_leg, ymin=[-0.5], ymax=[0.9]) + moveinput!(nmpc_leg, [-100]) + info = getinfo(nmpc_leg) + @test all(isapprox.(info[:Ŷ], -0.5; atol=1e-1)) + moveinput!(nmpc_leg, [100]) + info = getinfo(nmpc_leg) + @test all(isapprox.(info[:Ŷ], 0.9; atol=1e-1)) + setconstraint!(nmpc_leg, ymin=[-100], ymax=[100]) + + setconstraint!(nmpc_leg, Ymin=[-0.5; fill(-100, Hp-1)], Ymax=[0.9; fill(+100, Hp-1)]) + moveinput!(nmpc_leg, [-200]) + info = getinfo(nmpc_leg) + @test info[:Ŷ][end] ≈ -100 atol=1e-1 + @test info[:Ŷ][begin] ≈ -0.5 atol=1e-1 + moveinput!(nmpc_leg, [200]) + info = getinfo(nmpc_leg) + @test info[:Ŷ][end] ≈ 100 atol=1e-1 + @test info[:Ŷ][begin] ≈ 0.9 atol=1e-1 + setconstraint!(nmpc_leg, ymin=[-100], ymax=[100]) + + setconstraint!(nmpc_leg, x̂min=[-1e-6,-Inf], x̂max=[+1e-6,+Inf]) + moveinput!(nmpc_leg, [-10]) + info = getinfo(nmpc_leg) + @test info[:x̂end][1] ≈ 0 atol=1e-1 + moveinput!(nmpc_leg, [10]) + info = getinfo(nmpc_leg) + @test info[:x̂end][1] ≈ 0 atol=1e-1 + setconstraint!(nmpc_leg, x̂min=[-1e6,-Inf], x̂max=[1e6,+Inf]) + + nmpc_leg.p .= [1; 0] + moveinput!(nmpc_leg, [100]) + info = getinfo(nmpc_leg) + @test all(isapprox.(info[:U], 4.2; atol=1e-1)) + @test all(isapprox.(info[:gc][1:Hp], 0.0; atol=1e-1)) + + nmpc_leg.p .= [0; 1] + moveinput!(nmpc_leg, [100]) + info = getinfo(nmpc_leg) + @test all(isapprox.(info[:Ŷ], 3.14; atol=1e-1)) + @test all(isapprox.(info[:gc][Hp+1:end], 0.0; atol=1e-1)) + nmpc_ms = NonLinMPC( nonlinmodel; Hp, Hc=5, transcription=MultipleShooting(), gc, nc=2Hp, p=[0; 0] )