Merge pull request #94 from JuliaControl/soft_real_time_simple

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