Merge pull request #202 from JuliaControl/efficient_conversions

added: be more efficient with weights and conversion matrices for `NonLinMPC`

Author: franckgaga

Project.toml

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

src/controller/construct.jl

@@ -33,41 +33,64 @@ function PredictiveControllerBuffer(
 end
 
 "Include all the objective function weights of [`PredictiveController`](@ref)"
-struct ControllerWeights{NT<:Real}
-    M_Hp::Hermitian{NT, Matrix{NT}}
-    Ñ_Hc::Hermitian{NT, Matrix{NT}}
-    L_Hp::Hermitian{NT, Matrix{NT}}
+struct ControllerWeights{
+    NT<:Real,
+    # parameters to support both dense and Diagonal matrices (with specialization):
+    MW<:AbstractMatrix{NT}, 
+    NW<:AbstractMatrix{NT},  
+    LW<:AbstractMatrix{NT}, 
+}
+    M_Hp::Hermitian{NT, MW}
+    Ñ_Hc::Hermitian{NT, NW}
+    L_Hp::Hermitian{NT, LW}
     E   ::NT
     iszero_M_Hp::Vector{Bool}
     iszero_Ñ_Hc::Vector{Bool}
     iszero_L_Hp::Vector{Bool}
     iszero_E::Bool
+    isinf_C ::Bool
     function ControllerWeights{NT}(
-        model, Hp, Hc, M_Hp, N_Hc, L_Hp, Cwt=Inf, Ewt=0
-    ) where NT<:Real
+        model, Hp, Hc, M_Hp::MW, N_Hc::NW, L_Hp::LW, Cwt=Inf, Ewt=0
+    ) where {
+        NT<:Real, 
+        MW<:AbstractMatrix{NT}, 
+        NW<:AbstractMatrix{NT}, 
+        LW<:AbstractMatrix{NT}
+    }
         validate_weights(model, Hp, Hc, M_Hp, N_Hc, L_Hp, Cwt, Ewt)
-        # convert `Diagonal` to normal `Matrix` if required:
-        M_Hp = Hermitian(convert(Matrix{NT}, M_Hp), :L) 
-        N_Hc = Hermitian(convert(Matrix{NT}, N_Hc), :L)
-        L_Hp = Hermitian(convert(Matrix{NT}, L_Hp), :L)
         nΔU = size(N_Hc, 1)
         C = Cwt
-        if !isinf(Cwt)  
+        isinf_C = isinf(C)
+        if !isinf_C  
             # ΔŨ = [ΔU; ϵ] (ϵ is the slack variable)
-            Ñ_Hc = Hermitian([N_Hc zeros(NT, nΔU, 1); zeros(NT, 1, nΔU) C], :L)
+            Ñ_Hc = [N_Hc zeros(NT, nΔU, 1); zeros(NT, 1, nΔU) C]
+            isdiag(N_Hc) && (Ñ_Hc = Diagonal(Ñ_Hc)) # NW(Ñ_Hc) does not work on Julia 1.10
         else
             # ΔŨ = ΔU (only hard constraints)
             Ñ_Hc = N_Hc
-        end   
+        end
+        M_Hp = Hermitian(M_Hp, :L)
+        Ñ_Hc = Hermitian(Ñ_Hc, :L)
+        L_Hp = Hermitian(L_Hp, :L)
         E = Ewt
         iszero_M_Hp = [iszero(M_Hp)]
         iszero_Ñ_Hc = [iszero(Ñ_Hc)]
         iszero_L_Hp = [iszero(L_Hp)]
         iszero_E = iszero(E)
-        return new{NT}(M_Hp, Ñ_Hc, L_Hp, E, iszero_M_Hp, iszero_Ñ_Hc, iszero_L_Hp, iszero_E)
+        return new{NT, MW, NW, LW}(
+            M_Hp, Ñ_Hc, L_Hp, E, 
+            iszero_M_Hp, iszero_Ñ_Hc, iszero_L_Hp, iszero_E, isinf_C
+        )
     end
 end
 
+"Outer constructor to convert weight matrix number type to `NT` if necessary."
+function ControllerWeights{NT}(
+        model, Hp, Hc, M_Hp::MW, N_Hc::NW, L_Hp::LW, Cwt=Inf, Ewt=0
+    ) where {NT<:Real, MW<:AbstractMatrix, NW<:AbstractMatrix, LW<:AbstractMatrix}
+    return ControllerWeights{NT}(model, Hp, Hc, NT.(M_Hp), NT.(N_Hc), NT.(L_Hp), Cwt, Ewt)
+end
+
 "Include all the data for the constraints of [`PredictiveController`](@ref)"
 struct ControllerConstraint{NT<:Real, GCfunc<:Union{Nothing, Function}}
     # matrices for the terminal constraints:
@@ -478,8 +501,10 @@ end
 
 """
     init_defaultcon_mpc(
-        estim::StateEstimator, transcription::TranscriptionMethod,
-        Hp, Hc, C, 
+        estim::StateEstimator, 
+        weights::ControllerWeights
+        transcription::TranscriptionMethod,
+        Hp, Hc, 
         PΔu, Pu, E, 
         ex̂, fx̂, gx̂, jx̂, kx̂, vx̂, bx̂, 
         Eŝ, Fŝ, Gŝ, Jŝ, Kŝ, Vŝ, Bŝ,
@@ -491,16 +516,18 @@ Init `ControllerConstraint` struct with default parameters based on estimator `e
 Also return `P̃Δu`, `P̃u`, `Ẽ` and `Ẽŝ` matrices for the the augmented decision vector `Z̃`.
 """
 function init_defaultcon_mpc(
-    estim::StateEstimator{NT}, transcription::TranscriptionMethod,
-    Hp,  Hc, C, 
+    estim::StateEstimator{NT}, 
+    weights::ControllerWeights,
+    transcription::TranscriptionMethod,
+    Hp,  Hc, 
     PΔu, Pu, E, 
     ex̂, fx̂, gx̂, jx̂, kx̂, vx̂, bx̂, 
     Eŝ, Fŝ, Gŝ, Jŝ, Kŝ, Vŝ, Bŝ,
     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
+    nϵ = weights.isinf_C ? 0 : 1
     u0min,      u0max   = fill(convert(NT,-Inf), nu), fill(convert(NT,+Inf), nu)
     Δumin,      Δumax   = fill(convert(NT,-Inf), nu), fill(convert(NT,+Inf), nu)
     y0min,      y0max   = fill(convert(NT,-Inf), ny), fill(convert(NT,+Inf), ny)

src/controller/execute.jl

@@ -212,7 +212,7 @@ function initpred!(mpc::PredictiveController, model::LinModel, d, D̂, R̂y, R̂
         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
-    Cy, Cu, M_Hp_Ẽ, L_Hp_P̃U = mpc.buffer.Ŷ, mpc.buffer.U, mpc.buffer.Ẽ, mpc.buffer.P̃u
+    Cy, Cu, M_Hp_Ẽ, L_Hp_P̃u = mpc.buffer.Ŷ, mpc.buffer.U, mpc.buffer.Ẽ, mpc.buffer.P̃u
     q̃, r = mpc.q̃, mpc.r
     q̃ .= 0
     r .= 0
@@ -226,8 +226,8 @@ function initpred!(mpc::PredictiveController, model::LinModel, d, D̂, R̂y, R̂
     # --- input setpoint tracking term ---
     if !mpc.weights.iszero_L_Hp[]
         Cu .= mpc.Tu_lastu0 .+ mpc.Uop .- R̂u 
-        mul!(L_Hp_P̃U, mpc.weights.L_Hp, mpc.P̃u)
-        mul!(q̃, L_Hp_P̃U', Cu, 1, 1)             # q̃ = q̃ + L_Hp*P̃u'*Cu
+        mul!(L_Hp_P̃u, mpc.weights.L_Hp, mpc.P̃u)
+        mul!(q̃, L_Hp_P̃u', Cu, 1, 1)             # q̃ = q̃ + L_Hp*P̃u'*Cu
         r .+= dot(Cu, mpc.weights.L_Hp, Cu)     # r = r + Cu'*L_Hp*Cu
     end
     # --- finalize ---

src/controller/explicitmpc.jl

@@ -1,12 +1,16 @@
-struct ExplicitMPC{NT<:Real, SE<:StateEstimator} <: PredictiveController{NT}
+struct ExplicitMPC{
+    NT<:Real, 
+    SE<:StateEstimator, 
+    CW<:ControllerWeights
+} <: PredictiveController{NT}
     estim::SE
     transcription::SingleShooting
     Z̃::Vector{NT}
     ŷ::Vector{NT}
     Hp::Int
     Hc::Int
     nϵ::Int
-    weights::ControllerWeights{NT}
+    weights::CW
     R̂u::Vector{NT}
     R̂y::Vector{NT}
     P̃Δu::Matrix{NT}
@@ -34,17 +38,12 @@ struct ExplicitMPC{NT<:Real, SE<:StateEstimator} <: PredictiveController{NT}
     Dop::Vector{NT}
     buffer::PredictiveControllerBuffer{NT}
     function ExplicitMPC{NT}(
-        estim::SE, Hp, Hc, M_Hp, N_Hc, L_Hp
-    ) where {NT<:Real, SE<:StateEstimator}
+        estim::SE, Hp, Hc, weights::CW
+    ) where {NT<:Real, SE<:StateEstimator, CW<:ControllerWeights}
         model = estim.model
         nu, ny, nd, nx̂ = model.nu, model.ny, model.nd, estim.nx̂
         ŷ = copy(model.yop) # dummy vals (updated just before optimization)
         nϵ = 0    # no slack variable ϵ for ExplicitMPC
-        weights = ControllerWeights{NT}(model, Hp, Hc, M_Hp, N_Hc, L_Hp)
-        # convert `Diagonal` to normal `Matrix` if required:
-        M_Hp = Hermitian(convert(Matrix{NT}, M_Hp), :L) 
-        N_Hc = Hermitian(convert(Matrix{NT}, N_Hc), :L)
-        L_Hp = Hermitian(convert(Matrix{NT}, L_Hp), :L)
         # dummy vals (updated just before optimization):
         R̂y, R̂u, Tu_lastu0 = zeros(NT, ny*Hp), zeros(NT, nu*Hp), zeros(NT, nu*Hp)
         transcription = SingleShooting() # explicit MPC only supports SingleShooting
@@ -53,7 +52,7 @@ struct ExplicitMPC{NT<:Real, SE<:StateEstimator} <: PredictiveController{NT}
         E, G, J, K, V, B = init_predmat(model, estim, transcription, Hp, Hc)
         # dummy val (updated just before optimization):
         F, fx̂  = zeros(NT, ny*Hp), zeros(NT, nx̂)
-        P̃Δu, P̃u, Ñ_Hc, Ẽ = PΔu, Pu, N_Hc, E # no slack variable ϵ for ExplicitMPC
+        P̃Δu, P̃u, Ẽ = PΔu, Pu, E # no slack variable ϵ for ExplicitMPC
         H̃ = init_quadprog(model, weights, Ẽ, P̃Δu, P̃u)
         # dummy vals (updated just before optimization):
         q̃, r = zeros(NT, size(H̃, 1)), zeros(NT, 1)
@@ -65,7 +64,7 @@ struct ExplicitMPC{NT<:Real, SE<:StateEstimator} <: PredictiveController{NT}
         nZ̃ = get_nZ(estim, transcription, Hp, Hc) + nϵ
         Z̃ = zeros(NT, nZ̃)
         buffer = PredictiveControllerBuffer(estim, transcription, Hp, Hc, nϵ)
-        mpc = new{NT, SE}(
+        mpc = new{NT, SE, CW}(
             estim,
             transcription,
             Z̃, ŷ,
@@ -131,9 +130,9 @@ function ExplicitMPC(
     Mwt = fill(DEFAULT_MWT, model.ny),
     Nwt = fill(DEFAULT_NWT, model.nu),
     Lwt = fill(DEFAULT_LWT, model.nu),
-    M_Hp = diagm(repeat(Mwt, Hp)),
-    N_Hc = diagm(repeat(Nwt, Hc)),
-    L_Hp = diagm(repeat(Lwt, Hp)),
+    M_Hp = Diagonal(repeat(Mwt, Hp)),
+    N_Hc = Diagonal(repeat(Nwt, Hc)),
+    L_Hp = Diagonal(repeat(Lwt, Hp)),
     kwargs...
 ) 
     estim = SteadyKalmanFilter(model; kwargs...)
@@ -154,17 +153,18 @@ function ExplicitMPC(
     Mwt  = fill(DEFAULT_MWT, estim.model.ny),
     Nwt  = fill(DEFAULT_NWT, estim.model.nu),
     Lwt  = fill(DEFAULT_LWT, estim.model.nu),
-    M_Hp = diagm(repeat(Mwt, Hp)),
-    N_Hc = diagm(repeat(Nwt, Hc)),
-    L_Hp = diagm(repeat(Lwt, Hp)),
+    M_Hp = Diagonal(repeat(Mwt, Hp)),
+    N_Hc = Diagonal(repeat(Nwt, Hc)),
+    L_Hp = Diagonal(repeat(Lwt, Hp)),
 ) where {NT<:Real, SE<:StateEstimator{NT}}
     isa(estim.model, LinModel) || error("estim.model type must be a LinModel") 
     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 ExplicitMPC{NT}(estim, Hp, Hc, M_Hp, N_Hc, L_Hp)
+    weights = ControllerWeights{NT}(estim.model, Hp, Hc, M_Hp, N_Hc, L_Hp)
+    return ExplicitMPC{NT}(estim, Hp, Hc, weights)
 end
 
 setconstraint!(::ExplicitMPC; kwargs...) = error("ExplicitMPC does not support constraints.")

src/controller/linmpc.jl

@@ -4,6 +4,7 @@ const DEFAULT_LINMPC_TRANSCRIPTION = SingleShooting()
 struct LinMPC{
     NT<:Real, 
     SE<:StateEstimator, 
+    CW<:ControllerWeights,
     TM<:TranscriptionMethod,
     JM<:JuMP.GenericModel
 } <: PredictiveController{NT}
@@ -18,7 +19,7 @@ struct LinMPC{
     Hp::Int
     Hc::Int
     nϵ::Int
-    weights::ControllerWeights{NT}
+    weights::CW
     R̂u::Vector{NT}
     R̂y::Vector{NT}
     P̃Δu::Matrix{NT}
@@ -45,13 +46,18 @@ struct LinMPC{
     Dop::Vector{NT}
     buffer::PredictiveControllerBuffer{NT}
     function LinMPC{NT}(
-        estim::SE, Hp, Hc, M_Hp, N_Hc, L_Hp, Cwt, 
+        estim::SE, Hp, Hc, weights::CW, 
         transcription::TM, optim::JM
-    ) where {NT<:Real, SE<:StateEstimator, TM<:TranscriptionMethod, JM<:JuMP.GenericModel}
+    ) where {
+            NT<:Real, 
+            SE<:StateEstimator, 
+            CW<:ControllerWeights,
+            TM<:TranscriptionMethod, 
+            JM<:JuMP.GenericModel
+        }
         model = estim.model
         nu, ny, nd, nx̂ = model.nu, model.ny, model.nd, estim.nx̂
         ŷ = copy(model.yop) # dummy vals (updated just before optimization)
-        weights = ControllerWeights{NT}(model, Hp, Hc, M_Hp, N_Hc, L_Hp, Cwt)
         # dummy vals (updated just before optimization):
         R̂y, R̂u, Tu_lastu0 = zeros(NT, ny*Hp), zeros(NT, nu*Hp), zeros(NT, nu*Hp)
         PΔu = init_ZtoΔU(estim, transcription, Hp, Hc)
@@ -63,8 +69,9 @@ struct LinMPC{
         # dummy vals (updated just before optimization):
         F, fx̂, Fŝ  = zeros(NT, ny*Hp), zeros(NT, nx̂), zeros(NT, nx̂*Hp)
         con, nϵ, P̃Δu, P̃u, Ẽ, Ẽŝ = init_defaultcon_mpc(
-            estim, transcription,
-            Hp, Hc, Cwt, PΔu, Pu, E, 
+            estim, weights, transcription,
+            Hp, Hc, 
+            PΔu, Pu, E, 
             ex̂, fx̂, gx̂, jx̂, kx̂, vx̂, bx̂, 
             Eŝ, Fŝ, Gŝ, Jŝ, Kŝ, Vŝ, Bŝ
         )
@@ -78,7 +85,7 @@ struct LinMPC{
         nZ̃ = get_nZ(estim, transcription, Hp, Hc) + nϵ
         Z̃ = zeros(NT, nZ̃)
         buffer = PredictiveControllerBuffer(estim, transcription, Hp, Hc, nϵ)
-        mpc = new{NT, SE, TM, JM}(
+        mpc = new{NT, SE, CW, TM, JM}(
             estim, transcription, optim, con,
             Z̃, ŷ,
             Hp, Hc, nϵ,
@@ -140,9 +147,9 @@ arguments. This controller allocates memory at each time step for the optimizati
 - `Mwt=fill(1.0,model.ny)` : main diagonal of ``\mathbf{M}`` weight matrix (vector).
 - `Nwt=fill(0.1,model.nu)` : main diagonal of ``\mathbf{N}`` weight matrix (vector).
 - `Lwt=fill(0.0,model.nu)` : main diagonal of ``\mathbf{L}`` weight matrix (vector).
-- `M_Hp=diagm(repeat(Mwt,Hp))` : positive semidefinite symmetric matrix ``\mathbf{M}_{H_p}``.
-- `N_Hc=diagm(repeat(Nwt,Hc))` : positive semidefinite symmetric matrix ``\mathbf{N}_{H_c}``.
-- `L_Hp=diagm(repeat(Lwt,Hp))` : positive semidefinite symmetric matrix ``\mathbf{L}_{H_p}``.
+- `M_Hp=Diagonal(repeat(Mwt,Hp))` : positive semidefinite symmetric matrix ``\mathbf{M}_{H_p}``.
+- `N_Hc=Diagonal(repeat(Nwt,Hc))` : positive semidefinite symmetric matrix ``\mathbf{N}_{H_c}``.
+- `L_Hp=Diagonal(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.
 - `transcription=SingleShooting()` : a [`TranscriptionMethod`](@ref) for the optimization.
 - `optim=JuMP.Model(OSQP.MathOptInterfaceOSQP.Optimizer)` : quadratic optimizer used in
@@ -199,9 +206,9 @@ function LinMPC(
     Mwt  = fill(DEFAULT_MWT, model.ny),
     Nwt  = fill(DEFAULT_NWT, model.nu),
     Lwt  = fill(DEFAULT_LWT, model.nu),
-    M_Hp = diagm(repeat(Mwt, Hp)),
-    N_Hc = diagm(repeat(Nwt, Hc)),
-    L_Hp = diagm(repeat(Lwt, Hp)),
+    M_Hp = Diagonal(repeat(Mwt, Hp)),
+    N_Hc = Diagonal(repeat(Nwt, Hc)),
+    L_Hp = Diagonal(repeat(Lwt, Hp)),
     Cwt = DEFAULT_CWT,
     transcription::TranscriptionMethod = DEFAULT_LINMPC_TRANSCRIPTION,
     optim::JuMP.GenericModel = JuMP.Model(DEFAULT_LINMPC_OPTIMIZER, add_bridges=false),
@@ -242,9 +249,9 @@ function LinMPC(
     Mwt  = fill(DEFAULT_MWT, estim.model.ny),
     Nwt  = fill(DEFAULT_NWT, estim.model.nu),
     Lwt  = fill(DEFAULT_LWT, estim.model.nu),
-    M_Hp = diagm(repeat(Mwt, Hp)),
-    N_Hc = diagm(repeat(Nwt, Hc)),
-    L_Hp = diagm(repeat(Lwt, Hp)),
+    M_Hp = Diagonal(repeat(Mwt, Hp)),
+    N_Hc = Diagonal(repeat(Nwt, Hc)),
+    L_Hp = Diagonal(repeat(Lwt, Hp)),
     Cwt  = DEFAULT_CWT,
     transcription::TranscriptionMethod = DEFAULT_LINMPC_TRANSCRIPTION,
     optim::JM = JuMP.Model(DEFAULT_LINMPC_OPTIMIZER, add_bridges=false),
@@ -255,7 +262,8 @@ function LinMPC(
         @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 LinMPC{NT}(estim, Hp, Hc, M_Hp, N_Hc, L_Hp, Cwt, transcription, optim)
+    weights = ControllerWeights{NT}(estim.model, Hp, Hc, M_Hp, N_Hc, L_Hp, Cwt)
+    return LinMPC{NT}(estim, Hp, Hc, weights, transcription, optim)
 end
 
 """