Skip to content

Commit a0a37f8

Browse files
franckgagafranckgaga
committed
reduce allocation all PredictiveControllers
1 parent dbd1cd1 commit a0a37f8

File tree

4 files changed

+37
-37
lines changed

4 files changed

+37
-37
lines changed

‎src/controller/execute.jl

+16-22
Original file line numberDiff line numberDiff line change
@@ -106,9 +106,9 @@ function getinfo(mpc::PredictiveController{NT}) where NT<:Real
106106
info = Dict{Symbol, Union{JuMP._SolutionSummary, Vector{NT}, NT}}()
107107
Ŷ, x̂, u = similar(mpc.Ŷop), similar(mpc.estim.x̂), similar(mpc.estim.lastu0)
108108
Ŷ, x̂end = predict!(Ŷ, x̂, u, mpc, mpc.estim.model, mpc.ΔŨ)
109-
U = mpc.S̃*mpc.ΔŨ + mpc.T*(mpc.estim.lastu0 + mpc.estim.model.uop)
109+
U = mpc.S̃*mpc.ΔŨ + mpc.T_lastu
110110
Ȳ, Ū = similar(Ŷ), similar(U)
111-
J = obj_nonlinprog!(Ȳ, Ū, u, mpc, mpc.estim.model, Ŷ, mpc.ΔŨ)
111+
J = obj_nonlinprog!(Ȳ, Ū, mpc, mpc.estim.model, Ŷ, mpc.ΔŨ)
112112
info[:ΔU] = mpc.ΔŨ[1:mpc.Hc*mpc.estim.model.nu]
113113
info[:ϵ] = isinf(mpc.C) ? NaN : mpc.ΔŨ[end]
114114
info[:J] = J
@@ -155,6 +155,7 @@ They are computed with these equations using in-place operations:
155155
```
156156
"""
157157
function initpred!(mpc::PredictiveController, model::LinModel, d, ym, D̂, R̂y, R̂u)
158+
mul!(mpc.T_lastu, mpc.T, mpc.estim.lastu0 .+ model.uop)
158159
ŷ, F, q̃, p = mpc.ŷ, mpc.F, mpc.q̃, mpc.p
159160
ŷ .= evalŷ(mpc.estim, ym, d)
160161
predictstoch!(mpc, mpc.estim, d, ym) # init mpc.Ŷop for InternalModel
@@ -177,8 +178,7 @@ function initpred!(mpc::PredictiveController, model::LinModel, d, ym, D̂, R̂y,
177178
p .= dot(C_y, mpc.M_Hp, C_y)
178179
if ~mpc.noR̂u
179180
mpc.R̂u .= R̂u
180-
lastu = mpc.estim.lastu0 + model.uop
181-
C_u = mpc.T*lastu - mpc.R̂u
181+
C_u = mpc.T_lastu .- mpc.R̂u
182182
mpc.q̃ .+= lmul!(2, (mpc.L_Hp*mpc.S̃)'*C_u)
183183
mpc.p .+= dot(C_y, mpc.L_Hp, C_u)
184184
end
@@ -195,6 +195,7 @@ Init `ŷ, Ŷop, d0, D̂0` matrices when model is not a [`LinModel`](@ref).
195195
[`InternalModel`](@ref).
196196
"""
197197
function initpred!(mpc::PredictiveController, model::SimModel, d, ym, D̂, R̂y, R̂u)
198+
mul!(mpc.T_lastu, mpc.T, mpc.estim.lastu0 .+ model.uop)
198199
mpc.ŷ .= evalŷ(mpc.estim, ym, d)
199200
predictstoch!(mpc, mpc.estim, d, ym) # init mpc.Ŷop for InternalModel
200201
if model.nd ≠ 0
@@ -256,10 +257,9 @@ function linconstraint!(mpc::PredictiveController, model::LinModel)
256257
fx̂ .+= mul!(fx̂_LHS, mpc.con.gx̂, mpc.d0)
257258
fx̂ .+= mul!(fx̂_LHS, mpc.con.jx̂, mpc.D̂0)
258259
end
259-
T_lastu = mpc.T*(mpc.estim.lastu0 .+ model.uop)
260260
mpc.con.b .= [
261-
-mpc.con.Umin + T_lastu
262-
+mpc.con.Umax - T_lastu
261+
-mpc.con.Umin + mpc.T_lastu
262+
+mpc.con.Umax - mpc.T_lastu
263263
-mpc.con.ΔŨmin
264264
+mpc.con.ΔŨmax
265265
-mpc.con.Ymin + mpc.F
@@ -273,10 +273,9 @@ end
273273

274274
"Set `b` excluding predicted output constraints when `model` is not a [`LinModel`](@ref)."
275275
function linconstraint!(mpc::PredictiveController, model::SimModel)
276-
T_lastu = mpc.T*(mpc.estim.lastu0 .+ model.uop)
277276
mpc.con.b .= [
278-
-mpc.con.Umin + T_lastu
279-
+mpc.con.Umax - T_lastu
277+
-mpc.con.Umin + mpc.T_lastu
278+
+mpc.con.Umax - mpc.T_lastu
280279
-mpc.con.ΔŨmin
281280
+mpc.con.ΔŨmax
282281
]
@@ -331,23 +330,20 @@ function predict!(
331330
end
332331

333332
"""
334-
obj_nonlinprog!( _ , _ , u , mpc::PredictiveController, model::LinModel, Ŷ, ΔŨ)
333+
obj_nonlinprog!( _ , _ , mpc::PredictiveController, model::LinModel, Ŷ, ΔŨ)
335334
336335
Nonlinear programming objective function when `model` is a [`LinModel`](@ref).
337336
338337
The function is called by the nonlinear optimizer of [`NonLinMPC`](@ref) controllers. It can
339338
also be called on any [`PredictiveController`](@ref)s to evaluate the objective function `J`
340-
at specific input increments `ΔŨ` and predictions `Ŷ` values. This method mutate `u`
341-
argument.
339+
at specific input increments `ΔŨ` and predictions `Ŷ` values.
342340
"""
343341
function obj_nonlinprog!(
344-
_ , _ , u , mpc::PredictiveController, model::LinModel, Ŷ, ΔŨ::Vector{NT}
342+
_ , _ , mpc::PredictiveController, model::LinModel, Ŷ, ΔŨ::Vector{NT}
345343
) where {NT<:Real}
346344
J = obj_quadprog(ΔŨ, mpc.H̃, mpc.q̃) + mpc.p[]
347345
if !iszero(mpc.E)
348-
lastu = u
349-
lastu .= mpc.estim.lastu0 .+ model.uop
350-
U = mpc.S̃*ΔŨ + mpc.T*lastu
346+
U = mpc.S̃*ΔŨ .+ mpc.T_lastu
351347
UE = [U; U[(end - model.nu + 1):end]]
352348
ŶE = [mpc.ŷ; Ŷ]
353349
J += mpc.E*mpc.JE(UE, ŶE, mpc.D̂E)
@@ -356,14 +352,14 @@ function obj_nonlinprog!(
356352
end
357353

358354
"""
359-
obj_nonlinprog!(Ȳ, Ū, u, mpc::PredictiveController, model::SimModel, Ŷ, ΔŨ)
355+
obj_nonlinprog!(Ȳ, Ū, mpc::PredictiveController, model::SimModel, Ŷ, ΔŨ)
360356
361357
Nonlinear programming objective function when `model` is not a [`LinModel`](@ref). The
362358
function `dot(x, A, x)` is a performant way of calculating `x'*A*x`. This method mutates
363359
`Ȳ` and `Ū` vector arguments (output and input setpoint tracking error, respectively).
364360
"""
365361
function obj_nonlinprog!(
366-
Ȳ, Ū, u, mpc::PredictiveController, model::SimModel, Ŷ, ΔŨ::Vector{NT}
362+
Ȳ, Ū, mpc::PredictiveController, model::SimModel, Ŷ, ΔŨ::Vector{NT}
367363
) where {NT<:Real}
368364
# --- output setpoint tracking term ---
369365
Ȳ .= mpc.R̂y .- Ŷ
@@ -372,9 +368,7 @@ function obj_nonlinprog!(
372368
JΔŨ = dot(ΔŨ, mpc.Ñ_Hc, ΔŨ)
373369
# --- input over prediction horizon ---
374370
if !mpc.noR̂u || !iszero(mpc.E)
375-
lastu = u
376-
lastu .= mpc.estim.lastu0 .+ model.uop
377-
U = mpc.S̃*ΔŨ + mpc.T*lastu
371+
U = mpc.S̃*ΔŨ + mpc.T_lastu
378372
end
379373
# --- input setpoint tracking term ---
380374
if !mpc.noR̂u

‎src/controller/explicitmpc.jl

+3-2
Original file line numberDiff line numberDiff line change
@@ -14,6 +14,7 @@ struct ExplicitMPC{NT<:Real, SE<:StateEstimator} <: PredictiveController{NT}
1414
noR̂u::Bool
1515
S̃::Matrix{NT}
1616
T::Matrix{NT}
17+
T_lastu::Vector{NT}
1718
Ẽ::Matrix{NT}
1819
F::Vector{NT}
1920
G::Matrix{NT}
@@ -42,7 +43,7 @@ struct ExplicitMPC{NT<:Real, SE<:StateEstimator} <: PredictiveController{NT}
4243
validate_weights(model, Hp, Hc, M_Hp, N_Hc, L_Hp, Cwt)
4344
M_Hp, N_Hc, L_Hp = Diagonal{NT}(M_Hp), Diagonal{NT}(N_Hc), Diagonal{NT}(L_Hp) # debug julia 1.6
4445
# dummy vals (updated just before optimization):
45-
R̂y, R̂u = zeros(NT, ny*Hp), zeros(NT, nu*Hp)
46+
R̂y, R̂u, T_lastu = zeros(NT, ny*Hp), zeros(NT, nu*Hp), zeros(NT, nu*Hp)
4647
noR̂u = iszero(L_Hp)
4748
S, T = init_ΔUtoU(model, Hp, Hc)
4849
E, F, G, J, K, V = init_predmat(estim, model, Hp, Hc)
@@ -61,7 +62,7 @@ struct ExplicitMPC{NT<:Real, SE<:StateEstimator} <: PredictiveController{NT}
6162
Hp, Hc,
6263
M_Hp, Ñ_Hc, L_Hp, Cwt, Ewt,
6364
R̂u, R̂y, noR̂u,
64-
S̃, T,
65+
S̃, T, T_lastu,
6566
Ẽ, F, G, J, K, V, H̃, q̃, p,
6667
H̃_chol,
6768
Ks, Ps,

‎src/controller/linmpc.jl

+3-2
Original file line numberDiff line numberDiff line change
@@ -24,6 +24,7 @@ struct LinMPC{
2424
noR̂u::Bool
2525
S̃::Matrix{NT}
2626
T::Matrix{NT}
27+
T_lastu::Vector{NT}
2728
Ẽ::Matrix{NT}
2829
F::Vector{NT}
2930
G::Matrix{NT}
@@ -50,7 +51,7 @@ struct LinMPC{
5051
validate_weights(model, Hp, Hc, M_Hp, N_Hc, L_Hp, Cwt)
5152
M_Hp, N_Hc, L_Hp = Diagonal{NT}(M_Hp), Diagonal{NT}(N_Hc), Diagonal{NT}(L_Hp) # debug julia 1.6
5253
# dummy vals (updated just before optimization):
53-
R̂y, R̂u = zeros(NT, ny*Hp), zeros(NT, nu*Hp)
54+
R̂y, R̂u, T_lastu = zeros(NT, ny*Hp), zeros(NT, nu*Hp), zeros(NT, nu*Hp)
5455
noR̂u = iszero(L_Hp)
5556
S, T = init_ΔUtoU(model, Hp, Hc)
5657
E, F, G, J, K, V, ex̂, fx̂, gx̂, jx̂, kx̂, vx̂ = init_predmat(estim, model, Hp, Hc)
@@ -68,7 +69,7 @@ struct LinMPC{
6869
Hp, Hc,
6970
M_Hp, Ñ_Hc, L_Hp, Cwt, Ewt,
7071
R̂u, R̂y, noR̂u,
71-
S̃, T,
72+
S̃, T, T_lastu,
7273
Ẽ, F, G, J, K, V, H̃, q̃, p,
7374
Ks, Ps,
7475
d0, D̂0, D̂E,

‎src/controller/nonlinmpc.jl

+15-11
Original file line numberDiff line numberDiff line change
@@ -26,6 +26,7 @@ struct NonLinMPC{
2626
noR̂u::Bool
2727
S̃::Matrix{NT}
2828
T::Matrix{NT}
29+
T_lastu::Vector{NT}
2930
Ẽ::Matrix{NT}
3031
F::Vector{NT}
3132
G::Matrix{NT}
@@ -51,7 +52,7 @@ struct NonLinMPC{
5152
validate_weights(model, Hp, Hc, M_Hp, N_Hc, L_Hp, Cwt, Ewt)
5253
M_Hp, N_Hc, L_Hp = Diagonal{NT}(M_Hp), Diagonal{NT}(N_Hc), Diagonal{NT}(L_Hp) # debug julia 1.6
5354
# dummy vals (updated just before optimization):
54-
R̂y, R̂u = zeros(NT, ny*Hp), zeros(NT, nu*Hp)
55+
R̂y, R̂u, T_lastu = zeros(NT, ny*Hp), zeros(NT, nu*Hp), zeros(NT, nu*Hp)
5556
noR̂u = iszero(L_Hp)
5657
S, T = init_ΔUtoU(model, Hp, Hc)
5758
E, F, G, J, K, V, ex̂, fx̂, gx̂, jx̂, kx̂, vx̂ = init_predmat(estim, model, Hp, Hc)
@@ -69,7 +70,7 @@ struct NonLinMPC{
6970
Hp, Hc,
7071
M_Hp, Ñ_Hc, L_Hp, Cwt, Ewt, JE,
7172
R̂u, R̂y, noR̂u,
72-
S̃, T,
73+
S̃, T, T_lastu,
7374
Ẽ, F, G, J, K, V, H̃, q̃, p,
7475
Ks, Ps,
7576
d0, D̂0, D̂E,
@@ -309,36 +310,38 @@ function init_optimization!(mpc::NonLinMPC, optim::JuMP.GenericModel{JNT}) where
309310
Ū_cache::DiffCache{Vector{JNT}, Vector{JNT}} = DiffCache(zeros(JNT, nU), nΔŨ + 3)
310311
function Jfunc(ΔŨtup::JNT...)
311312
ΔŨ1 = ΔŨtup[begin]
312-
Ŷ, u = get_tmp(Ŷ_cache, ΔŨ1), get_tmp(u_cache, ΔŨ1)
313+
Ŷ = get_tmp(Ŷ_cache, ΔŨ1)
313314
ΔŨ = collect(ΔŨtup)
314315
if ΔŨtup !== last_ΔŨtup_float
315-
x̂, g = get_tmp(x̂_cache, ΔŨ1), get_tmp(g_cache, ΔŨ1)
316+
x̂, u = get_tmp(x̂_cache, ΔŨ1), get_tmp(u_cache, ΔŨ1)
316317
Ŷ, x̂end = predict!(Ŷ, x̂, u, mpc, model, ΔŨ)
318+
g = get_tmp(g_cache, ΔŨ1)
317319
g = con_nonlinprog!(g, mpc, model, x̂end, Ŷ, ΔŨ)
318320
last_ΔŨtup_float = ΔŨtup
319321
end
320322
Ȳ, Ū = get_tmp(Ȳ_cache, ΔŨ1), get_tmp(Ū_cache, ΔŨ1)
321-
return obj_nonlinprog!(Ȳ, Ū, u, mpc, model, Ŷ, ΔŨ)
323+
return obj_nonlinprog!(Ȳ, Ū, mpc, model, Ŷ, ΔŨ)
322324
end
323325
function Jfunc(ΔŨtup::ForwardDiff.Dual...)
324326
ΔŨ1 = ΔŨtup[begin]
325-
Ŷ, u = get_tmp(Ŷ_cache, ΔŨ1), get_tmp(u_cache, ΔŨ1)
327+
Ŷ = get_tmp(Ŷ_cache, ΔŨ1)
326328
ΔŨ = collect(ΔŨtup)
327329
if ΔŨtup !== last_ΔŨtup_dual
328-
x̂, g = get_tmp(x̂_cache, ΔŨ1), get_tmp(g_cache, ΔŨ1)
329-
g = get_tmp(g_cache, ΔŨ1)
330+
x̂, u = get_tmp(x̂_cache, ΔŨ1), get_tmp(u_cache, ΔŨ1)
330331
Ŷ, x̂end = predict!(Ŷ, x̂, u, mpc, model, ΔŨ)
332+
g = get_tmp(g_cache, ΔŨ1)
331333
g = con_nonlinprog!(g, mpc, model, x̂end, Ŷ, ΔŨ)
332334
last_ΔŨtup_dual = ΔŨtup
333335
end
334336
Ȳ, Ū = get_tmp(Ȳ_cache, ΔŨ1), get_tmp(Ū_cache, ΔŨ1)
335-
return obj_nonlinprog!(Ȳ, Ū, u, mpc, model, Ŷ, ΔŨ)
337+
return obj_nonlinprog!(Ȳ, Ū, mpc, model, Ŷ, ΔŨ)
336338
end
337339
function gfunc_i(i, ΔŨtup::NTuple{N, JNT}) where N
338340
ΔŨ1 = ΔŨtup[begin]
339341
g = get_tmp(g_cache, ΔŨ1)
340342
if ΔŨtup !== last_ΔŨtup_float
341-
Ŷ, u, x̂ = get_tmp(Ŷ_cache, ΔŨ1), get_tmp(u_cache, ΔŨ1), get_tmp(x̂_cache, ΔŨ1)
343+
x̂, u = get_tmp(x̂_cache, ΔŨ1), get_tmp(u_cache, ΔŨ1)
344+
Ŷ = get_tmp(Ŷ_cache, ΔŨ1)
342345
ΔŨ = collect(ΔŨtup)
343346
Ŷ, x̂end = predict!(Ŷ, x̂, u, mpc, model, ΔŨ)
344347
g = con_nonlinprog!(g, mpc, model, x̂end, Ŷ, ΔŨ)
@@ -350,7 +353,8 @@ function init_optimization!(mpc::NonLinMPC, optim::JuMP.GenericModel{JNT}) where
350353
ΔŨ1 = ΔŨtup[begin]
351354
g = get_tmp(g_cache, ΔŨ1)
352355
if ΔŨtup !== last_ΔŨtup_dual
353-
Ŷ, u, x̂ = get_tmp(Ŷ_cache, ΔŨ1), get_tmp(u_cache, ΔŨ1), get_tmp(x̂_cache, ΔŨ1)
356+
x̂, u = get_tmp(x̂_cache, ΔŨ1), get_tmp(u_cache, ΔŨ1)
357+
Ŷ = get_tmp(Ŷ_cache, ΔŨ1)
354358
ΔŨ = collect(ΔŨtup)
355359
Ŷ, x̂end = predict!(Ŷ, x̂, u, mpc, model, ΔŨ)
356360
g = con_nonlinprog!(g, mpc, model, x̂end, Ŷ, ΔŨ)

0 commit comments

Comments
 (0)