debug: ExplicitMPC updates with transcription methods

Author: franckgaga

src/controller/execute.jl

@@ -652,9 +652,9 @@ end
 
 "Update the prediction matrices, linear constraints and JuMP optimization."
 function setmodel_controller!(mpc::PredictiveController, x̂op_old)
-    estim, model = mpc.estim, mpc.estim.model
+    model, estim, transcription = mpc.estim.model, mpc.estim, mpc.transcription
     nu, ny, nd, Hp, Hc = model.nu, model.ny, model.nd, mpc.Hp, mpc.Hc
-    transcription, optim, con = mpc.transcription, mpc.optim, mpc.con
+    optim, con = mpc.optim, mpc.con
     # --- predictions matrices ---
     E, G, J, K, V, B, ex̂, gx̂, jx̂, kx̂, vx̂, bx̂ = init_predmat(
         model, estim, transcription, Hp, Hc

src/controller/explicitmpc.jl

@@ -193,7 +193,7 @@ The solution is ``\mathbf{Z̃ = - H̃^{-1} q̃}``, see [`init_quadprog`](@ref).
 optim_objective!(mpc::ExplicitMPC) = lmul!(-1, ldiv!(mpc.Z̃, mpc.H̃_chol, mpc.q̃))
 
 "Compute the predictions but not the terminal states if `mpc` is an [`ExplicitMPC`](@ref)."
-function predict!(Ŷ, x̂, _ , _ , _ , mpc::ExplicitMPC, ::LinModel, Z̃)
+function predict!(Ŷ, x̂, _ , _ , _ , mpc::ExplicitMPC, ::LinModel, ::TranscriptionMethod, Z̃)
     # in-place operations to reduce allocations :
     Ŷ .= mul!(Ŷ, mpc.Ẽ, Z̃) .+ mpc.F
     x̂ .= NaN
@@ -213,7 +213,7 @@ addinfo!(info, mpc::ExplicitMPC) = info
 
 "Update the prediction matrices and Cholesky factorization."
 function setmodel_controller!(mpc::ExplicitMPC, _ )
-    estim, model = mpc.estim, mpc.estim.model
+    model, estim, transcription = mpc.estim.model, mpc.estim, mpc.transcription
     nu, ny, nd, Hp, Hc = model.nu, model.ny, model.nd, mpc.Hp, mpc.Hc
     # --- predictions matrices ---
     E, G, J, K, V, B = init_predmat(model, estim, transcription, Hp, Hc)
@@ -225,7 +225,7 @@ function setmodel_controller!(mpc::ExplicitMPC, _ )
     mpc.V .= V
     mpc.B .= B
     # --- quadratic programming Hessian matrix ---
-    H̃ = init_quadprog(model, mpc.weights, mpc.Ẽ, mpc.S̃)
+    H̃ = init_quadprog(model, mpc.weights, mpc.Ẽ, mpc.P̃, mpc.S̃)
     mpc.H̃ .= H̃
     set_objective_hessian!(mpc)
     # --- operating points ---

src/controller/transcription.jl

@@ -35,17 +35,17 @@ The decision variable is (excluding ``ϵ``):
 thus it also includes the predicted states, expressed as deviation vectors from the
 operating point ``\mathbf{x̂_{op}}`` (see [`augment_model`](@ref)):
 ```math
-\mathbf{X̂_0} = \mathbf{X̂ - X̂_{op}}              \begin{bmatrix} 
+\mathbf{X̂_0} = \mathbf{X̂ - X̂_{op}} =            \begin{bmatrix} 
     \mathbf{x̂}_i(k+1)     - \mathbf{x̂_{op}}     \\ 
     \mathbf{x̂}_i(k+2)     - \mathbf{x̂_{op}}     \\ 
     \vdots                                      \\ 
     \mathbf{x̂}_i(k+H_p)   - \mathbf{x̂_{op}}     \end{bmatrix}
 ```
 where ``\mathbf{x̂}_i(k+j)`` is the state prediction for time ``k+j``, estimated by the
-observer at time ``i=k`` or ``i=k-1`` depending on the `direct` flag. This transcription
+observer at time ``i=k`` or ``i=k-1`` depending on its `direct` flag. This transcription
 method is generally more efficient for large control horizon ``H_c``, unstable or highly
-nonlinear plant models/constraints. Sparse optimizers like `OSQP.jl` or `Ipopt.jl` are
-recommended for large-scale problems.
+nonlinear plant models/constraints. Sparse optimizers like `OSQP` or `Ipopt` are recommended
+for large-scale problems.
 """
 struct MultipleShooting <: TranscriptionMethod end
 
@@ -365,13 +365,15 @@ function init_predmat(
     return E, G, J, K, V, B, ex̂, gx̂, jx̂, kx̂, vx̂, bx̂
 end
 
-"""
-    init_predmat(model::SimModel, estim, transcription::SingleShooting, Hp, Hc) 
+@doc raw"""
+    init_predmat(model::SimModel, estim, transcription::MultipleShooting, Hp, Hc)
+
+Return empty matrices except `ex̂` for [`SimModel`](@ref) and [`MultipleShooting`](@ref).
 
-Return empty matrices for [`SimModel`](@ref) and [`SingleShooting`](@ref) (N/A).
+The matrix is ``\mathbf{e_x̂} = [\begin{smallmatrix}\mathbf{0} & \mathbf{I}\end{smallmatrix}]``.
 """
 function init_predmat(
-    model::SimModel, estim::StateEstimator{NT}, transcription::SingleShooting, Hp, Hc
+    model::SimModel, estim::StateEstimator{NT}, transcription::MultipleShooting, Hp, Hc
 ) where {NT<:Real}
     nu, nx̂, nd = model.nu, estim.nx̂, model.nd
     nZ = get_nZ(estim, transcription, Hp, Hc)
@@ -381,19 +383,18 @@ function init_predmat(
     K  = zeros(NT, 0, nx̂)
     V  = zeros(NT, 0, nu)
     B  = zeros(NT, 0)
-    ex̂, gx̂, jx̂, kx̂, vx̂, bx̂ = E, G, J, K, V, B
+    ex̂ = [zeros(NT, nx̂, Hc*nu + (Hp-1)*nx̂) I]
+    gx̂, jx̂, kx̂, vx̂, bx̂ = G, J, K, V, B
     return E, G, J, K, V, B, ex̂, gx̂, jx̂, kx̂, vx̂, bx̂
 end
 
-@doc raw"""
-    init_predmat(model::SimModel, estim, transcription::MultipleShooting, Hp, Hc)
-
-Return empty matrices except `ex̂` for [`SimModel`](@ref) and [`MultipleShooting`](@ref).
+"""
+    init_predmat(model::SimModel, estim, transcription::TranscriptionMethod, Hp, Hc) 
 
-The matrix is ``\mathbf{ex̂} = [\begin{smallmatrix}\mathbf{0} & \mathbf{I}\end{smallmatrix}]``.
+Return empty matrices for all other cases.
 """
 function init_predmat(
-    model::SimModel, estim::StateEstimator{NT}, transcription::MultipleShooting, Hp, Hc
+    model::SimModel, estim::StateEstimator{NT}, transcription::TranscriptionMethod, Hp, Hc
 ) where {NT<:Real}
     nu, nx̂, nd = model.nu, estim.nx̂, model.nd
     nZ = get_nZ(estim, transcription, Hp, Hc)
@@ -403,30 +404,10 @@ function init_predmat(
     K  = zeros(NT, 0, nx̂)
     V  = zeros(NT, 0, nu)
     B  = zeros(NT, 0)
-    ex̂ = [zeros(NT, nx̂, Hc*nu + (Hp-1)*nx̂) I]
-    gx̂, jx̂, kx̂, vx̂, bx̂ = G, J, K, V, B
+    ex̂, gx̂, jx̂, kx̂, vx̂, bx̂ = E, G, J, K, V, B
     return E, G, J, K, V, B, ex̂, gx̂, jx̂, kx̂, vx̂, bx̂
 end
 
-"""
-    init_defectmat(model::SimModel, estim, transcription::SingleShooting, Hp, Hc)
-
-Return empty matrices if `transcription` is a [`SingleShooting`](@ref) (N/A).
-"""
-function init_defectmat(
-    model::SimModel, estim::StateEstimator{NT}, transcription::SingleShooting, Hp, Hc
-) where {NT<:Real}
-    nx̂, nu, nd = estim.nx̂, model.nu, model.nd
-    nZ = get_nZ(estim, transcription, Hp, Hc)
-    Eŝ = zeros(NT, 0, nZ)
-    Gŝ = zeros(NT, 0, nd)
-    Jŝ = zeros(NT, 0, nd*Hp)
-    Kŝ = zeros(NT, 0, nx̂)
-    Vŝ = zeros(NT, 0, nu)
-    Bŝ = zeros(NT, 0)
-    return Eŝ, Gŝ, Jŝ, Kŝ, Vŝ, Bŝ
-end
-
 @doc raw"""
     init_defectmat(model::LinModel, estim, transcription::MultipleShooting, Hp, Hc)
 
@@ -501,7 +482,11 @@ function init_defectmat(
     return Eŝ, Gŝ, Jŝ, Kŝ, Vŝ, Bŝ
 end
 
-"Return empty matrices if `model` is not a [`LinModel`](@ref) (N/A)."
+"""
+    init_defectmat(model::SimModel, estim, transcription::TranscriptionMethod, Hp, Hc)
+
+Return empty matrices for all other cases (N/A).
+"""
 function init_defectmat(
     model::SimModel, estim::StateEstimator{NT}, transcription::TranscriptionMethod, Hp, Hc
 ) where {NT<:Real}