diff --git a/README.md b/README.md index dc310f023..8ad573e0a 100644 --- a/README.md +++ b/README.md @@ -110,7 +110,9 @@ for more detailed examples. - [x] quickly compare multiple optimizers - [x] nonlinear solvers relying on automatic differentiation (exact derivative) - [x] additional information about the optimum to ease troubleshooting -- [x] implementation that carefully limits allocations for real-time applications +- [x] real-time control loop features: + - [x] implementations that carefully limits the allocations + - [x] simple soft real-time utilities ### State Estimation Features diff --git a/docs/src/public/generic_func.md b/docs/src/public/generic_func.md index 2f5e9d43e..bd07d608a 100644 --- a/docs/src/public/generic_func.md +++ b/docs/src/public/generic_func.md @@ -19,25 +19,27 @@ setconstraint! evaloutput ``` -## Prepare State x +## Change State x + +### Prepare State x ```@docs preparestate! ``` -## Update State x +### Update State x ```@docs updatestate! ``` -## Init State x +### Init State x ```@docs initstate! ``` -## Set State x +### Set State x ```@docs setstate! @@ -54,3 +56,21 @@ setmodel! ```@docs getinfo ``` + +## Real-Time Simulate and Control + +!!! danger "Disclaimer" + These utilities are for soft real-time applications. They are not suitable for hard + real-time environnement like safety-critical processes. + +### Save current time t + +```@docs +savetime! +``` + +### Period Sleep + +```@docs +periodsleep +``` diff --git a/src/ModelPredictiveControl.jl b/src/ModelPredictiveControl.jl index 1d884f63c..cd001f873 100644 --- a/src/ModelPredictiveControl.jl +++ b/src/ModelPredictiveControl.jl @@ -26,6 +26,7 @@ export SimModel, LinModel, NonLinModel export DiffSolver, RungeKutta export setop!, setname! export setstate!, setmodel!, preparestate!, updatestate!, evaloutput, linearize, linearize! +export savetime!, periodsleep export StateEstimator, InternalModel, Luenberger export SteadyKalmanFilter, KalmanFilter, UnscentedKalmanFilter, ExtendedKalmanFilter export MovingHorizonEstimator diff --git a/src/controller/execute.jl b/src/controller/execute.jl index 88c10aa2b..5b4e355e3 100644 --- a/src/controller/execute.jl +++ b/src/controller/execute.jl @@ -510,6 +510,20 @@ function updatestate!(mpc::PredictiveController, u, ym, d=mpc.estim.buffer.empty end updatestate!(::PredictiveController, _ ) = throw(ArgumentError("missing measured outputs ym")) +""" + savetime!(mpc::PredictiveController) -> t + +Call `savetime!(mpc.estim.model)` and return the time `t`. +""" +savetime!(mpc::PredictiveController) = savetime!(mpc.estim.model) + +""" + periodsleep(mpc::PredictiveController) -> nothing + +Call `periodsleep(mpc.estim.model)`. +""" +periodsleep(mpc::PredictiveController) = periodsleep(mpc.estim.model) + """ setstate!(mpc::PredictiveController, x̂) -> mpc diff --git a/src/estimator/execute.jl b/src/estimator/execute.jl index d38663895..2891936f4 100644 --- a/src/estimator/execute.jl +++ b/src/estimator/execute.jl @@ -274,6 +274,22 @@ function updatestate!(estim::StateEstimator, u, ym, d=estim.buffer.empty) end updatestate!(::StateEstimator, _ ) = throw(ArgumentError("missing measured outputs ym")) + +""" + savetime!(estim::StateEstimator) -> t + +Call `savetime!(estim.model)` and return the time `t`. +""" +savetime!(estim::StateEstimator) = savetime!(estim.model) + +""" + periodsleep(estim::StateEstimator) -> nothing + +Call `periodsleep(estim.model)`. +""" +periodsleep(estim::StateEstimator) = periodsleep(estim.model) + + """ validate_args(estim::StateEstimator, ym, d, u=nothing) diff --git a/src/model/linmodel.jl b/src/model/linmodel.jl index 2f7202820..4ccb7ffe0 100644 --- a/src/model/linmodel.jl +++ b/src/model/linmodel.jl @@ -6,6 +6,7 @@ struct LinModel{NT<:Real} <: SimModel{NT} Dd ::Matrix{NT} x0::Vector{NT} Ts::NT + t::Vector{NT} nu::Int nx::Int ny::Int @@ -44,12 +45,13 @@ struct LinModel{NT<:Real} <: SimModel{NT} yname = ["\$y_{$i}\$" for i in 1:ny] dname = ["\$d_{$i}\$" for i in 1:nd] xname = ["\$x_{$i}\$" for i in 1:nx] - x0 = zeros(NT, nx) + x0 = zeros(NT, nx) + t = zeros(NT, 1) buffer = SimModelBuffer{NT}(nu, nx, ny, nd) return new{NT}( A, Bu, C, Bd, Dd, x0, - Ts, + Ts, t, nu, nx, ny, nd, uop, yop, dop, xop, fop, uname, yname, dname, xname, diff --git a/src/model/nonlinmodel.jl b/src/model/nonlinmodel.jl index b983ae2f8..0952def94 100644 --- a/src/model/nonlinmodel.jl +++ b/src/model/nonlinmodel.jl @@ -4,6 +4,7 @@ struct NonLinModel{NT<:Real, F<:Function, H<:Function, DS<:DiffSolver} <: SimMod h!::H solver::DS Ts::NT + t::Vector{NT} nu::Int nx::Int ny::Int @@ -31,12 +32,14 @@ struct NonLinModel{NT<:Real, F<:Function, H<:Function, DS<:DiffSolver} <: SimMod yname = ["\$y_{$i}\$" for i in 1:ny] dname = ["\$d_{$i}\$" for i in 1:nd] xname = ["\$x_{$i}\$" for i in 1:nx] - x0 = zeros(NT, nx) + x0 = zeros(NT, nx) + t = zeros(NT, 1) buffer = SimModelBuffer{NT}(nu, nx, ny, nd) return new{NT, F, H, DS}( x0, f!, h!, - solver, Ts, + solver, + Ts, t, nu, nx, ny, nd, uop, yop, dop, xop, fop, uname, yname, dname, xname, diff --git a/src/sim_model.jl b/src/sim_model.jl index a435c0c30..4f52353b1 100644 --- a/src/sim_model.jl +++ b/src/sim_model.jl @@ -285,6 +285,67 @@ function evaloutput(model::SimModel{NT}, d=model.buffer.empty) where NT <: Real return y end +""" + savetime!(model::SimModel) -> t + +Set `model.t` to `time()` and return the value. + +Used in conjunction with [`periodsleep`](@ref) for simple soft real-time simulations. Call +this function before any other in the simulation loop. +""" +function savetime!(model::SimModel) + model.t[] = time() + return model.t[] +end + +""" + periodsleep(model::SimModel, busywait=false) -> nothing + +Sleep for `model.Ts` s minus the time elapsed since the last call to [`savetime!`](@ref). + +It calls [`sleep`](https://docs.julialang.org/en/v1/base/parallel/#Base.sleep) if `busywait` +is `false`. Else, a simple `while` loop implements busy-waiting. As a rule-of-thumb, +busy-waiting should be used if `model.Ts < 0.1` s, since the accuracy of `sleep` is around 1 +ms. Can be used to implement simple soft real-time simulations, see the example below. + +!!! warning + The allocations in Julia are garbage-collected (GC) automatically. This can affect the + timings. In such cases, you can temporarily stop the GC with `GC.enable(false)`, and + restart it at a convenient time e.g.: just before calling `periodsleep`. + +# Examples +```jldoctest +julia> model = LinModel(tf(2, [0.3, 1]), 0.1); + +julia> function sim_realtime!(model) + t_0 = time() + for i=1:3 + t = savetime!(model) # first function called + println(round(t - t_0, digits=3)) + updatestate!(model, [1]) + periodsleep(model, true) # last function called + end + end; + +julia> sim_realtime!(model) +0.0 +0.1 +0.2 +``` +""" +function periodsleep(model::SimModel, busywait=false) + if !busywait + model.Ts < 0.1 && @warn "busy-waiting is recommended for Ts < 0.1 s" + computing_time = time() - model.t[] + sleep_time = model.Ts - computing_time + sleep_time > 0 && sleep(sleep_time) + else + while time() - model.t[] < model.Ts + end + end + return nothing +end + """ validate_args(model::SimModel, d, u=nothing) diff --git a/test/test_predictive_control.jl b/test/test_predictive_control.jl index ca80fc045..5424e92f3 100644 --- a/test/test_predictive_control.jl +++ b/test/test_predictive_control.jl @@ -302,6 +302,19 @@ end @test mpc.L_Hp ≈ diagm(1.1:1000.1) end +@testset "LinMPC real-time simulations" begin + linmodel1 = LinModel(tf(2, [10, 1]), 0.1) + mpc1 = LinMPC(linmodel1) + times1 = zeros(5) + for i=1:5 + times1[i] = savetime!(mpc1) + preparestate!(mpc1, [1]) + updatestate!(mpc1, [1], [1]) + periodsleep(mpc1) + end + @test all(isapprox.(diff(times1[2:end]), 0.1, atol=0.01)) +end + @testset "ExplicitMPC construction" begin model = LinModel(sys, Ts, i_d=[3]) mpc1 = ExplicitMPC(model, Hp=15) diff --git a/test/test_sim_model.jl b/test/test_sim_model.jl index 9f547b592..e88142172 100644 --- a/test/test_sim_model.jl +++ b/test/test_sim_model.jl @@ -111,6 +111,25 @@ end @test_throws DimensionMismatch evaloutput(linmodel1, zeros(1)) end +@testset "LinModel real time simulations" begin + linmodel1 = LinModel(tf(2, [10, 1]), 0.1) + 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) + 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)) +end + @testset "NonLinModel construction" begin linmodel1 = LinModel(sys,Ts,i_u=[1,2]) f1(x,u,_) = linmodel1.A*x + linmodel1.Bu*u @@ -274,4 +293,33 @@ end updatestate!(linmodel3, u, d) end @test all(isapprox.(Ynl, Yl, atol=1e-6)) +end + +@testset "NonLinModel real time simulations" begin + linmodel1 = LinModel(tf(2, [10, 1]), 0.1) + nonlinmodel1 = NonLinModel( + (x,u,_)->linmodel1.A*x + linmodel1.Bu*u, + (x,_)->linmodel1.C*x, + linmodel1.Ts, 1, 1, 1, 0, solver=nothing + ) + times1 = zeros(5) + for i=1:5 + times1[i] = savetime!(nonlinmodel1) + updatestate!(nonlinmodel1, [1]) + periodsleep(nonlinmodel1) + end + @test all(isapprox.(diff(times1[2:end]), 0.1, atol=0.01)) + linmodel2 = LinModel(tf(2, [0.1, 1]), 0.001) + nonlinmodel2 = NonLinModel( + (x,u,_)->linmodel2.A*x + linmodel2.Bu*u, + (x,_)->linmodel2.C*x, + linmodel2.Ts, 1, 1, 1, 0, solver=nothing + ) + times2 = zeros(5) + for i=1:5 + times2[i] = savetime!(nonlinmodel2) + updatestate!(nonlinmodel2, [1]) + periodsleep(nonlinmodel2, true) + end + @test all(isapprox.(diff(times2[2:end]), 0.001, atol=0.0001)) end \ No newline at end of file diff --git a/test/test_state_estim.jl b/test/test_state_estim.jl index 68b15fe5b..372215377 100644 --- a/test/test_state_estim.jl +++ b/test/test_state_estim.jl @@ -118,6 +118,19 @@ end skalmanfilter = SteadyKalmanFilter(linmodel, nint_ym=0) @test_throws ErrorException setmodel!(skalmanfilter, linmodel) end + +@testset "SteadyKalmanFilter real-time simulations" begin + linmodel1 = LinModel(tf(2, [10, 1]), 0.1) + skalmanfilter1 = SteadyKalmanFilter(linmodel1) + times1 = zeros(5) + for i=1:5 + times1[i] = savetime!(skalmanfilter1) + preparestate!(skalmanfilter1, [1]) + updatestate!(skalmanfilter1, [1], [1]) + periodsleep(skalmanfilter1) + end + @test all(isapprox.(diff(times1[2:end]), 0.1, atol=0.01)) +end @testset "KalmanFilter construction" begin linmodel1 = setop!(LinModel(sys,Ts,i_u=[1,2]), uop=[10,50], yop=[50,30])