Skip to content

Commit 599d582

Browse files
franckgagafranckgaga
committed
removed mutable struct OptimInfo
1 parent 1ee666d commit 599d582

File tree

4 files changed

+31
-76
lines changed

4 files changed

+31
-76
lines changed

‎example/juMPC.jl‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -129,7 +129,7 @@ end
129129

130130
@time u_data, y_data, r_data, d_data = test_mpc(linModel4, mpc)
131131

132-
@profview u_data, y_data, r_data, d_data = test_mpc(linModel4, mpc)
132+
133133
@time u_data, y_data, r_data, d_data = test_mpc(linModel4, nmpc)
134134

135135
using PlotThemes, Plots

‎src/controller/linmpc.jl‎

Lines changed: 8 additions & 29 deletions
Original file line numberDiff line numberDiff line change
@@ -1,8 +1,8 @@
11
struct LinMPC{S<:StateEstimator} <: PredictiveController
22
estim::S
33
optim::JuMP.Model
4-
info::OptimInfo
54
con::ControllerConstraint
5+
ΔŨ::Vector{Float64}
66
x̂d::Vector{Float64}
77
x̂s::Vector{Float64}
88
ŷ ::Vector{Float64}
@@ -74,19 +74,10 @@ struct LinMPC{S<:StateEstimator} <: PredictiveController
7474
Ks, Ps = init_stochpred(estim, Hp)
7575
d0, D̂0 = zeros(nd), zeros(nd*Hp)
7676
Uop, Yop, Dop = repeat(model.uop, Hp), repeat(model.yop, Hp), repeat(model.dop, Hp)
77-
@variable(optim, ΔŨ[1:nvar])
78-
A = con.A[con.i_b, :]
79-
b = con.b[con.i_b]
80-
@constraint(optim, linconstraint, A*ΔŨ .≤ b)
81-
ΔŨ0 = zeros(nvar)
82-
ϵ = isinf(C) ? NaN : 0.0 # C = Inf means hard constraints only
83-
u, U = copy(model.uop), repeat(model.uop, Hp)
84-
ŷ, Ŷ = copy(model.yop), repeat(model.yop, Hp)
85-
ŷs, Ŷs = zeros(ny), zeros(ny*Hp)
86-
info = OptimInfo(ΔŨ0, ϵ, 0, u, U, ŷ, Ŷ, ŷs, Ŷs)
77+
ΔŨ = zeros(nvar)
8778
mpc = new(
88-
estim, optim, info, con,
89-
x̂d, x̂s, ŷ,
79+
estim, optim, con,
80+
ΔŨ, x̂d, x̂s, ŷ,
9081
Hp, Hc,
9182
M_Hp, Ñ_Hc, L_Hp, Cwt, R̂u,
9283
S̃_Hp, T_Hp, T_Hc,
@@ -95,6 +86,10 @@ struct LinMPC{S<:StateEstimator} <: PredictiveController
9586
d0, D̂0,
9687
Uop, Yop, Dop,
9788
)
89+
@variable(optim, ΔŨ[1:nvar])
90+
A = con.A[con.i_b, :]
91+
b = con.b[con.i_b]
92+
@constraint(optim, linconstraint, A*ΔŨ .≤ b)
9893
@objective(optim, Min, obj_quadprog(ΔŨ, mpc.P̃, mpc.q̃))
9994
set_silent(optim)
10095
return mpc
@@ -228,19 +223,3 @@ end
228223
function init_objective(mpc::LinMPC, ΔŨ)
229224
set_objective_function(mpc.optim, obj_quadprog(ΔŨ, mpc.P̃, mpc.q̃))
230225
end
231-
232-
"""
233-
write_info!(mpc::LinMPC, ΔŨ, ϵ, J, info, ŷs, Ŷs, lastu, F, ym, d)
234-
235-
Write `mpc.info` with the [`LinMPC`](@ref) optimization results.
236-
"""
237-
function write_info!(mpc::LinMPC, ΔŨ, J, ŷs, Ŷs)
238-
mpc.info.ΔŨ = ΔŨ
239-
mpc.info.ϵ = isinf(mpc.C) ? NaN : ΔŨ[end]
240-
mpc.info.J = J
241-
mpc.info.U = mpc.S̃_Hp*ΔŨ + mpc.T_Hp*(mpc.estim.lastu0 + mpc.estim.model.uop)
242-
mpc.info.u = mpc.info.U[1:mpc.estim.model.nu]
243-
mpc.info.ŷ = mpc.ŷ
244-
mpc.info.Ŷ = mpc.Ẽ*ΔŨ + mpc.F
245-
mpc.info.ŷs, mpc.info.Ŷs = ŷs, Ŷs
246-
end

‎src/controller/nonlinmpc.jl‎

Lines changed: 11 additions & 22 deletions
Original file line numberDiff line numberDiff line change
@@ -1,8 +1,8 @@
11
struct NonLinMPC{S<:StateEstimator, JEFunc<:Function} <: PredictiveController
22
estim::S
33
optim::JuMP.Model
4-
info::OptimInfo
54
con::ControllerConstraint
5+
ΔŨ::Vector{Float64}
66
x̂d::Vector{Float64}
77
x̂s::Vector{Float64}
88
ŷ ::Vector{Float64}
@@ -78,21 +78,10 @@ struct NonLinMPC{S<:StateEstimator, JEFunc<:Function} <: PredictiveController
7878
Ks, Ps = init_stochpred(estim, Hp)
7979
d0, D̂0 = zeros(nd), zeros(nd*Hp)
8080
Uop, Yop, Dop = repeat(model.uop, Hp), repeat(model.yop, Hp), repeat(model.dop, Hp)
81-
@variable(optim, ΔŨ[1:nvar])
82-
A = [A_Umin; A_Umax; A_ΔŨmin; A_ΔŨmax; A_Ŷmin; A_Ŷmax]
83-
A = con.A[con.i_b, :]
84-
b = con.b[con.i_b]
85-
@constraint(optim, linconstraint, A*ΔŨ .≤ b)
86-
ΔŨ0 = zeros(nvar)
87-
ϵ = isinf(C) ? NaN : 0.0 # C = Inf means hard constraints only
88-
u, U = copy(model.uop), repeat(model.uop, Hp)
89-
ŷ, Ŷ = copy(model.yop), repeat(model.yop, Hp)
90-
ŷs, Ŷs = zeros(ny), zeros(ny*Hp)
91-
info = OptimInfo(ΔŨ0, ϵ, 0, u, U, ŷ, Ŷ, ŷs, Ŷs)
92-
# dummy x̂d, F, d0, D̂0, q̃ values (updated just before optimization)
81+
ΔŨ = zeros(nvar)
9382
mpc = new(
94-
estim, optim, info, con,
95-
x̂d, x̂s, ŷ,
83+
estim, optim, con,
84+
ΔŨ, x̂d, x̂s, ŷ,
9685
Hp, Hc,
9786
M_Hp, Ñ_Hc, L_Hp, Cwt, Ewt, JE, R̂u,
9887
S̃_Hp, T_Hp, T_Hc,
@@ -101,7 +90,13 @@ struct NonLinMPC{S<:StateEstimator, JEFunc<:Function} <: PredictiveController
10190
d0, D̂0,
10291
Uop, Yop, Dop,
10392
)
104-
J = (ΔŨ...) -> obj_nonlinprog(mpc, model, ΔŨ)
93+
@variable(optim, ΔŨ[1:nvar])
94+
A = con.A[con.i_b, :]
95+
b = con.b[con.i_b]
96+
@constraint(optim, linconstraint, A*ΔŨ .≤ b)
97+
J = let mpc=mpc, model=model # capture mpc and model variables
98+
(ΔŨ...) -> obj_nonlinprog(mpc, model, ΔŨ)
99+
end
105100
register(mpc.optim, :J, nvar, J, autodiff=true)
106101
@NLobjective(mpc.optim, Min, J(ΔŨ...))
107102
set_silent(optim)
@@ -229,9 +224,3 @@ function obj_nonlinprog(mpc::NonLinMPC, model::SimModel, ΔŨ::NTuple{N, T}) wh
229224
#println("yoSimModel")
230225
return J
231226
end
232-
233-
234-
235-
function write_info!(mpc::NonLinMPC, ΔŨ, J, ŷs, Ŷs)
236-
return nothing
237-
end

‎src/predictive_control.jl‎

Lines changed: 11 additions & 24 deletions
Original file line numberDiff line numberDiff line change
@@ -19,19 +19,6 @@ julia> u = mpc([5]); round.(u, digits=3)
1919
"""
2020
abstract type PredictiveController end
2121

22-
"Include the additional information about the optimum to ease troubleshooting."
23-
mutable struct OptimInfo
24-
ΔŨ::Vector{Float64}
25-
ϵ ::Float64
26-
J ::Float64
27-
u ::Vector{Float64}
28-
U ::Vector{Float64}
29-
ŷ ::Vector{Float64}
30-
Ŷ ::Vector{Float64}
31-
ŷs::Vector{Float64}
32-
Ŷs::Vector{Float64}
33-
end
34-
3522
"Include all the data for the constraints of [`PredictiveController`](@ref)"
3623
struct ControllerConstraint
3724
Umin ::Vector{Float64}
@@ -260,8 +247,7 @@ function moveinput!(
260247
ŷs, Ŷs = predictstoch!(mpc, mpc.estim, d, ym)
261248
p = initpred!(mpc, mpc.estim.model, d, D̂, Ŷs, R̂y)
262249
linconstaint!(mpc, mpc.estim.model)
263-
ΔŨ, J = optim_objective!(mpc, p)
264-
write_info!(mpc, ΔŨ, J, ŷs, Ŷs)
250+
ΔŨ, _ = optim_objective!(mpc, p)
265251
Δu = ΔŨ[1:mpc.estim.model.nu] # receding horizon principle: only Δu(k) is used (1st one)
266252
u = mpc.estim.lastu0 + mpc.estim.model.uop + Δu
267253
return u
@@ -277,10 +263,10 @@ setstate!(mpc::PredictiveController, x̂) = (setstate!(mpc.estim, x̂); return m
277263
"""
278264
initstate!(mpc::PredictiveController, u, ym, d=Float64[])
279265
280-
Init `mpc.info` and the states of `mpc.estim` [`StateEstimator`](@ref).
266+
Init `mpc.ΔŨ` for warm-starting and the states of `mpc.estim` [`StateEstimator`](@ref).
281267
"""
282268
function initstate!(mpc::PredictiveController, u, ym, d=Float64[])
283-
mpc.info.ΔŨ .= 0
269+
mpc.ΔŨ .= 0
284270
return initstate!(mpc.estim, u, ym, d)
285271
end
286272

@@ -390,7 +376,7 @@ end
390376
@doc raw"""
391377
linconstraint!(mpc::PredictiveController, model::LinModel)
392378
393-
Calc `b` vector for the linear model inequality constraints (``\mathbf{A ΔŨ ≤ b}``).
379+
Set `b` vector for the linear model inequality constraints (``\mathbf{A ΔŨ ≤ b}``).
394380
"""
395381
function linconstaint!(mpc::PredictiveController, model::LinModel)
396382
mpc.con.b[:] = [
@@ -401,12 +387,13 @@ function linconstaint!(mpc::PredictiveController, model::LinModel)
401387
-mpc.con.Ŷmin + mpc.F
402388
+mpc.con.Ŷmax - mpc.F
403389
]
390+
set_normalized_rhs.(mpc.optim[:linconstraint], mpc.con.b[mpc.con.i_b])
404391
end
405392

406393
@doc raw"""
407394
linconstraint!(mpc::PredictiveController, model::NonLinModel)
408395
409-
Calc `b` without predicted output ``\mathbf{Ŷ}`` constraints for [`NonLinModel`](@ref).
396+
Set `b` that excludes predicted output ``\mathbf{Ŷ}`` constraints for [`NonLinModel`](@ref).
410397
"""
411398
function linconstaint!(mpc::PredictiveController, model::NonLinModel)
412399
mpc.con.b[:] = [
@@ -415,6 +402,7 @@ function linconstaint!(mpc::PredictiveController, model::NonLinModel)
415402
-mpc.con.ΔŨmin
416403
+mpc.con.ΔŨmax
417404
]
405+
set_normalized_rhs.(mpc.optim[:linconstraint], mpc.con.b[mpc.con.i_b])
418406
end
419407

420408
"""
@@ -426,8 +414,7 @@ function optim_objective!(mpc::PredictiveController, p)
426414
optim = mpc.optim
427415
model = mpc.estim.model
428416
ΔŨ::Vector{VariableRef} = optim[:ΔŨ]
429-
lastΔŨ = mpc.info.ΔŨ
430-
set_normalized_rhs.(optim[:linconstraint], mpc.con.b[mpc.con.i_b])
417+
lastΔŨ = mpc.ΔŨ
431418
# initial ΔŨ (warm-start): [Δu_{k-1}(k); Δu_{k-1}(k+1); ... ; 0_{nu × 1}]
432419
ΔŨ0 = [lastΔŨ[(model.nu+1):(mpc.Hc*model.nu)]; zeros(model.nu)]
433420
# if soft constraints, append the last slack value ϵ_{k-1}:
@@ -450,9 +437,9 @@ function optim_objective!(mpc::PredictiveController, p)
450437
@warn "MPC termination status not OPTIMAL or LOCALLY_SOLVED ($status)"
451438
@debug solution_summary(optim)
452439
end
453-
ΔŨ_val = isfatal(status) ? ΔŨ0 : value.(ΔŨ) # fatal status : use last value
454-
J_val = objective_value(optim) + p # optimal objective value by adding constant p
455-
return ΔŨ_val, J_val
440+
mpc.ΔŨ[:] = isfatal(status) ? ΔŨ0 : value.(ΔŨ) # fatal status : use last value
441+
J_val = objective_value(optim) + p # add LinModel p constant (p=0 for NonLinModel)
442+
return mpc.ΔŨ, J_val
456443
end
457444

458445

0 commit comments

Comments
 (0)