test: NonLinMPC construction with JE and gc keyword argument

JuliaControl · Nov 26, 2024 · 6980f69 · 6980f69
1 parent be57e4a
commit 6980f69
Show file tree

Hide file tree

Showing 2 changed files with 32 additions and 11 deletions.
diff --git a/src/controller/nonlinmpc.jl b/src/controller/nonlinmpc.jl
@@ -406,13 +406,14 @@ should ease troubleshooting of simple bugs e.g.: the user forgets to set the `nc
 function test_custom_functions(NT, model::SimModel, JE, gc!, nc, Uop, Yop, Dop, p)
     uop, dop, yop = model.uop, model.dop, model.yop
     Ue, Ŷe, D̂e = [Uop; uop], [yop; Yop], [dop; Dop]
-    try 
-        JE(Ue, Ŷe, D̂e, p)
+    try
+        val::NT = JE(Ue, Ŷe, D̂e, p)
     catch err
         @warn(
             """
             Calling the JE function with Ue, Ŷe, D̂e arguments fixed at uop=$uop, 
-            yop=$yop, dop=$dop failed with the following stacktrace.
+            yop=$yop, dop=$dop failed with the following stacktrace. Did you forget
+            to set the keyword argument p?
             """, 
             exception=(err, catch_backtrace())
         )
@@ -425,7 +426,7 @@ function test_custom_functions(NT, model::SimModel, JE, gc!, nc, Uop, Yop, Dop,
             """
             Calling the gc function with Ue, Ŷe, D̂e, ϵ arguments fixed at uop=$uop,
             yop=$yop, dop=$dop, ϵ=0 failed with the following stacktrace. Did you 
-            forget to set the keyword argument nc?
+            forget to set the keyword argument p or nc?
             """, 
             exception=(err, catch_backtrace())
         )

diff --git a/test/test_predictive_control.jl b/test/test_predictive_control.jl
@@ -18,7 +18,10 @@ sys = [ tf(1.90,[1800.0,1])   tf(1.90,[1800.0,1])   tf(1.90,[1800.0,1]);
     @test mpc5.Ñ_Hc ≈ Diagonal(diagm([repeat(Float64[3, 4], 5); [1e3]]))
     mpc6 = LinMPC(model, Lwt=[0,1], Hp=15)
     @test mpc6.L_Hp ≈ Diagonal(diagm(repeat(Float64[0, 1], 15)))
-    mpc7 = LinMPC(model, optim=JuMP.Model(DAQP.Optimizer))
+    mpc7 = @test_logs(
+        (:warn, "Solving time limit is not supported by the optimizer."), 
+        LinMPC(model, optim=JuMP.Model(DAQP.Optimizer))
+    )
     @test solver_name(mpc7.optim) == "DAQP"
     kf = KalmanFilter(model)
     mpc8 = LinMPC(kf)
@@ -37,7 +40,12 @@ sys = [ tf(1.90,[1800.0,1])   tf(1.90,[1800.0,1])   tf(1.90,[1800.0,1]);
     @test isa(mpc13, LinMPC{Float32})
     @test isa(mpc13.optim, JuMP.GenericModel{Float64}) # OSQP does not support Float32
 
-    @test_throws ArgumentError LinMPC(model, Hp=0)
+    @test_logs(
+        (:warn, 
+        "prediction horizon Hp (0) ≤ estimated number of delays in model (0), the "*
+        "closed-loop system may be unstable or zero-gain (unresponsive)"), 
+        @test_throws ArgumentError LinMPC(model, Hp=0)
+    )
     @test_throws ArgumentError LinMPC(model, Hc=0)
     @test_throws ArgumentError LinMPC(model, Hp=1, Hc=2)
     @test_throws ArgumentError LinMPC(model, Mwt=[1])
@@ -134,8 +142,11 @@ end
     preparestate!(mpc1, [50, 30])
     updatestate!(mpc1, mpc1.estim.model.uop, [50, 30])
     @test mpc1.estim.x̂0 ≈ [0,0,0,0]
-    # do not call preparestate! before moveinput! for the warning:
-    moveinput!(mpc1, [10, 50])
+    @test_logs(
+        (:warn, "preparestate! should be called before moveinput! with current estimators"), 
+        (:warn, "preparestate! should be called before evaloutput with current estimators"),
+        moveinput!(mpc1, [10, 50])
+    )
     @test_throws ArgumentError updatestate!(mpc1, [0,0])
 end
 
@@ -491,9 +502,9 @@ end
     @test nmpc5.Ñ_Hc ≈ Diagonal(diagm([repeat(Float64[3, 4], 5); [1e3]]))
     nmpc6 = NonLinMPC(nonlinmodel, Hp=15, Lwt=[0,1])
     @test nmpc6.L_Hp ≈ Diagonal(diagm(repeat(Float64[0, 1], 15)))
-    nmpc7 = NonLinMPC(nonlinmodel, Hp=15, Ewt=1e-3, JE=(Ue,Ŷe,D̂e,p) -> p*Ue.*Ŷe.*D̂e, p=2)
+    nmpc7 = NonLinMPC(nonlinmodel, Hp=15, Ewt=1e-3, JE=(Ue,Ŷe,D̂e,p) -> p*dot(Ue,Ŷe)+sum(D̂e), p=10)
     @test nmpc7.E == 1e-3
-    @test nmpc7.JE([1,2],[3,4],[4,6],2) == 2*[1,2].*[3,4].*[4,6]
+    @test nmpc7.JE([1,2],[3,4],[4,6],10) == 10*dot([1,2],[3,4])+sum([4,6])
     optim = JuMP.Model(optimizer_with_attributes(Ipopt.Optimizer, "nlp_scaling_max_gradient"=>1.0))
     nmpc8 = NonLinMPC(nonlinmodel, Hp=15, optim=optim)
     @test solver_name(nmpc8.optim) == "Ipopt"
@@ -513,6 +524,10 @@ end
     @test nmpc13.Ñ_Hc ≈ Diagonal([0.1,0.11,0.12,0.13])
     nmcp14 = NonLinMPC(nonlinmodel, Hp=10, L_Hp=Diagonal(collect(0.001:0.001:0.02)))
     @test nmcp14.L_Hp ≈ Diagonal(collect(0.001:0.001:0.02))
+    nmpc15 = NonLinMPC(nonlinmodel, Hp=10, gc=(Ue,Ŷe,D̂e,p,ϵ)-> [p*dot(Ue,Ŷe)+sum(D̂e)+ϵ], nc=1, p=10)
+    LHS = zeros(1)
+    nmpc15.con.gc!(LHS,[1,2],[3,4],[4,6],10,0.1) 
+    @test LHS ≈ [10*dot([1,2],[3,4])+sum([4,6])+0.1]
 
     nonlinmodel2 = NonLinModel{Float32}(f, h, Ts, 2, 4, 2, 1, solver=nothing)
     nmpc15  = NonLinMPC(nonlinmodel2, Hp=15)
@@ -521,7 +536,12 @@ end
 
     @test_throws ArgumentError NonLinMPC(nonlinmodel, Hp=15, Ewt=[1, 1])
     @test_throws ArgumentError NonLinMPC(nonlinmodel)
-    @test_throws ErrorException NonLinMPC(nonlinmodel, Hp=15, JE=(_,_,_)->0.0)
+    @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_logs (:warn, Regex(".*")) NonLinMPC(nonlinmodel, Hp=15, JE=(Ue,_,_,_)->Ue)
+    @test_logs (:warn, Regex(".*")) NonLinMPC(nonlinmodel, Hp=15, gc=(Ue,_,_,_,_)->Ue, nc=0)    
 end
 
 @testset "NonLinMPC moves and getinfo" begin