changed: added slack ϵ in the JE function signature of NonLinMPC

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(τ, ω)

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(τ.*ω)

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,15 +428,16 @@ 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
 
 "No custom nonlinear constraints `gc` by default, return `gc` unchanged." 
 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
 

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
 

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(Ȳ, Ȳ)

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