Skip to content

Commit 321e3ce

Browse files
franckgagafranckgaga
authored
Merge pull request #17 from franckgaga/nonFloat64
Support non-`Float64` numbers in all objects
2 parents fc8ebb0 + 0bef5f1 commit 321e3ce

18 files changed

+707
-586
lines changed

‎src/controller/explicitmpc.jl‎

Lines changed: 41 additions & 38 deletions
Original file line numberDiff line numberDiff line change
@@ -1,45 +1,48 @@
1-
struct ExplicitMPC{SE<:StateEstimator} <: PredictiveController
1+
struct ExplicitMPC{NT<:Real, SE<:StateEstimator} <: PredictiveController{NT}
22
estim::SE
3-
ΔŨ::Vector{Float64}
4-
ŷ ::Vector{Float64}
3+
ΔŨ::Vector{NT}
4+
ŷ ::Vector{NT}
55
Hp::Int
66
Hc::Int
7-
M_Hp::Diagonal{Float64, Vector{Float64}}
8-
Ñ_Hc::Diagonal{Float64, Vector{Float64}}
9-
L_Hp::Diagonal{Float64, Vector{Float64}}
10-
C::Float64
11-
E::Float64
12-
R̂u::Vector{Float64}
13-
R̂y::Vector{Float64}
7+
M_Hp::Diagonal{NT, Vector{NT}}
8+
Ñ_Hc::Diagonal{NT, Vector{NT}}
9+
L_Hp::Diagonal{NT, Vector{NT}}
10+
C::NT
11+
E::NT
12+
R̂u::Vector{NT}
13+
R̂y::Vector{NT}
1414
noR̂u::Bool
1515
S̃::Matrix{Bool}
1616
T::Matrix{Bool}
17-
Ẽ::Matrix{Float64}
18-
F::Vector{Float64}
19-
G::Matrix{Float64}
20-
J::Matrix{Float64}
21-
K::Matrix{Float64}
22-
V::Matrix{Float64}
23-
P̃::Hermitian{Float64, Matrix{Float64}}
24-
q̃::Vector{Float64}
25-
p::Vector{Float64}
26-
P̃_chol::Cholesky{Float64, Matrix{Float64}}
27-
Ks::Matrix{Float64}
28-
Ps::Matrix{Float64}
29-
d0::Vector{Float64}
30-
D̂0::Vector{Float64}
31-
D̂E::Vector{Float64}
32-
Ŷop::Vector{Float64}
33-
Dop::Vector{Float64}
34-
function ExplicitMPC{SE}(estim::SE, Hp, Hc, M_Hp, N_Hc, L_Hp) where {SE<:StateEstimator}
17+
Ẽ::Matrix{NT}
18+
F::Vector{NT}
19+
G::Matrix{NT}
20+
J::Matrix{NT}
21+
K::Matrix{NT}
22+
V::Matrix{NT}
23+
P̃::Hermitian{NT, Matrix{NT}}
24+
q̃::Vector{NT}
25+
p::Vector{NT}
26+
P̃_chol::Cholesky{NT, Matrix{NT}}
27+
Ks::Matrix{NT}
28+
Ps::Matrix{NT}
29+
d0::Vector{NT}
30+
D̂0::Vector{NT}
31+
D̂E::Vector{NT}
32+
Ŷop::Vector{NT}
33+
Dop::Vector{NT}
34+
function ExplicitMPC{NT, SE}(
35+
estim::SE, Hp, Hc, M_Hp, N_Hc, L_Hp
36+
) where {NT<:Real, SE<:StateEstimator}
3537
model = estim.model
3638
nu, ny, nd = model.nu, model.ny, model.nd
3739
ŷ = copy(model.yop) # dummy vals (updated just before optimization)
3840
Cwt = Inf # no slack variable ϵ for ExplicitMPC
3941
Ewt = 0 # economic costs not supported for ExplicitMPC
4042
validate_weights(model, Hp, Hc, M_Hp, N_Hc, L_Hp, Cwt)
4143
M_Hp, N_Hc, L_Hp = float(M_Hp), float(N_Hc), float(L_Hp) # debug julia 1.6
42-
R̂y, R̂u = zeros(ny*Hp), zeros(nu*Hp) # dummy vals (updated just before optimization)
44+
# dummy vals (updated just before optimization):
45+
R̂y, R̂u = zeros(NT, ny*Hp), zeros(NT, nu*Hp)
4346
noR̂u = iszero(L_Hp)
4447
S, T = init_ΔUtoU(nu, Hp, Hc)
4548
E, F, G, J, K, V = init_predmat(estim, model, Hp, Hc)
@@ -48,11 +51,11 @@ struct ExplicitMPC{SE<:StateEstimator} <: PredictiveController
4851
P̃_chol = cholesky(P̃)
4952
Ks, Ps = init_stochpred(estim, Hp)
5053
# dummy vals (updated just before optimization):
51-
d0, D̂0, D̂E = zeros(nd), zeros(nd*Hp), zeros(nd + nd*Hp)
54+
d0, D̂0, D̂E = zeros(NT, nd), zeros(NT, nd*Hp), zeros(NT, nd + nd*Hp)
5255
Ŷop, Dop = repeat(model.yop, Hp), repeat(model.dop, Hp)
5356
nvar = size(Ẽ, 2)
54-
ΔŨ = zeros(nvar)
55-
mpc = new(
57+
ΔŨ = zeros(NT, nvar)
58+
mpc = new{NT, SE}(
5659
estim,
5760
ΔŨ, ŷ,
5861
Hp, Hc,
@@ -161,13 +164,13 @@ function ExplicitMPC(
161164
M_Hp = nothing,
162165
N_Hc = nothing,
163166
L_Hp = nothing
164-
) where {SE<:StateEstimator}
167+
) where {NT<:Real, SE<:StateEstimator{NT}}
165168
isa(estim.model, LinModel) || error("estim.model type must be LinModel")
166169
Hp = default_Hp(estim.model, Hp)
167-
isnothing(M_Hp) && (M_Hp = Diagonal(repeat(Mwt, Hp)))
168-
isnothing(N_Hc) && (N_Hc = Diagonal(repeat(Nwt, Hc)))
169-
isnothing(L_Hp) && (L_Hp = Diagonal(repeat(Lwt, Hp)))
170-
return ExplicitMPC{SE}(estim, Hp, Hc, M_Hp, N_Hc, L_Hp)
170+
isnothing(M_Hp) && (M_Hp = Diagonal{NT}(repeat(Mwt, Hp)))
171+
isnothing(N_Hc) && (N_Hc = Diagonal{NT}(repeat(Nwt, Hc)))
172+
isnothing(L_Hp) && (L_Hp = Diagonal{NT}(repeat(Lwt, Hp)))
173+
return ExplicitMPC{NT, SE}(estim, Hp, Hc, M_Hp, N_Hc, L_Hp)
171174
end
172175

173176
setconstraint!(::ExplicitMPC,kwargs...) = error("ExplicitMPC does not support constraints.")
@@ -197,7 +200,7 @@ function optim_objective!(mpc::ExplicitMPC)
197200
end
198201

199202
"Compute the predictions but not the terminal states if `mpc` is an [`ExplicitMPC`](@ref)."
200-
function predict!(Ŷ, x̂, mpc::ExplicitMPC, ::LinModel, ΔŨ::Vector{T}) where {T<:Real}
203+
function predict!(Ŷ, x̂, mpc::ExplicitMPC, ::LinModel, ΔŨ::Vector{NT}) where {NT<:Real}
201204
# in-place operations to reduce allocations :
202205
mul!(Ŷ, mpc.Ẽ, ΔŨ) + mpc.F
203206
x̂[:] .= NaN

‎src/controller/linmpc.jl‎

Lines changed: 53 additions & 44 deletions
Original file line numberDiff line numberDiff line change
@@ -1,59 +1,68 @@
11
const DEFAULT_LINMPC_OPTIMIZER = OSQP.MathOptInterfaceOSQP.Optimizer
22

3-
struct LinMPC{SE<:StateEstimator} <: PredictiveController
3+
struct LinMPC{
4+
NT<:Real,
5+
SE<:StateEstimator,
6+
JM<:JuMP.GenericModel
7+
} <: PredictiveController{NT}
48
estim::SE
5-
optim::JuMP.Model
6-
con::ControllerConstraint
7-
ΔŨ::Vector{Float64}
8-
ŷ ::Vector{Float64}
9+
# note: `NT` and the number type `JNT` in `JuMP.GenericModel{JNT}` can be
10+
# different since solvers that support non-Float64 are scarce.
11+
optim::JM
12+
con::ControllerConstraint{NT}
13+
ΔŨ::Vector{NT}
14+
ŷ ::Vector{NT}
915
Hp::Int
1016
Hc::Int
11-
M_Hp::Diagonal{Float64, Vector{Float64}}
12-
Ñ_Hc::Diagonal{Float64, Vector{Float64}}
13-
L_Hp::Diagonal{Float64, Vector{Float64}}
14-
C::Float64
15-
E::Float64
16-
R̂u::Vector{Float64}
17-
R̂y::Vector{Float64}
17+
M_Hp::Diagonal{NT, Vector{NT}}
18+
Ñ_Hc::Diagonal{NT, Vector{NT}}
19+
L_Hp::Diagonal{NT, Vector{NT}}
20+
C::NT
21+
E::NT
22+
R̂u::Vector{NT}
23+
R̂y::Vector{NT}
1824
noR̂u::Bool
1925
S̃::Matrix{Bool}
2026
T::Matrix{Bool}
21-
Ẽ::Matrix{Float64}
22-
F::Vector{Float64}
23-
G::Matrix{Float64}
24-
J::Matrix{Float64}
25-
K::Matrix{Float64}
26-
V::Matrix{Float64}
27-
P̃::Hermitian{Float64, Matrix{Float64}}
28-
q̃::Vector{Float64}
29-
p::Vector{Float64}
30-
Ks::Matrix{Float64}
31-
Ps::Matrix{Float64}
32-
d0::Vector{Float64}
33-
D̂0::Vector{Float64}
34-
D̂E::Vector{Float64}
35-
Ŷop::Vector{Float64}
36-
Dop::Vector{Float64}
37-
function LinMPC{SE}(estim::SE, Hp, Hc, M_Hp, N_Hc, L_Hp, Cwt, optim) where {SE<:StateEstimator}
27+
Ẽ::Matrix{NT}
28+
F::Vector{NT}
29+
G::Matrix{NT}
30+
J::Matrix{NT}
31+
K::Matrix{NT}
32+
V::Matrix{NT}
33+
P̃::Hermitian{NT, Matrix{NT}}
34+
q̃::Vector{NT}
35+
p::Vector{NT}
36+
Ks::Matrix{NT}
37+
Ps::Matrix{NT}
38+
d0::Vector{NT}
39+
D̂0::Vector{NT}
40+
D̂E::Vector{NT}
41+
Ŷop::Vector{NT}
42+
Dop::Vector{NT}
43+
function LinMPC{NT, SE, JM}(
44+
estim::SE, Hp, Hc, M_Hp, N_Hc, L_Hp, Cwt, optim::JM
45+
) where {NT<:Real, SE<:StateEstimator, JM<:JuMP.GenericModel}
3846
model = estim.model
3947
nu, ny, nd = model.nu, model.ny, model.nd
4048
ŷ = copy(model.yop) # dummy vals (updated just before optimization)
4149
Ewt = 0 # economic costs not supported for LinMPC
4250
validate_weights(model, Hp, Hc, M_Hp, N_Hc, L_Hp, Cwt)
4351
M_Hp, N_Hc, L_Hp = float(M_Hp), float(N_Hc), float(L_Hp) # debug julia 1.6
44-
R̂y, R̂u = zeros(ny*Hp), zeros(nu*Hp) # dummy vals (updated just before optimization)
52+
# dummy vals (updated just before optimization):
53+
R̂y, R̂u = zeros(NT, ny*Hp), zeros(NT, nu*Hp)
4554
noR̂u = iszero(L_Hp)
4655
S, T = init_ΔUtoU(nu, Hp, Hc)
4756
E, F, G, J, K, V, ex̂, fx̂, gx̂, jx̂, kx̂, vx̂ = init_predmat(estim, model, Hp, Hc)
4857
con, S̃, Ñ_Hc, Ẽ = init_defaultcon(estim, Hp, Hc, Cwt, S, N_Hc, E, ex̂, fx̂, gx̂, jx̂, kx̂, vx̂)
4958
P̃, q̃, p = init_quadprog(model, Ẽ, S̃, M_Hp, Ñ_Hc, L_Hp)
5059
Ks, Ps = init_stochpred(estim, Hp)
5160
# dummy vals (updated just before optimization):
52-
d0, D̂0, D̂E = zeros(nd), zeros(nd*Hp), zeros(nd + nd*Hp)
61+
d0, D̂0, D̂E = zeros(NT, nd), zeros(NT, nd*Hp), zeros(NT, nd + nd*Hp)
5362
Ŷop, Dop = repeat(model.yop, Hp), repeat(model.dop, Hp)
5463
nvar = size(Ẽ, 2)
55-
ΔŨ = zeros(nvar)
56-
mpc = new(
64+
ΔŨ = zeros(NT, nvar)
65+
mpc = new{NT, SE, JM}(
5766
estim, optim, con,
5867
ΔŨ, ŷ,
5968
Hp, Hc,
@@ -65,7 +74,7 @@ struct LinMPC{SE<:StateEstimator} <: PredictiveController
6574
d0, D̂0, D̂E,
6675
Ŷop, Dop,
6776
)
68-
init_optimization!(mpc)
77+
init_optimization!(mpc, optim)
6978
return mpc
7079
end
7180
end
@@ -163,7 +172,7 @@ function LinMPC(
163172
M_Hp = nothing,
164173
N_Hc = nothing,
165174
L_Hp = nothing,
166-
optim::JuMP.Model = JuMP.Model(DEFAULT_LINMPC_OPTIMIZER, add_bridges=false),
175+
optim::JuMP.GenericModel = JuMP.Model(DEFAULT_LINMPC_OPTIMIZER, add_bridges=false),
167176
kwargs...
168177
)
169178
estim = SteadyKalmanFilter(model; kwargs...)
@@ -204,24 +213,24 @@ function LinMPC(
204213
M_Hp = nothing,
205214
N_Hc = nothing,
206215
L_Hp = nothing,
207-
optim::JuMP.Model = JuMP.Model(DEFAULT_LINMPC_OPTIMIZER, add_bridges=false),
208-
) where {SE<:StateEstimator}
216+
optim::JM = JuMP.Model(DEFAULT_LINMPC_OPTIMIZER, add_bridges=false),
217+
) where {NT<:Real, SE<:StateEstimator{NT}, JM<:JuMP.GenericModel}
209218
isa(estim.model, LinModel) || error("estim.model type must be LinModel")
210219
Hp = default_Hp(estim.model, Hp)
211-
isnothing(M_Hp) && (M_Hp = Diagonal(repeat(Mwt, Hp)))
212-
isnothing(N_Hc) && (N_Hc = Diagonal(repeat(Nwt, Hc)))
213-
isnothing(L_Hp) && (L_Hp = Diagonal(repeat(Lwt, Hp)))
214-
return LinMPC{SE}(estim, Hp, Hc, M_Hp, N_Hc, L_Hp, Cwt, optim)
220+
isnothing(M_Hp) && (M_Hp = Diagonal{NT}(repeat(Mwt, Hp)))
221+
isnothing(N_Hc) && (N_Hc = Diagonal{NT}(repeat(Nwt, Hc)))
222+
isnothing(L_Hp) && (L_Hp = Diagonal{NT}(repeat(Lwt, Hp)))
223+
return LinMPC{NT, SE, JM}(estim, Hp, Hc, M_Hp, N_Hc, L_Hp, Cwt, optim)
215224
end
216225

217226
"""
218-
init_optimization!(mpc::LinMPC)
227+
init_optimization!(mpc::LinMPC, optim::JuMP.GenericModel)
219228
220229
Init the quadratic optimization for [`LinMPC`](@ref) controllers.
221230
"""
222-
function init_optimization!(mpc::LinMPC)
231+
function init_optimization!(mpc::LinMPC, optim::JuMP.GenericModel)
223232
# --- variables and linear constraints ---
224-
optim, con = mpc.optim, mpc.con
233+
con = mpc.con
225234
nvar = length(mpc.ΔŨ)
226235
set_silent(optim)
227236
limit_solve_time(mpc)

0 commit comments

Comments
 (0)