Fixes for MTK v8

Project.toml

@@ -12,7 +12,7 @@ Symbolics = "0c5d862f-8b57-4792-8d23-62f2024744c7"
 
 [compat]
 IfElse = "0.1"
-ModelingToolkit = "5.26, 6, 7"
+ModelingToolkit = "8"
 OffsetArrays = "1"
 OrdinaryDiffEq = "5.56, 6"
 Symbolics = "0.1, 1, 2, 3, 4"

src/Blocks/Blocks.jl

@@ -1,29 +1,29 @@
 """
 The module `Blocks` contains common input-output components, referred to as blocks.
-
-In general, input-output blocks follow the convention 
-```
-     ┌───────────┐
- u   │  ẋ=f(x,u) │  y
-────►│  y=g(x,u) ├────►
-     │           │
-     └───────────┘
-```
-where `u` are inputs, `x` are state variables and `y` are outputs. `x,u,y` are all implemented as `@variables` internally, `u` are marked as `[input=true]` and `y` are marked `[output=true]`.
 """
 module Blocks
 using ModelingToolkit, Symbolics, IfElse, OrdinaryDiffEq
+using IfElse: ifelse
 
 @parameters t
-Dₜ = Differential(t)
+D = Differential(t)
+
+export RealInput, RealOutput, SISO
+include("utils.jl")
 
-export Gain, Sum
+export Gain, Sum, MatrixGain, Sum, Feedback, Add, Product, Division
+export Abs, Sign, Sqrt, Sin, Cos, Tan, Asin, Acos, Atan, Atan2, Sinh, Cosh, Tanh, Exp
+export Log, Log10
 include("math.jl")
 
-export Saturation, DeadZone
+export Constant, Sine, Cosine, Clock, Ramp, Step, ExpSine
+include("sources.jl")
+
+export Limiter, DeadZone, SlewRateLimiter
 include("nonlinear.jl")
 
-export Constant, Integrator, Derivative, FirstOrder, SecondOrder, PID, StateSpace
+export Integrator, Derivative, FirstOrder, SecondOrder, StateSpace
+export PI, LimPI, PID
 include("continuous.jl")
 
 end

src/Blocks/continuous.jl

@@ -1,31 +1,18 @@
-# TODO: remove initial values for all inputs once IO handling in MTK is in place
-"""
-    Constant(val; name)
-
-Outputs a constant value `val`.
-"""
-function Constant(val; name)
-    @variables y(t)=val [output=true]
-    @parameters val=val
-    eqs = [
-        y ~ val
-    ]
-    ODESystem(eqs, t, name=name)
-end
-
 """
     Integrator(; k=1, name)
 
 Outputs `y = ∫k*u dt`, corresponding to the transfer function `1/s`.
 """
-function Integrator(; k=1, name)
-    @variables x(t)=0 u(t)=0 [input=true] y(t)=0 [output=true]
-    @parameters k=k
+function Integrator(;name, k=1, x0=0.0)
+    @named siso = SISO()
+    @unpack u, y = siso
+    sts = @variables x(t)=x0
+    pars = @parameters k=k
     eqs = [
-        Dₜ(x) ~ k*u
+        D(x) ~ k * u
         y ~ x
     ]
-    ODESystem(eqs, t, name=name)
+    extend(ODESystem(eqs, t, sts, pars; name=name), siso)
 end
 
 """
@@ -43,14 +30,16 @@ and a state-space realization is given by `ss(-1/T, 1/T, -k/T, k/T)`
 where `T` is the time constant of the filter.
 A smaller `T` leads to a more ideal approximation of the derivative.
 """
-function Derivative(; k=1, T, name)
-    @variables x(t)=0 u(t)=0 [input=true] y(t)=0 [output=true]
-    @parameters T=T k=k
+function Derivative(; name, k=1, T=10, x0=0)
+    @named siso = SISO()
+    @unpack u, y = siso
+    sts = @variables x(t)=x0
+    pars = @parameters T=T k=k
     eqs = [
-        Dₜ(x) ~ (u - x) / T
-        y ~ (k/T)*(u - x)
+        D(x) ~ (u - x) / T
+        y ~ (k / T) * (u - x)
     ]
-    ODESystem(eqs, t, name=name)
+    extend(ODESystem(eqs, t, sts, pars; name=name), siso)
 end
 
 """
@@ -65,13 +54,15 @@ sT + 1
 ```
 """
 function FirstOrder(; k=1, T, name)
-    @variables x(t)=0 u(t)=0 [input=true] y(t) [output=true]
-    @parameters T=T k=k
+    @named siso = SISO()
+    @unpack u, y = siso
+    sts = @variables x(t)=0
+    pars = @parameters T=T k=k
     eqs = [
-        Dₜ(x) ~ (-x + k*u) / T
+        D(x) ~ (u - x) / T
         y ~ x
     ]
-    ODESystem(eqs, t, name=name)
+    extend(ODESystem(eqs, t, sts, pars; name=name), siso)
 end
 
 """
@@ -88,14 +79,75 @@ Critical damping corresponds to `d=1`, which yields the fastest step response wi
 `d = 1/√2` corresponds to a Butterworth filter of order 2 (maximally flat frequency response).
 """
 function SecondOrder(; k=1, w, d, name)
-    @variables x(t)=0 xd(t)=0 u(t)=0 [input=true] y(t) [output=true]
-    @parameters k=k w=w d=d
+    @named siso = SISO()
+    @unpack u, y = siso
+    sts = @variables x(t)=0 xd(t)=0
+    pars = @parameters k=k w=w d=d
     eqs = [
-        Dₜ(x) ~ xd
-        Dₜ(xd) ~ w*(w*(k*u - x) - 2*d*xd)
+        D(x) ~ xd
+        D(xd) ~ w*(w*(k*u - x) - 2*d*xd)
         y ~ x
     ]
-    ODESystem(eqs, t, name=name)
+    extend(ODESystem(eqs, t, sts, pars; name=name), siso)
+end
+
+"""
+PI-controller without actuator saturation and anti-windup measure.
+"""
+function PI(;name, k=1, T=1, x_start=0)
+    @named e = RealInput() # control error
+    @named u = RealOutput() # control signal
+    @variables x(t)=x_start
+    T > 0 || error("Time constant `T` has to be strictly positive")
+    pars = @parameters k=k T=T
+    eqs = [
+        D(x) ~ 1 / T * e.u
+        u.u ~ k * (x + e.u)
+    ]
+    compose(ODESystem(eqs, t, [x], pars; name=name), [e, u])
+end
+
+"""
+Text-book version of a PID-controller.
+"""
+function PID(;name, k=1, Ti=1, Td=1, Nd=10, xi_start=0, xd_start=0)
+    @named err_input = RealInput() # control error
+    @named ctr_output = RealOutput() # control signal
+    Ti > 0 || error("Time constant `Ti` has to be strictly positive")
+    Td > 0 || error("Time constant `Td` has to be strictly positive")
+    Nd > 0 || error("`Nd` has to be strictly positive")
+    @named gain = Gain(k)
+    @named int = Integrator(k=1/Ti, x0=xi_start)
+    @named der = Derivative(k=1/Td, T=1/Nd, x0=xd_start)
+    @named add = Add3()
+    eqs = [
+        connect(err_input, add.input1),
+        connect(err_input, int.input),
+        connect(err_input, der.input),
+        connect(int.output, add.input2),
+        connect(der.output, add.input3),
+        connect(add.output, gain.input),
+        connect(gain.output, ctr_output)
+    ]
+    ODESystem(eqs, t, [], []; name=name, systems=[gain, int, der, add, err_input, ctr_output])
+end
+
+"""
+PI-controller with actuator saturation and anti-windup measure.
+"""
+function LimPI(;name, k=1, T=1, u_max=1, u_min=-u_max, Ta=1)
+    @named e = RealInput() # control error
+    @named u = RealOutput() # control signal
+    @variables x(t)=0.0 u_star(t)=0.0
+    Ta > 0 || error("Time constant `Ta` has to be strictly positive")
+    T > 0 || error("Time constant `T` has to be strictly positive")
+    pars = @parameters k=k T=T u_max=u_max u_min=u_min
+    eqs = [
+        D(x) ~ e.u * k / T + 1 / Ta * (-u_star + u.u)
+        u.u ~ max(min(u_star, u_max), u_min)      
+        u_star ~ x + k * e.u
+    ]
+    compose(ODESystem(eqs, t, [x, u_star], pars; name=name), [e, u])
 end
 
 """
@@ -124,16 +176,16 @@ where the transfer function for the derivative includes additional filtering, se
 - `wd`: Set-point weighting in the derivative part.
 - `Nd`: Derivative limit, limits the derivative gain to Nd/Td. Reasonable values are ∈ [8, 20]. A higher value gives a better approximation of an ideal derivative at the expense of higher noise amplification.
 - `Ni`: `Ni*Ti` controls the time constant `Tₜ` of anti-windup tracking. A common (default) choice is `Tₜ = √(Ti*Td)` which is realized by `Ni = √(Td / Ti)`. Anti-windup can be effectively turned off by setting `Ni = Inf`.
-`gains`: If `gains = true`, `Ti` and `Td` will be interpreted as gains with a fundamental PID transfer function on parallel form `ki=Ti, kd=Td, k + ki/s + kd*s`
+` `gains`: If `gains = true`, `Ti` and `Td` will be interpreted as gains with a fundamental PID transfer function on parallel form `ki=Ti, kd=Td, k + ki/s + kd*s`
 """
-function PID(; k, Ti=false, Td=false, wp=1, wd=1,
+function LimPID(; k, Ti=false, Td=false, wp=1, wd=1,
     Ni    = Ti == 0 ? Inf : √(max(Td / Ti, 1e-6)),
     Nd    = 12,
-    y_max = Inf,
-    y_min = y_max > 0 ? -y_max : -Inf,
+    u_max = Inf,
+    u_min = u_max > 0 ? -u_max : -Inf,
     gains = false,
     name
-)
+    )
     if gains
         Ti = k / Ti
         Td = Td / k
@@ -142,28 +194,31 @@ function PID(; k, Ti=false, Td=false, wp=1, wd=1,
     0 ≤ wd ≤ 1 || throw(ArgumentError("wd out of bounds, got $(wd) but expected wd ∈ [0, 1]"))
     Ti ≥ 0     || throw(ArgumentError("Ti out of bounds, got $(Ti) but expected Ti ≥ 0"))
     Td ≥ 0     || throw(ArgumentError("Td out of bounds, got $(Td) but expected Td ≥ 0"))
-    y_max ≥ y_min || throw(ArgumentError("y_min must be smaller than y_max"))
+    u_max ≥ u_min || throw(ArgumentError("u_min must be smaller than u_max"))
 
-    @variables x(t)=0 u_r(t)=0 [input=true] u_y(t)=0 [input=true] y(t) [output=true] e(t)=0 ep(t)=0 ed(t)=0 ea(t)=0
-
-
+    @named r = RealInput() # reference
+    @named y = RealInput() # measurement
+    @named u = RealOutput() # control signal
+
+    sts = @variables x(t)=0 e(t)=0 ep(t)=0 ed(t)=0 ea(t)=0
+
     @named D = Derivative(k = Td, T = Td/Nd) # NOTE: consider T = max(Td/Nd, 100eps()), but currently errors since a symbolic variable appears in a boolean expression in `max`.
     if isequal(Ti, false)
-        @named I = Gain(false)
+        @named I = Gain(1)
     else
         @named I = Integrator(k = 1/Ti)
     end
-    @named sat = Saturation(; y_min, y_max)
+    @named sat = Limiter(; y_min=y_min, y_max=y_max)
     derivative_action = Td > 0
-    @parameters k=k Td=Td wp=wp wd=wd Ni=Ni Nd=Nd # TODO: move this line above the subsystem definitions when symbolic default values for parameters works. https://github.com/SciML/ModelingToolkit.jl/issues/1013
+    pars = @parameters k=k Td=Td wp=wp wd=wd Ni=Ni Nd=Nd # TODO: move this line above the subsystem definitions when symbolic default values for parameters works. https://github.com/SciML/ModelingToolkit.jl/issues/1013
     # NOTE: Ti is not included as a parameter since we cannot support setting it to false after this constructor is called. Maybe Integrator can be tested with Ti = false setting k to 0 with IfElse?
 
     eqs = [
-        e ~ u_r - u_y # Control error
-        ep ~ wp*u_r - u_y  # Control error for proportional part with setpoint weight
-        ea ~ sat.y - sat.u # Actuator error due to saturation
-        I.u ~ e + 1/(k*Ni)*ea  # Connect integrator block. The integrator integrates the control error and the anti-wind up tracking. Note the apparent tracking time constant 1/(k*Ni), since k appears after the integration and 1/Ti appears in the integrator block, the final tracking gain will be 1/(Ti*Ni) 
-        sat.u ~ derivative_action ? k*(ep + I.y + D.y) : k*(ep + I.y) # unsaturated output = P + I + D
+        e ~ r.u - y.u # Control error
+        ep ~ wp * r.u - y.u  # Control error for proportional part with setpoint weight
+        ea ~ sat.y.u - sat.u.u # Actuator error due to saturation
+        I.u ~ e + 1 / (k * Ni) * ea  # Connect integrator block. The integrator integrates the control error and the anti-wind up tracking. Note the apparent tracking time constant 1/(k*Ni), since k appears after the integration and 1/Ti appears in the integrator block, the final tracking gain will be 1/(Ti*Ni) 
+        sat.u ~ derivative_action ? k * (ep + I.y + D.y) : k * (ep + I.y) # unsaturated output = P + I + D
         y ~ sat.y
     ]
     systems = [I, sat]
@@ -172,7 +227,7 @@ function PID(; k, Ti=false, Td=false, wp=1, wd=1,
         push!(eqs, D.u ~ ed) # Connect derivative block
         push!(systems, D)
     end
-    ODESystem(eqs, t, name=name, systems=systems)
+    ODESystem(eqs, t, sts, pars, name=name, systems=systems)
 end
 
 """
@@ -185,28 +240,26 @@ y = Cx + Du
 ```
 Transfer functions can also be simulated by converting them to a StateSpace form.
 """
-function StateSpace(A, B, C, D=0; x0=zeros(size(A,1)), name)
-    nx = size(A,1)
-    nu = size(B,2)
-    ny = size(C,1)
-    if nx == 0
-        length(C) == length(B) == 0 || throw(ArgumentError("Dimension mismatch between A,B,C matrices"))
-        return Gain(D; name=name)
-    end
+function StateSpace(;A, B, C, D=nothing, x0=zeros(size(A,1)), name)
+    nx, nu, ny = size(A,1), size(B,2), size(C,1)
+    size(A,2) == nx || error("`A` has to be a square matrix.")
+    size(B,1) == nx || error("`B` has to be of dimension ($nx x $nu).")
+    size(C,2) == nx || error("`C` has to be of dimension ($ny x $nx).")
     if B isa AbstractVector
         B = reshape(B, length(B), 1)
     end
-    if D == 0
+    if isnothing(D)
         D = zeros(ny, nu)
+    else
+        size(D) == (ny,nu) || error("`D` has to be of dimension ($ny x $nu).")
     end
-    @variables x[1:nx](t)=x0 u[1:nu](t)=0 [input=true] y[1:ny](t)=C*x0 [output=true]
-    x = collect(x) # https://github.com/JuliaSymbolics/Symbolics.jl/issues/379
-    u = collect(u)
-    y = collect(y)
-    # @parameters A=A B=B C=C D=D # This is buggy
-    eqs = [
-        Dₜ.(x) .~ A*x .+ B*u
-        y      .~ C*x .+ D*u
+    @named input = RealInput(nin=nu)
+    @named output = RealOutput(nout=ny)
+    @variables x[1:nx](t)=x0
+    # pars = @parameters A=A B=B C=C D=D # This is buggy
+    eqs = [ # FIXME: if array equations work
+        [Differential(t)(x[i]) ~ sum(A[i,k] * x[k] for k in 1:nx) + sum(B[i,j] * input.u[j] for j in 1:nu) for i in 1:nx]..., # cannot use D here
+        [output.u[j] ~ sum(C[j,i] * x[i] for i in 1:nx) + sum(D[j,k] * input.u[k] for k in 1:nu) for j in 1:ny]...,
     ]
-    ODESystem(eqs, t, name=name)
+    compose(ODESystem(eqs, t, vcat(x...), [], name=name), [input, output])
 end