Merge pull request #186 from JuliaControl/debug_linearize

debug: do not call `jacobian!` if `nd==0` in `linearize`

Author: franckgaga

Project.toml

@@ -1,7 +1,7 @@
 name = "ModelPredictiveControl"
 uuid = "61f9bdb8-6ae4-484a-811f-bbf86720c31c"
 authors = ["Francis Gagnon"]
-version = "1.5.1"
+version = "1.5.2"
 
 [deps]
 ControlSystemsBase = "aaaaaaaa-a6ca-5380-bf3e-84a91bcd477e"

src/controller/execute.jl

@@ -208,7 +208,7 @@ function initpred!(mpc::PredictiveController, model::LinModel, d, D̂, R̂y, R̂
     F .+= mpc.B                                 # F = F + B
     mul!(F, mpc.K, mpc.estim.x̂0, 1, 1)          # F = F + K*x̂0
     mul!(F, mpc.V, mpc.estim.lastu0, 1, 1)      # F = F + V*lastu0
-    if model.nd ≠ 0
+    if model.nd > 0
         mul!(F, mpc.G, mpc.d0, 1, 1)            # F = F + G*d0
         mul!(F, mpc.J, mpc.D̂0, 1, 1)            # F = F + J*D̂0
     end
@@ -256,7 +256,7 @@ is an [`InternalModel`](@ref). The function returns `mpc.F`.
 function initpred_common!(mpc::PredictiveController, model::SimModel, d, D̂, R̂y, R̂u)
     mul!(mpc.Tu_lastu0, mpc.Tu, mpc.estim.lastu0)
     mpc.ŷ .= evaloutput(mpc.estim, d)
-    if model.nd ≠ 0
+    if model.nd > 0
         mpc.d0 .= d .- model.dop
         mpc.D̂0 .= D̂ .- mpc.Dop
         mpc.D̂e[1:model.nd]     .= d

src/controller/transcription.jl

@@ -302,7 +302,7 @@ function init_predmat(
     G  = Matrix{NT}(undef, Hp*ny, nd)
     jx̂ = Matrix{NT}(undef, nx̂, Hp*nd)
     J  = repeatdiag(D̂d, Hp)
-    if nd ≠ 0
+    if nd > 0
         for j=1:Hp
             iRow = (1:ny) .+ ny*(j-1)
             G[iRow,:] = Ĉ*getpower(Âpow, j-1)*B̂d
@@ -853,7 +853,7 @@ function linconstraint!(mpc::PredictiveController, model::LinModel, ::Transcript
     fx̂ .= mpc.con.bx̂
     mul!(fx̂, mpc.con.kx̂, mpc.estim.x̂0, 1, 1)
     mul!(fx̂, mpc.con.vx̂, mpc.estim.lastu0, 1, 1)
-    if model.nd ≠ 0
+    if model.nd > 0
         mul!(fx̂, mpc.con.gx̂, mpc.d0, 1, 1)
         mul!(fx̂, mpc.con.jx̂, mpc.D̂0, 1, 1)
     end
@@ -939,7 +939,7 @@ function linconstrainteq!(mpc::PredictiveController, model::LinModel, ::Multiple
     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 model.nd ≠ 0
+    if model.nd > 0
         mul!(Fŝ, mpc.con.Gŝ, mpc.d0, 1, 1)
         mul!(Fŝ, mpc.con.Jŝ, mpc.D̂0, 1, 1)
     end

src/estimator/mhe/execute.jl

@@ -191,7 +191,7 @@ function add_data_windows!(estim::MovingHorizonEstimator, y0m, d0, u0=estim.last
     if ismoving
         estim.Y0m[1:end-nym]     .= @views estim.Y0m[nym+1:end]
         estim.Y0m[end-nym+1:end] .= y0m
-        if nd ≠ 0
+        if nd > 0
             estim.D0[1:end-nd]       .= @views estim.D0[nd+1:end]
             estim.D0[end-nd+1:end]   .= d0
         end
@@ -204,7 +204,7 @@ function add_data_windows!(estim::MovingHorizonEstimator, y0m, d0, u0=estim.last
         estim.Nk .= estim.He
     else
         estim.Y0m[(1 + nym*(Nk-1)):(nym*Nk)]  .= y0m
-        if nd ≠ 0
+        if nd > 0
             # D0 include 1 additional measured disturbance if direct==true (p==0):
             estim.D0[(1 + nd*(Nk-p)):(nd*Nk+1-p)] .= d0 
         end  
@@ -252,7 +252,7 @@ function initpred!(estim::MovingHorizonEstimator, model::LinModel)
     # --- update F and fx̄ vectors for MHE predictions ---
     F .= estim.Y0m .+ estim.B
     mul!(F, estim.G, estim.U0, 1, 1)
-    if model.nd ≠ 0
+    if model.nd > 0
         mul!(F, estim.J, estim.D0, 1, 1)
     end
     estim.fx̄ .= estim.x̂0arr_old
@@ -290,7 +290,7 @@ function linconstraint!(estim::MovingHorizonEstimator, model::LinModel)
     Fx̂  = estim.con.Fx̂
     Fx̂ .= estim.con.Bx̂
     mul!(Fx̂, estim.con.Gx̂, estim.U0, 1, 1)
-    if model.nd ≠ 0
+    if model.nd > 0
         mul!(Fx̂, estim.con.Jx̂, estim.D0, 1, 1)
     end
     X̂0min, X̂0max = trunc_bounds(estim, estim.con.X̂0min, estim.con.X̂0max, estim.nx̂)

src/model/linearization.jl

@@ -39,9 +39,11 @@ function get_linearization_func(NT, solver_f!, solver_h!, nu, nx, ny, nd, p, sol
         # all the arguments before `backend` are mutated in this function
         jacobian!(f_x!, xnext, A,  A_prep,  backend, x, cache_k, cst_u, cst_d)
         jacobian!(f_u!, xnext, Bu, Bu_prep, backend, u, cache_k, cst_x, cst_d)
-        jacobian!(f_d!, xnext, Bd, Bd_prep, backend, d, cache_k, cst_x, cst_u)
         jacobian!(h_x!, y,     C,  C_prep,  backend, x, cst_d)
-        jacobian!(h_d!, y,     Dd, Dd_prep, backend, d, cst_x)
+        if nd > 0
+            jacobian!(f_d!, xnext, Bd, Bd_prep, backend, d, cache_k, cst_x, cst_u)
+            jacobian!(h_d!, y,     Dd, Dd_prep, backend, d, cst_x)
+        end
         return nothing
     end
     return linfunc!

src/plot_sim.jl

@@ -379,10 +379,10 @@ plot_recipe(::Nothing, ::SimResult{<:Real, <:SimModel}) = nothing
     nx = length(indices_x)
 
     layout_mat = Matrix{Tuple{Int64, Int64}}(undef, 1, 0)
-    ny ≠ 0 && (layout_mat = [layout_mat (ny, 1)])
-    nu ≠ 0 && (layout_mat = [layout_mat (nu, 1)])
-    nd ≠ 0 && (layout_mat = [layout_mat (nd, 1)])
-    nx ≠ 0 && (layout_mat = [layout_mat (nx, 1)])
+    ny > 0 && (layout_mat = [layout_mat (ny, 1)])
+    nu > 0 && (layout_mat = [layout_mat (nu, 1)])
+    nd > 0 && (layout_mat = [layout_mat (nd, 1)])
+    nx > 0 && (layout_mat = [layout_mat (nx, 1)])
     layout := layout_mat
 
     # --- outputs y ---

test/1_test_sim_model.jl

@@ -355,6 +355,11 @@ end
         Ynl, Yl
     end
     @test all(isapprox.(Ynl, Yl, atol=1e-6))
+
+    f2!(xnext, x, u, _, _) = (xnext .= x .+ u)
+    h2!(y, x, _, _) = (y .= x)
+    nonlinmodel4 = NonLinModel(f2!,h2!,Ts,1,1,1,0,solver=nothing,jacobian=AutoFiniteDiff())
+    @test_nowarn linearize(nonlinmodel4, x=[1], u=[2])
 end
 
 @testitem "NonLinModel real time simulations" setup=[SetupMPCtests] begin