debug: always update linear eq. constraints for OrthogonalCollocation

Project.toml

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

src/controller/transcription.jl

@@ -1243,25 +1243,47 @@ end
         mpc::PredictiveController, ::SimModel, ::StateEstimator, ::TranscriptionMethod
     )
 
-Do the same for [`SimModel`](@ref), but using simpler equations (stochastic defects only).
+By default, fallback to doing same the but using the shorter equations.
+
+The linear equality constraints include the stochastic defects only, and the continuity
+constraints of [`OrthogonalCollocation`](@ref), if applicable. See [`init_defectmat`](@ref)
+for the equation.
+"""
+function linconstrainteq!(
+    mpc::PredictiveController, ::SimModel, ::StateEstimator, ::TranscriptionMethod
+)
+    FS  = mpc.con.FS
+    mul!(FS, mpc.con.KS, mpc.estim.x̂0) # the only non-zero matrix is KS
+    mpc.con.beq .= @. -FS
+    linconeq = mpc.optim[:linconstrainteq]
+    JuMP.set_normalized_rhs(linconeq, mpc.con.beq)
+    return nothing
+end
+
+"""
+    linconstrainteq!(
+        mpc::PredictiveController, ::NonLinModel, ::InternalModel, ::OrthogonalCollocation
+    )
+
+Same as above but only for the continuity constraints of [`OrthogonalCollocation`](@ref).
+
+There are no stochastic defects, the [`InternalModel`](@ref) does not augment the states.
 """
 function linconstrainteq!(
-    mpc::PredictiveController, ::SimModel, estim::StateEstimator, ::TranscriptionMethod
+    mpc::PredictiveController, ::NonLinModel, ::InternalModel, ::OrthogonalCollocation
 )
-    (estim.nxs < 1) && return nothing # no stochastic state ⟹ no linear eq. constraint
     FS  = mpc.con.FS
-    # the only non-zeros matrices are ES and KS:
-    mul!(FS, mpc.con.KS, mpc.estim.x̂0)
+    mul!(FS, mpc.con.KS, mpc.estim.x̂0) # the only non-zero matrix is KS
     mpc.con.beq .= @. -FS
     linconeq = mpc.optim[:linconstrainteq]
     JuMP.set_normalized_rhs(linconeq, mpc.con.beq)
     return nothing
 end
-"No linear equality constraints for [`InternalModel`](@ref) (state is not augmented)."
+"No linear equality constraints for other cases of [`InternalModel`](@ref)."
 linconstrainteq!(::PredictiveController, ::NonLinModel, ::InternalModel, ::TranscriptionMethod) = nothing
-"No linear equality constraints for [`SingleShooting`(@ref) (N/A).]"
-linconstrainteq!(::PredictiveController, ::SimModel,    ::StateEstimator, ::SingleShooting)  = nothing
-linconstrainteq!(::PredictiveController, ::NonLinModel, ::InternalModel,  ::SingleShooting)  = nothing
+"No linear equality constraints for all cases of [`SingleShooting`](@ref) (N/A)."
+linconstrainteq!(::PredictiveController, ::SimModel,    ::StateEstimator, ::SingleShooting) = nothing 
+linconstrainteq!(::PredictiveController, ::NonLinModel, ::InternalModel,  ::SingleShooting) = nothing 
 
 @doc raw"""
     set_warmstart!(mpc::PredictiveController, ::SingleShooting, Z̃var) -> Z̃s

test/3_test_predictive_control.jl

@@ -1030,7 +1030,7 @@ end
     @test u ≈ [1.0] atol=5e-2
 
     transcription = TrapezoidalCollocation(1)    
-    nmpc5_1 = NonLinMPC(InternalModel(nonlinmodel_c); Nwt=[0], Hp=100, Hc=1, transcription)
+    nmpc5_1 = NonLinMPC(nonlinmodel_c; Nwt=[0], Hp=100, Hc=1, transcription)
     preparestate!(nmpc5_1, [0.0])
     u = moveinput!(nmpc5_1, [1/0.001])
     @test u ≈ [1.0] atol=5e-2
@@ -1042,10 +1042,13 @@ end
     @test u ≈ [1.0] atol=5e-2
 
     transcription = OrthogonalCollocation(1, roots=:gausslegendre)
-    nmpc6_1 = NonLinMPC(nonlinmodel_c; Nwt=[0], Hp=100, Hc=1, transcription)
+    nmpc6_1 = NonLinMPC(InternalModel(nonlinmodel_c); Nwt=[0], Hp=100, Hc=1, transcription)
     preparestate!(nmpc6_1, [0.0])
     u = moveinput!(nmpc6_1, [1/0.001])
     @test u ≈ [1.0] atol=5e-2
+    setstate!(nmpc6_1, [5.0])
+    moveinput!(nmpc6_1)
+    @test nmpc6_1.con.FS ≈ nmpc6_1.con.KS*nmpc6_1.estim.x̂0 # OC + IMC: special lin. eq. method
 
     nonlinmodel2 = NonLinModel{Float32}(f, h, 3000.0, 1, 2, 1, 1, solver=nothing, p=linmodel2)
     nmpc7  = NonLinMPC(nonlinmodel2, Hp=10)