Merge pull request #326 from JuliaControl/epsilon_economic_function

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

Author: franckgaga

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

@@ -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
 
 """
@@ -425,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 
@@ -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