Merge branch 'main' into multiple_shooting

Author: franckgaga

Project.toml

@@ -1,7 +1,7 @@
 name = "ModelPredictiveControl"
 uuid = "61f9bdb8-6ae4-484a-811f-bbf86720c31c"
 authors = ["Francis Gagnon"]
-version = "1.3.1"
+version = "1.3.2"
 
 [deps]
 ControlSystemsBase = "aaaaaaaa-a6ca-5380-bf3e-84a91bcd477e"
@@ -28,13 +28,15 @@ PrecompileTools = "1"
 ProgressLogging = "0.1"
 Random = "1.6"
 RecipesBase = "1"
+TestItemRunner = "1.1"
 julia = "1.10"
 
 [extras]
 DAQP = "c47d62df-3981-49c8-9651-128b1cd08617"
 Documenter = "e30172f5-a6a5-5a46-863b-614d45cd2de4"
 Plots = "91a5bcdd-55d7-5caf-9e0b-520d859cae80"
 Test = "8dfed614-e22c-5e08-85e1-65c5234f0b40"
+TestItemRunner = "f8b46487-2199-4994-9208-9a1283c18c0a"
 
 [targets]
-test = ["Test", "Documenter", "Plots", "DAQP"]
+test = ["Test", "TestItemRunner", "Documenter", "Plots", "DAQP"]

src/controller/execute.jl

@@ -126,10 +126,7 @@ function getinfo(mpc::PredictiveController{NT}) where NT<:Real
     U, Ŷ = Ū, Ȳ
     U   .= mul!(U, mpc.S̃, mpc.ΔŨ) .+ mpc.T_lastu 
     Ŷ   .= Ŷ0 .+ mpc.Yop
-    oldF = copy(mpc.F)
-    F    = predictstoch!(mpc, mpc.estim) 
-    Ŷs  .= F # predictstoch! init mpc.F with Ŷs value if estim is an InternalModel
-    F   .= oldF  # restore old F value
+    predictstoch!(Ŷs, mpc, mpc.estim)
     info[:ΔU]   = mpc.ΔŨ[1:mpc.Hc*model.nu]
     info[:ϵ]    = mpc.nϵ == 1 ? mpc.ΔŨ[end] : zero(NT)
     info[:J]    = J
@@ -230,7 +227,7 @@ function initpred!(mpc::PredictiveController, model::SimModel, d, D̂, R̂y, R̂
 end
 
 """
-    initpred_common!(mpc::PredictiveController, model::SimModel, d, D̂, R̂y, R̂u) -> F
+    initpred_common!(mpc::PredictiveController, model::SimModel, d, D̂, R̂y, R̂u) -> mpc.F
 
 Common computations of `initpred!` for all types of [`SimModel`](@ref).
 
@@ -250,23 +247,22 @@ function initpred_common!(mpc::PredictiveController, model::SimModel, d, D̂, R
     end
     mpc.R̂y .= R̂y
     mpc.R̂u .= R̂u
-    F = predictstoch!(mpc, mpc.estim)  # init mpc.F with Ŷs for InternalModel
-    return F
+    predictstoch!(mpc.F, mpc, mpc.estim)
+    return mpc.F
 end
 
 @doc raw"""
-    predictstoch!(mpc::PredictiveController, estim::InternalModel) -> F
+    predictstoch!(Ŷs, mpc::PredictiveController, estim::InternalModel) -> nothing
 
-Init `mpc.F` vector with ``\mathbf{F = Ŷ_s}`` when `estim` is an [`InternalModel`](@ref).
+Fill `Ŷs` in-place with stochastic predictions if `estim` is an [`InternalModel`](@ref).
 """
-function predictstoch!(mpc::PredictiveController, estim::InternalModel)
-    Ŷs = mpc.F
+function predictstoch!(Ŷs, mpc::PredictiveController, estim::InternalModel)
     mul!(Ŷs, mpc.Ks, estim.x̂s)
     mul!(Ŷs, mpc.Ps, estim.ŷs, 1, 1)
-    return mpc.F
+    return nothing
 end
-"Separate stochastic predictions are not needed if `estim` is not [`InternalModel`](@ref)."
-predictstoch!(mpc::PredictiveController, ::StateEstimator) = (mpc.F .= 0)
+"Fill `Ŷs` vector with 0 values when `estim` is not an [`InternalModel`](@ref)."
+predictstoch!(Ŷs, mpc::PredictiveController, ::StateEstimator) = (Ŷs .= 0; nothing)
 
 @doc raw"""
     linconstraint!(mpc::PredictiveController, model::LinModel)
@@ -588,11 +584,11 @@ Call `savetime!(mpc.estim.model)` and return the time `t`.
 savetime!(mpc::PredictiveController) = savetime!(mpc.estim.model)
 
 """
-    periodsleep(mpc::PredictiveController) -> nothing
+    periodsleep(mpc::PredictiveController, busywait=false) -> nothing
 
 Call `periodsleep(mpc.estim.model)`.
 """
-periodsleep(mpc::PredictiveController) = periodsleep(mpc.estim.model)
+periodsleep(mpc::PredictiveController, busywait=false) = periodsleep(mpc.estim.model, busywait)
 
 """
     setstate!(mpc::PredictiveController, x̂) -> mpc

src/estimator/execute.jl

@@ -291,11 +291,11 @@ Call `savetime!(estim.model)` and return the time `t`.
 savetime!(estim::StateEstimator) = savetime!(estim.model)
 
 """
-    periodsleep(estim::StateEstimator) -> nothing
+    periodsleep(estim::StateEstimator, busywait=false) -> nothing
 
 Call `periodsleep(estim.model)`.
 """
-periodsleep(estim::StateEstimator) = periodsleep(estim.model)
+periodsleep(estim::StateEstimator, busywait=false) = periodsleep(estim.model, busywait)
 
 
 """

test/0_test_module.jl

@@ -0,0 +1,10 @@
+@testmodule SetupMPCtests begin
+    using ControlSystemsBase
+    Ts = 400.0
+    sys = [ tf(1.90,[1800.0,1])   tf(1.90,[1800.0,1])   tf(1.90,[1800.0,1]);
+            tf(-0.74,[800.0,1])   tf(0.74,[800.0,1])    tf(-0.74,[800.0,1])   ] 
+    sys_ss = minreal(ss(sys))
+    Gss = c2d(sys_ss[:,1:2], Ts, :zoh)
+    Gss2 = c2d(sys_ss[:,1:2], 0.5Ts, :zoh)
+    export Ts, sys, sys_ss, Gss, Gss2
+end

test/test_sim_model.jl renamed to test/1_test_sim_model.jl

@@ -1,11 +1,5 @@
-Ts = 4.0
-sys = [ tf(1.90,[18.0,1])   tf(1.90,[18.0,1])   tf(1.90,[18.0,1]);
-        tf(-0.74,[8.0,1])   tf(0.74,[8.0,1])    tf(-0.74,[8.0,1])   ] 
-sys_ss = minreal(ss(sys))
-Gss = c2d(sys_ss[:,1:2], Ts, :zoh)
-Gss2 = c2d(sys_ss[:,1:2], 0.5Ts, :zoh)
-
-@testset "LinModel construction" begin
+@testitem "LinModel construction" setup=[SetupMPCtests] begin
+    using .SetupMPCtests, ControlSystemsBase, LinearAlgebra
     linmodel1 = LinModel(sys, Ts, i_u=1:2)
     @test linmodel1.nx == 2
     @test linmodel1.nu == 2
@@ -29,7 +23,7 @@ Gss2 = c2d(sys_ss[:,1:2], 0.5Ts, :zoh)
     @test linmodel2.dop ≈ zeros(0,1)
 
     linmodel3 = LinModel(Gss, 0.5Ts)
-    @test linmodel3.Ts == 2.0
+    @test linmodel3.Ts == 200.0
     @test linmodel3.A ≈ Gss2.A
     @test linmodel3.C ≈ Gss2.C
 
@@ -55,7 +49,7 @@ Gss2 = c2d(sys_ss[:,1:2], 0.5Ts, :zoh)
     @test linmodel5.yop ≈ [50,30]
     @test linmodel5.dop ≈ [20]
 
-    linmodel6 = LinModel([delay(4) delay(4)]*sys,Ts,i_d=[3])
+    linmodel6 = LinModel([delay(Ts) delay(Ts)]*sys,Ts,i_d=[3])
     @test linmodel6.nx == 3
     @test sum(eigvals(linmodel6.A) .≈ 0) == 1
 
@@ -103,7 +97,8 @@ Gss2 = c2d(sys_ss[:,1:2], 0.5Ts, :zoh)
     @test_throws ErrorException LinModel(sys_ss,Ts)
 end
 
-@testset "LinModel sim methods" begin
+@testitem "LinModel sim methods" setup=[SetupMPCtests] begin
+    using .SetupMPCtests, ControlSystemsBase
     linmodel1 = setop!(LinModel(Gss), uop=[10,50], yop=[50,30])
     @test updatestate!(linmodel1, [10, 50]) ≈ zeros(2)
     @test updatestate!(linmodel1, [10, 50], Float64[]) ≈ zeros(2)
@@ -123,26 +118,28 @@ end
     @test_throws DimensionMismatch evaloutput(linmodel1, zeros(1))
 end
 
-@testset "LinModel real time simulations" begin
-    linmodel1 = LinModel(tf(2, [10, 1]), 0.1)
+@testitem "LinModel real time simulations" setup=[SetupMPCtests] begin
+    using .SetupMPCtests, ControlSystemsBase, LinearAlgebra
+    linmodel1 = LinModel(tf(2, [10, 1]), 0.25)
     times1 = zeros(5)
     for i=1:5
         times1[i] = savetime!(linmodel1)
         updatestate!(linmodel1, [1])
         periodsleep(linmodel1)
     end
-    @test all(isapprox.(diff(times1[2:end]), 0.1, atol=0.01))
-    linmodel2 = LinModel(tf(2, [0.1, 1]), 0.001)
+    @test all(isapprox.(diff(times1[2:end]), 0.25, atol=0.01))
+    linmodel2 = LinModel(tf(2, [0.1, 1]), 0.25)
     times2 = zeros(5)
     for i=1:5
         times2[i] = savetime!(linmodel2)
         updatestate!(linmodel2, [1])
         periodsleep(linmodel2, true)
     end
-    @test all(isapprox.(diff(times2[2:end]), 0.001, atol=0.0001))
+    @test all(isapprox.(diff(times2[2:end]), 0.25, atol=0.0001))
 end
 
-@testset "NonLinModel construction" begin
+@testitem "NonLinModel construction" setup=[SetupMPCtests] begin
+    using .SetupMPCtests, ControlSystemsBase, LinearAlgebra
     linmodel1 = LinModel(sys,Ts,i_u=[1,2])
     f1(x,u,_,model) = model.A*x + model.Bu*u
     h1(x,_,model)   = model.C*x
@@ -249,7 +246,8 @@ end
         (x,_)->linmodel1.C*x, Ts, 2, 4, 2, 1, solver=nothing)
 end
 
-@testset "NonLinModel sim methods" begin
+@testitem "NonLinModel sim methods" setup=[SetupMPCtests] begin
+    using .SetupMPCtests, ControlSystemsBase, LinearAlgebra
     linmodel1 = LinModel(sys,Ts,i_u=[1,2])
     f1(x,u,_,model) = model.A*x + model.Bu*u
     h1(x,_,model)   = model.C*x
@@ -268,7 +266,8 @@ end
     @test_throws DimensionMismatch evaloutput(nonlinmodel, zeros(1))
 end
 
-@testset "NonLinModel linearization" begin
+@testitem "NonLinModel linearization" setup=[SetupMPCtests] begin
+    using .SetupMPCtests, ControlSystemsBase, LinearAlgebra, ForwardDiff
     Ts = 1.0
     f1(x,u,d,_) = x.^5 + u.^4 + d.^3
     h1(x,d,_)   = x.^2 + d
@@ -313,17 +312,18 @@ end
     linmodel3 = linearize(nonlinmodel3; x, u, d)
     for i=1:N
         ynl = nonlinmodel3(d)
-        yl  = linmodel3(d)
+        global yl  = linmodel3(d)
         Ynl[i] = ynl[1]
         Yl[i]  = yl[1]
-        linmodel3 = linearize(nonlinmodel3; u, d)
+        global linmodel3 = linearize(nonlinmodel3; u, d)
         updatestate!(nonlinmodel3, u, d)
         updatestate!(linmodel3, u, d)
     end
     @test all(isapprox.(Ynl, Yl, atol=1e-6))
 end
 
-@testset "NonLinModel real time simulations" begin
+@testitem "NonLinModel real time simulations" setup=[SetupMPCtests] begin
+    using .SetupMPCtests, ControlSystemsBase, LinearAlgebra
     linmodel1 = LinModel(tf(2, [10, 1]), 0.1)
     nonlinmodel1 = NonLinModel(
         (x,u,_,_)->linmodel1.A*x + linmodel1.Bu*u,