Merge pull request #110 from JuliaControl/mtk_corr

doc: MTK example throw error for non-strictly proper systems

Author: franckgaga

docs/src/manual/mtk.md

@@ -60,39 +60,61 @@ function generate_f_h(model, inputs, 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)
+    nu, nx, ny = length(inputs), length(dvs), length(outputs)
     vx = string.(dvs)
-    par = varmap_to_vars(defaults(io_sys), psym)
-    function f!(ẋ, x, u, _ , _ )
-        f_ip(ẋ, x, u, par, 1)
-        nothing
+    p = varmap_to_vars(defaults(io_sys), psym)
+    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
-    function h!(y, x, _ , _ )
-        y .= h_(x, 1, par, 1)
-        nothing
+    h_ = ModelingToolkit.build_explicit_observed_function(io_sys, outputs; inputs)
+    u_nothing = fill(nothing, nu)
+    function h!(y, x, _ , p)
+        y .= try
+            # MTK.jl supports a `u` argument in `h_` function but not this package. We set
+            # `u` as a vector of nothing and `h_` function will presumably throw an
+            # MethodError it this argument is used inside the function
+            h_(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
-    return f!, h!, nx, vx
+    return f!, h!, p, nu, nx, ny, 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
+f!, h!, p, nu, nx, ny, vx = generate_f_h(mtk_model, inputs, outputs)
+Ts = 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)
+model = setname!(NonLinModel(f!, h!, Ts, nu, nx, ny; p); 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)
+f_plant, h_plant, p = generate_f_h(mtk_model, inputs, outputs)
+plant = setname!(NonLinModel(f_plant, h_plant, Ts, nu, nx, ny; p); u=vu, x=vx, y=vy)
 ```
 
 ## Controller Design

src/model/linmodel.jl

@@ -150,7 +150,8 @@ function LinModel(
     sysu = sminreal(sys[:,i_u]) # remove states associated to measured disturbances d
     sysd = sminreal(sys[:,i_d]) # remove states associated to manipulates inputs u
     if !iszero(sysu.D)
-        error("State matrix D must be 0 for columns associated to manipulated inputs u")
+        error("LinModel only supports strictly proper systems (state matrix D must be 0 "*
+              "for columns associated to manipulated inputs u)")
     end
     if iscontinuous(sys)
         isnothing(Ts) && error("Sample time Ts must be specified if sys is continuous")

src/model/solver.jl

@@ -54,9 +54,9 @@ function get_solver_functions(NT::DataType, solver::RungeKutta, fc!, hc!, Ts, _
         k2   = get_tmp(k2_cache, var)
         k3   = get_tmp(k3_cache, var)
         k4   = get_tmp(k4_cache, var)
+        xterm = xnext
         @. xcur = x
         for i=1:solver.supersample
-            xterm = xnext # TODO: move this out of the loop, just above (to test) ?
             @. xterm = xcur
             fc!(k1, xterm, u, d, p)
             @. xterm = xcur + k1 * Ts_inner/2