added: linearize! is now allocation-free

Author: franckgaga

src/model/linearization.jl

@@ -146,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/nonlinmodel.jl

@@ -144,7 +144,7 @@ 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)
-    jacobian = JacobianBuffer{NT}(f!, h!, nu, nx, ny, nd)
+    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

src/sim_model.jl

@@ -34,9 +34,9 @@ struct JacobianBuffer{
     CHX<:ForwardDiff.JacobianConfig
 }
     xnext::Vector{NT}
+    u::Vector{NT}
     x::Vector{NT}
     y::Vector{NT}
-    u::Vector{NT}
     d::Vector{NT}
     f_x!::FX
     f_u!::FU
@@ -48,33 +48,34 @@ struct JacobianBuffer{
     f_d_cfg::CFD
     h_x_cfg::CHX
     h_d_cfg::CHU
+    
 end
 
 """
-    JacobianBuffer(NT::DataType, f!::Function, h!::Function, nx, nu, ny, nd)
+    JacobianBuffer(NT::DataType, f!::Function, h!::Function, nu, nx, ny, nd)
 
 Buffer object for calling `ForwardDiff.jacobian!` on `SimModel` without any allocation.
 """
 function JacobianBuffer{NT}(
-    f!::Function, h!::Function, nx::Int, nu::Int, ny::Int, nd::Int
+    f!::Function, h!::Function, nu::Int, nx::Int, ny::Int, nd::Int, p
 ) where NT <: Real 
     xnext = Vector{NT}(undef, nx)
+    u = Vector{NT}(undef, nu)
     x = Vector{NT}(undef, nx)
     y = Vector{NT}(undef, ny)
-    u = Vector{NT}(undef, nu)
     d = Vector{NT}(undef, nd)
-    f_x!(y, x) = f!(y, x, u, d)
-    f_u!(y, u) = f!(y, x, u, d)
-    f_d!(y, d) = f!(y, x, u, d)
-    h_x!(y, x) = h!(y, x, d)
-    h_d!(y, d) = h!(y, x, d)
+    f_x!(xnext, x) = f!(xnext, x, u, d, p)
+    f_u!(xnext, u) = f!(xnext, x, u, d, p)
+    f_d!(xnext, d) = f!(xnext, x, u, d, p)
+    h_x!(y, x) = h!(y, x, d, p)
+    h_d!(y, d) = h!(y, x, d, p)
     f_x_cfg = ForwardDiff.JacobianConfig(f_x!, xnext, x)
     f_u_cfg = ForwardDiff.JacobianConfig(f_u!, xnext, u)
     f_d_cfg = ForwardDiff.JacobianConfig(f_d!, xnext, d)
     h_x_cfg = ForwardDiff.JacobianConfig(h_x!, y, x)
     h_d_cfg = ForwardDiff.JacobianConfig(h_d!, y, d)
     return JacobianBuffer(
-        xnext,      x,          y,          u,          d,
+        xnext,      u,          x,          y,           d,
         f_x!,       f_u!,       f_d!,       h_x!,       h_d!, 
         f_x_cfg,    f_u_cfg,    f_d_cfg,    h_x_cfg,    h_d_cfg
     )