wip: multiple shooting transcription

Author: franckgaga

src/controller/construct.jl

@@ -495,7 +495,7 @@ function init_quadprog(::LinModel, weights::ControllerWeights, Ẽ, P̃, S̃)
     return H̃
 end
 "Return empty matrix if `model` is not a [`LinModel`](@ref)."
-function init_quadprog(::SimModel{NT}, weights::ControllerWeights, _, _) where {NT<:Real}
+function init_quadprog(::SimModel{NT}, weights::ControllerWeights, _, _, _) where {NT<:Real}
     H̃ = Hermitian(zeros(NT, 0, 0), :L)
     return H̃
 end

src/controller/linmpc.jl

@@ -274,6 +274,9 @@ function init_optimization!(mpc::LinMPC, model::LinModel, optim)
     A = con.A[con.i_b, :]
     b = con.b[con.i_b]
     @constraint(optim, linconstraint, A*ΔŨvar .≤ b)
+    Aeq = con.A
+    beq = con.b
+    @constraint(optim, lineqconstraint, Aeq*ΔŨvar .== beq)
     set_objective_hessian!(mpc, ΔŨvar)
     return nothing
 end

src/controller/nonlinmpc.jl

@@ -492,6 +492,9 @@ function init_optimization!(mpc::NonLinMPC, model::SimModel, optim)
     A = con.A[con.i_b, :]
     b = con.b[con.i_b]
     @constraint(optim, linconstraint, A*ΔŨvar .≤ b)
+    Aeq = con.A
+    beq = con.b
+    @constraint(optim, lineqconstraint, Aeq*ΔŨvar .== beq)
     # --- nonlinear optimization init ---
     if mpc.nϵ == 1 && JuMP.solver_name(optim) == "Ipopt"
         C = mpc.weights.Ñ_Hc[end]