diff --git a/benchmark/3_bench_predictive_control.jl b/benchmark/3_bench_predictive_control.jl index 6388bbde4..19e8c386d 100644 --- a/benchmark/3_bench_predictive_control.jl +++ b/benchmark/3_bench_predictive_control.jl @@ -421,7 +421,7 @@ model2, p = pendulum_model2, pendulum_p2 plant2 = deepcopy(model2) plant2.p[3] = 1.25*p[3] # plant-model mismatch estim2 = UnscentedKalmanFilter(model2; σQ, σR, nint_u, σQint_u, i_ym=[1]) -function JE(UE, ŶE, _ , p) +function JE(UE, ŶE, _ , p, _) Ts = p τ, ω = @views UE[1:end-1], ŶE[2:2:end-1] return Ts*dot(τ, ω) diff --git a/docs/src/manual/nonlinmpc.md b/docs/src/manual/nonlinmpc.md index 8ec651fe4..5335354a9 100644 --- a/docs/src/manual/nonlinmpc.md +++ b/docs/src/manual/nonlinmpc.md @@ -191,7 +191,7 @@ output vector of `plant` argument corresponds to the model output vector in `mpc We can now define the ``J_E`` function and the `empc` controller: ```@example man_nonlin -function JE(Ue, Ŷe, _ , p) +function JE(Ue, Ŷe, _ , p, _ ) Ts = p τ, ω = Ue[1:end-1], Ŷe[2:2:end-1] return Ts*sum(τ.*ω) diff --git a/src/controller/execute.jl b/src/controller/execute.jl index 80134fffa..2164255ad 100644 --- a/src/controller/execute.jl +++ b/src/controller/execute.jl @@ -155,7 +155,7 @@ function getinfo(mpc::PredictiveController{NT}) where NT<:Real U .= U0 .+ mpc.Uop Ŷ .= Ŷ0 .+ mpc.Yop D̂ .= mpc.D̂0 + mpc.Dop - J = obj_nonlinprog!(Ŷ0, U0, mpc, model, Ue, Ŷe, ΔŨ) + J = obj_nonlinprog!(Ŷ0, U0, mpc, Ue, Ŷe, ΔŨ) Ŷs = similar(mpc.Yop) predictstoch!(Ŷs, mpc, mpc.estim) info[:ΔU] = Z̃[1:mpc.Hc*model.nu] @@ -185,13 +185,16 @@ function getinfo(mpc::PredictiveController{NT}) where NT<:Real return info end -""" - getϵ(mpc::PredictiveController, Z̃) -> ϵ +@doc raw""" + getϵ(mpc::PredictiveController, Z̃orΔŨ) -> ϵ + +Get the slack `ϵ` from `Z̃orΔŨ` if present, otherwise return 0. -Get the slack `ϵ` from the decision vector `Z̃` if present, otherwise return 0. +The argument `Z̃orΔŨ` can be the augmented decision vector ``\mathbf{Z̃}`` or the augmented +input increment vector ``\mathbf{ΔŨ}``, it works with both. """ -function getϵ(mpc::PredictiveController, Z̃::AbstractVector{NT}) where NT<:Real - return mpc.nϵ ≠ 0 ? Z̃[end] : zero(NT) +function getϵ(mpc::PredictiveController, Z̃orΔŨ::AbstractVector{NT}) where NT<:Real + return mpc.nϵ ≠ 0 ? Z̃orΔŨ[end] : zero(NT) end """ @@ -387,33 +390,20 @@ end iszero_nc(mpc::PredictiveController) = (mpc.con.nc == 0) """ - obj_nonlinprog!( _ , _ , mpc::PredictiveController, model::LinModel, Ue, Ŷe, _ , Z̃) - -Nonlinear programming objective function when `model` is a [`LinModel`](@ref). - -The method is called by the nonlinear optimizer of [`NonLinMPC`](@ref) controllers. It can -also be called on any [`PredictiveController`](@ref)s to evaluate the objective function `J` -at specific `Ue`, `Ŷe` and `Z̃`, values. It does not mutate any argument. -""" -function obj_nonlinprog!( - _, _, mpc::PredictiveController, model::LinModel, Ue, Ŷe, _ , Z̃::AbstractVector{NT} -) where NT <: Real - JQP = obj_quadprog(Z̃, mpc.H̃, mpc.q̃) + mpc.r[] - E_JE = obj_econ(mpc, model, Ue, Ŷe) - return JQP + E_JE -end - -""" - obj_nonlinprog!(Ȳ, Ū, mpc::PredictiveController, model::SimModel, Ue, Ŷe, ΔŨ) + obj_nonlinprog!(Ȳ, Ū, mpc::PredictiveController, Ue, Ŷe, ΔŨ) Nonlinear programming objective method when `model` is not a [`LinModel`](@ref). The function `dot(x, A, x)` is a performant way of calculating `x'*A*x`. This method mutates `Ȳ` and `Ū` arguments, without assuming any initial values (it recuperates the values in `Ŷe` and `Ue` arguments). + +Note that a specialized version on [`LinModel`](@ref) that uses the Hessian matrix `mpc.H̃` +is actually slower in the [`MultipleShooting`](@ref) case, so only one method is defined. """ function obj_nonlinprog!( - Ȳ, Ū, mpc::PredictiveController, model::SimModel, Ue, Ŷe, ΔŨ::AbstractVector{NT} + Ȳ, Ū, mpc::PredictiveController, Ue, Ŷe, ΔŨ::AbstractVector{NT} ) where NT<:Real + model = mpc.estim.model nu, ny = model.nu, model.ny # --- output setpoint tracking term --- if mpc.weights.iszero_M_Hp[] @@ -438,7 +428,8 @@ function obj_nonlinprog!( JR̂u = dot(Ū, mpc.weights.L_Hp, Ū) end # --- economic term --- - E_JE = obj_econ(mpc, model, Ue, Ŷe) + ϵ = getϵ(mpc, ΔŨ) + E_JE = obj_econ(mpc, model, Ue, Ŷe, ϵ) return JR̂y + JΔŨ + JR̂u + E_JE end @@ -446,7 +437,7 @@ end con_custom!(gc, ::PredictiveController, _ , _, _ ) = gc "By default, the economic term is zero." -function obj_econ(::PredictiveController, ::SimModel, _ , ::AbstractVector{NT}) where NT +function obj_econ(::PredictiveController, ::SimModel, _ , ::AbstractVector{NT}, _ ) where NT return zero(NT) end diff --git a/src/controller/nonlinmpc.jl b/src/controller/nonlinmpc.jl index 4f2a3c943..00e0343e2 100644 --- a/src/controller/nonlinmpc.jl +++ b/src/controller/nonlinmpc.jl @@ -162,7 +162,7 @@ controller minimizes the following objective function at each discrete time ``k` + \mathbf{(ΔU)}' \mathbf{N}_{H_c} \mathbf{(ΔU)} \\& + \mathbf{(R̂_u - U)}' \mathbf{L}_{H_p} \mathbf{(R̂_u - U)} + C ϵ^2 - + E J_E(\mathbf{U_e}, \mathbf{Ŷ_e}, \mathbf{D̂_e}, \mathbf{p}) + + E J_E(\mathbf{U_e}, \mathbf{Ŷ_e}, \mathbf{D̂_e}, \mathbf{p}, ϵ) \end{aligned} ``` subject to [`setconstraint!`](@ref) bounds, and the custom inequality constraints: @@ -219,8 +219,8 @@ This controller allocates memory at each time step for the optimization. - `Wd=nothing` : custom linear constraint matrix for meas. disturbance (see Extended Help). - `Wr=nothing` : custom linear constraint matrix for output setpoint (see Extended Help). - `Ewt=0.0` : economic costs weight ``E`` (scalar). -- `JE=(_,_,_,_)->0.0` : economic or custom cost function ``J_E(\mathbf{U_e}, \mathbf{Ŷ_e}, - \mathbf{D̂_e}, \mathbf{p})``. +- `JE=(_,_,_,_,_)->0.0` : economic or custom cost function ``J_E(\mathbf{U_e}, \mathbf{Ŷ_e}, + \mathbf{D̂_e}, \mathbf{p}, ϵ)``. - `gc=(_,_,_,_,_,_)->nothing` or `gc!` : custom nonlinear inequality constraint function ``\mathbf{g_c}(\mathbf{U_e}, \mathbf{Ŷ_e}, \mathbf{D̂_e}, \mathbf{p}, ϵ)``, mutating or not (details in Extended Help). @@ -275,8 +275,8 @@ NonLinMPC controller with a sample time Ts = 10.0 s: The economic cost ``J_E`` and custom constraint ``\mathbf{g_c}`` functions receive the extended vectors ``\mathbf{U_e}`` (`nu*Hp+nu` elements), ``\mathbf{Ŷ_e}`` (`ny+ny*Hp` elements) and ``\mathbf{D̂_e}`` (`nd+nd*Hp` elements) as arguments. They all include the - values from ``k`` to ``k + H_p`` (inclusively). The custom constraint also receives the - slack ``ϵ`` (scalar), which is always zero if `Cwt=Inf`. + values from ``k`` to ``k + H_p`` (inclusively). They also receives the slack ``ϵ`` + (scalar), which is always zero if `Cwt=Inf`. More precisely, the last two time steps in ``\mathbf{U_e}`` are forced to be equal, i.e. ``\mathbf{u}(k+H_p) = \mathbf{u}(k+H_p-1)``, since ``H_c ≤ H_p`` implies that @@ -343,7 +343,7 @@ function NonLinMPC( Wr = nothing, Cwt = DEFAULT_CWT, Ewt = DEFAULT_EWT, - JE ::Function = (_,_,_,_) -> 0.0, + JE ::Function = (_,_,_,_,_) -> 0.0, gc!::Function = (_,_,_,_,_,_) -> nothing, gc ::Function = gc!, nc::Int = 0, @@ -414,7 +414,7 @@ function NonLinMPC( Wr = nothing, Cwt = DEFAULT_CWT, Ewt = DEFAULT_EWT, - JE ::Function = (_,_,_,_) -> 0.0, + JE ::Function = (_,_,_,_,_) -> 0.0, gc!::Function = (_,_,_,_,_,_) -> nothing, gc ::Function = gc!, nc = 0, @@ -454,11 +454,11 @@ default_jacobian(::TranscriptionMethod) = DEFAULT_NONLINMPC_JACSPARSE Validate `JE` function argument signature. """ function validate_JE(NT, JE) - # Ue, Ŷe, D̂e, p - if !hasmethod(JE, Tuple{Vector{NT}, Vector{NT}, Vector{NT}, Any}) + # Ue, Ŷe, D̂e, p, ϵ + if !hasmethod(JE, Tuple{Vector{NT}, Vector{NT}, Vector{NT}, Any, NT}) error( "the economic cost function has no method with type signature "* - "JE(Ue::Vector{$(NT)}, Ŷe::Vector{$(NT)}, D̂e::Vector{$(NT)}, p::Any)" + "JE(Ue::Vector{$(NT)}, Ŷe::Vector{$(NT)}, D̂e::Vector{$(NT)}, p::Any, ϵ::$(NT))" ) end return nothing @@ -513,19 +513,20 @@ should ease troubleshooting of simple bugs e.g.: the user forgets to set the `nc function test_custom_functions(NT, model::SimModel, JE, gc!, nc, Uop, Yop, Dop, p) uop, dop, yop = model.uop, model.dop, model.yop Ue, Ŷe, D̂e = [Uop; uop], [yop; Yop], [dop; Dop] + ϵ = zero(NT) try - val::NT = JE(Ue, Ŷe, D̂e, p) + val::NT = JE(Ue, Ŷe, D̂e, p, ϵ) catch err @warn( """ - Calling the JE function with Ue, Ŷe, D̂e arguments fixed at uop=$uop, - yop=$yop, dop=$dop failed with the following stacktrace. Did you forget + Calling the JE function with Ue, Ŷe, D̂e, ϵ arguments fixed at uop=$uop, + yop=$yop, dop=$dop, ϵ=0 failed with the following stacktrace. Did you forget to set the keyword argument p? """, exception=(err, catch_backtrace()) ) end - ϵ, gc = zero(NT), Vector{NT}(undef, nc) + gc = Vector{NT}(undef, nc) try gc!(gc, Ue, Ŷe, D̂e, p, ϵ) catch err @@ -553,7 +554,7 @@ function addinfo!(info, mpc::NonLinMPC{NT}) where NT<:Real Ue = [U; U[(end - mpc.estim.model.nu + 1):end]] Ŷe = [ŷ; Ŷ] D̂e = [d; D̂] - JE = mpc.JE(Ue, Ŷe, D̂e, mpc.p) + JE = mpc.JE(Ue, Ŷe, D̂e, mpc.p, ϵ) LHS = Vector{NT}(undef, mpc.con.nc) mpc.con.gc!(LHS, Ue, Ŷe, D̂e, mpc.p, ϵ) info[:JE] = JE @@ -584,7 +585,7 @@ function addinfo!(info, mpc::NonLinMPC{NT}) where NT<:Real ) function J!(Z̃, ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, K, X̂0, gc, g, geq) update_predictions!(ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, K, X̂0, gc, g, geq, mpc, Z̃) - return obj_nonlinprog!(Ŷ0, U0, mpc, model, Ue, Ŷe, ΔŨ) + return obj_nonlinprog!(Ŷ0, U0, mpc, Ue, Ŷe, ΔŨ) end if !isnothing(mpc.hessian) _, ∇J, ∇²J = value_gradient_and_hessian(J!, mpc.hessian, mpc.Z̃, J_cache...) @@ -780,7 +781,7 @@ function get_nonlinobj_op(mpc::NonLinMPC, optim::JuMP.GenericModel{JNT}) where J geq::Vector{JNT} = zeros(JNT, neq) function J!(Z̃, ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, K, X̂0, gc, g, geq) update_predictions!(ΔŨ, x̂0end, Ue, Ŷe, U0, Ŷ0, Û0, K, X̂0, gc, g, geq, mpc, Z̃) - return obj_nonlinprog!(Ŷ0, U0, mpc, model, Ue, Ŷe, ΔŨ) + return obj_nonlinprog!(Ŷ0, U0, mpc, Ue, Ŷe, ΔŨ) end Z̃_J = fill(myNaN, nZ̃) # NaN to force update at first call J_cache = ( @@ -1092,9 +1093,9 @@ end "Evaluate the economic term `E*JE` of the objective function for [`NonLinMPC`](@ref)." function obj_econ( - mpc::NonLinMPC, ::SimModel, Ue, Ŷe::AbstractVector{NT} + mpc::NonLinMPC, ::SimModel, Ue, Ŷe::AbstractVector{NT}, ϵ ) where NT<:Real - E_JE = mpc.weights.iszero_E ? zero(NT) : mpc.weights.E*mpc.JE(Ue, Ŷe, mpc.D̂e, mpc.p) + E_JE = mpc.weights.iszero_E ? zero(NT) : mpc.weights.E*mpc.JE(Ue, Ŷe, mpc.D̂e, mpc.p, ϵ) return E_JE end diff --git a/src/precompile.jl b/src/precompile.jl index 74c096bb1..2408a2017 100644 --- a/src/precompile.jl +++ b/src/precompile.jl @@ -21,7 +21,7 @@ function h!(y, x, _ , p) end p = (sys2.A, sys2.B, sys2.C) -function JE( _ , Ŷe, _ , R̂y) +function JE( _ , Ŷe, _ , R̂y , _ ) Ŷ = @views Ŷe[3:end] Ȳ = R̂y - Ŷ return dot(Ȳ, Ȳ) diff --git a/test/3_test_predictive_control.jl b/test/3_test_predictive_control.jl index efb767865..75a736a83 100644 --- a/test/3_test_predictive_control.jl +++ b/test/3_test_predictive_control.jl @@ -782,9 +782,9 @@ end nmpc6 = NonLinMPC(linmodel1, Hp=15, Lwt=[0,1]) @test nmpc6.weights.L_Hp ≈ Diagonal(diagm(repeat(Float64[0, 1], 15))) @test nmpc6.weights.L_Hp isa Hermitian{Float64, Diagonal{Float64, Vector{Float64}}} - nmpc7 = NonLinMPC(linmodel1, Hp=15, Ewt=1e-3, JE=(Ue,Ŷe,D̂e,p) -> p*dot(Ue,Ŷe)+sum(D̂e), p=10) + nmpc7 = NonLinMPC(linmodel1, Hp=15, Ewt=1e-3, JE=(Ue,Ŷe,D̂e,p,_) -> p*dot(Ue,Ŷe)+sum(D̂e), p=10) @test nmpc7.weights.E == 1e-3 - @test nmpc7.JE([1,2],[3,4],[4,6],10) == 10*dot([1,2],[3,4])+sum([4,6]) + @test nmpc7.JE([1,2],[3,4],[4,6],10,0) == 10*dot([1,2],[3,4])+sum([4,6]) nmpc10 = NonLinMPC(linmodel1, nint_u=[1, 1], nint_ym=[0, 0]) @test nmpc10.estim.nint_u == [1, 1] @test nmpc10.estim.nint_ym == [0, 0] @@ -808,11 +808,12 @@ end @test_throws DimensionMismatch NonLinMPC(linmodel1, Hp=15, Ewt=[1, 1]) @test_throws ErrorException NonLinMPC(linmodel1, Hp=15, JE = (_,_,_)->0.0) + @test_throws ErrorException NonLinMPC(linmodel1, Hp=15, JE = (_,_,_,_)->0.0) @test_throws ErrorException NonLinMPC(linmodel1, Hp=15, gc = (_,_,_,_)->[0.0], nc=1) @test_throws ErrorException NonLinMPC(linmodel1, Hp=15, gc! = (_,_,_,_)->[0.0], nc=1) @test_throws ArgumentError NonLinMPC(linmodel1, transcription=TrapezoidalCollocation()) - @test_logs (:warn, Regex(".*")) NonLinMPC(linmodel1, Hp=15, JE=(Ue,_,_,_)->Ue) + @test_logs (:warn, Regex(".*")) NonLinMPC(linmodel1, Hp=15, JE=(Ue,_,_,_,_)->Ue) @test_logs (:warn, Regex(".*")) NonLinMPC(linmodel1, Hp=15, gc=(Ue,_,_,_,_)->Ue, nc=0) end @@ -903,7 +904,7 @@ end setmodel!(nmpc_lin; Mwt=[0], Lwt=[1]) u = moveinput!(nmpc_lin; R̂u=fill(ru[1], Hp)) @test u ≈ [4] atol=5e-2 - function JE(Ue, Ŷe, _ , p) + function JE(Ue, Ŷe, _ , p, _ ) Wy, R̂y, Wu, R̂u = p return Wy*sum((R̂y-Ŷe[2:end]).^2) + Wu*sum((R̂u-Ue[1:end-1]).^2) end