wip: multiple shooting transcription

Author: franckgaga

src/controller/construct.jl

@@ -472,7 +472,7 @@ function validate_args(mpc::PredictiveController, ry, d, D̂, R̂y, R̂u)
 end
 
 @doc raw"""
-    init_quadprog(model::LinModel, weights::ControllerWeights, Ẽ, S) -> H̃
+    init_quadprog(model::LinModel, weights::ControllerWeights, Ẽ, P̃, S̃) -> H̃
 
 Init the quadratic programming Hessian `H̃` for MPC.
 
@@ -489,9 +489,9 @@ The vector ``\mathbf{q̃}`` and scalar ``r`` need recalculation each control per
 [`initpred!`](@ref). ``r`` does not impact the minima position. It is thus useless at
 optimization but required to evaluate the minimal ``J`` value.
 """
-function init_quadprog(::LinModel, weights::ControllerWeights, Ẽ, S̃)
+function init_quadprog(::LinModel, weights::ControllerWeights, Ẽ, P̃, S̃)
     M_Hp, Ñ_Hc, L_Hp = weights.M_Hp, weights.Ñ_Hc, weights.L_Hp
-    H̃ = Hermitian(2*(Ẽ'*M_Hp*Ẽ + Ñ_Hc + S̃'*L_Hp*S̃), :L)
+    H̃ = Hermitian(2*(Ẽ'*M_Hp*Ẽ + P̃'*Ñ_Hc*P̃ + S̃'*L_Hp*S̃), :L)
     return H̃
 end
 "Return empty matrix if `model` is not a [`LinModel`](@ref)."
@@ -502,7 +502,7 @@ end
 
 """
     init_defaultcon_mpc(
-        estim::StateEstimator,
+        estim::StateEstimator, transcription::TranscriptionMethod,
         Hp, Hc, C, 
         P, S, E, 
         ex̂, fx̂, gx̂, jx̂, kx̂, vx̂, bx̂, 
@@ -515,7 +515,7 @@ Init `ControllerConstraint` struct with default parameters based on estimator `e
 Also return `P̃`, `S̃`, `Ẽ` and `Ẽŝ` matrices for the the augmented decision vector `Z̃`.
 """
 function init_defaultcon_mpc(
-    estim::StateEstimator{NT}, 
+    estim::StateEstimator{NT}, transcription::TranscriptionMethod,
     Hp, Hc, C, 
     P, S, E, 
     ex̂, fx̂, gx̂, jx̂, kx̂, vx̂, bx̂, 
@@ -538,7 +538,9 @@ function init_defaultcon_mpc(
     C_umin, C_umax, C_Δumin, C_Δumax, C_ymin, C_ymax = 
         repeat_constraints(Hp, Hc, c_umin, c_umax, c_Δumin, c_Δumax, c_ymin, c_ymax)
     A_Umin,  A_Umax, S̃  = relaxU(model, nϵ, C_umin, C_umax, S)
-    A_ΔŨmin, A_ΔŨmax, ΔŨmin, ΔŨmax, P̃ = relaxΔU(model, nϵ, C_Δumin, C_Δumax, ΔUmin, ΔUmax, P)
+    A_ΔŨmin, A_ΔŨmax, ΔŨmin, ΔŨmax, P̃ = relaxΔU(
+        model, transcription, nϵ, C_Δumin, C_Δumax, ΔUmin, ΔUmax, P
+    )
     A_Ymin,  A_Ymax, Ẽ  = relaxŶ(model, nϵ, C_ymin, C_ymax, E)
     A_x̂min,  A_x̂max, ẽx̂ = relaxterminal(model, nϵ, c_x̂min, c_x̂max, ex̂)
     A_ŝ, Ẽŝ = augmentdefect(model, nϵ, Eŝ)
@@ -619,15 +621,17 @@ end
 
 @doc raw"""
     relaxΔU(
-        model, nϵ, C_Δumin, C_Δumax, ΔUmin, ΔUmax, P
+        model, transcription, nϵ, C_Δumin, C_Δumax, ΔUmin, ΔUmax, P
     ) -> A_ΔŨmin, A_ΔŨmax, ΔŨmin, ΔŨmax, P̃
 
 Augment input increments constraints with slack variable ϵ for softening.
 
 Denoting the decision variables augmented with the slack variable 
 ``\mathbf{Z̃} = [\begin{smallmatrix} \mathbf{Z} \\ ϵ \end{smallmatrix}]``, it returns the
 augmented conversion matrix ``\mathbf{P̃}``, similar to the one described at
-[`init_ZtoΔU`](@ref). Knowing that ``0 ≤ ϵ ≤ ∞``, it also returns the augmented bounds 
+[`init_ZtoΔU`](@ref), but extracting the input increments augmented with the slack variable
+``\mathbf{ΔŨ} = [\begin{smallmatrix} \mathbf{ΔU} \\ ϵ \end{smallmatrix}] = \mathbf{P̃ Z̃}``.
+Also, knowing that ``0 ≤ ϵ ≤ ∞``, it also returns the augmented bounds 
 ``\mathbf{ΔŨ_{min}} = [\begin{smallmatrix} \mathbf{ΔU_{min}} \\ 0 \end{smallmatrix}]`` and
 ``\mathbf{ΔŨ_{max}} = [\begin{smallmatrix} \mathbf{ΔU_{min}} \\ ∞ \end{smallmatrix}]``,
 and the ``\mathbf{A}`` matrices for the inequality constraints:
@@ -646,13 +650,16 @@ bound, which is more specific than a linear inequality constraint. However, it i
 convenient to treat it as a linear inequality constraint since the optimizer `OSQP.jl` does
 not support pure bounds on the decision variables.
 """
-function relaxΔU(::SimModel{NT}, nϵ, C_Δumin, C_Δumax, ΔUmin, ΔUmax, P) where NT<:Real
+function relaxΔU(
+    ::SimModel{NT}, transcription::TranscriptionMethod, 
+    nϵ, C_Δumin, C_Δumax, ΔUmin, ΔUmax, P
+) where NT<:Real
     nZ = size(P, 2)
     if nϵ == 1 # Z̃ = [Z; ϵ]
         ΔŨmin, ΔŨmax = [ΔUmin; NT[0.0]], [ΔUmax; NT[Inf]] # 0 ≤ ϵ ≤ ∞
         A_ϵ = [zeros(NT, 1, nZ) NT[1.0]]
         A_ΔŨmin, A_ΔŨmax = -[P  C_Δumin; A_ϵ], [P -C_Δumax; A_ϵ]
-        P̃ = [P zeros(NT, size(P, 1), 1)]
+        P̃ = [P zeros(NT, size(P, 1), 1); zeros(NT, 1, size(P, 2)) NT[1.0]]
     else # Z̃ = Z (only hard constraints)
         ΔŨmin, ΔŨmax = ΔUmin, ΔUmax
         A_ΔŨmin, A_ΔŨmax = -P,  P

src/controller/execute.jl

@@ -177,10 +177,10 @@ They are computed with these equations using in-place operations:
                             + \mathbf{V u_0}(k-1) + \mathbf{B} + \mathbf{Ŷ_s}           \\
     \mathbf{C_y}     &= \mathbf{F} + \mathbf{Y_{op}} - \mathbf{R̂_y}                     \\
     \mathbf{C_u}     &= \mathbf{T}\mathbf{u}(k-1)    - \mathbf{R̂_u}                     \\
-    \mathbf{q̃}       &= 2[(\mathbf{M}_{H_p} \mathbf{Ẽ})' \mathbf{C_y} 
-                            + (\mathbf{L}_{H_p} \mathbf{S̃})' \mathbf{C_u}]              \\
-    r                &= \mathbf{C_y}' \mathbf{M}_{H_p} \mathbf{C_y} 
-                            + \mathbf{C_u}' \mathbf{L}_{H_p} \mathbf{C_u}
+    \mathbf{q̃}       &= 2[    (\mathbf{M}_{H_p} \mathbf{Ẽ})' \mathbf{C_y} 
+                            + (\mathbf{L}_{H_p} \mathbf{S̃})' \mathbf{C_u}   ]           \\
+    r                &=     \mathbf{C_y}' \mathbf{M}_{H_p} \mathbf{C_y} 
+                          + \mathbf{C_u}' \mathbf{L}_{H_p} \mathbf{C_u}
 \end{aligned}
 ```
 """
@@ -765,7 +765,7 @@ function setmodel_controller!(mpc::PredictiveController, x̂op_old)
     JuMP.unregister(optim, :linconstraint)
     @constraint(optim, linconstraint, A*ΔŨvar .≤ 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, ΔŨvar)
     return nothing

src/controller/explicitmpc.jl

@@ -54,7 +54,7 @@ struct ExplicitMPC{NT<:Real, SE<:StateEstimator} <: PredictiveController{NT}
         # dummy val (updated just before optimization):
         F, fx̂  = zeros(NT, ny*Hp), zeros(NT, nx̂)
         P̃, S̃, Ñ_Hc, Ẽ = P, S, N_Hc, E # no slack variable ϵ for ExplicitMPC
-        H̃ = init_quadprog(model, weights ,Ẽ, S̃)
+        H̃ = init_quadprog(model, weights, Ẽ, P̃, S̃)
         # dummy vals (updated just before optimization):
         q̃, r = zeros(NT, size(H̃, 1)), zeros(NT, 1)
         H̃_chol = cholesky(H̃)

src/controller/linmpc.jl

@@ -63,11 +63,12 @@ struct LinMPC{
         # dummy vals (updated just before optimization):
         F, fx̂, Fŝ  = zeros(NT, ny*Hp), zeros(NT, nx̂), zeros(NT, nx̂*Hp)
         con, nϵ, P̃, S̃, Ẽ, Ẽŝ = init_defaultcon_mpc(
-            estim, Hp, Hc, Cwt, P, S, E, 
+            estim, transcription,
+            Hp, Hc, Cwt, P, S, E, 
             ex̂, fx̂, gx̂, jx̂, kx̂, vx̂, bx̂, 
             Eŝ, Fŝ, Gŝ, Jŝ, Kŝ, Vŝ, Bŝ
         )
-        H̃ = init_quadprog(model, weights, Ẽ, S̃)
+        H̃ = init_quadprog(model, weights, Ẽ, P̃, S̃)
         # dummy vals (updated just before optimization):
         q̃, r = zeros(NT, size(H̃, 1)), zeros(NT, 1)
         Ks, Ps = init_stochpred(estim, Hp)

src/controller/nonlinmpc.jl

@@ -77,12 +77,13 @@ struct NonLinMPC{
         # dummy vals (updated just before optimization):
         F, fx̂, Fŝ  = zeros(NT, ny*Hp), zeros(NT, nx̂), zeros(NT, nx̂*Hp)
         con, nϵ, P̃, S̃, Ẽ, Ẽŝ = init_defaultcon_mpc(
-            estim, Hp, Hc, Cwt, P, S, E, 
+            estim, transcription,
+            Hp, Hc, Cwt, P, S, E, 
             ex̂, fx̂, gx̂, jx̂, kx̂, vx̂, bx̂, 
             Eŝ, Fŝ, Gŝ, Jŝ, Kŝ, Vŝ, Bŝ,
             gc!, nc
         )
-        H̃ = init_quadprog(model, weights, Ẽ, S̃)
+        H̃ = init_quadprog(model, weights, Ẽ, P̃, S̃)
         # dummy vals (updated just before optimization):
         q̃, r = zeros(NT, size(H̃, 1)), zeros(NT, 1)
         Ks, Ps = init_stochpred(estim, Hp)

src/controller/transcription.jl

@@ -67,21 +67,38 @@ in which ``\mathbf{P} is defined in the Extended Help section.
     - ``\mathbf{P} = [\begin{smallmatrix}\mathbf{I} \mathbf{0} \end{smallmatrix}]`` if 
       `transcription isa MultipleShooting`
 """
-function init_ZtoU end
+function init_ZtoΔU end
 
 function init_ZtoΔU(
     estim::StateEstimator{NT}, transcription::SingleShooting, _ , Hc
 ) where {NT<:Real}
-    return Matrix{NT}(I, estim.model.nu*Hc, estim.model.nu*Hc)
+    P = Matrix{NT}(I, estim.model.nu*Hc, estim.model.nu*Hc)
+    return P
 end
 
 function init_ZtoΔU(
     estim::StateEstimator{NT}, transcription::MultipleShooting, Hp, Hc
 ) where {NT<:Real}
     I_nu_Hc = Matrix{NT}(I, estim.model.nu*Hc, estim.model.nu*Hc)
-    return [I_nu_Hc zeros(NT, estim.model.nu*Hc, estim.nx̂*Hp)]
+    P = [I_nu_Hc zeros(NT, estim.model.nu*Hc, estim.nx̂*Hp)]
+    return P
 end
 
+#=
+function init_Z̃toΔŨ(
+    estim::StateEstimator{NT}, transcription::SingleShooting, Hp, Hc
+) where {NT<:Real}
+    return Matrix{NT}(I, model.nu*Hc, model.nu*Hc)
+end
+
+function init_Z̃toΔŨ(
+    estim::StateEstimator{NT}, transcription::MultipleShooting, Hp, Hc
+) where {NT<:Real}
+    I_nu_Hc = Matrix{NT}(I, model.nu*Hc, model.nu*Hc)
+    return [I_nu_Hc zeros(NT, model.nu*Hc, model.nx̂*Hp)]
+end
+=#
+
 @doc raw"""
     init_ZtoU(estim, transcription, Hp, Hc) -> S, T
 
@@ -133,14 +150,14 @@ function init_ZtoU(
     I_nu = Matrix{NT}(I, model.nu, model.nu)
     S_Hc = LowerTriangular(repeat(I_nu, Hc, Hc))
     Sdagger = [S_Hc; repeat(I_nu, Hp - Hc, Hc)]
-    S = init_ZtoU_Smat(estim, transcription, Hp, Hc, Sdagger)
+    S = init_Smat(estim, transcription, Hp, Hc, Sdagger)
     T = repeat(I_nu, Hp)
     return S, T
 end
 
-init_ZtoU_Smat( _ , transcription::SingleShooting, _ , _ , Sdagger) = Sdagger
+init_Smat( _ , transcription::SingleShooting, _ , _ , Sdagger) = Sdagger
 
-function init_ZtoU_Smat(estim, transcription::MultipleShooting, Hp, _ , Sdagger)
+function init_Smat(estim, transcription::MultipleShooting, Hp, _ , Sdagger)
     return [Sdagger zeros(eltype(Sdagger), estim.model.nu*Hp, estim.nx̂*Hp)]
 end
 
@@ -339,7 +356,7 @@ function init_predmat(
     J  = repeatdiag(D̂d, Hp)
     jx̂ = zeros(NT, nx̂, Hp*nd)
     # --- state x̂op and state update f̂op operating points ---
-    B  = zeros(NT, Hp*ny, 1)
+    B  = zeros(NT, Hp*ny)
     bx̂ = zeros(NT, nx̂)
     return E, G, J, K, V, B, ex̂, gx̂, jx̂, kx̂, vx̂, bx̂
 end