added: gE and nE field in ControllerConstraint

Author: franckgaga

src/controller/construct.jl

@@ -1,5 +1,5 @@
 "Include all the data for the constraints of [`PredictiveController`](@ref)"
-struct ControllerConstraint{NT<:Real}
+struct ControllerConstraint{NT<:Real, GEfunc<:Function}
     ẽx̂      ::Matrix{NT}
     fx̂      ::Vector{NT}
     gx̂      ::Matrix{NT}
@@ -31,6 +31,8 @@ struct ControllerConstraint{NT<:Real}
     c_x̂min  ::Vector{NT}
     c_x̂max  ::Vector{NT}
     i_g     ::BitVector
+    gE      ::GEfunc
+    nE      ::Int
 end
 
 @doc raw"""
@@ -625,16 +627,18 @@ function init_quadprog(::SimModel{NT}, Ẽ, S̃, M_Hp, Ñ_Hc, L_Hp) where {NT<:
 end
 
 """
-    init_defaultcon_mpc(estim, C, S, N_Hc, E, ex̂, fx̂, gx̂, jx̂, kx̂, vx̂) -> con, S̃, Ñ_Hc, Ẽ
+    init_defaultcon_mpc(
+        estim, C, S, N_Hc, E, ex̂, fx̂, gx̂, jx̂, kx̂, vx̂, gE=()->nothing, nE=0
+    ) -> con, S̃, Ñ_Hc, Ẽ
 
 Init `ControllerConstraint` struct with default parameters based on estimator `estim`.
 
 Also return `S̃`, `Ñ_Hc` and `Ẽ` matrices for the the augmented decision vector `ΔŨ`.
 """
 function init_defaultcon_mpc(
     estim::StateEstimator{NT}, 
-    Hp, Hc, C, S, N_Hc, E, ex̂, fx̂, gx̂, jx̂, kx̂, vx̂, bx̂
-) where {NT<:Real}
+    Hp, Hc, C, S, N_Hc, E, ex̂, fx̂, gx̂, jx̂, kx̂, vx̂, bx̂, gE::GEfunc=()->nothing, nE=0
+) where {NT<:Real, GEfunc<:Function}
     model = estim.model
     nu, ny, nx̂ = model.nu, model.ny, estim.nx̂
     nϵ = isinf(C) ? 0 : 1
@@ -666,11 +670,12 @@ function init_defaultcon_mpc(
         A_Umin, A_Umax, A_ΔŨmin, A_ΔŨmax, A_Ymin, A_Ymax, A_x̂max, A_x̂min
     )
     b = zeros(NT, size(A, 1)) # dummy b vector (updated just before optimization)
-    con = ControllerConstraint{NT}(
+    con = ControllerConstraint{NT, GEfunc}(
         ẽx̂      , fx̂    , gx̂     , jx̂       , kx̂     , vx̂     , bx̂     ,
         U0min   , U0max , ΔŨmin  , ΔŨmax    , Y0min  , Y0max  , x̂0min  , x̂0max,
         A_Umin  , A_Umax, A_ΔŨmin, A_ΔŨmax  , A_Ymin , A_Ymax , A_x̂min , A_x̂max,
-        A       , b     , i_b    , C_ymin   , C_ymax , c_x̂min , c_x̂max , i_g
+        A       , b     , i_b    , C_ymin   , C_ymax , c_x̂min , c_x̂max , i_g,
+        gE      , nE
     )
     return con, nϵ, S̃, Ñ_Hc, Ẽ
 end

src/controller/nonlinmpc.jl

@@ -48,9 +48,9 @@ struct NonLinMPC{
     Yop::Vector{NT}
     Dop::Vector{NT}
     buffer::PredictiveControllerBuffer{NT}
-    function NonLinMPC{NT, SE, JM, JEFunc, P}(
-        estim::SE, Hp, Hc, M_Hp, N_Hc, L_Hp, Cwt, Ewt, JE::JEFunc, p::P, optim::JM
-    ) where {NT<:Real, SE<:StateEstimator, JM<:JuMP.GenericModel, JEFunc<:Function, P<:Any}
+    function NonLinMPC{NT, SE, JM, JEfunc, P}(
+        estim::SE, Hp, Hc, M_Hp, N_Hc, L_Hp, Cwt, Ewt, JE::JEfunc, gE, nE, p::P, optim::JM
+    ) where {NT<:Real, SE<:StateEstimator, JM<:JuMP.GenericModel, JEfunc<:Function, P<:Any}
         model = estim.model
         nu, ny, nd, nx̂ = model.nu, model.ny, model.nd, estim.nx̂
         ŷ = copy(model.yop) # dummy vals (updated just before optimization)
@@ -68,7 +68,7 @@ struct NonLinMPC{
         # dummy vals (updated just before optimization):
         F, fx̂  = zeros(NT, ny*Hp), zeros(NT, nx̂)
         con, nϵ, S̃, Ñ_Hc, Ẽ = init_defaultcon_mpc(
-            estim, Hp, Hc, Cwt, S, N_Hc, E, ex̂, fx̂, gx̂, jx̂, kx̂, vx̂, bx̂
+            estim, Hp, Hc, Cwt, S, N_Hc, E, ex̂, fx̂, gx̂, jx̂, kx̂, vx̂, bx̂, gE, nE
         )
         H̃ = init_quadprog(model, Ẽ, S̃, M_Hp, Ñ_Hc, L_Hp)
         # dummy vals (updated just before optimization):
@@ -80,7 +80,7 @@ struct NonLinMPC{
         nΔŨ = size(Ẽ, 2)
         ΔŨ = zeros(NT, nΔŨ)
         buffer = PredictiveControllerBuffer{NT}(nu, ny, nd, Hp)
-        mpc = new{NT, SE, JM, JEFunc, P}(
+        mpc = new{NT, SE, JM, JEfunc, P}(
             estim, optim, con,
             ΔŨ, ŷ,
             Hp, Hc, nϵ,
@@ -115,14 +115,14 @@ controller minimizes the following objective function at each discrete time ``k`
                        + E J_E(\mathbf{U}_E, \mathbf{Ŷ}_E, \mathbf{D̂}_E, \mathbf{p})
 \end{aligned}
 ```
-subject to [`setconstraint!`](@ref) bounds, and the custom economic inequality constraints:
+subject to [`setconstraint!`](@ref) bounds, and the economic inequality constraints:
 ```math
-\mathbf{g}_E(\mathbf{U}_E, \mathbf{Ŷ}_E, \mathbf{D̂}_E, \mathbf{p}, ε) ≤ \mathbf{0}
-````
+\mathbf{g}_E(\mathbf{U}_E, \mathbf{Ŷ}_E, \mathbf{D̂}_E, ϵ, \mathbf{p}) ≤ \mathbf{0}
+```
 The economic function ``J_E`` can penalizes solutions with high economic costs. Setting all
 the weights to 0 except ``E``  creates a pure economic model predictive controller (EMPC).
-The arguments of ``J_E`` include the manipulated inputs, the predicted outputs and measured
-disturbances from ``k`` to ``k+H_p`` inclusively:
+The arguments of ``J_E`` and ``\mathbf{g}_E`` include the manipulated inputs, the predicted
+outputs and measured disturbances from ``k`` to ``k+H_p`` inclusively:
 ```math
     \mathbf{U}_E = \begin{bmatrix} \mathbf{U}      \\ \mathbf{u}(k+H_p-1)   \end{bmatrix}  , \quad
     \mathbf{Ŷ}_E = \begin{bmatrix} \mathbf{ŷ}(k)   \\ \mathbf{Ŷ}            \end{bmatrix}  , \quad
@@ -134,7 +134,8 @@ over ``H_p``. The argument ``\mathbf{p}`` is a custom parameter object of any ty
 mutable one if you want to modify it later e.g.: a vector.
 
 !!! tip
-    Replace any of the 4 arguments with `_` if not needed (see `JE` default value below).
+    Replace any of the arguments of ``J_E`` and ``\mathbf{g}_E`` functions with `_` if not
+    needed (details in Extended Help).
 
 See [`LinMPC`](@ref) for the definition of the other variables. This method uses the default
 state estimator :
@@ -158,11 +159,12 @@ state estimator :
 - `L_Hp=diagm(repeat(Lwt,Hp))` : positive semidefinite symmetric matrix ``\mathbf{L}_{H_p}``.
 - `Cwt=1e5` : slack variable weight ``C`` (scalar), use `Cwt=Inf` for hard constraints only.
 - `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})``.
-- `gE=(_,_,_,_,_)->[]` : economic (or custom) constraint function
-   ``\mathbf{g}_E(\mathbf{U}_E, \mathbf{Ŷ}_E, \mathbf{D̂}_E, \mathbf{p}, ε)``.
-- `p=model.p` : ``J_E`` function parameter ``\mathbf{p}`` (any type).
+- `JE=(_,_,_,_)->0.0` : economic (or custom) cost function ``J_E(\mathbf{U}_E, \mathbf{Ŷ}_E,
+   \mathbf{D̂}_E, \mathbf{p})`` (details in Extended Help).
+- `gE=(_,_,_,_,_,_)->nothing` : economic (or custom) constraint function ``\mathbf{g}_E(
+   \mathbf{U}_E, \mathbf{Ŷ}_E, \mathbf{D̂}_E, ϵ, \mathbf{p})`` (details in Extended Help).
+- `nE=0` : number of economic constraints.
+- `p=model.p` : ``J_E`` and ``\mathbf{g}_E`` functions parameter ``\mathbf{p}`` (any type).
 - `optim=JuMP.Model(Ipopt.Optimizer)` : nonlinear optimizer used in the predictive
    controller, provided as a [`JuMP.Model`](https://jump.dev/JuMP.jl/stable/api/JuMP/#JuMP.Model)
    (default to [`Ipopt`](https://github.com/jump-dev/Ipopt.jl) optimizer).
@@ -191,6 +193,15 @@ NonLinMPC controller with a sample time Ts = 10.0 s, Ipopt optimizer, UnscentedK
     algebra instead of a `for` loop. This feature can accelerate the optimization, especially
     for the constraint handling, and is not available in any other package, to my knowledge.
 
+    The J
+
+1. **Non-mutating functions** (out-of-place): define them as `f(x, u, d, p) -> ẋ` and
+   `h(x, d, p) -> y`. This syntax is simple and intuitive but it allocates more memory.
+2. **Mutating functions** (in-place): define them as `f!(ẋ, x, u, d, p) -> nothing` and
+   `h!(y, x, d, p) -> nothing`. This syntax reduces the allocations and potentially the 
+   computational burden as well.
+
+
     The optimization relies on [`JuMP`](https://github.com/jump-dev/JuMP.jl) automatic 
     differentiation (AD) to compute the objective and constraint derivatives. Optimizers 
     generally benefit from exact derivatives like AD. However, the [`NonLinModel`](@ref) 
@@ -212,13 +223,18 @@ function NonLinMPC(
     L_Hp = diagm(repeat(Lwt, Hp)),
     Cwt  = DEFAULT_CWT,
     Ewt  = DEFAULT_EWT,
-    JE::Function = (args...) -> 0.0,
+    JE::Function = (_,_,_,_) -> 0.0,
+    gE::Function = (_,_,_,_,_,_) -> nothing,
+    nE = 0,
     p = model.p,
     optim::JuMP.GenericModel = JuMP.Model(DEFAULT_NONLINMPC_OPTIMIZER, add_bridges=false),
     kwargs...
 )
     estim = UnscentedKalmanFilter(model; kwargs...)
-    NonLinMPC(estim; Hp, Hc, Mwt, Nwt, Lwt, Cwt, Ewt, JE, p, M_Hp, N_Hc, L_Hp, optim)
+    return NonLinMPC(
+        estim; 
+        Hp, Hc, Mwt, Nwt, Lwt, Cwt, Ewt, JE, gE, nE, p, M_Hp, N_Hc, L_Hp, optim
+    )
 end
 
 function NonLinMPC(
@@ -233,13 +249,18 @@ function NonLinMPC(
     L_Hp = diagm(repeat(Lwt, Hp)),
     Cwt  = DEFAULT_CWT,
     Ewt  = DEFAULT_EWT,
-    JE::Function = (args...) -> 0.0,
+    JE::Function = (_,_,_,_) -> 0.0,
+    gE::Function = (_,_,_,_,_,_) -> nothing,
+    nE = 0,
     p = model.p,
     optim::JuMP.GenericModel = JuMP.Model(DEFAULT_NONLINMPC_OPTIMIZER, add_bridges=false),
     kwargs...
 )
     estim = SteadyKalmanFilter(model; kwargs...)
-    NonLinMPC(estim; Hp, Hc, Mwt, Nwt, Lwt, Cwt, Ewt, JE, p, M_Hp, N_Hc, L_Hp, optim)
+    return NonLinMPC(
+        estim; 
+        Hp, Hc, Mwt, Nwt, Lwt, Cwt, Ewt, JE, gE, nE, p, M_Hp, N_Hc, L_Hp, optim
+    )
 end
 
 
@@ -278,17 +299,26 @@ function NonLinMPC(
     L_Hp = diagm(repeat(Lwt, Hp)),
     Cwt  = DEFAULT_CWT,
     Ewt  = DEFAULT_EWT,
-    JE::JEFunc = (args...) -> 0.0,
+    JE::JEfunc = (_,_,_,_) -> 0.0,
+    gE::GEfunc = (_,_,_,_,_,_) -> nothing,
+    nE = 0,
     p::P = estim.model.p,
     optim::JM = JuMP.Model(DEFAULT_NONLINMPC_OPTIMIZER, add_bridges=false),
-) where {NT<:Real, SE<:StateEstimator{NT}, JM<:JuMP.GenericModel, JEFunc<:Function, P<:Any}
+) where {
+    NT<:Real, 
+    SE<:StateEstimator{NT}, 
+    JM<:JuMP.GenericModel, 
+    JEfunc<:Function, 
+    GEfunc<:Function,
+    P<:Any
+}
     nk = estimate_delays(estim.model)
     if Hp ≤ nk
         @warn("prediction horizon Hp ($Hp) ≤ estimated number of delays in model "*
               "($nk), the closed-loop system may be unstable or zero-gain (unresponsive)")
     end
-    return NonLinMPC{NT, SE, JM, JEFunc, P}(
-        estim, Hp, Hc, M_Hp, N_Hc, L_Hp, Cwt, Ewt, JE, p, optim
+    return NonLinMPC{NT, SE, JM, JEfunc, P}(
+        estim, Hp, Hc, M_Hp, N_Hc, L_Hp, Cwt, Ewt, JE, gE, nE, p, optim
     )
 end