Merge pull request #132 from JuliaControl/reduce_alloc_nmpc

Added: `iszero_W` field in for the 4 weights in `ControllerWeights` struct

Author: franckgaga

src/controller/construct.jl

@@ -4,6 +4,10 @@ struct ControllerWeights{NT<:Real}
     Ñ_Hc::Hermitian{NT, Matrix{NT}}
     L_Hp::Hermitian{NT, Matrix{NT}}
     E   ::NT
+    iszero_M_Hp::Vector{Bool}
+    iszero_Ñ_Hc::Vector{Bool}
+    iszero_L_Hp::Vector{Bool}
+    iszero_E::Bool
     function ControllerWeights{NT}(
         model, Hp, Hc, M_Hp, N_Hc, L_Hp, Cwt=Inf, Ewt=0
     ) where NT<:Real
@@ -21,8 +25,12 @@ struct ControllerWeights{NT<:Real}
             # ΔŨ = ΔU (only hard constraints)
             Ñ_Hc = N_Hc
         end   
-        E = Ewt         
-        return new{NT}(M_Hp, Ñ_Hc, L_Hp, E)
+        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)
     end
 end
 

src/controller/execute.jl

@@ -179,7 +179,7 @@ They are computed with these equations using in-place operations:
 \begin{aligned}
     \mathbf{F}       &= \mathbf{G d_0}(k) + \mathbf{J D̂_0} + \mathbf{K x̂_0}(k) 
                             + \mathbf{V u_0}(k-1) + \mathbf{B} + \mathbf{Ŷ_s}           \\
-    \mathbf{C_y}     &= \mathbf{F}                 - (\mathbf{R̂_y - Y_{op}})            \\
+    \mathbf{C_y}     &= \mathbf{F}                   - (\mathbf{R̂_y - Y_{op}})          \\
     \mathbf{C_u}     &= \mathbf{T} \mathbf{u_0}(k-1) - (\mathbf{R̂_u - U_{op}})          \\
     \mathbf{q̃}       &= 2[(\mathbf{M}_{H_p} \mathbf{Ẽ})' \mathbf{C_y} 
                             + (\mathbf{L}_{H_p} \mathbf{S̃})' \mathbf{C_u}]              \\
@@ -193,32 +193,38 @@ function initpred!(mpc::PredictiveController, model::LinModel, d, D̂, R̂y, R̂
     ŷ, 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 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
+    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)        # F = F + G*d0
-        mul!(F, mpc.J, mpc.D̂0, 1, 1)        # F = F + J*D̂0
+        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
+    q̃ .= 0
+    r .= 0
     # --- output setpoint tracking term ---
     mpc.R̂y .= R̂y
-    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.weights.iszero_M_Hp[]
+        Cy .= F .- (R̂y .- mpc.Yop)
+        mul!(M_Hp_Ẽ, mpc.weights.M_Hp, mpc.Ẽ)
+        mul!(q̃, M_Hp_Ẽ', Cy, 1, 1)              # q̃ = q̃ + M_Hp*Ẽ'*Cy
+        r .+= dot(Cy, mpc.weights.M_Hp, Cy)     # r = r + Cy'*M_Hp*Cy
+    end
     # --- 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)
+    if !mpc.weights.iszero_L_Hp[]
+        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)     # r = r + Cu'*L_Hp*Cu
+    end
     # --- finalize ---
-    lmul!(2, q̃)                             # q̃ = 2*q̃
+    lmul!(2, q̃)                                 # q̃ = 2*q̃
     return nothing
 end
 
@@ -230,7 +236,7 @@ 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 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
@@ -412,22 +418,36 @@ function obj_nonlinprog!(
 ) where NT<:Real
     nu, ny = model.nu, model.ny
     # --- output setpoint tracking term ---
-    Ȳ  .= @views Ŷe[ny+1:end]
-    Ȳ  .= mpc.R̂y .- Ȳ
-    JR̂y = dot(Ȳ, mpc.weights.M_Hp, Ȳ)
+    if mpc.weights.iszero_M_Hp[]
+        JR̂y = zero(NT)
+    else
+        Ȳ  .= @views Ŷe[ny+1:end]
+        Ȳ  .= mpc.R̂y .- Ȳ
+        JR̂y = dot(Ȳ, mpc.weights.M_Hp, Ȳ)
+    end
     # --- move suppression and slack variable term ---
-    JΔŨ = dot(ΔŨ, mpc.weights.Ñ_Hc, ΔŨ)
+    if mpc.weights.iszero_Ñ_Hc[]
+        JΔŨ = zero(NT)
+    else
+        JΔŨ = dot(ΔŨ, mpc.weights.Ñ_Hc, ΔŨ)
+    end
     # --- input setpoint tracking term ---
-    Ū  .= @views Ue[1:end-nu]
-    Ū  .= mpc.R̂u .- Ū
-    JR̂u = dot(Ū, mpc.weights.L_Hp, Ū)
+    if mpc.weights.iszero_L_Hp[]
+        JR̂u = zero(NT)
+    else
+        Ū  .= @views Ue[1:end-nu]
+        Ū  .= mpc.R̂u .- Ū
+        JR̂u = dot(Ū, mpc.weights.L_Hp, Ū)
+    end
     # --- economic term ---
     E_JE = obj_econ(mpc, model, Ue, Ŷe)
     return JR̂y + JΔŨ + JR̂u + E_JE
 end
 
 "By default, the economic term is zero."
-obj_econ(::PredictiveController, ::SimModel, _ , _ ) = 0.0
+function obj_econ(::PredictiveController, ::SimModel, _ , ::AbstractVector{NT}) where NT
+    return zero(NT)
+end
 
 @doc raw"""
     optim_objective!(mpc::PredictiveController) -> ΔŨ
@@ -615,35 +635,41 @@ function setmodel!(
         for i=1:ny*Hp
             mpc.weights.M_Hp[i, i] = Mwt[(i-1) % ny + 1]
         end
+        mpc.weights.iszero_M_Hp[] = iszero(mpc.weights.M_Hp)
     elseif !isnothing(M_Hp)
         M_Hp = to_hermitian(M_Hp)
         nŶ = ny*Hp
         size(M_Hp) == (nŶ, nŶ) || throw(ArgumentError("M_Hp size should be ($nŶ, $nŶ)"))
         mpc.weights.M_Hp .= M_Hp
+        mpc.weights.iszero_M_Hp[] = iszero(mpc.weights.M_Hp)
     end
     if isnothing(Ñ_Hc) && !isnothing(Nwt)
         size(Nwt) == (nu,) || throw(ArgumentError("Nwt should be a vector of length $nu"))
         any(x -> x < 0, Nwt) && throw(ArgumentError("Nwt values should be nonnegative"))
         for i=1:nu*Hc
             mpc.weights.Ñ_Hc[i, i] = Nwt[(i-1) % nu + 1]
         end
+        mpc.weights.iszero_Ñ_Hc[] = iszero(mpc.weights.Ñ_Hc)
     elseif !isnothing(Ñ_Hc)
         Ñ_Hc = to_hermitian(Ñ_Hc)
         nΔŨ = nu*Hc+nϵ
         size(Ñ_Hc) == (nΔŨ, nΔŨ) || throw(ArgumentError("Ñ_Hc size should be ($nΔŨ, $nΔŨ)"))
         mpc.weights.Ñ_Hc .= Ñ_Hc
+        mpc.weights.iszero_Ñ_Hc[] = iszero(mpc.weights.Ñ_Hc)
     end
     if isnothing(L_Hp) && !isnothing(Lwt)
         size(Lwt) == (nu,) || throw(ArgumentError("Lwt should be a vector of length $nu"))
         any(x -> x < 0, Lwt) && throw(ArgumentError("Lwt values should be nonnegative"))
         for i=1:nu*Hp
             mpc.weights.L_Hp[i, i] = Lwt[(i-1) % nu + 1]
         end
+        mpc.weights.iszero_L_Hp[] = iszero(mpc.weights.L_Hp)
     elseif !isnothing(L_Hp)
         L_Hp = to_hermitian(L_Hp)
         nU = nu*Hp
         size(L_Hp) == (nU, nU) || throw(ArgumentError("L_Hp size should be ($nU, $nU)"))
         mpc.weights.L_Hp .= L_Hp
+        mpc.weights.iszero_L_Hp[] = iszero(mpc.weights.L_Hp)
     end
     setmodel_controller!(mpc, x̂op_old)
     return mpc

src/controller/nonlinmpc.jl

@@ -525,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] : 0 # ϵ = 0 if nϵ == 0 (meaning no relaxation)
+        ϵ = (nϵ ≠ 0) ? ΔŨ[end] : zero(T) # ϵ = 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
@@ -544,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] : 0 # ϵ = 0 if nϵ == 0 (meaning no relaxation)
+            ϵ = (nϵ ≠ 0) ? ΔŨ[end] : zero(T) # ϵ = 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
@@ -702,7 +702,9 @@ function con_nonlinprog!(g, mpc::NonLinMPC, ::SimModel, x̂0end, Ŷ0, gc, ϵ)
 end
 
 "Evaluate the economic term `E*JE` of the objective function for [`NonLinMPC`](@ref)."
-function obj_econ(mpc::NonLinMPC, model::SimModel, Ue, Ŷe)
-    E_JE = iszero(mpc.weights.E) ? 0.0 : mpc.weights.E*mpc.JE(Ue, Ŷe, mpc.D̂e, mpc.p)
+function obj_econ(
+    mpc::NonLinMPC, model::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)
     return E_JE
 end