Merge pull request #141 from JuliaControl/reduce_alloc_jacobian!

added: `linearize!` is now allocation free

Author: franckgaga

src/controller/explicitmpc.jl

@@ -31,7 +31,7 @@ struct ExplicitMPC{NT<:Real, SE<:StateEstimator} <: PredictiveController{NT}
     Yop::Vector{NT}
     Dop::Vector{NT}
     buffer::PredictiveControllerBuffer{NT}
-    function ExplicitMPC{NT, SE}(
+    function ExplicitMPC{NT}(
         estim::SE, Hp, Hc, M_Hp, N_Hc, L_Hp
     ) where {NT<:Real, SE<:StateEstimator}
         model = estim.model
@@ -158,7 +158,7 @@ function ExplicitMPC(
         @warn("prediction horizon Hp ($Hp) ≤ estimated number of delays in model "*
               "($nk), the closed-loop system may be unstable or zero-gain (unresponsive)")
     end
-    return ExplicitMPC{NT, SE}(estim, Hp, Hc, M_Hp, N_Hc, L_Hp)
+    return ExplicitMPC{NT}(estim, Hp, Hc, M_Hp, N_Hc, L_Hp)
 end
 
 setconstraint!(::ExplicitMPC; kwargs...) = error("ExplicitMPC does not support constraints.")

src/controller/linmpc.jl

@@ -40,7 +40,7 @@ struct LinMPC{
     Yop::Vector{NT}
     Dop::Vector{NT}
     buffer::PredictiveControllerBuffer{NT}
-    function LinMPC{NT, SE, JM}(
+    function LinMPC{NT}(
         estim::SE, Hp, Hc, M_Hp, N_Hc, L_Hp, Cwt, optim::JM
     ) where {NT<:Real, SE<:StateEstimator, JM<:JuMP.GenericModel}
         model = estim.model
@@ -237,7 +237,7 @@ function LinMPC(
         @warn("prediction horizon Hp ($Hp) ≤ estimated number of delays in model "*
               "($nk), the closed-loop system may be unstable or zero-gain (unresponsive)")
     end
-    return LinMPC{NT, SE, JM}(estim, Hp, Hc, M_Hp, N_Hc, L_Hp, Cwt, optim)
+    return LinMPC{NT}(estim, Hp, Hc, M_Hp, N_Hc, L_Hp, Cwt, optim)
 end
 
 """

src/controller/nonlinmpc.jl

@@ -4,9 +4,9 @@ struct NonLinMPC{
     NT<:Real, 
     SE<:StateEstimator,
     JM<:JuMP.GenericModel, 
+    P<:Any,
     JEfunc<:Function,
-    GCfunc<:Function,
-    P<:Any
+    GCfunc<:Function
 } <: PredictiveController{NT}
     estim::SE
     # note: `NT` and the number type `JNT` in `JuMP.GenericModel{JNT}` can be
@@ -45,16 +45,16 @@ struct NonLinMPC{
     Yop::Vector{NT}
     Dop::Vector{NT}
     buffer::PredictiveControllerBuffer{NT}
-    function NonLinMPC{NT, SE, JM, JEfunc, GCfunc, P}(
+    function NonLinMPC{NT}(
         estim::SE, 
         Hp, Hc, M_Hp, N_Hc, L_Hp, Cwt, Ewt, JE::JEfunc, gc!::GCfunc, nc, p::P, optim::JM
     ) where {
             NT<:Real, 
             SE<:StateEstimator, 
-            JM<:JuMP.GenericModel, 
+            JM<:JuMP.GenericModel,
+            P<:Any,
             JEfunc<:Function, 
             GCfunc<:Function, 
-            P<:Any
         }
         model = estim.model
         nu, ny, nd, nx̂ = model.nu, model.ny, model.nd, estim.nx̂
@@ -80,7 +80,7 @@ struct NonLinMPC{
         nΔŨ = size(Ẽ, 2)
         ΔŨ = zeros(NT, nΔŨ)
         buffer = PredictiveControllerBuffer{NT}(nu, ny, nd, Hp, Hc, nϵ)
-        mpc = new{NT, SE, JM, JEfunc, GCfunc, P}(
+        mpc = new{NT, SE, JM, P, JEfunc, GCfunc}(
             estim, optim, con,
             ΔŨ, ŷ,
             Hp, Hc, nϵ,
@@ -317,7 +317,7 @@ function NonLinMPC(
     L_Hp = diagm(repeat(Lwt, Hp)),
     Cwt  = DEFAULT_CWT,
     Ewt  = DEFAULT_EWT,
-    JE ::JEfunc   = (_,_,_,_) -> 0.0,
+    JE ::Function = (_,_,_,_) -> 0.0,
     gc!::Function = (_,_,_,_,_,_) -> nothing,
     gc ::Function = gc!,
     nc = 0,
@@ -327,7 +327,6 @@ function NonLinMPC(
     NT<:Real, 
     SE<:StateEstimator{NT}, 
     JM<:JuMP.GenericModel, 
-    JEfunc<:Function,
     P<:Any
 }
     nk = estimate_delays(estim.model)
@@ -337,8 +336,7 @@ function NonLinMPC(
     end
     validate_JE(NT, JE)
     gc! = get_mutating_gc(NT, gc)
-    GCfunc = get_type_mutating_gc(gc!)
-    return NonLinMPC{NT, SE, JM, JEfunc, GCfunc, P}(
+    return NonLinMPC{NT}(
         estim, Hp, Hc, M_Hp, N_Hc, L_Hp, Cwt, Ewt, JE, gc!, nc, p, optim
     )
 end
@@ -396,9 +394,6 @@ function get_mutating_gc(NT, gc)
     return gc!
 end
 
-"Get the type of the mutating version of the custom constrain function `gc!`."
-get_type_mutating_gc(::GCfunc) where {GCfunc<:Function} = GCfunc
-
 """
     test_custom_functions(NT, model::SimModel, JE, gc!, nc, Uop, Yop, Dop, p)
 

src/estimator/internal_model.jl

@@ -29,7 +29,7 @@ struct InternalModel{NT<:Real, SM<:SimModel} <: StateEstimator{NT}
     direct::Bool
     corrected::Vector{Bool}
     buffer::StateEstimatorBuffer{NT}
-    function InternalModel{NT, SM}(
+    function InternalModel{NT}(
         model::SM, i_ym, Asm, Bsm, Csm, Dsm
     ) where {NT<:Real, SM<:SimModel}
         nu, ny, nd = model.nu, model.ny, model.nd
@@ -117,7 +117,7 @@ function InternalModel(
             stoch_ym   = c2d(stoch_ym_c, model.Ts, :tustin)
         end
     end
-    return InternalModel{NT, SM}(model, i_ym, stoch_ym.A, stoch_ym.B, stoch_ym.C, stoch_ym.D)
+    return InternalModel{NT}(model, i_ym, stoch_ym.A, stoch_ym.B, stoch_ym.C, stoch_ym.D)
 end
 
 "Validate if deterministic `model` and stochastic model `Csm, Dsm` for `InternalModel`s."

src/estimator/kalman.jl

@@ -27,7 +27,7 @@ struct SteadyKalmanFilter{NT<:Real, SM<:LinModel} <: StateEstimator{NT}
     direct::Bool
     corrected::Vector{Bool}
     buffer::StateEstimatorBuffer{NT}
-    function SteadyKalmanFilter{NT, SM}(
+    function SteadyKalmanFilter{NT}(
         model::SM, i_ym, nint_u, nint_ym, Q̂, R̂; direct=true
     ) where {NT<:Real, SM<:LinModel}
         nu, ny, nd = model.nu, model.ny, model.nd
@@ -182,7 +182,7 @@ function SteadyKalmanFilter(
     # estimated covariances matrices (variance = σ²) :
     Q̂  = Hermitian(diagm(NT[σQ;  σQint_u;  σQint_ym ].^2), :L)
     R̂  = Hermitian(diagm(NT[σR;].^2), :L)
-    return SteadyKalmanFilter{NT, SM}(model, i_ym, nint_u, nint_ym, Q̂, R̂; direct)
+    return SteadyKalmanFilter{NT}(model, i_ym, nint_u, nint_ym, Q̂, R̂; direct)
 end
 
 @doc raw"""
@@ -196,7 +196,7 @@ function SteadyKalmanFilter(
     model::SM, i_ym, nint_u, nint_ym, Q̂, R̂; direct=true
 ) where {NT<:Real, SM<:LinModel{NT}}
     Q̂, R̂ = to_mat(Q̂), to_mat(R̂)
-    return SteadyKalmanFilter{NT, SM}(model, i_ym, nint_u, nint_ym, Q̂, R̂; direct)
+    return SteadyKalmanFilter{NT}(model, i_ym, nint_u, nint_ym, Q̂, R̂; direct)
 end
 
 "Throw an error if `setmodel!` is called on a SteadyKalmanFilter w/o the default values."
@@ -308,7 +308,7 @@ struct KalmanFilter{NT<:Real, SM<:LinModel} <: StateEstimator{NT}
     direct::Bool
     corrected::Vector{Bool}
     buffer::StateEstimatorBuffer{NT}
-    function KalmanFilter{NT, SM}(
+    function KalmanFilter{NT}(
         model::SM, i_ym, nint_u, nint_ym, P̂_0, Q̂, R̂; direct=true
     ) where {NT<:Real, SM<:LinModel}
         nu, ny, nd = model.nu, model.ny, model.nd
@@ -419,7 +419,7 @@ function KalmanFilter(
     P̂_0 = Hermitian(diagm(NT[σP_0; σPint_u_0; σPint_ym_0].^2), :L)
     Q̂  = Hermitian(diagm(NT[σQ;  σQint_u;  σQint_ym ].^2), :L)
     R̂  = Hermitian(diagm(NT[σR;].^2), :L)
-    return KalmanFilter{NT, SM}(model, i_ym, nint_u, nint_ym, P̂_0, Q̂ , R̂; direct)
+    return KalmanFilter{NT}(model, i_ym, nint_u, nint_ym, P̂_0, Q̂ , R̂; direct)
 end
 
 @doc raw"""
@@ -433,7 +433,7 @@ function KalmanFilter(
     model::SM, i_ym, nint_u, nint_ym, P̂_0, Q̂, R̂; direct=true
 ) where {NT<:Real, SM<:LinModel{NT}}
     P̂_0, Q̂, R̂ = to_mat(P̂_0), to_mat(Q̂), to_mat(R̂)
-    return KalmanFilter{NT, SM}(model, i_ym, nint_u, nint_ym, P̂_0, Q̂, R̂; direct)
+    return KalmanFilter{NT}(model, i_ym, nint_u, nint_ym, P̂_0, Q̂, R̂; direct)
 end
 
 @doc raw"""
@@ -530,7 +530,7 @@ struct UnscentedKalmanFilter{NT<:Real, SM<:SimModel} <: StateEstimator{NT}
     direct::Bool
     corrected::Vector{Bool}
     buffer::StateEstimatorBuffer{NT}
-    function UnscentedKalmanFilter{NT, SM}(
+    function UnscentedKalmanFilter{NT}(
         model::SM, i_ym, nint_u, nint_ym, P̂_0, Q̂, R̂, α, β, κ; direct=true
     ) where {NT<:Real, SM<:SimModel{NT}}
         nu, ny, nd = model.nu, model.ny, model.nd
@@ -681,7 +681,7 @@ function UnscentedKalmanFilter(
     P̂_0 = Hermitian(diagm(NT[σP_0; σPint_u_0; σPint_ym_0].^2), :L)
     Q̂  = Hermitian(diagm(NT[σQ;  σQint_u;  σQint_ym ].^2), :L)
     R̂  = Hermitian(diagm(NT[σR;].^2), :L)
-    return UnscentedKalmanFilter{NT, SM}(
+    return UnscentedKalmanFilter{NT}(
         model, i_ym, nint_u, nint_ym, P̂_0, Q̂, R̂, α, β, κ; direct
     )
 end
@@ -699,7 +699,7 @@ function UnscentedKalmanFilter(
     model::SM, i_ym, nint_u, nint_ym, P̂_0, Q̂, R̂, α=1e-3, β=2, κ=0; direct=true
 ) where {NT<:Real, SM<:SimModel{NT}}
     P̂_0, Q̂, R̂ = to_mat(P̂_0), to_mat(Q̂), to_mat(R̂)
-    return UnscentedKalmanFilter{NT, SM}(
+    return UnscentedKalmanFilter{NT}(
         model, i_ym, nint_u, nint_ym, P̂_0, Q̂ , R̂, α, β, κ; direct
     )
 end
@@ -905,7 +905,7 @@ struct ExtendedKalmanFilter{NT<:Real, SM<:SimModel} <: StateEstimator{NT}
     direct::Bool
     corrected::Vector{Bool}
     buffer::StateEstimatorBuffer{NT}
-    function ExtendedKalmanFilter{NT, SM}(
+    function ExtendedKalmanFilter{NT}(
         model::SM, i_ym, nint_u, nint_ym, P̂_0, Q̂, R̂; direct=true
     ) where {NT<:Real, SM<:SimModel}
         nu, ny, nd = model.nu, model.ny, model.nd
@@ -994,7 +994,7 @@ function ExtendedKalmanFilter(
     P̂_0 = Hermitian(diagm(NT[σP_0; σPint_u_0; σPint_ym_0].^2), :L)
     Q̂  = Hermitian(diagm(NT[σQ;  σQint_u;  σQint_ym ].^2), :L)
     R̂  = Hermitian(diagm(NT[σR;].^2), :L)
-    return ExtendedKalmanFilter{NT, SM}(model, i_ym, nint_u, nint_ym, P̂_0, Q̂, R̂; direct)
+    return ExtendedKalmanFilter{NT}(model, i_ym, nint_u, nint_ym, P̂_0, Q̂, R̂; direct)
 end
 
 @doc raw"""
@@ -1008,7 +1008,7 @@ function ExtendedKalmanFilter(
     model::SM, i_ym, nint_u, nint_ym,P̂_0, Q̂, R̂; direct=true
 ) where {NT<:Real, SM<:SimModel{NT}}
     P̂_0, Q̂, R̂ = to_mat(P̂_0), to_mat(Q̂), to_mat(R̂)
-    return ExtendedKalmanFilter{NT, SM}(model, i_ym, nint_u, nint_ym, P̂_0, Q̂, R̂; direct)
+    return ExtendedKalmanFilter{NT}(model, i_ym, nint_u, nint_ym, P̂_0, Q̂, R̂; direct)
 end
 
 """

src/estimator/mhe/construct.jl

@@ -107,7 +107,7 @@ struct MovingHorizonEstimator{
     direct::Bool
     corrected::Vector{Bool}
     buffer::StateEstimatorBuffer{NT}
-    function MovingHorizonEstimator{NT, SM, JM, CE}(
+    function MovingHorizonEstimator{NT}(
         model::SM, He, i_ym, nint_u, nint_ym, P̂_0, Q̂, R̂, Cwt, optim::JM, covestim::CE;
         direct=true
     ) where {NT<:Real, SM<:SimModel{NT}, JM<:JuMP.GenericModel, CE<:StateEstimator{NT}}
@@ -406,7 +406,7 @@ function MovingHorizonEstimator(
     covestim::CE = default_covestim_mhe(model, i_ym, nint_u, nint_ym, P̂_0, Q̂, R̂; direct)
 ) where {NT<:Real, SM<:SimModel{NT}, JM<:JuMP.GenericModel, CE<:StateEstimator{NT}}
     P̂_0, Q̂, R̂ = to_mat(P̂_0), to_mat(Q̂), to_mat(R̂)
-    return MovingHorizonEstimator{NT, SM, JM, CE}(
+    return MovingHorizonEstimator{NT}(
         model, He, i_ym, nint_u, nint_ym, P̂_0, Q̂ , R̂, Cwt, optim, covestim; direct
     )
 end

src/model/linearization.jl

@@ -1,3 +1,31 @@
+
+function jacobianA!(A, jb::JacobianBuffer, model::SimModel, x, u, d)
+    jb.x .= x; jb.u .= u; jb.d .= d
+    return ForwardDiff.jacobian!(A, jb.f_x!, jb.xnext, jb.x, jb.f_x_cfg)
+end
+jacobianA!( _ , _ , model::LinModel, _ , _ , _ ) = model.A
+function jacobianBu!(Bu, jb::JacobianBuffer, model::SimModel, x, u, d)
+    jb.x .= x; jb.u .= u; jb.d .= d
+    return ForwardDiff.jacobian!(Bu, jb.f_u!, jb.xnext, jb.u, jb.f_u_cfg)
+end
+jacobianBu!( _ , _ , model::LinModel, _ , _ , _ ) = model.Bu
+function jacobianBd!(Bd, jb::JacobianBuffer, model::SimModel, x, u, d)
+    jb.x .= x; jb.u .= u; jb.d .= d
+    return ForwardDiff.jacobian!(Bd, jb.f_d!, jb.xnext, jb.d, jb.f_d_cfg)
+end
+jacobianBd!( _ , _ , model::LinModel, _ , _ , _ ) = model.Bd
+function jacobianC!(C, jb::JacobianBuffer, model::SimModel, x, d)
+    jb.x .= x; jb.d .= d
+    return ForwardDiff.jacobian!(C, jb.h_x!, jb.y, jb.x, jb.h_x_cfg)
+end
+jacobianC!( _ , _ , model::LinModel, _ , _ ) = model.C
+function jacobianDd!(Dd, jb::JacobianBuffer, model::SimModel, x, d)
+    jb.x .= x; jb.d .= d
+    return ForwardDiff.jacobian!(Dd, jb.h_d!, jb.y, jb.d, jb.h_d_cfg)
+end
+jacobianDd!( _ , _ , model::LinModel, _ , _ ) = model.Dd
+
+
 """
     LinModel(model::NonLinModel; x=model.x0+model.xop, u=model.uop, d=model.dop)
 
@@ -118,19 +146,25 @@ function linearize!(
     d0 .= d .- nonlinmodel.dop
     x0 .= x .- nonlinmodel.xop
     # --- compute the Jacobians at linearization points ---
-    A::Matrix{NT}, Bu::Matrix{NT}, Bd::Matrix{NT}  = linmodel.A, linmodel.Bu, linmodel.Bd
-    C::Matrix{NT}, Dd::Matrix{NT} = linmodel.C, linmodel.Dd
+    #A::Matrix{NT}, Bu::Matrix{NT}, Bd::Matrix{NT}  = linmodel.A, linmodel.Bu, linmodel.Bd
+    #C::Matrix{NT}, Dd::Matrix{NT} = linmodel.C, linmodel.Dd
     xnext0::Vector{NT}, y0::Vector{NT} = linmodel.buffer.x, linmodel.buffer.y
-    myf_x0!(xnext0, x0) = f!(xnext0, nonlinmodel, x0, u0, d0, model.p)
-    myf_u0!(xnext0, u0) = f!(xnext0, nonlinmodel, x0, u0, d0, model.p)
-    myf_d0!(xnext0, d0) = f!(xnext0, nonlinmodel, x0, u0, d0, model.p)
-    myh_x0!(y0, x0) = h!(y0, nonlinmodel, x0, d0, model.p)
-    myh_d0!(y0, d0) = h!(y0, nonlinmodel, x0, d0, model.p)
-    ForwardDiff.jacobian!(A,  myf_x0!, xnext0, x0)
-    ForwardDiff.jacobian!(Bu, myf_u0!, xnext0, u0)
-    ForwardDiff.jacobian!(Bd, myf_d0!, xnext0, d0)
-    ForwardDiff.jacobian!(C,  myh_x0!, y0, x0)
-    ForwardDiff.jacobian!(Dd, myh_d0!, y0, d0)
+    #myf_x0!(xnext0, x0) = f!(xnext0, nonlinmodel, x0, u0, d0, model.p)
+    #myf_u0!(xnext0, u0) = f!(xnext0, nonlinmodel, x0, u0, d0, model.p)
+    #myf_d0!(xnext0, d0) = f!(xnext0, nonlinmodel, x0, u0, d0, model.p)
+    #myh_x0!(y0, x0) = h!(y0, nonlinmodel, x0, d0, model.p)
+    #myh_d0!(y0, d0) = h!(y0, nonlinmodel, x0, d0, model.p)
+    #ForwardDiff.jacobian!(A,  myf_x0!, xnext0, x0)
+    #ForwardDiff.jacobian!(Bu, myf_u0!, xnext0, u0)
+    #ForwardDiff.jacobian!(Bd, myf_d0!, xnext0, d0)
+    #ForwardDiff.jacobian!(C,  myh_x0!, y0, x0)
+    #ForwardDiff.jacobian!(Dd, myh_d0!, y0, d0)
+    jb = nonlinmodel.buffer.jacobian
+    jacobianA!(linmodel.A,   jb, nonlinmodel, x0, u0, d0)
+    jacobianBu!(linmodel.Bu, jb, nonlinmodel, x0, u0, d0)
+    jacobianBd!(linmodel.Bd, jb, nonlinmodel, x0, u0, d0)
+    jacobianC!(linmodel.C,   jb, nonlinmodel, x0, d0)
+    jacobianDd!(linmodel.Dd, jb, nonlinmodel, x0, d0)
     # --- compute the nonlinear model output at operating points ---
     h!(y0, nonlinmodel, x0, d0, model.p)
     y  = y0

src/model/linmodel.jl

@@ -21,7 +21,7 @@ struct LinModel{NT<:Real} <: SimModel{NT}
     yname::Vector{String}
     dname::Vector{String}
     xname::Vector{String}
-    buffer::SimModelBuffer{NT}
+    buffer::SimModelBuffer{NT, Nothing}
     function LinModel{NT}(A, Bu, C, Bd, Dd, Ts) where {NT<:Real}
         A, Bu = to_mat(A, 1, 1), to_mat(Bu, 1, 1)
         nu, nx = size(Bu, 2), size(A, 2)

src/model/nonlinmodel.jl

@@ -1,5 +1,5 @@
 struct NonLinModel{
-    NT<:Real, F<:Function, H<:Function, P<:Any, DS<:DiffSolver
+    NT<:Real, F<:Function, H<:Function, P<:Any, DS<:DiffSolver, SMB<:SimModelBuffer
 } <: SimModel{NT}
     x0::Vector{NT}
     f!::F
@@ -21,10 +21,10 @@ struct NonLinModel{
     yname::Vector{String}
     dname::Vector{String}
     xname::Vector{String}
-    buffer::SimModelBuffer{NT}
-    function NonLinModel{NT, F, H, P, DS}(
-        f!::F, h!::H, Ts, nu, nx, ny, nd, p::P, solver::DS
-    ) where {NT<:Real, F<:Function, H<:Function, P<:Any, DS<:DiffSolver}
+    buffer::SMB
+    function NonLinModel{NT}(
+        f!::F, h!::H, Ts, nu, nx, ny, nd, p::P, solver::DS, buffer::SMB
+    ) where {NT<:Real, F<:Function, H<:Function, P<:Any, DS<:DiffSolver, SMB<:SimModelBuffer}
         Ts > 0 || error("Sampling time Ts must be positive")
         uop = zeros(NT, nu)
         yop = zeros(NT, ny)
@@ -37,8 +37,7 @@ struct NonLinModel{
         xname = ["\$x_{$i}\$" for i in 1:nx]
         x0 = zeros(NT, nx)
         t  = zeros(NT, 1)
-        buffer = SimModelBuffer{NT}(nu, nx, ny, nd)
-        return new{NT, F, H, P, DS}(
+        return new{NT, F, H, P, DS, SMB}(
             x0, 
             f!, h!,
             p,
@@ -145,8 +144,9 @@ function NonLinModel{NT}(
     isnothing(solver) && (solver=EmptySolver())
     f!, h! = get_mutating_functions(NT, f, h)
     f!, h! = get_solver_functions(NT, solver, f!, h!, Ts, nu, nx, ny, nd)
-    F, H, P, DS = get_types(f!, h!, p, solver)
-    return NonLinModel{NT, F, H, P, DS}(f!, h!, Ts, nu, nx, ny, nd, p, solver)
+    jacobian = JacobianBuffer{NT}(f!, h!, nu, nx, ny, nd, p)
+    buffer = SimModelBuffer{NT}(nu, nx, ny, nd, jacobian)
+    return NonLinModel{NT}(f!, h!, Ts, nu, nx, ny, nd, p, solver, buffer)
 end
 
 function NonLinModel(
@@ -224,13 +224,6 @@ function validate_h(NT, h)
     return ismutating
 end
 
-"Get the types of `f!`, `h!` and `solver` to construct a `NonLinModel`."
-function get_types(
-    ::F, ::H, ::P, ::DS
-) where {F<:Function, H<:Function, P<:Any, DS<:DiffSolver} 
-    return F, H, P, DS
-end
-
 "Do nothing if `model` is a [`NonLinModel`](@ref)."
 steadystate!(::SimModel, _ , _ ) = nothing