reduce allocations for NonLinMPC

franckgaga · franckgaga · commit 8365f23459a3 · 2023-09-21T17:10:46.000-04:00
in-place `predict!` and `con_nonlinprog!` methods
diff --git a/src/controller/nonlinmpc.jl b/src/controller/nonlinmpc.jl
@@ -269,8 +269,8 @@ function init_optimization!(mpc::NonLinMPC)
             ΔŨ = collect(ΔŨtup)
             if ΔŨtup != last_ΔŨtup_float
                 C = get_tmp(C_cache, ΔŨtup[1])
-                Ŷ[:] = predict(mpc, model, ΔŨ)
-                C[:] = con_nonlinprog(mpc, model, Ŷ, ΔŨ)
+                predict!(Ŷ, mpc, model, ΔŨ)
+                con_nonlinprog!(C, mpc, model, Ŷ, ΔŨ)
                 last_ΔŨtup_float = ΔŨtup
             end
             return obj_nonlinprog(mpc, model, Ŷ, ΔŨ)
@@ -280,8 +280,8 @@ function init_optimization!(mpc::NonLinMPC)
             ΔŨ = collect(ΔŨtup)
             if ΔŨtup != last_ΔŨtup_dual
                 C = get_tmp(C_cache, ΔŨtup[1])
-                Ŷ[:] = predict(mpc, model, ΔŨ)
-                C[:] = con_nonlinprog(mpc, model, Ŷ, ΔŨ)
+                predict!(Ŷ, mpc, model, ΔŨ)
+                con_nonlinprog!(C, mpc, model, Ŷ, ΔŨ)
                 last_ΔŨtup_dual = ΔŨtup
             end
             return obj_nonlinprog(mpc, model, Ŷ, ΔŨ)
@@ -291,8 +291,8 @@ function init_optimization!(mpc::NonLinMPC)
             if ΔŨtup != last_ΔŨtup_float
                 Ŷ = get_tmp(Ŷ_cache, ΔŨtup[1])
                 ΔŨ = collect(ΔŨtup)
-                Ŷ[:] = predict(mpc, model, ΔŨ)
-                C[:] = con_nonlinprog(mpc, model, Ŷ, ΔŨ)
+                predict!(Ŷ, mpc, model, ΔŨ)
+                con_nonlinprog!(C, mpc, model, Ŷ, ΔŨ)
                 last_ΔŨtup_float = ΔŨtup
             end
             return C[i]
@@ -302,8 +302,8 @@ function init_optimization!(mpc::NonLinMPC)
             if ΔŨtup != last_ΔŨtup_dual
                 Ŷ = get_tmp(Ŷ_cache, ΔŨtup[1])
                 ΔŨ = collect(ΔŨtup)
-                Ŷ[:] = predict(mpc, model, ΔŨ)
-                C[:] = con_nonlinprog(mpc, model, Ŷ, ΔŨ)
+                predict!(Ŷ, mpc, model, ΔŨ)
+                con_nonlinprog!(C, mpc, model, Ŷ, ΔŨ)
                 last_ΔŨtup_dual = ΔŨtup
             end
             return C[i]
@@ -351,30 +351,28 @@ end
 
 
 """
-    con_nonlinprog(mpc::NonLinMPC, ::LinModel, ΔŨ::Vector{Real})
+    con_nonlinprog!(C, mpc::NonLinMPC, model::LinModel, ΔŨ::Vector{Real})
 
 Nonlinear constraints for [`NonLinMPC`](@ref) when `model` is a [`LinModel`](@ref).
 """
-function con_nonlinprog(mpc::NonLinMPC, model::LinModel, _, ΔŨ::Vector{T}) where {T<:Real}
-    return zeros(T, 0)
+function con_nonlinprog!(C, ::NonLinMPC, ::LinModel, _ , ::Vector{T}) where {T<:Real}
+    return C
 end
 """
-    con_nonlinprog(mpc::NonLinMPC, model::NonLinModel, ΔŨ::Vector{Real})
+    con_nonlinprog!(C, mpc::NonLinMPC, model::NonLinModel, ΔŨ::Vector{Real})
 
 Nonlinear constrains for [`NonLinMPC`](@ref) when `model` is not a [`LinModel`](@ref).
 """
-function con_nonlinprog(mpc::NonLinMPC, ::SimModel, Ŷ, ΔŨ::Vector{T}) where {T<:Real}
+function con_nonlinprog!(C, mpc::NonLinMPC, model::SimModel, Ŷ, ΔŨ::Vector{T}) where {T<:Real}
+    ny, Hp = model.ny, mpc.Hp
     if !isinf(mpc.C) # constraint softening activated :
         ϵ = ΔŨ[end]
-        C_Ymin = (mpc.con.Ymin - Ŷ) - ϵ*mpc.con.c_Ymin
-        C_Ymax = (Ŷ - mpc.con.Ymax) - ϵ*mpc.con.c_Ymax
+        C[begin:(Hp*ny)] = (mpc.con.Ymin - Ŷ) - ϵ*mpc.con.c_Ymin
+        C[(Hp*ny+1):end] = (Ŷ - mpc.con.Ymax) - ϵ*mpc.con.c_Ymax
     else # no constraint softening :
-        C_Ymin = (mpc.con.Ymin - Ŷ)
-        C_Ymax = (Ŷ - mpc.con.Ymax)
+        C[begin:(Hp*ny)] = (mpc.con.Ymin - Ŷ)
+        C[(Hp*ny+1):end] = (Ŷ - mpc.con.Ymax)
     end
-    # replace -Inf with 0 to avoid INVALID_MODEL error :
-    C_Ymin[isinf.(C_Ymin)] .= 0
-    C_Ymax[isinf.(C_Ymax)] .= 0
-    C = [C_Ymin; C_Ymax]
+    C[isinf.(C)] .= 0 # replace ±Inf with 0 to avoid INVALID_MODEL error
     return C
 end
diff --git a/src/predictive_control.jl b/src/predictive_control.jl
@@ -334,8 +334,8 @@ julia> sol_summary, info = getinfo(mpc); round.(info[:Ŷ], digits=3)
 ```
 """
 function getinfo(mpc::PredictiveController)
-    sol_summary = get_summary(mpc) 
-    Ŷ = predict(mpc, mpc.estim.model, mpc.ΔŨ)
+    sol_summary = get_summary(mpc)
+    Ŷ = similar(mpc.Ŷop)
     info = Dict{Symbol, InfoDictType}()
     info[:ΔU]  = mpc.ΔŨ[1:mpc.Hc*mpc.estim.model.nu]
     info[:ϵ]   = isinf(mpc.C) ? NaN : mpc.ΔŨ[end]
@@ -345,7 +345,7 @@ function getinfo(mpc::PredictiveController)
     info[:d]   = mpc.d
     info[:D̂]   = mpc.D̂
     info[:ŷ]   = mpc.ŷ
-    info[:Ŷ]   = Ŷ
+    info[:Ŷ]   = predict!(Ŷ, mpc, mpc.estim.model, mpc.ΔŨ)
     info[:Ŷs]  = mpc.Ŷop - repeat(mpc.estim.model.yop, mpc.Hp) # Ŷop = Ŷs + Yop
     info[:R̂y]  = mpc.R̂y
     info[:R̂u]  = mpc.R̂u
@@ -461,34 +461,34 @@ end
 predictstoch!(mpc, estim::StateEstimator, _ , _ ) = nothing
 
 @doc raw"""
-    predict(mpc::PredictiveController, model::LinModel, ΔŨ)
+    predict!(Ŷ, mpc::PredictiveController, model::LinModel, ΔŨ) -> Ŷ
 
 Evaluate the outputs predictions ``\mathbf{Ŷ}`` when `model` is a [`LinModel`](@ref).
 """
-function predict(mpc::PredictiveController, ::LinModel, ΔŨ::Vector{T}) where {T<:Real}
-    return mpc.Ẽ*ΔŨ + mpc.F
+function predict!(Ŷ, mpc::PredictiveController, ::LinModel, ΔŨ::Vector{T}) where {T<:Real}
+    Ŷ[:] = mpc.Ẽ*ΔŨ + mpc.F
+    return Ŷ
 end
 
-# TODO: remplacer S_p et T_Hp par un if pour éviter les allocs
 @doc raw"""
-    predict(mpc::PredictiveController, model::SimModel, ΔŨ)
+    predict!(Ŷ, mpc::PredictiveController, model::SimModel, ΔŨ) -> Ŷ
 
 Evaluate  ``\mathbf{Ŷ}`` when `model` is not a [`LinModel`](@ref).
 """
-function predict(mpc::PredictiveController, model::SimModel, ΔŨ::Vector{T}) where {T<:Real}
-    nu, ny, nd, Hp = model.nu, model.ny, model.nd, mpc.Hp
-    yop, dop = model.yop, model.dop
-    U0 = mpc.S̃_Hp*ΔŨ + mpc.T_Hp*mpc.estim.lastu0
-    Ŷ ::Vector{T} = copy(mpc.Ŷop) # Ŷop = Ŷs + Yop
-    u0::Vector{T} = Vector{T}(undef, nu)
+function predict!(Ŷ, mpc::PredictiveController, model::SimModel, ΔŨ::Vector{T}) where {T<:Real}
+    nu, ny, nd, Hp, Hc = model.nu, model.ny, model.nd, mpc.Hp, mpc.Hc
     x̂ ::Vector{T} = copy(mpc.estim.x̂)
-    d0 = mpc.d - dop
+    u0::Vector{T} = copy(mpc.estim.lastu0)
+    d0 = mpc.d - model.dop
     for j=1:Hp
-        u0[:] = @views U0[(1 + nu*(j-1)):(nu*j)]
+        if j ≤ Hc
+            u0[:] += @views ΔŨ[(1 + nu*(j-1)):(nu*j)]
+        end
         x̂[:]  = f̂(mpc.estim, x̂, u0, d0)
-        d0[:] = @views mpc.D̂[(1 + nd*(j-1)):(nd*j)] - dop
-        Ŷ[(1 + ny*(j-1)):(ny*j)] += ĥ(mpc.estim, x̂, d0)
+        d0[:] = @views mpc.D̂[(1 + nd*(j-1)):(nd*j)] - model.dop
+        Ŷ[(1 + ny*(j-1)):(ny*j)] = ĥ(mpc.estim, x̂, d0)
     end
+    Ŷ[:] += mpc.Ŷop # Ŷop = Ŷs + Yop, and Ŷs=0 if mpc.estim is not an InternalModel
     return Ŷ
 end
 
