Merge pull request #130 from JuliaControl/fix_type_instability

Fix type instability in `LinMPC` and `NonLinMPC` (introduced in 1.1.0)

Author: franckgaga

Project.toml

@@ -1,7 +1,7 @@
 name = "ModelPredictiveControl"
 uuid = "61f9bdb8-6ae4-484a-811f-bbf86720c31c"
 authors = ["Francis Gagnon"]
-version = "1.1.0"
+version = "1.1.1"
 
 [deps]
 ControlSystemsBase = "aaaaaaaa-a6ca-5380-bf3e-84a91bcd477e"

src/controller/construct.jl

@@ -27,7 +27,7 @@ struct ControllerWeights{NT<:Real}
 end
 
 "Include all the data for the constraints of [`PredictiveController`](@ref)"
-struct ControllerConstraint{NT<:Real, GCfunc<:Function}
+struct ControllerConstraint{NT<:Real, GCfunc<:Union{Nothing, Function}}
     ẽx̂      ::Matrix{NT}
     fx̂      ::Vector{NT}
     gx̂      ::Matrix{NT}
@@ -434,9 +434,7 @@ function validate_args(mpc::PredictiveController, ry, d, D̂, R̂y, R̂u)
     size(d)  ≠ (nd,)    && throw(DimensionMismatch("d size $(size(d)) ≠ measured dist. size ($nd,)"))
     size(D̂)  ≠ (nd*Hp,) && throw(DimensionMismatch("D̂ size $(size(D̂)) ≠ measured dist. size × Hp ($(nd*Hp),)"))
     size(R̂y) ≠ (ny*Hp,) && throw(DimensionMismatch("R̂y size $(size(R̂y)) ≠ output size × Hp ($(ny*Hp),)"))
-    if ~mpc.noR̂u
-        size(R̂u) ≠ (nu*Hp,) && throw(DimensionMismatch("R̂u size $(size(R̂u)) ≠ manip. input size × Hp ($(nu*Hp),)"))
-    end
+    size(R̂u) ≠ (nu*Hp,) && throw(DimensionMismatch("R̂u size $(size(R̂u)) ≠ manip. input size × Hp ($(nu*Hp),)"))
 end
 
 
@@ -668,18 +666,17 @@ end
 """
     init_defaultcon_mpc(
         estim, C, S, E, ex̂, fx̂, gx̂, jx̂, kx̂, vx̂, 
-        gc!=(_,_,_,_,_,_)->nothing, nc=0
+        gc!=nothing, nc=0
     ) -> con, S̃, Ẽ
 
 Init `ControllerConstraint` struct with default parameters based on estimator `estim`.
 
 Also return `S̃` and `Ẽ` matrices for the the augmented decision vector `ΔŨ`.
 """
 function init_defaultcon_mpc(
-    estim::StateEstimator{NT}, 
-    Hp, Hc, C, S, E, ex̂, fx̂, gx̂, jx̂, kx̂, vx̂, bx̂, 
-    gc!::GCfunc=(_,_,_,_,_,_)->nothing, nc=0
-) where {NT<:Real, GCfunc<:Function}
+    estim::StateEstimator{NT}, Hp, Hc, C, S, E, ex̂, fx̂, gx̂, jx̂, kx̂, vx̂, bx̂, 
+    gc!::GCfunc=nothing, nc=0
+) where {NT<:Real, GCfunc<:Union{Nothing, Function}}
     model = estim.model
     nu, ny, nx̂ = model.nu, model.ny, estim.nx̂
     nϵ = isinf(C) ? 0 : 1

src/controller/execute.jl

@@ -191,32 +191,34 @@ They are computed with these equations using in-place operations:
 function initpred!(mpc::PredictiveController, model::LinModel, d, D̂, R̂y, R̂u)
     mul!(mpc.T_lastu0, mpc.T, mpc.estim.lastu0)
     ŷ, F, q̃, r = mpc.ŷ, mpc.F, mpc.q̃, mpc.r
+    Cy, Cu, M_Hp_Ẽ, L_Hp_S̃ = mpc.buffer.Ŷ, mpc.buffer.U, mpc.buffer.Ẽ, mpc.buffer.S̃
     ŷ .= evaloutput(mpc.estim, d)
-    predictstoch!(mpc, mpc.estim) # init mpc.F with Ŷs for InternalModel
-    F .+= mpc.B
-    mul!(F, mpc.K, mpc.estim.x̂0, 1, 1) 
-    mul!(F, mpc.V, mpc.estim.lastu0, 1, 1)
+    predictstoch!(mpc, mpc.estim)           # init F with Ŷs for InternalModel
+    F .+= mpc.B                             # F = F + B
+    mul!(F, mpc.K, mpc.estim.x̂0, 1, 1)      # F = F + K*x̂0
+    mul!(F, mpc.V, mpc.estim.lastu0, 1, 1)  # F = F + V*lastu0
     if model.nd ≠ 0
         mpc.d0 .= d .- model.dop
         mpc.D̂0 .= D̂ .- mpc.Dop
         mpc.D̂e[1:model.nd]     .= d
         mpc.D̂e[model.nd+1:end] .= D̂
-        mul!(F, mpc.G, mpc.d0, 1, 1)
-        mul!(F, mpc.J, mpc.D̂0, 1, 1)
+        mul!(F, mpc.G, mpc.d0, 1, 1)        # F = F + G*d0
+        mul!(F, mpc.J, mpc.D̂0, 1, 1)        # F = F + J*D̂0
     end
+    # --- output setpoint tracking term ---
     mpc.R̂y .= R̂y
-    Cy = F .- (R̂y .- mpc.Yop)
-    M_Hp_Ẽ = mpc.weights.M_Hp*mpc.Ẽ
-    mul!(q̃, M_Hp_Ẽ', Cy)
+    Cy .= F .- (R̂y .- mpc.Yop)
+    mul!(M_Hp_Ẽ, mpc.weights.M_Hp, mpc.Ẽ)
+    mul!(q̃, M_Hp_Ẽ', Cy)                    # q̃ = M_Hp*Ẽ'*Cy
     r .= dot(Cy, mpc.weights.M_Hp, Cy)
-    if ~mpc.noR̂u
-        mpc.R̂u .= R̂u
-        Cu = mpc.T_lastu0 .- (R̂u .- mpc.Uop) 
-        L_Hp_S̃ = mpc.weights.L_Hp*mpc.S̃
-        mul!(q̃, L_Hp_S̃', Cu, 1, 1)
-        r .+= dot(Cu, mpc.weights.L_Hp, Cu)
-    end
-    lmul!(2, q̃)
+    # --- input setpoint tracking term ---
+    mpc.R̂u .= R̂u
+    Cu .= mpc.T_lastu0 .- (R̂u .- mpc.Uop) 
+    mul!(L_Hp_S̃, mpc.weights.L_Hp, mpc.S̃)
+    mul!(q̃, L_Hp_S̃', Cu, 1, 1)              # q̃ = q̃ + L_Hp*S̃'*Cu
+    r .+= dot(Cu, mpc.weights.L_Hp, Cu)
+    # --- finalize ---
+    lmul!(2, q̃)                             # q̃ = 2*q̃
     return nothing
 end
 
@@ -228,17 +230,15 @@ Init `ŷ, F, d0, D̂0, D̂e, R̂y, R̂u` vectors when model is not a [`LinModel
 function initpred!(mpc::PredictiveController, model::SimModel, d, D̂, R̂y, R̂u)
     mul!(mpc.T_lastu0, mpc.T, mpc.estim.lastu0)
     mpc.ŷ .= evaloutput(mpc.estim, d)
-    predictstoch!(mpc, mpc.estim) # init mpc.F with Ŷs for InternalModel
+    predictstoch!(mpc, mpc.estim)           # init F with Ŷs for InternalModel
     if model.nd ≠ 0
         mpc.d0 .= d .- model.dop
         mpc.D̂0 .= D̂ .- mpc.Dop
         mpc.D̂e[1:model.nd]     .= d
         mpc.D̂e[model.nd+1:end] .= D̂
     end
     mpc.R̂y .= R̂y
-    if ~mpc.noR̂u
-        mpc.R̂u .= R̂u 
-    end
+    mpc.R̂u .= R̂u
     return nothing
 end
 
@@ -371,15 +371,15 @@ The function mutates `Ue`, `Ŷe` and `Ū` in arguments, without assuming any i
 function extended_predictions!(Ue, Ŷe, Ū, mpc, model, Ŷ0, ΔŨ)
     ny, nu = model.ny, model.nu
     # --- extended manipulated inputs Ue = [U; u(k+Hp-1)] ---
-    U0 = Ū
-    U0 .= mul!(U0, mpc.S̃, ΔŨ) .+ mpc.T_lastu0
-    Ue[1:end-nu] .= U0 .+ mpc.Uop
+    U  = Ū
+    U .= mul!(U, mpc.S̃, ΔŨ) .+ mpc.T_lastu0 .+ mpc.Uop
+    Ue[1:end-nu] .= U
     # u(k + Hp) = u(k + Hp - 1) since Δu(k+Hp) = 0 (because Hc ≤ Hp):
-    Ue[end-nu+1:end] .= @views Ue[end-2nu+1:end-nu]
+    Ue[end-nu+1:end] .= @views U[end-nu+1:end]
     # --- extended output predictions Ŷe = [ŷ(k); Ŷ] ---
     Ŷe[1:ny]     .= mpc.ŷ
     Ŷe[ny+1:end] .= Ŷ0 .+ mpc.Yop
-    return Ue, Ŷe
+    return Ue, Ŷe 
 end
 
 """
@@ -418,13 +418,9 @@ function obj_nonlinprog!(
     # --- move suppression and slack variable term ---
     JΔŨ = dot(ΔŨ, mpc.weights.Ñ_Hc, ΔŨ)
     # --- input setpoint tracking term ---
-    if !mpc.noR̂u
-        Ū  .= @views Ue[1:end-nu]
-        Ū  .= mpc.R̂u .- Ū
-        JR̂u = dot(Ū, mpc.weights.L_Hp, Ū)
-    else
-        JR̂u = 0.0
-    end
+    Ū  .= @views Ue[1:end-nu]
+    Ū  .= mpc.R̂u .- Ū
+    JR̂u = dot(Ū, mpc.weights.L_Hp, Ū)
     # --- economic term ---
     E_JE = obj_econ(mpc, model, Ue, Ŷe)
     return JR̂y + JΔŨ + JR̂u + E_JE

src/controller/explicitmpc.jl

@@ -8,7 +8,6 @@ struct ExplicitMPC{NT<:Real, SE<:StateEstimator} <: PredictiveController{NT}
     weights::ControllerWeights{NT}
     R̂u::Vector{NT}
     R̂y::Vector{NT}
-    noR̂u::Bool
     S̃::Matrix{NT} 
     T::Matrix{NT}
     T_lastu0::Vector{NT}
@@ -46,7 +45,6 @@ struct ExplicitMPC{NT<:Real, SE<:StateEstimator} <: PredictiveController{NT}
         L_Hp = Hermitian(convert(Matrix{NT}, L_Hp), :L)
         # dummy vals (updated just before optimization):
         R̂y, R̂u, T_lastu0 = zeros(NT, ny*Hp), zeros(NT, nu*Hp), zeros(NT, nu*Hp)
-        noR̂u = iszero(L_Hp)
         S, T = init_ΔUtoU(model, Hp, Hc)
         E, G, J, K, V, B = init_predmat(estim, model, Hp, Hc)
         # dummy val (updated just before optimization):
@@ -62,13 +60,13 @@ struct ExplicitMPC{NT<:Real, SE<:StateEstimator} <: PredictiveController{NT}
         Uop, Yop, Dop = repeat(model.uop, Hp), repeat(model.yop, Hp), repeat(model.dop, Hp)
         nΔŨ = size(Ẽ, 2)
         ΔŨ = zeros(NT, nΔŨ)
-        buffer = PredictiveControllerBuffer{NT}(nu, ny, nd, Hp)
+        buffer = PredictiveControllerBuffer{NT}(nu, ny, nd, Hp, Hc, nϵ)
         mpc = new{NT, SE}(
             estim,
             ΔŨ, ŷ,
             Hp, Hc, nϵ,
             weights,
-            R̂u, R̂y, noR̂u,
+            R̂u, R̂y,
             S̃, T, T_lastu0,
             Ẽ, F, G, J, K, V, B,
             H̃, q̃, r,

src/controller/linmpc.jl

@@ -9,7 +9,7 @@ struct LinMPC{
     # note: `NT` and the number type `JNT` in `JuMP.GenericModel{JNT}` can be
     # different since solvers that support non-Float64 are scarce.
     optim::JM
-    con::ControllerConstraint{NT}
+    con::ControllerConstraint{NT, Nothing}
     ΔŨ::Vector{NT}
     ŷ ::Vector{NT}
     Hp::Int
@@ -18,7 +18,6 @@ struct LinMPC{
     weights::ControllerWeights{NT}
     R̂u::Vector{NT}
     R̂y::Vector{NT}
-    noR̂u::Bool
     S̃::Matrix{NT} 
     T::Matrix{NT}
     T_lastu0::Vector{NT}
@@ -50,7 +49,6 @@ struct LinMPC{
         weights = ControllerWeights{NT}(model, Hp, Hc, M_Hp, N_Hc, L_Hp, Cwt)
         # dummy vals (updated just before optimization):
         R̂y, R̂u, T_lastu0 = zeros(NT, ny*Hp), zeros(NT, nu*Hp), zeros(NT, nu*Hp)
-        noR̂u = iszero(L_Hp)
         S, T = init_ΔUtoU(model, Hp, Hc)
         E, G, J, K, V, B, ex̂, gx̂, jx̂, kx̂, vx̂, bx̂ = init_predmat(estim, model, Hp, Hc)
         # dummy vals (updated just before optimization):
@@ -67,13 +65,13 @@ struct LinMPC{
         Uop, Yop, Dop = repeat(model.uop, Hp), repeat(model.yop, Hp), repeat(model.dop, Hp)
         nΔŨ = size(Ẽ, 2)
         ΔŨ = zeros(NT, nΔŨ)
-        buffer = PredictiveControllerBuffer{NT}(nu, ny, nd, Hp)
+        buffer = PredictiveControllerBuffer{NT}(nu, ny, nd, Hp, Hc, nϵ)
         mpc = new{NT, SE, JM}(
             estim, optim, con,
             ΔŨ, ŷ,
             Hp, Hc, nϵ,
             weights,
-            R̂u, R̂y, noR̂u,
+            R̂u, R̂y,
             S̃, T, T_lastu0,
             Ẽ, F, G, J, K, V, B, 
             H̃, q̃, r,

src/controller/nonlinmpc.jl

@@ -2,16 +2,17 @@ const DEFAULT_NONLINMPC_OPTIMIZER = optimizer_with_attributes(Ipopt.Optimizer,"s
 
 struct NonLinMPC{
     NT<:Real, 
-    SE<:StateEstimator, 
+    SE<:StateEstimator,
     JM<:JuMP.GenericModel, 
     JEfunc<:Function,
+    GCfunc<:Function,
     P<:Any
 } <: PredictiveController{NT}
     estim::SE
     # note: `NT` and the number type `JNT` in `JuMP.GenericModel{JNT}` can be
     # different since solvers that support non-Float64 are scarce.
     optim::JM
-    con::ControllerConstraint{NT}
+    con::ControllerConstraint{NT, GCfunc}
     ΔŨ::Vector{NT}
     ŷ ::Vector{NT}
     Hp::Int
@@ -22,7 +23,6 @@ struct NonLinMPC{
     p::P
     R̂u::Vector{NT}
     R̂y::Vector{NT}
-    noR̂u::Bool
     S̃::Matrix{NT}
     T::Matrix{NT}
     T_lastu0::Vector{NT}
@@ -45,18 +45,23 @@ 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, gc, nc, p::P, optim::JM
-    ) where {NT<:Real, SE<:StateEstimator, JM<:JuMP.GenericModel, JEfunc<:Function, P<:Any}
+    function NonLinMPC{NT, SE, JM, JEfunc, GCfunc, P}(
+        estim::SE, 
+        Hp, Hc, M_Hp, N_Hc, L_Hp, Cwt, Ewt, JE::JEfunc, gc!::GCfunc, nc, p::P, optim::JM
+    ) where {
+            NT<:Real, 
+            SE<:StateEstimator, 
+            JM<:JuMP.GenericModel, 
+            JEfunc<:Function, 
+            GCfunc<: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)
-        validate_JE(NT, JE)
-        gc! = get_mutating_gc(NT, gc)
         weights = ControllerWeights{NT}(model, Hp, Hc, M_Hp, N_Hc, L_Hp, Cwt, Ewt)
         # dummy vals (updated just before optimization):
         R̂y, R̂u, T_lastu0 = zeros(NT, ny*Hp), zeros(NT, nu*Hp), zeros(NT, nu*Hp)
-        noR̂u = iszero(L_Hp)
         S, T = init_ΔUtoU(model, Hp, Hc)
         E, G, J, K, V, B, ex̂, gx̂, jx̂, kx̂, vx̂, bx̂ = init_predmat(estim, model, Hp, Hc)
         # dummy vals (updated just before optimization):
@@ -74,14 +79,14 @@ struct NonLinMPC{
         test_custom_functions(NT, model, JE, gc!, nc, Uop, Yop, Dop, p)
         nΔŨ = size(Ẽ, 2)
         ΔŨ = zeros(NT, nΔŨ)
-        buffer = PredictiveControllerBuffer{NT}(nu, ny, nd, Hp)
-        mpc = new{NT, SE, JM, JEfunc, P}(
+        buffer = PredictiveControllerBuffer{NT}(nu, ny, nd, Hp, Hc, nϵ)
+        mpc = new{NT, SE, JM, JEfunc, GCfunc, P}(
             estim, optim, con,
             ΔŨ, ŷ,
             Hp, Hc, nϵ,
             weights,
             JE, p,
-            R̂u, R̂y, noR̂u,
+            R̂u, R̂y,
             S̃, T, T_lastu0,
             Ẽ, F, G, J, K, V, B,
             H̃, q̃, r,
@@ -234,7 +239,7 @@ function NonLinMPC(
     L_Hp = diagm(repeat(Lwt, Hp)),
     Cwt  = DEFAULT_CWT,
     Ewt  = DEFAULT_EWT,
-    JE::Function = (_,_,_,_) -> 0.0,
+    JE ::Function = (_,_,_,_) -> 0.0,
     gc!::Function = (_,_,_,_,_,_) -> nothing,
     gc ::Function = gc!,
     nc::Int = 0,
@@ -312,7 +317,7 @@ function NonLinMPC(
     L_Hp = diagm(repeat(Lwt, Hp)),
     Cwt  = DEFAULT_CWT,
     Ewt  = DEFAULT_EWT,
-    JE ::JEfunc = (_,_,_,_) -> 0.0,
+    JE ::JEfunc   = (_,_,_,_) -> 0.0,
     gc!::Function = (_,_,_,_,_,_) -> nothing,
     gc ::Function = gc!,
     nc = 0,
@@ -330,8 +335,11 @@ function NonLinMPC(
         @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, gc, nc, p, optim
+    validate_JE(NT, JE)
+    gc! = get_mutating_gc(NT, gc)
+    GCfunc = get_type_mutating_gc(gc!)
+    return NonLinMPC{NT, SE, JM, JEfunc, GCfunc, P}(
+        estim, Hp, Hc, M_Hp, N_Hc, L_Hp, Cwt, Ewt, JE, gc!, nc, p, optim
     )
 end
 
@@ -388,6 +396,9 @@ function get_mutating_gc(NT, gc)
     return gc!
 end
 
+"Get the type of the mutating version of the custom constrain function `gc!`."
+get_type_mutating_gc(::GCfunc) where {GCfunc<:Function} = GCfunc
+
 """
     test_custom_functions(NT, model::SimModel, JE, gc!, nc, Uop, Yop, Dop, p)
 
@@ -514,7 +525,7 @@ function get_optim_functions(mpc::NonLinMPC, ::JuMP.GenericModel{JNT}) where JNT
         gc         = get_tmp(gc_cache, ΔŨ1)
         Ŷ0, x̂0end  = predict!(Ȳ, x̂0, x̂0next, u0, û0, mpc, model, ΔŨ)
         Ue, Ŷe     = extended_predictions!(Ue, Ŷe, Ū, mpc, model, Ŷ0, ΔŨ)
-        ϵ = (nϵ ≠ 0) ? ΔŨ[end] : zero(T) # ϵ = 0 if nϵ == 0 (meaning no relaxation)
+        ϵ = (nϵ ≠ 0) ? ΔŨ[end] : 0 # ϵ = 0 if nϵ == 0 (meaning no relaxation)
         mpc.con.gc!(gc, Ue, Ŷe, mpc.D̂e, mpc.p, ϵ)
         g = con_nonlinprog!(g, mpc, model, x̂0end, Ŷ0, gc, ϵ)
         return obj_nonlinprog!(Ȳ, Ū, mpc, model, Ue, Ŷe, ΔŨ)::T
@@ -533,7 +544,7 @@ function get_optim_functions(mpc::NonLinMPC, ::JuMP.GenericModel{JNT}) where JNT
             gc         = get_tmp(gc_cache, ΔŨ1)
             Ŷ0, x̂0end  = predict!(Ȳ, x̂0, x̂0next, u0, û0, mpc, model, ΔŨ)
             Ue, Ŷe     = extended_predictions!(Ue, Ŷe, Ū, mpc, model, Ŷ0, ΔŨ)
-            ϵ = (nϵ ≠ 0) ? ΔŨ[end] : zero(T) # ϵ = 0 if nϵ == 0 (meaning no relaxation)
+            ϵ = (nϵ ≠ 0) ? ΔŨ[end] : 0 # ϵ = 0 if nϵ == 0 (meaning no relaxation)
             mpc.con.gc!(gc, Ue, Ŷe, mpc.D̂e, mpc.p, ϵ)
             g = con_nonlinprog!(g, mpc, model, x̂0end, Ŷ0, gc, ϵ)
         end