Manual: ModelingToolkit Integration
Pendulum Model
This example integrates the simple pendulum model of the last section in ModelingToolkit
(MTK) framework and extracts appropriate f!
and h!
functions to construct a NonLinModel
. An NonLinMPC
is designed from this model and simulated to reproduce the results of the last section.
This simple example is not an official interface to ModelingToolkit.jl
. It is provided as a basic starting 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:
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)
\[ \begin{align} \frac{\mathrm{d} \theta\left( t \right)}{\mathrm{d}t} &= \omega\left( t \right) \\ \frac{\mathrm{d} \omega\left( t \right)}{\mathrm{d}t} &= \frac{ - g \sin\left( \theta\left( t \right) \right)}{L} + \frac{\tau\left( t \right)}{L^{2} m} + \frac{ - K \omega\left( t \right)}{m} \\ y\left( t \right) &= 57.296 \theta\left( t \right) \end{align} \]
We than convert the MTK model to an input-output system:
function generate_f_h(model, inputs, outputs)
(_, f_ip), x_sym, p_sym, 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
nu, nx, ny = length(inputs), length(x_sym), length(outputs)
function f!(ẋ, x, u, _ , p)
try
f_ip(ẋ, x, u, p, nothing)
catch err
if err isa MethodError
error("NonLinModel does not support a time argument t in the f function, "*
"see the constructor docstring for a workaround.")
else
rethrow()
end
end
return nothing
end
(_, h_ip) = ModelingToolkit.build_explicit_observed_function(
io_sys, outputs; inputs, return_inplace = true
)
u_nothing = fill(nothing, nu)
function h!(y, x, _ , p)
try
# MTK.jl supports a `u` argument in `h_ip` function but not this package. We set
# `u` as a vector of nothing and `h_ip` function will presumably throw an
# MethodError it this argument is used inside the function
h_ip(y, x, u_nothing, p, nothing)
catch err
if err isa MethodError
error("NonLinModel only support strictly proper systems (no manipulated "*
"input argument u in the output function h)")
else
rethrow()
end
end
return nothing
end
p = varmap_to_vars(defaults(io_sys), p_sym)
return f!, h!, p, x_sym, nu, nx, ny
end
inputs, outputs = [mtk_model.τ], [mtk_model.y]
f!, h!, p, x_sym, nu, nx, ny = generate_f_h(mtk_model, inputs, outputs)
x_sym
2-element Vector{SymbolicUtils.BasicSymbolic{Real}}:
ω(t)
θ(t)
Since MTK is an acausal modeling framework, we do not have the control on the state realization chosen by the package. The content of x_sym
above shows it settled for the state vector $\mathbf{x}(t) = [\begin{smallmatrix}ω(t) && θ(t)\end{smallmatrix}]'$, that is, the states of the last section in the reverse order. We can now construct a NonLinModel
with this specific state realization:
vu, vx, vy = ["\$τ\$ (Nm)"], ["\$ω\$ (rad/s)", "\$θ\$ (rad)"], ["\$θ\$ (°)"]
Ts = 0.1
model = setname!(NonLinModel(f!, h!, Ts, nu, nx, ny; p); u=vu, x=vx, y=vy)
NonLinModel with a sample time Ts = 0.1 s:
├ solver: RungeKutta(4)
├ jacobian: AutoForwardDiff
└ dimensions:
├ 1 manipulated inputs u
├ 2 states x
├ 1 outputs y
└ 0 measured disturbances d
We also instantiate a plant model with a 25 % larger friction coefficient $K$:
@named mtk_plant = Pendulum(K=1.25*defaults(mtk_model)[mtk_model.K])
mtk_plant = complete(mtk_plant)
inputs, outputs = [mtk_plant.τ], [mtk_plant.y]
f2!, h2!, p2 = generate_f_h(mtk_plant, inputs, outputs)
plant = setname!(NonLinModel(f2!, h2!, Ts, nu, nx, ny; p=p2), u=vu, x=vx, y=vy)
NonLinModel with a sample time Ts = 0.1 s:
├ solver: RungeKutta(4)
├ jacobian: AutoForwardDiff
└ dimensions:
├ 1 manipulated inputs u
├ 2 states x
├ 1 outputs y
└ 0 measured disturbances d
Controller Design
We can than reproduce the Kalman filter and the controller design of the last section by reversing the order of σQ
vector, because of the different state realization:
α=0.01; σQ=[1.0, 0.1]; σ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)
NonLinMPC controller with a sample time Ts = 0.1 s:
├ estimator: UnscentedKalmanFilter
├ model: NonLinModel
├ optimizer: Ipopt
├ transcription: SingleShooting
├ gradient: AutoForwardDiff
├ jacobian: AutoForwardDiff
└ dimensions:
├ 20 prediction steps Hp
├ 2 control steps Hc
├ 0 slack variable ϵ (control constraints)
├ 1 manipulated inputs u (1 integrating states)
├ 3 estimated states x̂
├ 1 measured outputs ym (0 integrating states)
├ 0 unmeasured outputs yu
└ 0 measured disturbances d
The 180° setpoint response is identical:
using Plots
N = 35
res_ry = sim!(nmpc, N, [180.0], plant=plant, x_0=[0, 0], x̂_0=[0, 0, 0])
plot(res_ry)
and also the output disturbance rejection:
res_yd = sim!(nmpc, N, [180.0], plant=plant, x_0=[0, π], x̂_0=[0, π, 0], y_step=[10])
plot(res_yd)
Acknowledgement
Authored by 1-Bart-1
and baggepinnen
, thanks for the contribution.