Added: soft real time utilities

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
 

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
+```

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

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
 

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)
 

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,

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,

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)
 

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)

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

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])