added: unit tests and benchmarks for TrapezoidalCollocation

Project.toml

@@ -1,7 +1,7 @@
 name = "ModelPredictiveControl"
 uuid = "61f9bdb8-6ae4-484a-811f-bbf86720c31c"
 authors = ["Francis Gagnon"]
-version = "1.8.2"
+version = "1.9.0"
 
 [deps]
 ControlSystemsBase = "aaaaaaaa-a6ca-5380-bf3e-84a91bcd477e"

benchmark/0_bench_setup.jl

@@ -16,6 +16,11 @@ linmodel = setop!(LinModel(sys, Ts, i_d=[3]), uop=[10, 50], yop=[50, 30], dop=[5
 nonlinmodel = NonLinModel(f_lin!, h_lin!, Ts, 2, 4, 2, 1, p=linmodel, solver=nothing)
 nonlinmodel = setop!(nonlinmodel, uop=[10, 50], yop=[50, 30], dop=[5])
 u, d, y = [10, 50], [5], [50, 30]
+nonlinmodel_c = NonLinModel(
+    (ẋ,x,u,d,_) -> ẋ .= -0.001x .+ u .+ d , 
+    (y,x,d,_) -> y .= x .+ 0.001d, 500, 1, 1, 1, 1
+)
+u_c, d_c, y_c = [1], [0], [0]
 
 G = [ tf(1.90, [18, 1]) tf(1.90, [18, 1]);
       tf(-0.74,[8, 1])  tf(0.74, [8, 1]) ]

benchmark/3_bench_predictive_control.jl

@@ -50,6 +50,10 @@ nmpc_nonlin_ms = NonLinMPC(
     nonlinmodel, transcription=MultipleShooting(),
     Mwt=[1, 1], Nwt=[0.1, 0.1], Lwt=[0.1, 0.1], Hp=10    
 )
+nmpc_nonlin_tc = NonLinMPC(
+    nonlinmodel_c, transcription=TrapezoidalCollocation(),
+    Mwt=[1], Nwt=[0.1], Lwt=[0.1], Hp=10    
+)
 
 samples, evals, seconds = 10000, 1, 60
 UNIT_MPC["NonLinMPC"]["moveinput!"]["LinModel"]["SingleShooting"] =
@@ -76,6 +80,12 @@ UNIT_MPC["NonLinMPC"]["moveinput!"]["NonLinModel"]["MultipleShooting"] =
         setup=preparestate!($nmpc_nonlin_ms, $y, $d),
         samples=samples, evals=evals, seconds=seconds
     )
+UNIT_MPC["NonLinMPC"]["moveinput!"]["NonLinModel"]["TrapezoidalCollocation"] =
+    @benchmarkable(
+        moveinput!($nmpc_nonlin_tc, $y_c, $d_c),
+        setup=preparestate!($nmpc_nonlin_tc, $y_c, $d_c),
+        samples=samples, evals=evals, seconds=seconds
+    )
 
 ## ----------------------------------------------------------------------------------------
 ## ---------------------- CASE STUDIES ----------------------------------------------------
@@ -230,7 +240,6 @@ CASE_MPC["CSTR"]["LinMPC"]["With feedforward"]["Ipopt"]["MultipleShooting"] =
         samples=samples, evals=evals
     )
 
-
 # ----------------- Case study: Pendulum noneconomic -----------------------------
 model, p = pendulum_model, pendulum_p
 σQ = [0.1, 1.0]; σR=[5.0]; nint_u=[1]; σQint_u=[0.1]
@@ -255,13 +264,20 @@ nmpc_ipopt_ms = NonLinMPC(estim; Hp, Hc, Mwt, Nwt, Cwt, optim, transcription)
 nmpc_ipopt_ms = setconstraint!(nmpc_ipopt_ms; umin, umax)
 JuMP.unset_time_limit_sec(nmpc_ipopt_ms.optim)
 
+optim = JuMP.Model(optimizer_with_attributes(Ipopt.Optimizer,"sb"=>"yes"), add_bridges=false)
+transcription = TrapezoidalCollocation()
+nmpc_ipopt_tc = NonLinMPC(estim; Hp, Hc, Mwt, Nwt, Cwt, optim, transcription)
+nmpc_ipopt_tc = setconstraint!(nmpc_ipopt_tc; umin, umax)
+JuMP.unset_time_limit_sec(nmpc_ipopt_tc.optim)
+
 optim = JuMP.Model(MadNLP.Optimizer, add_bridges=false)
 transcription = SingleShooting()
 nmpc_madnlp_ss = NonLinMPC(estim; Hp, Hc, Mwt, Nwt, Cwt, optim, transcription)
 nmpc_madnlp_ss = setconstraint!(nmpc_madnlp_ss; umin, umax)
 JuMP.unset_time_limit_sec(nmpc_madnlp_ss.optim) 
 
-# TODO: does not work well with MadNLP and MultipleShooting, figure out why. Current theory: 
+# TODO: does not work well with MadNLP and MultipleShooting or TrapezoidalCollocation, 
+# figure out why. Current theory: 
 # MadNLP LBFGS approximation is less robust than Ipopt version. Re-test when exact Hessians
 # will be supported in ModelPredictiveControl.jl. The following attributes kinda work with 
 # the MadNLP LBFGS approximation but super slow (~1000 times slower than Ipopt):
@@ -285,6 +301,11 @@ CASE_MPC["Pendulum"]["NonLinMPC"]["Noneconomic"]["Ipopt"]["MultipleShooting"] =
         sim!($nmpc_ipopt_ms, $N, $ry; plant=$plant, x_0=$x_0, x̂_0=$x̂_0),
         samples=samples, evals=evals, seconds=seconds
     )
+CASE_MPC["Pendulum"]["NonLinMPC"]["Noneconomic"]["Ipopt"]["TrapezoidalCollocation"] =
+    @benchmarkable(
+        sim!($nmpc_ipopt_tc, $N, $ry; plant=$plant, x_0=$x_0, x̂_0=$x̂_0),
+        samples=samples, evals=evals, seconds=seconds
+    )
 CASE_MPC["Pendulum"]["NonLinMPC"]["Noneconomic"]["MadNLP"]["SingleShooting"] = 
     @benchmarkable(
         sim!($nmpc_madnlp_ss, $N, $ry; plant=$plant, x_0=$x_0, x̂_0=$x̂_0),
@@ -316,13 +337,19 @@ empc_ipopt_ms = NonLinMPC(estim2; Hp, Hc, Nwt, Mwt=Mwt2, Cwt, JE, Ewt, optim, tr
 empc_ipopt_ms = setconstraint!(empc_ipopt_ms; umin, umax)
 JuMP.unset_time_limit_sec(empc_ipopt_ms.optim)
 
+optim = JuMP.Model(optimizer_with_attributes(Ipopt.Optimizer,"sb"=>"yes"), add_bridges=false)
+transcription = TrapezoidalCollocation()
+empc_ipopt_tc = NonLinMPC(estim2; Hp, Hc, Nwt, Mwt=Mwt2, Cwt, JE, Ewt, optim, transcription, p)
+empc_ipopt_tc = setconstraint!(empc_ipopt_tc; umin, umax)
+JuMP.unset_time_limit_sec(empc_ipopt_tc.optim)
+
 optim = JuMP.Model(MadNLP.Optimizer, add_bridges=false)
 transcription = SingleShooting()
 empc_madnlp_ss = NonLinMPC(estim2; Hp, Hc, Nwt, Mwt=Mwt2, Cwt, JE, Ewt, optim, transcription, p)
 empc_madnlp_ss = setconstraint!(empc_madnlp_ss; umin, umax)
 JuMP.unset_time_limit_sec(empc_madnlp_ss.optim)
 
-# TODO: test EMPC with MadNLP and MultipleShooting, see comment above.
+# TODO: test EMPC with MadNLP and MultipleShooting and TrapezoidalCollocation, see comment above.
 
 samples, evals, seconds = 100, 1, 15*60
 CASE_MPC["Pendulum"]["NonLinMPC"]["Economic"]["Ipopt"]["SingleShooting"] = 
@@ -335,6 +362,11 @@ CASE_MPC["Pendulum"]["NonLinMPC"]["Economic"]["Ipopt"]["MultipleShooting"] =
         sim!($empc_ipopt_ms, $N, $ry; plant=$plant2, x_0=$x_0, x̂_0=$x̂_0),
         samples=samples, evals=evals, seconds=seconds
     )
+CASE_MPC["Pendulum"]["NonLinMPC"]["Economic"]["Ipopt"]["TrapezoidalCollocation"] =
+    @benchmarkable(
+        sim!($empc_ipopt_tc, $N, $ry; plant=$plant2, x_0=$x_0, x̂_0=$x̂_0),
+        samples=samples, evals=evals, seconds=seconds
+    )
 CASE_MPC["Pendulum"]["NonLinMPC"]["Economic"]["MadNLP"]["SingleShooting"] = 
     @benchmarkable(
         sim!($empc_madnlp_ss, $N, $ry; plant=$plant2, x_0=$x_0, x̂_0=$x̂_0),
@@ -373,8 +405,17 @@ nmpc2_ipopt_ms = NonLinMPC(estim2;
 nmpc2_ipopt_ms = setconstraint!(nmpc2_ipopt_ms; umin, umax)
 JuMP.unset_time_limit_sec(nmpc2_ipopt_ms.optim)
 
+optim = JuMP.Model(optimizer_with_attributes(Ipopt.Optimizer,"sb"=>"yes"), add_bridges=false)
+transcription = TrapezoidalCollocation()
+nmpc2_ipopt_tc = NonLinMPC(estim2; 
+    Hp, Hc, Nwt=Nwt, Mwt=[0.5, 0], Cwt, gc!, nc, p=Pmax, optim, transcription
+)
+nmpc2_ipopt_tc = setconstraint!(nmpc2_ipopt_tc; umin, umax)
+JuMP.unset_time_limit_sec(nmpc2_ipopt_tc.optim)
+
 # TODO: test custom constraints with MadNLP and SingleShooting, see comment above.
 # TODO: test custom constraints with MadNLP and MultipleShooting, see comment above.
+# TODO: test custom constraints with MadNLP and TrapezoidalCollocation, see comment above.
 
 samples, evals, seconds = 100, 1, 15*60
 CASE_MPC["Pendulum"]["NonLinMPC"]["Custom constraints"]["Ipopt"]["SingleShooting"] = 
@@ -387,6 +428,11 @@ CASE_MPC["Pendulum"]["NonLinMPC"]["Custom constraints"]["Ipopt"]["MultipleShooti
         sim!($nmpc2_ipopt_ms, $N, $ry; plant=$plant2, x_0=$x_0, x̂_0=$x̂_0),
         samples=samples, evals=evals, seconds=seconds
     )
+CASE_MPC["Pendulum"]["NonLinMPC"]["Custom constraints"]["Ipopt"]["TrapezoidalCollocation"] =
+    @benchmarkable(
+        sim!($nmpc2_ipopt_tc, $N, $ry; plant=$plant2, x_0=$x_0, x̂_0=$x̂_0),
+        samples=samples, evals=evals, seconds=seconds
+    )
 
 # ----------------- Case study: Pendulum successive linearization -------------------------
 linmodel = linearize(model, x=[0, 0], u=[0])

test/3_test_predictive_control.jl

@@ -699,6 +699,12 @@ end
     @test length(nmpc16.Z̃) == nonlinmodel.nu*nmpc16.Hc + nmpc16.estim.nx̂*nmpc16.Hp + nmpc16.nϵ
     @test nmpc16.con.neq == nmpc16.estim.nx̂*nmpc16.Hp
     @test nmpc16.con.nc == 10
+    nonlinmodel_c = NonLinModel((ẋ,x,u,_,_)->ẋ .= -0.1x .+ u, (y,x,_,_)->y.=x, 1, 1, 1, 1)
+    nmpc16 = NonLinMPC(nonlinmodel_c, Hp=10, transcription=TrapezoidalCollocation(), nc=10, gc=gc!)
+    @test nmpc16.transcription == TrapezoidalCollocation()
+    @test length(nmpc16.Z̃) == nonlinmodel_c.nu*nmpc16.Hc + nmpc16.estim.nx̂*nmpc16.Hp + nmpc16.nϵ
+    @test nmpc16.con.neq == nmpc16.estim.nx̂*nmpc16.Hp
+    @test nmpc16.con.nc == 10
     nmpc17 = NonLinMPC(linmodel1, Hp=10, transcription=MultipleShooting())
     @test nmpc17.transcription == MultipleShooting()
     @test length(nmpc17.Z̃) == linmodel1.nu*nmpc17.Hc + nmpc17.estim.nx̂*nmpc17.Hp + nmpc17.nϵ
@@ -721,6 +727,7 @@ end
     @test_throws ErrorException NonLinMPC(nonlinmodel, Hp=15, JE  = (_,_,_)->0.0)
     @test_throws ErrorException NonLinMPC(nonlinmodel, Hp=15, gc  = (_,_,_,_)->[0.0], nc=1)
     @test_throws ErrorException NonLinMPC(nonlinmodel, Hp=15, gc! = (_,_,_,_)->[0.0], nc=1)
+    @test_throws ArgumentError NonLinMPC(nonlinmodel, transcription=TrapezoidalCollocation())
 
     @test_logs (:warn, Regex(".*")) NonLinMPC(nonlinmodel, Hp=15, JE=(Ue,_,_,_)->Ue)
     @test_logs (:warn, Regex(".*")) NonLinMPC(nonlinmodel, Hp=15, gc=(Ue,_,_,_,_)->Ue, nc=0)    
@@ -792,6 +799,13 @@ end
     # execute update_predictions! branch in `geqfunc_i` for coverage:
     geq_end = nmpc5.optim[:geq_2].func
     @test_nowarn geq_end(5.0, 4.0, 3.0, 2.0)
+    f! = (ẋ,x,u,_,_) -> ẋ .= -0.001x .+ u 
+    h! = (y,x,_,_) -> y .= x 
+    nonlinmodel_c = NonLinModel(f!, h!, 500, 1, 1, 1)
+    nmpc5 = NonLinMPC(nonlinmodel_c, Nwt=[0], Hp=100, Hc=1, transcription=TrapezoidalCollocation())
+    preparestate!(nmpc5, [0.0])
+    u = moveinput!(nmpc5, [1/0.001])
+    @test u ≈ [1.0] atol=5e-2
     nmpc6  = NonLinMPC(linmodel3, Hp=10)
     preparestate!(nmpc6, [0])
     @test moveinput!(nmpc6, [0]) ≈ [0.0] atol=5e-2