Merge pull request #301 from JuliaControl/debug_moveblocking

debug: correctly handle custom move blocking for `LinModel`

Author: franckgaga

Project.toml

@@ -1,6 +1,6 @@
 name = "ModelPredictiveControl"
 uuid = "61f9bdb8-6ae4-484a-811f-bbf86720c31c"
-version = "1.14.3"
+version = "1.14.4"
 authors = ["Francis Gagnon"]
 
 [deps]

docs/src/manual/mtk.md

@@ -22,6 +22,10 @@ the last section.
     as a basic starting template to combine both packages. There is no guarantee that it
     will work for all corner cases.
 
+!!! compat
+    The example works on `ModelingToolkit.jl` v10 (corresponding to the following `[compat]`
+    entry: `ModelingToolkit = "10"`).
+
 We first construct and instantiate the pendulum model:
 
 ```@example 1

src/controller/construct.jl

@@ -499,15 +499,16 @@ strictly positive integers):
 ```math
     \mathbf{n_b} = \begin{bmatrix} n_1 & n_2 & \cdots & n_{H_c} \end{bmatrix}'
 ```
-The vector that includes all the manipulated input increments ``\mathbf{Δu}`` is then
+Introducing the notation ``j_ℓ = ∑_{i=1}^{ℓ} n_i`` to convert from block lengths to discrete
+time steps, the vector that includes all the free moves of the manipulated input is then
 defined as:
 ```math
 \mathbf{ΔU} = \begin{bmatrix}
-    \mathbf{Δu}(k + 0)                                  \\[0.1em]
-    \mathbf{Δu}(k + ∑_{i=1}^1 n_i)                      \\[0.1em]
-    \mathbf{Δu}(k + ∑_{i=1}^2 n_i)                      \\[0.1em]
-    \vdots                                              \\[0.1em]
-    \mathbf{Δu}(k + ∑_{i=1}^{H_c-1} n_i)   
+    \mathbf{Δu}(k + 0)                        \\
+    \mathbf{Δu}(k + j_1)                      \\
+    \mathbf{Δu}(k + j_2)                      \\
+    \vdots                                    \\
+    \mathbf{Δu}(k + j_{H_c-1})   
 \end{bmatrix}
 ```
 The provided `nb` vector is modified to ensure `sum(nb) == Hp`:

src/controller/execute.jl

@@ -624,11 +624,11 @@ end
 function setmodel_controller!(mpc::PredictiveController, uop_old, x̂op_old)
     model, estim, transcription = mpc.estim.model, mpc.estim, mpc.transcription
     weights = mpc.weights
-    nu, ny, nd, Hp, Hc = model.nu, model.ny, model.nd, mpc.Hp, mpc.Hc
+    nu, ny, nd, Hp, Hc, nb = model.nu, model.ny, model.nd, mpc.Hp, mpc.Hc, mpc.nb
     optim, con = mpc.optim, mpc.con
     # --- prediction matrices ---
     E, G, J, K, V, B, ex̂, gx̂, jx̂, kx̂, vx̂, bx̂ = init_predmat(
-        model, estim, transcription, Hp, Hc
+        model, estim, transcription, Hp, Hc, nb
     )
     A_Ymin, A_Ymax, Ẽ = relaxŶ(E, con.C_ymin, con.C_ymax, mpc.nϵ)
     A_x̂min, A_x̂max, ẽx̂ = relaxterminal(ex̂, con.c_x̂min, con.c_x̂max, mpc.nϵ)
@@ -639,7 +639,7 @@ function setmodel_controller!(mpc::PredictiveController, uop_old, x̂op_old)
     mpc.V .= V
     mpc.B .= B
     # --- defect matrices ---
-    Eŝ, Gŝ, Jŝ, Kŝ, Vŝ, Bŝ = init_defectmat(model, estim, transcription, Hp, Hc)
+    Eŝ, Gŝ, Jŝ, Kŝ, Vŝ, Bŝ = init_defectmat(model, estim, transcription, Hp, Hc, nb)
     A_ŝ, Ẽŝ = augmentdefect(Eŝ, mpc.nϵ)
     con.Ẽŝ .= Ẽŝ
     con.Gŝ .= Gŝ

src/controller/explicitmpc.jl

@@ -53,7 +53,7 @@ struct ExplicitMPC{
         validate_transcription(model, transcription)
         PΔu = init_ZtoΔU(estim, transcription, Hp, Hc)
         Pu, Tu = init_ZtoU(estim, transcription, Hp, Hc, nb)
-        E, G, J, K, V, B = init_predmat(model, estim, transcription, Hp, Hc)
+        E, G, J, K, V, B = init_predmat(model, estim, transcription, Hp, Hc, nb)
         # dummy val (updated just before optimization):
         F = zeros(NT, ny*Hp)
         P̃Δu, P̃u, Ẽ = PΔu, Pu, E # no slack variable ϵ for ExplicitMPC
@@ -226,9 +226,9 @@ addinfo!(info, mpc::ExplicitMPC) = info
 function setmodel_controller!(mpc::ExplicitMPC, uop_old, _ )
     model, estim, transcription = mpc.estim.model, mpc.estim, mpc.transcription
     weights = mpc.weights
-    nu, ny, nd, Hp, Hc = model.nu, model.ny, model.nd, mpc.Hp, mpc.Hc
+    nu, ny, nd, Hp, Hc, nb = model.nu, model.ny, model.nd, mpc.Hp, mpc.Hc, mpc.nb
     # --- predictions matrices ---
-    E, G, J, K, V, B = init_predmat(model, estim, transcription, Hp, Hc)
+    E, G, J, K, V, B = init_predmat(model, estim, transcription, Hp, Hc, nb)
     Ẽ = E  # no slack variable ϵ for ExplicitMPC
     mpc.Ẽ .= Ẽ
     mpc.G .= G

src/controller/linmpc.jl

@@ -67,9 +67,9 @@ struct LinMPC{
         PΔu = init_ZtoΔU(estim, transcription, Hp, Hc)
         Pu, Tu = init_ZtoU(estim, transcription, Hp, Hc, nb)
         E, G, J, K, V, B, ex̂, gx̂, jx̂, kx̂, vx̂, bx̂ = init_predmat(
-            model, estim, transcription, Hp, Hc
+            model, estim, transcription, Hp, Hc, nb
         )
-        Eŝ, Gŝ, Jŝ, Kŝ, Vŝ, Bŝ = init_defectmat(model, estim, transcription, Hp, Hc)
+        Eŝ, Gŝ, Jŝ, Kŝ, Vŝ, Bŝ = init_defectmat(model, estim, transcription, Hp, Hc, nb)
         # dummy vals (updated just before optimization):
         F, fx̂, Fŝ  = zeros(NT, ny*Hp), zeros(NT, nx̂), zeros(NT, nx̂*Hp)
         con, nϵ, P̃Δu, P̃u, Ẽ = init_defaultcon_mpc(

src/controller/nonlinmpc.jl

@@ -95,9 +95,9 @@ struct NonLinMPC{
         PΔu = init_ZtoΔU(estim, transcription, Hp, Hc)
         Pu, Tu = init_ZtoU(estim, transcription, Hp, Hc, nb)
         E, G, J, K, V, B, ex̂, gx̂, jx̂, kx̂, vx̂, bx̂ = init_predmat(
-            model, estim, transcription, Hp, Hc
+            model, estim, transcription, Hp, Hc, nb
         )
-        Eŝ, Gŝ, Jŝ, Kŝ, Vŝ, Bŝ = init_defectmat(model, estim, transcription, Hp, Hc)
+        Eŝ, Gŝ, Jŝ, Kŝ, Vŝ, Bŝ = init_defectmat(model, estim, transcription, Hp, Hc, nb)
         # dummy vals (updated just before optimization):
         F, fx̂, Fŝ  = zeros(NT, ny*Hp), zeros(NT, nx̂), zeros(NT, nx̂*Hp)
         con, nϵ, P̃Δu, P̃u, Ẽ = init_defaultcon_mpc(