wip: multiple shooting transcription

Author: franckgaga

docs/src/internals/predictive_control.md

@@ -36,5 +36,6 @@ ModelPredictiveControl.linconstraint!(::PredictiveController, ::LinModel)
 
 ```@docs
 ModelPredictiveControl.optim_objective!(::PredictiveController)
+ModelPredictiveControl.set_warmstart!
 ModelPredictiveControl.getinput
 ```

src/controller/construct.jl

@@ -1,3 +1,37 @@
+struct PredictiveControllerBuffer{NT<:Real}
+    u::Vector{NT}
+    Z̃::Vector{NT}
+    D̂::Vector{NT}
+    Ŷ::Vector{NT}
+    U::Vector{NT}
+    Ẽ::Matrix{NT}
+    S̃::Matrix{NT}
+    empty::Vector{NT}
+end
+
+@doc raw"""
+    PredictiveControllerBuffer(estim, transcription, Hp, Hc, nϵ)
+
+Create a buffer for `PredictiveController` objects.
+
+The buffer is used to store intermediate results during computation without allocating.
+"""
+function PredictiveControllerBuffer(
+    estim::StateEstimator{NT}, transcription::TranscriptionMethod, Hp::Int, Hc::Int, nϵ::Int
+) where NT <: Real
+    nu, ny, nd, nx̂ = estim.model.nu, estim.model.ny, estim.model.nd, estim.nx̂
+    nZ̃ = get_nZ̃(estim, transcription, Hp, Hc, nϵ)
+    u = Vector{NT}(undef, nu)
+    Z̃ = Vector{NT}(undef, nZ̃)
+    D̂ = Vector{NT}(undef, nd*Hp)
+    Ŷ = Vector{NT}(undef, ny*Hp)
+    U = Vector{NT}(undef, nu*Hp)
+    Ẽ = Matrix{NT}(undef, ny*Hp, nZ̃)
+    S̃ = Matrix{NT}(undef, nu*Hp, nZ̃)
+    empty = Vector{NT}(undef, 0)
+    return PredictiveControllerBuffer{NT}(u, Z̃, D̂, Ŷ, U, Ẽ, S̃, empty)
+end
+
 "Include all the objective function weights of [`PredictiveController`](@ref)"
 struct ControllerWeights{NT<:Real}
     M_Hp::Hermitian{NT, Matrix{NT}}

src/controller/execute.jl

@@ -1,10 +1,10 @@
 @doc raw"""
     initstate!(mpc::PredictiveController, u, ym, d=[]) -> x̂
 
-Init the states of `mpc.estim` [`StateEstimator`](@ref) and warm start `mpc.ΔŨ` at zero.
+Init the states of `mpc.estim` [`StateEstimator`](@ref) and warm start `mpc.Z̃` at zero.
 """
 function initstate!(mpc::PredictiveController, u, ym, d=mpc.estim.buffer.empty)
-    mpc.ΔŨ .= 0
+    mpc.Z̃ .= 0
     return initstate!(mpc.estim, u, ym, d)
 end
 
@@ -67,8 +67,9 @@ function moveinput!(
     validate_args(mpc, ry, d, D̂, R̂y, R̂u)
     initpred!(mpc, mpc.estim.model, d, D̂, R̂y, R̂u)
     linconstraint!(mpc, mpc.estim.model)
-    ΔŨ = optim_objective!(mpc)
-    return getinput(mpc, ΔŨ)
+    linconstrainteq!(mpc, mpc.transcription)
+    Z̃ = optim_objective!(mpc)
+    return getinput(mpc, Z̃)
 end
 
 @doc raw"""
@@ -323,6 +324,22 @@ function linconstraint!(mpc::PredictiveController, ::SimModel)
     return nothing
 end
 
+linconstrainteq!(mpc::PredictiveController, transcription::SingleShooting) = nothing
+function linconstrainteq!(mpc::PredictiveController, transcription::MultipleShooting)
+    nx̂, Fŝ = mpc.estim.nx̂, mpc.con.Fŝ
+    Fŝ .= mpc.con.Bŝ
+    mul!(Fŝ, mpc.con.Kŝ, mpc.estim.x̂0, 1, 1)
+    mul!(Fŝ, mpc.con.Vŝ, mpc.estim.lastu0, 1, 1)
+    if mpc.estim.model.nd ≠ 0
+        mul!(Fŝ, mpc.con.Gŝ, mpc.d0, 1, 1)
+        mul!(Fŝ, mpc.con.Jŝ, mpc.D̂0, 1, 1)
+    end
+    mpc.con.beq .= @. -Fŝ
+    linconeq = mpc.optim[:linconstrainteq]
+    JuMP.set_normalized_rhs(linconeq, mpc.con.beq)
+    return nothing
+end
+
 @doc raw"""
     predict!(Ŷ0, x̂0, _, _, _, mpc::PredictiveController, model::LinModel, ΔŨ) -> Ŷ0, x̂0end
 
@@ -467,39 +484,21 @@ function obj_econ(::PredictiveController, ::SimModel, _ , ::AbstractVector{NT})
 end
 
 @doc raw"""
-    optim_objective!(mpc::PredictiveController) -> ΔŨ
+    optim_objective!(mpc::PredictiveController) -> Z̃
 
-Optimize the objective function of `mpc` [`PredictiveController`](@ref) and return the solution `ΔŨ`.
+Optimize the objective function of `mpc` [`PredictiveController`](@ref) and return the solution `Z̃`.
 
-If supported by `mpc.optim`, it warm-starts the solver at:
-```math
-\mathbf{ΔŨ} = 
-\begin{bmatrix}
-    \mathbf{Δu}_{k-1}(k+0)      \\ 
-    \mathbf{Δu}_{k-1}(k+1)      \\ 
-    \vdots                      \\
-    \mathbf{Δu}_{k-1}(k+H_c-2)  \\
-    \mathbf{0}                  \\
-    ϵ_{k-1}
-\end{bmatrix}
-```
-where ``\mathbf{Δu}_{k-1}(k+j)`` is the input increment for time ``k+j`` computed at the 
-last control period ``k-1``. It then calls `JuMP.optimize!(mpc.optim)` and extract the
-solution. A failed optimization prints an `@error` log in the REPL and returns the 
-warm-start value. A failed optimization also prints [`getinfo`](@ref) results in
-the debug log [if activated](https://docs.julialang.org/en/v1/stdlib/Logging/#Example:-Enable-debug-level-messages).
+If first warm-starts the solver with [`set_warm_start!`](@ref). It then calls 
+`JuMP.optimize!(mpc.optim)` and extract the solution. A failed optimization prints an 
+`@error` log in the REPL and returns the warm-start value. A failed optimization also prints
+[`getinfo`](@ref) results in the debug log [if activated](https://docs.julialang.org/en/v1/stdlib/Logging/#Example:-Enable-debug-level-messages).
 """
 function optim_objective!(mpc::PredictiveController{NT}) where {NT<:Real}
     model, optim = mpc.estim.model, mpc.optim
-    nu, Hc = model.nu, mpc.Hc
-    ΔŨvar::Vector{JuMP.VariableRef} = optim[:ΔŨvar]
-    # initial ΔŨ (warm-start): [Δu_{k-1}(k); Δu_{k-1}(k+1); ... ; 0_{nu × 1}; ϵ_{k-1}]
-    ΔŨ0 = mpc.buffer.ΔŨ
-    ΔŨ0[1:(Hc*nu-nu)] .= @views mpc.ΔŨ[nu+1:Hc*nu]
-    ΔŨ0[(Hc*nu-nu+1):(Hc*nu)] .= 0
-    mpc.nϵ == 1 && (ΔŨ0[end] = mpc.ΔŨ[end])
-    JuMP.set_start_value.(ΔŨvar, ΔŨ0)
-    set_objective_linear_coef!(mpc, ΔŨvar)
+    nu, Hc = model.nu, mpc.Hc 
+    Z̃var::Vector{JuMP.VariableRef} = optim[:Z̃var]
+    Z̃0 = set_warmstart!(mpc, mpc.transcription, Z̃var)
+    set_objective_linear_coef!(mpc, Z̃var)
     try
         JuMP.optimize!(optim)
     catch err
@@ -529,11 +528,11 @@ function optim_objective!(mpc::PredictiveController{NT}) where {NT<:Real}
         @debug info2debugstr(getinfo(mpc))
     end
     if iserror(optim)
-        mpc.ΔŨ .= ΔŨ0
+        mpc.Z̃ .= Z̃0
     else
-        mpc.ΔŨ .= JuMP.value.(ΔŨvar)
+        mpc.Z̃ .= JuMP.value.(Z̃var)
     end
-    return mpc.ΔŨ
+    return mpc.Z̃
 end
 
 "By default, no need to modify the objective function."
@@ -549,17 +548,17 @@ function preparestate!(mpc::PredictiveController, ym, d=mpc.estim.buffer.empty)
 end
 
 @doc raw"""
-    getinput(mpc::PredictiveController, ΔŨ) -> u
+    getinput(mpc::PredictiveController, Z̃) -> u
 
-Get current manipulated input `u` from a [`PredictiveController`](@ref) solution `ΔŨ`.
+Get current manipulated input `u` from a [`PredictiveController`](@ref) solution `Z̃`.
 
-The first manipulated input ``\mathbf{u}(k)`` is extracted from the input increments vector
-``\mathbf{ΔŨ}`` and applied on the plant (from the receding horizon principle).
+The first manipulated input ``\mathbf{u}(k)`` is extracted from the decision vector
+``\mathbf{Z̃}`` and applied on the plant (from the receding horizon principle).
 """
-function getinput(mpc, ΔŨ)
+function getinput(mpc, Z̃)
     Δu  = mpc.buffer.u
     for i in 1:mpc.estim.model.nu
-        Δu[i] = ΔŨ[i]
+        Δu[i] = Z̃[i]
     end
     u   = Δu
     u .+= mpc.estim.lastu0 .+ mpc.estim.model.uop

src/controller/explicitmpc.jl

@@ -1,8 +1,8 @@
 struct ExplicitMPC{NT<:Real, SE<:StateEstimator} <: PredictiveController{NT}
     estim::SE
     transcription::SingleShooting
-    ΔŨ::Vector{NT}
-    ŷ ::Vector{NT}
+    Z̃::Vector{NT}
+    ŷ::Vector{NT}
     Hp::Int
     Hc::Int
     nϵ::Int
@@ -62,13 +62,13 @@ struct ExplicitMPC{NT<:Real, SE<:StateEstimator} <: PredictiveController{NT}
         # dummy vals (updated just before optimization):
         d0, D̂0, D̂e = zeros(NT, nd), zeros(NT, nd*Hp), zeros(NT, nd + nd*Hp)
         Uop, Yop, Dop = repeat(model.uop, Hp), repeat(model.yop, Hp), repeat(model.dop, Hp)
-        nΔŨ = size(Ẽ, 2)
-        ΔŨ = zeros(NT, nΔŨ)
-        buffer = PredictiveControllerBuffer{NT}(nu, ny, nd, Hp, Hc, nϵ)
+        nZ̃ = get_nZ̃(estim, transcription, Hp, Hc, nϵ)
+        Z̃ = zeros(NT, nZ̃)
+        buffer = PredictiveControllerBuffer(estim, transcription, Hp, Hc, nϵ)
         mpc = new{NT, SE}(
             estim,
             transcription,
-            ΔŨ, ŷ,
+            Z̃, ŷ,
             Hp, Hc, nϵ,
             weights,
             R̂u, R̂y,

src/controller/linmpc.jl

@@ -13,8 +13,8 @@ struct LinMPC{
     # different since solvers that support non-Float64 are scarce.
     optim::JM
     con::ControllerConstraint{NT, Nothing}
-    ΔŨ::Vector{NT}
-    ŷ ::Vector{NT}
+    Z̃::Vector{NT}
+    ŷ::Vector{NT}
     Hp::Int
     Hc::Int
     nϵ::Int
@@ -75,12 +75,12 @@ struct LinMPC{
         # dummy vals (updated just before optimization):
         d0, D̂0, D̂e = zeros(NT, nd), zeros(NT, nd*Hp), zeros(NT, nd + nd*Hp)
         Uop, Yop, Dop = repeat(model.uop, Hp), repeat(model.yop, Hp), repeat(model.dop, Hp)
-        nΔŨ = size(Ẽ, 2)
-        ΔŨ = zeros(NT, nΔŨ)
-        buffer = PredictiveControllerBuffer{NT}(nu, ny, nd, Hp, Hc, nϵ)
+        nZ̃ = get_nZ̃(estim, transcription, Hp, Hc, nϵ)
+        Z̃ = zeros(NT, nZ̃)
+        buffer = PredictiveControllerBuffer(estim, transcription, Hp, Hc, nϵ)
         mpc = new{NT, SE, TM, JM}(
             estim, transcription, optim, con,
-            ΔŨ, ŷ,
+            Z̃, ŷ,
             Hp, Hc, nϵ,
             weights,
             R̂u, R̂y,
@@ -266,29 +266,29 @@ Init the quadratic optimization for [`LinMPC`](@ref) controllers.
 function init_optimization!(mpc::LinMPC, model::LinModel, optim)
     # --- variables and linear constraints ---
     con = mpc.con
-    nΔŨ = length(mpc.ΔŨ)
+    nZ̃ = length(mpc.Z̃)
     JuMP.num_variables(optim) == 0 || JuMP.empty!(optim)
     JuMP.set_silent(optim)
     limit_solve_time(mpc.optim, model.Ts)
-    @variable(optim, ΔŨvar[1:nΔŨ])
+    @variable(optim, Z̃var[1:nZ̃])
     A = con.A[con.i_b, :]
     b = con.b[con.i_b]
-    @constraint(optim, linconstraint, A*ΔŨvar .≤ b)
-    Aeq = con.A
-    beq = con.b
-    @constraint(optim, lineqconstraint, Aeq*ΔŨvar .== beq)
-    set_objective_hessian!(mpc, ΔŨvar)
+    @constraint(optim, linconstraint, A*Z̃var .≤ b)
+    Aeq = con.Aeq
+    beq = con.beq
+    @constraint(optim, linconstrainteq, Aeq*Z̃var .== beq)
+    set_objective_hessian!(mpc, Z̃var)
     return nothing
 end
 
 "For [`LinMPC`](@ref), set the QP linear coefficient `q̃` just before optimization."
-function set_objective_linear_coef!(mpc::LinMPC, ΔŨvar)
-    JuMP.set_objective_coefficient(mpc.optim, ΔŨvar, mpc.q̃)
+function set_objective_linear_coef!(mpc::LinMPC, Z̃var)
+    JuMP.set_objective_coefficient(mpc.optim, Z̃var, mpc.q̃)
     return nothing
 end
 
 "Update the quadratic objective function for [`LinMPC`](@ref) controllers."
-function set_objective_hessian!(mpc::LinMPC, ΔŨvar)
-    @objective(mpc.optim, Min, obj_quadprog(ΔŨvar, mpc.H̃, mpc.q̃))
+function set_objective_hessian!(mpc::LinMPC, Z̃var)
+    @objective(mpc.optim, Min, obj_quadprog(Z̃var, mpc.H̃, mpc.q̃))
     return nothing
 end

src/controller/nonlinmpc.jl

@@ -16,8 +16,8 @@ struct NonLinMPC{
     # different since solvers that support non-Float64 are scarce.
     optim::JM
     con::ControllerConstraint{NT, GCfunc}
-    ΔŨ::Vector{NT}
-    ŷ ::Vector{NT}
+    Z̃::Vector{NT}
+    ŷ::Vector{NT}
     Hp::Int
     Hc::Int
     nϵ::Int
@@ -91,12 +91,12 @@ struct NonLinMPC{
         d0, D̂0, D̂e = zeros(NT, nd), zeros(NT, nd*Hp), zeros(NT, nd + nd*Hp)
         Uop, Yop, Dop = repeat(model.uop, Hp), repeat(model.yop, Hp), repeat(model.dop, Hp)
         test_custom_functions(NT, model, JE, gc!, nc, Uop, Yop, Dop, p)
-        nΔŨ = size(Ẽ, 2)
-        ΔŨ = zeros(NT, nΔŨ)
-        buffer = PredictiveControllerBuffer{NT}(nu, ny, nd, Hp, Hc, nϵ)
+        nZ̃ = get_nZ̃(estim, transcription, Hp, Hc, nϵ)
+        Z̃ = zeros(NT, nZ̃)
+        buffer = PredictiveControllerBuffer(estim, transcription, Hp, Hc, nϵ)
         mpc = new{NT, SE, TM, JM, PT, JEfunc, GCfunc}(
             estim, transcription, optim, con,
-            ΔŨ, ŷ,
+            Z̃, ŷ,
             Hp, Hc, nϵ,
             weights,
             JE, p,
@@ -484,17 +484,17 @@ Init the nonlinear optimization for [`NonLinMPC`](@ref) controllers.
 function init_optimization!(mpc::NonLinMPC, model::SimModel, optim)
     # --- variables and linear constraints ---
     con = mpc.con
-    nΔŨ = length(mpc.ΔŨ)
+    nZ̃ = length(mpc.Z̃)
     JuMP.num_variables(optim) == 0 || JuMP.empty!(optim)
     JuMP.set_silent(optim)
     limit_solve_time(mpc.optim, mpc.estim.model.Ts)
-    @variable(optim, ΔŨvar[1:nΔŨ])
+    @variable(optim, Z̃var[1:nZ̃])
     A = con.A[con.i_b, :]
     b = con.b[con.i_b]
-    @constraint(optim, linconstraint, A*ΔŨvar .≤ b)
+    @constraint(optim, linconstraint, A*Z̃var .≤ b)
     Aeq = con.A
     beq = con.b
-    @constraint(optim, lineqconstraint, Aeq*ΔŨvar .== beq)
+    @constraint(optim, lineqconstraint, Aeq*Z̃var .== beq)
     # --- nonlinear optimization init ---
     if mpc.nϵ == 1 && JuMP.solver_name(optim) == "Ipopt"
         C = mpc.weights.Ñ_Hc[end]
@@ -506,8 +506,8 @@ function init_optimization!(mpc::NonLinMPC, model::SimModel, optim)
         end
     end
     Jfunc, gfuncs = get_optim_functions(mpc, mpc.optim)
-    @operator(optim, J, nΔŨ, Jfunc)
-    @objective(optim, Min, J(ΔŨvar...))
+    @operator(optim, J, nZ̃, Jfunc)
+    @objective(optim, Min, J(Z̃var...))
     init_nonlincon!(mpc, model, gfuncs)
     set_nonlincon!(mpc, model, mpc.optim)
     return nothing