added: simple ModelingToolkit example in manual

Author: franckgaga

docs/Project.toml

@@ -5,10 +5,15 @@ Documenter = "e30172f5-a6a5-5a46-863b-614d45cd2de4"
 JuMP = "4076af6c-e467-56ae-b986-b466b2749572"
 LinearAlgebra = "37e2e46d-f89d-539d-b4ee-838fcccc9c8e"
 Logging = "56ddb016-857b-54e1-b83d-db4d58db5568"
+ModelingToolkit = "961ee093-0014-501f-94e3-6117800e7a78"
 Plots = "91a5bcdd-55d7-5caf-9e0b-520d859cae80"
 
 [compat]
 ControlSystemsBase = "1"
+DAQP = "0.5"
 Documenter = "1"
+JuMP = "1"
 LinearAlgebra = "1.6"
 Logging = "1.6"
+ModelingToolkit = "9"
+Plots = "1"

docs/make.jl

@@ -27,6 +27,7 @@ makedocs(
             "Examples" => [
                 "Linear Design" => "manual/linmpc.md",
                 "Nonlinear Design" => "manual/nonlinmpc.md",
+                "MTK Integration" =>  "manual/mtk.md",
             ],
         ],
         "Functions" => [

docs/src/index.md

@@ -29,6 +29,7 @@ Pages = [
     "manual/installation.md",
     "manual/linmpc.md",
     "manual/nonlinmpc.md",
+    "manual/mtk.md"
 ]
 ```
 

docs/src/manual/mtk.md

@@ -0,0 +1,129 @@
+# [Manual: ModelingToolkit Integration](@id man_mtk)
+
+```@contents
+Pages = ["mtk.md"]
+```
+
+```@setup 1
+using Logging; errlogger = ConsoleLogger(stderr, Error);
+old_logger = global_logger(); global_logger(errlogger);
+```
+
+## Pendulum Example
+
+This example integrates the simple pendulum model of the [last section](@man_nonlin) in the
+[ModelingToolkit.jl](https://docs.sciml.ai/ModelingToolkit/stable/) (MTK) framework and
+extracts appropriate `f!` and `h!` functions to construct a [`NonLinModel`](@ref).
+
+!!! danger "Disclaimer"
+    This simple example is not an official interface to `ModelingToolkit.jl`. It is provided
+    as a basic template to combine both packages. There is no guarantee that it will work
+    for all corner cases.
+
+We first construct and instantiate the pendulum model:
+
+```@example 1
+using ModelPredictiveControl, ModelingToolkit
+using ModelingToolkit: D_nounits as D, t_nounits as t, varmap_to_vars
+@mtkmodel Pendulum begin
+    @parameters begin
+        g = 9.8
+        L = 0.4
+        K = 1.2
+        m = 0.3
+    end
+    @variables begin
+        θ(t) # state
+        ω(t) # state
+        τ(t) # input
+        y(t) # output
+    end
+    @equations begin
+        D(θ)    ~ ω
+        D(ω)    ~ -g/L*sin(θ) - K/m*ω + τ/m/L^2
+        y       ~ θ * 180 / π
+    end
+end
+@named mtk_model = Pendulum()
+mtk_model = complete(mtk_model)
+```
+
+We than convert the MTK model to an [input-output system](https://docs.sciml.ai/ModelingToolkit/stable/basics/InputOutput/):
+
+```@example 1
+function generate_f_h(model, inputs, outputs)
+    (_, f_ip), dvs, psym, io_sys = ModelingToolkit.generate_control_function(
+        model, inputs, split=false; outputs
+    )
+    if any(ModelingToolkit.is_alg_equation, equations(io_sys)) 
+        error("Systems with algebraic equations are not supported")
+    end
+    h_ = ModelingToolkit.build_explicit_observed_function(io_sys, outputs; inputs = inputs)
+    nx = length(dvs)
+    vx = string.(dvs)
+    par = varmap_to_vars(defaults(io_sys), psym)
+    function f!(ẋ, x, u, _ , _ )
+        f_ip(ẋ, x, u, par, 1)
+        nothing
+    end
+    function h!(y, x, _ , _ )
+        y .= h_(x, 1, par, 1)
+        nothing
+    end
+    return f!, h!, nx, vx
+end
+inputs, outputs = [mtk_model.τ], [mtk_model.y]
+f!, h!, nx, vx = generate_f_h(mtk_model, inputs, outputs)
+nu, ny, Ts = length(inputs), length(outputs), 0.1
+vu, vy = ["\$τ\$ (Nm)"], ["\$θ\$ (°)"]
+nothing # hide
+```
+
+A [`NonLinModel`](@ref) can now be constructed:
+
+```@example 1
+model = setname!(NonLinModel(f!, h!, Ts, nu, nx, ny); u=vu, x=vx, y=vy)
+```
+
+We also instantiate a plant model with a 25 % larger friction coefficient ``K``:
+
+```@example 1
+mtk_model.K = defaults(mtk_model)[mtk_model.K] * 1.25
+f_plant, h_plant, _, _ = generate_f_h(mtk_model, inputs, outputs)
+plant = setname!(NonLinModel(f_plant, h_plant, Ts, nu, nx, ny); u=vu, x=vx, y=vy)
+```
+
+We can than reproduce the Kalman filter and the controller design of the [last section](@man_nonlin):
+
+```@example 1
+α=0.01; σQ=[0.1, 1.0]; σR=[5.0]; nint_u=[1]; σQint_u=[0.1]
+estim = UnscentedKalmanFilter(model; α, σQ, σR, nint_u, σQint_u)
+Hp, Hc, Mwt, Nwt = 20, 2, [0.5], [2.5]
+nmpc = NonLinMPC(estim; Hp, Hc, Mwt, Nwt, Cwt=Inf)
+umin, umax = [-1.5], [+1.5]
+nmpc = setconstraint!(nmpc; umin, umax)
+```
+
+The angular setpoint response is identical:
+
+```@example 1
+res_ry = sim!(nmpc, N, [180.0], plant=plant, x_0=[0, 0], x̂_0=[0, 0, 0])
+display(plot(res_ry))
+savefig("plot1_MTK.svg"); nothing # hide
+```
+
+![plot1_MTK](plot1_MTK.svg)
+
+and also the output disturbance rejection:
+
+```@example 1
+res_yd = sim!(nmpc, N, [180.0], plant=plant, x_0=[π, 0], x̂_0=[π, 0, 0], y_step=[10])
+display(plot(res_yd))
+savefig("plot2_MTK.svg"); nothing # hide
+```
+
+![plot2_MTK](plot2_MTK.svg)
+
+```@setup 1
+global_logger(old_logger);
+```

docs/src/manual/nonlinmpc.md

@@ -64,10 +64,10 @@ model = setname!(NonLinModel(f, h, Ts, nu, nx, ny; p=p_model); u=vu, x=vx, y=vy)
 
 The output function ``\mathbf{h}`` converts the ``θ`` angle to degrees. Note that special
 characters like ``θ`` can be typed in the Julia REPL or VS Code by typing `\theta` and
-pressing the `<TAB>` key. Note that the model parameter `p` can be any 
- A 4th order [`RungeKutta`](@ref) method solves the differential
-equations by default. It is good practice to first simulate `model` using [`sim!`](@ref) as
-a quick sanity check:
+pressing the `<TAB>` key. Note that the parameter `p` can be of any type but use a mutable
+type like a vector of you want to modify it later. A 4th order [`RungeKutta`](@ref) method
+solves the differential equations by default. It is good practice to first simulate `model`
+using [`sim!`](@ref) as a quick sanity check:
 
 ```@example 1
 using Plots
@@ -194,7 +194,8 @@ output vector of `plant` argument corresponds to the model output vector in `mpc
 We can now define the ``J_E`` function and the `empc` controller:
 
 ```@example 1
-function JE(UE, ŶE, _ , Ts)
+function JE(UE, ŶE, _ , p)
+    Ts = p
     τ, ω = UE[1:end-1], ŶE[2:2:end-1]
     return Ts*sum(τ.*ω)
 end