Better integration with ModellingToolkit

[ufechner7]

baggepinnen wrote on Discourse what you need to do if you want to use an MTK model with this package:

• Build a dynamics function that includes inputs, docs here.
• Write a wrapper that changes the order of the input arguments to match the order assumed by ModelPredictiveControl.jl
• Possibly build an output function, docs on the same page
• Manually keep track of the order of state variables in the generated function

As a first step it would be nice to have a simple, working example for this.

[baggepinnen]

Please consider working through and contributing this example @ufechner7

[franckgaga]

FYI, I'm working on some (final) breaking change for the v1.0.0 release that should simplify these steps. I will change the order of the input arguments to match the order of MTK and I will officially support a time $t$ and a parameter (instead of a closure, like in the manual) arguments.

[baggepinnen]

A problem I see with including an MTK example is that it is rather involved to cover all corner cases that MTK exposes. Committing to an interface to MTK is a substantial amount of nontrivial work, and we do not want to give users false indications of a well supported interface.

[franckgaga]

I don't plan on making an official interface to MTK, but a section in the manual that exposes the steps for that would be clearly useful (with a disclaimer that it's not an official interface). I personally don't have much experience with MTK so it would not be easy task for me (and my little experience with MTK was rather bad in term of clear error messages)

Anyway, there is already JuliaSimControl that is tightly integrated with MTK by design. No need to overlap on this specific feature.

[1-Bart-1]

I can try making a working example. And yes, JuliaSimControl is tightly integrated with MTK, but it is not open source. So I think more people would be interested in documentation for using MTK with ModelPredictiveControl.jl.

[1-Bart-1]

@franckgaga do you know how soon you will make those breaking changes?

[baggepinnen]

You can use this example as a guide https://baggepinnen.github.io/ControlSystemIdentification.jl/dev/nonlinear/#Identifying-parameters-in-a-ModelingToolkit-model

[franckgaga]

@franckgaga do you know how soon you will make those breaking changes?

I will work on these feature this week and in the next week. I would say in max 2 weeks these changes will be released.

[1-Bart-1]

The following example works until the last 4 lines. In these last lines, ForwardDiff tries to run f! with ForwardDiff.Dual numbers, but this does not work with the generated f! function. Any ideas on how to fix this?

using ModelPredictiveControl
using ModelingToolkit
using ModelingToolkit: D_nounits as D, t_nounits as t

@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

function generate_f_h(model, par)
    @named mtkmodel = model()
    mtkmodel = complete(mtkmodel)
    inputs = [mtkmodel.τ]
    outputs = [mtkmodel.y]
    (f_, f_ip), dvs, psym, io_sys = ModelingToolkit.generate_control_function(mtkmodel, inputs, split=false; outputs=outputs)
    h_ = ModelingToolkit.build_explicit_observed_function(io_sys, outputs; inputs = inputs)
    function f!(dx, x, u, _)
        @show dx
        dx .= f_(x, u, par, 1)
    end
    function h!(y, x, _)
        y .= h_(x, 1, par, 1)
    end
    return f!, h!
end

const par = (9.8, 0.4, 1.2, 0.3)
f!, h! = generate_f_h(Pendulum, par)
nu, nx, ny, Ts = 1, 2, 1, 0.1
vu, vx, vy = ["\$τ\$ (Nm)"], ["\$θ\$ (rad)", "\$ω\$ (rad/s)"], ["\$θ\$ (°)"]
model = setname!(NonLinModel(f!, h!, Ts, nu, nx, ny); u=vu, x=vx, y=vy)

using Plots
u = [0.5]
N = 35
res = sim!(model, N, u)
display(plot(res, plotu=false))

α=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)

const par_plant = (par[1], par[2], 1.25*par[3], par[4])
f_plant, h_plant = generate_f_h(Pendulum, par_plant)
plant = setname!(NonLinModel(f_plant, h_plant, Ts, nu, nx, ny); u=vu, x=vx, y=vy)
@time res = sim!(estim, N, [0.5], plant=plant, y_noise=[0.5])
display(plot(res, plotu=false, plotxwithx̂=true))

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)

res_ry = sim!(nmpc, N, [180.0], plant=plant, x_0=[0, 0], x̂_0=[0, 0, 0])
display(plot(res_ry))

res_yd = sim!(nmpc, N, [180.0], plant=plant, x_0=[π, 0], x̂_0=[π, 0, 0], y_step=[10])
display(plot(res_yd))

[baggepinnen]

In

    function f!(dx, x, u, _)
        @show dx
        dx .= f_(x, u, par, 1)
    end

you could use f_ip instead, this is "in place" and accepts dx as the first argument. What type does dx have?

This works

function generate_f_h(model, par)
    @named mtkmodel = model()
    mtkmodel = complete(mtkmodel)
    inputs = [mtkmodel.τ]
    outputs = [mtkmodel.y]
    (f_, f_ip), dvs, psym, io_sys = ModelingToolkit.generate_control_function(mtkmodel, inputs, split=false; outputs=outputs)
    any(ModelingToolkit.is_alg_equation, equations(io_sys)) && error("Systems with algebraic equations are not supported")
    h_ = ModelingToolkit.build_explicit_observed_function(io_sys, outputs; inputs = inputs)
    function f!(dx, x, u, _)
        f_ip(dx, x, u, par, 1)
        dx
    end
    function h!(y, x, _)
        y .= h_(x, 1, par, 1)
    end
    return f!, h!
end

[baggepinnen]

Another note, MTK does not guarantee that the state realization remains constant between versions (even patch versions are allowed to change this), the way you assign vx is thus fragile and subject to breakage at any time. Use something like vx = string.(dvs) instead. nx = 2 is also not a valid assumption, nx = length(dvs) is more robust. For safety, you should also check whether or not the simplified system contains algebraic equations, if it does, an error should be thrown since the RK4 integrator used by NonLinModel does not support algebraic equations. If someone tries to use this example on a system with algebraic equations they will be silently treated as differential equations and you get a very confusing situation :) You check this by

any(ModelingToolkit.is_alg_equation, equations(io_sys)) && error("Systems with algebraic equations are not supported")

[1-Bart-1]

The updated example with your comments works well:

using ModelPredictiveControl
using ModelingToolkit
using ModelingToolkit: D_nounits as D, t_nounits as t

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

function generate_f_h(model, par, inputs, outputs)
    (_, f_ip), dvs, _, io_sys = ModelingToolkit.generate_control_function(model, inputs, split=false; outputs=outputs)
    any(ModelingToolkit.is_alg_equation, equations(io_sys)) && error("Systems with algebraic equations are not supported")
    h_ = ModelingToolkit.build_explicit_observed_function(io_sys, outputs; inputs = inputs)
    nx = length(dvs)
    vx = string.(dvs)
    function f!(dx, x, u, _)
        f_ip(dx, 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]

const par = (9.8, 0.4, 1.2, 0.3)
f!, h!, nx, vx = generate_f_h(mtk_model, par, inputs, outputs)
nu, ny, Ts = 1, 1, 0.1
vu, vy = ["\$τ\$ (Nm)"], ["\$θ\$ (°)"]
model = setname!(NonLinModel(f!, h!, Ts, nu, nx, ny); u=vu, x=vx, y=vy)

using Plots
u = [0.5]
N = 35
res = sim!(model, N, u)
display(plot(res, plotu=false))

α=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)

const par_plant = (par[1], par[2], 1.25*par[3], par[4])
f_plant, h_plant, _, _ = generate_f_h(mtk_model, par_plant, inputs, outputs)
plant = setname!(NonLinModel(f_plant, h_plant, Ts, nu, nx, ny); u=vu, x=vx, y=vy)
res = sim!(estim, N, [0.5], plant=plant, y_noise=[0.5])
display(plot(res, plotu=false, plotxwithx̂=true))

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)

res_ry = sim!(nmpc, N, [180.0], plant=plant, x_0=[0, 0], x̂_0=[0, 0, 0])
display(plot(res_ry))

res_yd = sim!(nmpc, N, [180.0], plant=plant, x_0=[π, 0], x̂_0=[π, 0, 0], y_step=[10])
display(plot(res_yd))

[baggepinnen]

Where does this particular order of parameters come from?

const par = (9.8, 0.4, 1.2, 0.3)

it is most likely incorrect, the generated dynamics have parameters in this order

julia> psym
4-element Vector{Num}:
 L
 g
 K
 m

This is a safer way to get the parameters in the correct order

julia> getindex.(Ref(defaults(io_sys)), psym)
4-element Vector{Float64}:
 0.4
 9.8
 1.2
 0.3

It is generally unsafe to assume the order of elements of any variable in MTK. The one exception is when the order is explicitly indicated by the user, such as the order of inputs and outputs to generate_control_function and build_explicit_observed_function. This page contains some utilities and explanations as to why
https://docs.sciml.ai/ModelingToolkit/stable/basics/FAQ/

The final system that has all the required information about unknowns and variable order is io_sys, and the unknowns dvs and parameters psym are returned from generate_control_function precisely because the layout of those variables will be required for correct usage of the generated function.

[1-Bart-1]

But in which order does f_ip want its parameters? In the Ref(defaults(io_sys)) order or in the psym order? And somehow I get a different order than you:
getindex.(Ref(defaults(io_sys)), psym) = [9.8, 0.4, 1.2, 0.3]
Ref(defaults(io_sys)) = Base.RefValue{Dict{Any, Any}}(Dict{Any, Any}(L => 0.4, g => 9.8, K => 1.2, m => 0.3))

[baggepinnen]

defaults(io_sys) is a Dict so it does not have an order defined.

And somehow I get a different order than you

This is exactly why it is tricky to get things like this right with MTK ;)

But in which order does f_ip want its parameters? In the Ref(defaults(io_sys)) order or in the psym order?

in the psym order