clean up code structure

Author: franckgaga

src/controller/linmpc.jl

@@ -30,7 +30,6 @@ struct LinMPC{S<:StateEstimator} <: PredictiveController
     Ps::Matrix{Float64}
     d0::Vector{Float64}
     D̂0::Vector{Float64}
-    Uop::Vector{Float64}
     Yop::Vector{Float64}
     Dop::Vector{Float64}
     function LinMPC{S}(estim::S, Hp, Hc, Mwt, Nwt, Lwt, Cwt, ru, optim) where {S<:StateEstimator}
@@ -48,11 +47,11 @@ struct LinMPC{S<:StateEstimator} <: PredictiveController
         S_Hp, T_Hp, S_Hc, T_Hc = init_ΔUtoU(nu, Hp, Hc)
         E, F, G, J, Kd, Q = init_deterpred(model, Hp, Hc)
         con, S̃_Hp, Ñ_Hc, Ẽ = init_defaultcon(model, Hp, Hc, C, S_Hp, S_Hc, N_Hc, E)
-        nvar = size(Ẽ, 2)
         P̃, q̃ = init_quadprog(model, Ẽ, S̃_Hp, M_Hp, Ñ_Hc, L_Hp)
         Ks, Ps = init_stochpred(estim, Hp)
         d0, D̂0 = zeros(nd), zeros(nd*Hp)
-        Uop, Yop, Dop = repeat(model.uop, Hp), repeat(model.yop, Hp), repeat(model.dop, Hp)
+        Yop, Dop = repeat(model.yop, Hp), repeat(model.dop, Hp)
+        nvar = size(Ẽ, 2)
         ΔŨ = zeros(nvar)
         mpc = new(
             estim, optim, con,
@@ -63,14 +62,9 @@ struct LinMPC{S<:StateEstimator} <: PredictiveController
             Ẽ, F, G, J, Kd, Q, P̃, q̃,
             Ks, Ps,
             d0, D̂0,
-            Uop, Yop, Dop,
+            Yop, Dop,
         )
-        @variable(optim, ΔŨ[1:nvar])
-        A = con.A[con.i_b, :]
-        b = con.b[con.i_b]
-        @constraint(optim, linconstraint, A*ΔŨ .≤ b)
-        @objective(optim, Min, obj_quadprog(ΔŨ, mpc.P̃, mpc.q̃))
-        set_silent(optim)
+        init_optimization!(mpc)
         return mpc
     end
 end
@@ -199,11 +193,27 @@ function LinMPC(
     return LinMPC{S}(estim, Hp, Hc, Mwt, Nwt, Lwt, Cwt, ru, optim)
 end
 
-"No nonlinear constraint for [`LinMPC`](@ref) controller."
-setnonlincon!(::LinMPC, ::LinModel) = nothing
+"""
+    init_optimization!(mpc::LinMPC)
+
+Init the quadratic optimization for [`LinMPC`](@ref) controllers.
+"""
+function init_optimization!(mpc::LinMPC)
+    # --- variables and linear constraints ---
+    optim, con = mpc.optim, mpc.con
+    nvar = length(mpc.ΔŨ)
+    set_silent(optim)
+    @variable(optim, ΔŨvar[1:nvar])
+    A = con.A[con.i_b, :]
+    b = con.b[con.i_b]
+    @constraint(optim, linconstraint, A*ΔŨvar .≤ b)
+    # --- quadratic optimization init ---
+    @objective(mpc.optim, Min, obj_quadprog(ΔŨvar, mpc.P̃, mpc.q̃))
+    return nothing
+end
 
-"Init quadratic programming `q̃` vector just before optimization."
-function init_objective!(mpc::LinMPC, ΔŨ)
+"Set quadratic programming `q̃` vector just before optimization."
+function set_objective!(mpc::LinMPC, ΔŨ)
     set_objective_function(mpc.optim, obj_quadprog(ΔŨ, mpc.P̃, mpc.q̃))
     return nothing
 end

src/controller/nonlinmpc.jl

@@ -32,7 +32,6 @@ struct NonLinMPC{S<:StateEstimator, JEFunc<:Function} <: PredictiveController
     Ps::Matrix{Float64}
     d0::Vector{Float64}
     D̂0::Vector{Float64}
-    Uop::Vector{Float64}
     Yop::Vector{Float64}
     Dop::Vector{Float64}
     function NonLinMPC{S, JEFunc}(
@@ -52,11 +51,11 @@ struct NonLinMPC{S<:StateEstimator, JEFunc<:Function} <: PredictiveController
         S_Hp, T_Hp, S_Hc, T_Hc = init_ΔUtoU(nu, Hp, Hc)
         E, F, G, J, Kd, Q = init_deterpred(model, Hp, Hc)
         con, S̃_Hp, Ñ_Hc, Ẽ = init_defaultcon(model, Hp, Hc, C, S_Hp, S_Hc, N_Hc, E)
-        nvar = size(Ẽ, 2)
         P̃, q̃ = init_quadprog(model, Ẽ, S̃_Hp, M_Hp, Ñ_Hc, L_Hp)
         Ks, Ps = init_stochpred(estim, Hp)
         d0, D̂0 = zeros(nd), zeros(nd*Hp)
-        Uop, Yop, Dop = repeat(model.uop, Hp), repeat(model.yop, Hp), repeat(model.dop, Hp)
+        Yop, Dop = repeat(model.yop, Hp), repeat(model.dop, Hp)
+        nvar = size(Ẽ, 2)
         ΔŨ = zeros(nvar)
         mpc = new(
             estim, optim, con,
@@ -67,14 +66,9 @@ struct NonLinMPC{S<:StateEstimator, JEFunc<:Function} <: PredictiveController
             Ẽ, F, G, J, Kd, Q, P̃, q̃,
             Ks, Ps,
             d0, D̂0,
-            Uop, Yop, Dop,
+            Yop, Dop,
         )
-        @variable(optim, ΔŨ[1:nvar])
-        A = con.A[con.i_b, :]
-        b = con.b[con.i_b]
-        @constraint(optim, linconstraint, A*ΔŨ .≤ b)
-        init_nonlinprog!(mpc, model)
-        set_silent(optim)
+        init_optimization!(mpc)
         return mpc
     end
 end
@@ -194,10 +188,23 @@ function NonLinMPC(
 end
 
 
-function init_nonlinprog!(mpc::NonLinMPC, model::SimModel)
+"""
+    init_optimization!(mpc::NonLinMPC)
+
+Init the nonlinear optimization for [`NonLinMPC`](@ref) controllers.
+"""
+function init_optimization!(mpc::NonLinMPC)
+    # --- variables and linear constraints ---
     optim, con = mpc.optim, mpc.con
-    ΔŨvar = optim[:ΔŨ]
-    nvar = length(ΔŨvar)
+    nvar = length(mpc.ΔŨ)
+    set_silent(optim)
+    @variable(optim, ΔŨvar[1:nvar])
+    ΔŨvar = optim[:ΔŨvar]
+    A = con.A[con.i_b, :]
+    b = con.b[con.i_b]
+    @constraint(optim, linconstraint, A*ΔŨvar .≤ b)
+    # --- nonlinear optimization init ---
+    model = mpc.estim.model
     last_ΔŨtup, C, Ŷ = nothing, nothing, nothing
     last_dΔŨtup, dC, dŶ = nothing, nothing, nothing
     function Jfunc(ΔŨtup::Float64...)
@@ -250,6 +257,7 @@ function init_nonlinprog!(mpc::NonLinMPC, model::SimModel)
         sym = Symbol("C_Ŷmax_$i")
         register(optim, sym, nvar, Cfunc[n + i], autodiff=true)
     end
+    return nothing
 end
 
 
@@ -259,24 +267,20 @@ setnontlincon!(::NonLinMPC, ::LinModel) = nothing
 "Set the nonlinear constraints on the output predictions `Ŷ`."
 function setnonlincon!(mpc::NonLinMPC, model::NonLinModel)
     optim = mpc.optim
-    ΔŨ = mpc.optim[:ΔŨ]
+    ΔŨvar = mpc.optim[:ΔŨvar]
     con = mpc.con
     map(con -> delete(optim, con), all_nonlinear_constraints(optim))
     for i in findall(.!isinf.(con.Ŷmin))
         f_sym = Symbol("C_Ŷmin_$(i)")
-        add_nonlinear_constraint(optim, :($(f_sym)($(ΔŨ...)) <= 0))
+        add_nonlinear_constraint(optim, :($(f_sym)($(ΔŨvar...)) <= 0))
     end
     for i in findall(.!isinf.(con.Ŷmax))
         f_sym = Symbol("C_Ŷmax_$(i)")
-        add_nonlinear_constraint(optim, :($(f_sym)($(ΔŨ...)) <= 0))
+        add_nonlinear_constraint(optim, :($(f_sym)($(ΔŨvar...)) <= 0))
     end
     return nothing
 end
 
-"Nonlinear programming objective does not require any initialization."
-init_objective!(mpc::NonLinMPC, _ ) = nothing
-
-
 """
     obj_nonlinprog(mpc::NonLinMPC, model::LinModel, ΔŨ::Vector{Real})
 

src/predictive_control.jl

@@ -183,15 +183,15 @@ function setconstraint!(
     )
     A = con.A[con.i_b, :]
     b = con.b[con.i_b]
-    ΔŨ = mpc.optim[:ΔŨ]
+    ΔŨvar = mpc.optim[:ΔŨvar]
     delete(mpc.optim, mpc.optim[:linconstraint])
     unregister(mpc.optim, :linconstraint)
-    @constraint(mpc.optim, linconstraint, A*ΔŨ .≤ b)
+    @constraint(mpc.optim, linconstraint, A*ΔŨvar .≤ b)
     setnonlincon!(mpc, model)
     return mpc
 end
 
-"By default, there is no nonlinear constraint"
+"By default, there is no nonlinear constraint, thus do nothing."
 setnonlincon!(::PredictiveController, ::SimModel) = nothing
 
 @doc raw"""
@@ -448,14 +448,14 @@ Optimize the objective function ``J`` of `mpc` controller.
 function optim_objective!(mpc::PredictiveController, p)
     optim = mpc.optim
     model = mpc.estim.model
-    ΔŨ::Vector{VariableRef} = optim[:ΔŨ]
+    ΔŨvar::Vector{VariableRef} = optim[:ΔŨvar]
     lastΔŨ = mpc.ΔŨ
     # initial ΔŨ (warm-start): [Δu_{k-1}(k); Δu_{k-1}(k+1); ... ; 0_{nu × 1}]
     ΔŨ0 = [lastΔŨ[(model.nu+1):(mpc.Hc*model.nu)]; zeros(model.nu)]
     # if soft constraints, append the last slack value ϵ_{k-1}:
     !isinf(mpc.C) && (ΔŨ0 = [ΔŨ0; lastΔŨ[end]])
-    set_start_value.(ΔŨ, ΔŨ0)
-    init_objective!(mpc, ΔŨ)
+    set_start_value.(ΔŨvar, ΔŨ0)
+    set_objective!(mpc, ΔŨvar)
     try
         optimize!(optim)
     catch err
@@ -472,11 +472,13 @@ function optim_objective!(mpc::PredictiveController, p)
         @warn "MPC termination status not OPTIMAL or LOCALLY_SOLVED ($status)"
         @debug solution_summary(optim)
     end
-    mpc.ΔŨ[:] = isfatal(status) ? ΔŨ0 : value.(ΔŨ) # fatal status : use last value
+    mpc.ΔŨ[:] = isfatal(status) ? ΔŨ0 : value.(ΔŨvar) # fatal status : use last value
     J_val = objective_value(optim) + p # add LinModel p constant (p=0 for NonLinModel) 
     return mpc.ΔŨ, J_val
 end
 
+"By default, no change to the objective function."
+set_objective!(::PredictiveController, _ ) = nothing
 
 "Evaluate current output of `InternalModel` estimator."
 evalŷ!(estim::InternalModel, ym, d) = (mpc.ŷ[:] = estim(ym, d))