add tests

Author: baggepinnen

Project.toml

@@ -20,16 +20,16 @@ DataInterpolations = "3, 4, 5, 6, 7"
 ModelingToolkit = "9.61"
 ModelingToolkitStandardLibrary = "2"
 MonteCarloMeasurements = "1.1"
-OrdinaryDiffEq = "6"
+OrdinaryDiffEqRosenbrock = "1.4.0"
 RobustAndOptimalControl = "0.4.14"
 Symbolics = "5, 6"
 UnPack = "1"
 julia = "1.6"
 
 [extras]
 ControlSystems = "a6e380b2-a6ca-5380-bf3e-84a91bcd477e"
-OrdinaryDiffEq = "1dea7af3-3e70-54e6-95c3-0bf5283fa5ed"
+OrdinaryDiffEqRosenbrock = "43230ef6-c299-4910-a778-202eb28ce4ce"
 Test = "8dfed614-e22c-5e08-85e1-65c5234f0b40"
 
 [targets]
-test = ["Test", "ControlSystems", "OrdinaryDiffEq"]
+test = ["Test", "ControlSystems", "OrdinaryDiffEqRosenbrock"]

src/ode_system.jl

@@ -202,10 +202,9 @@ function RobustAndOptimalControl.named_ss(
     matrices, ssys = ModelingToolkit.linearize(sys, inputs, outputs; kwargs...)
     symstr(x) = Symbol(x isa AnalysisPoint ? x.name : string(x))
     unames = symstr.(inputs)
-    fm(x) = convert(Matrix{Float64}, x)
     if nu > 0 && size(matrices.B, 2) == 2nu
         # This indicates that input derivatives are present
-        duinds = findall(any(!iszero, eachcol(sys.B[:, nu+1:end]))) .+ nu
+        duinds = findall(any(!iszero, eachcol(matrices.B[:, nu+1:end]))) .+ nu
         u2du = (1:nu) .=> duinds # This maps inputs to their derivatives
         lsys = causal_simplification(matrices, u2du)
     else
@@ -232,6 +231,8 @@ function causal_simplification(sys, u2duinds::Vector{Pair{Int, Int}})
     Iu = u_with_du_inds .== (1:nu)'
     E = [I(nx) -B̄; zeros(ndu, nx+ndu)]
 
+    fm(x) = convert(Matrix{Float64}, x)
+
     Ae = cat(sys.A, -I(ndu), dims=(1,2))
     Be = [B; Iu]
     Ce = [fm(sys.C) zeros(ny, ndu)]

test/test_ODESystem.jl

@@ -1,5 +1,5 @@
 using ControlSystemsMTK,
-    ControlSystemsBase, ModelingToolkit, OrdinaryDiffEq, RobustAndOptimalControl
+    ControlSystemsBase, ModelingToolkit, OrdinaryDiffEqRosenbrock, RobustAndOptimalControl
 import ModelingToolkitStandardLibrary.Blocks as Blocks
 conn = ModelingToolkit.connect
 connect = ModelingToolkit.connect
@@ -165,7 +165,7 @@ x0 = Pair[
 p = Pair[]
 
 prob = ODEProblem(simplified_sys, x0, (0.0, 20.0), p, jac = true)
-sol = solve(prob, OrdinaryDiffEq.Rodas5(), saveat = 0:0.01:20)
+sol = solve(prob, Rodas5(), saveat = 0:0.01:20)
 if isinteractive()
     @show sol.retcode
     plot(sol, layout = length(unknowns(simplified_sys)) + 1)
@@ -177,7 +177,7 @@ if isinteractive()
 end
 
 ## Double-mass model in MTK
-using ModelingToolkit, OrdinaryDiffEq, LinearAlgebra
+using ModelingToolkit, OrdinaryDiffEqRosenbrock, LinearAlgebra
 using ModelingToolkitStandardLibrary.Mechanical.Rotational
 using ModelingToolkitStandardLibrary.Blocks: Sine
 using ModelingToolkit: connect
@@ -280,4 +280,70 @@ Sn = get_named_sensitivity(sys_outer, [sys_outer.inner.plant_input, sys_outer.in
 P = named_ss(ssrand(1,1,1), u=:jörgen, y=:solis)
 @named Pode = ODESystem(P)
 ModelingToolkit.isconnector(Pode.jörgen)
-ModelingToolkit.isconnector(Pode.solis)
+ModelingToolkit.isconnector(Pode.solis)
+
+
+
+## Test causal simplification
+using LinearAlgebra
+using ModelingToolkit
+using ModelingToolkitStandardLibrary
+using ModelingToolkitStandardLibrary.Blocks
+using ModelingToolkitStandardLibrary.Mechanical.MultiBody2D
+using ModelingToolkitStandardLibrary.Mechanical.TranslationalPosition
+using Test
+
+using ControlSystemsMTK
+using ControlSystemsMTK.ControlSystemsBase: sminreal, minreal, poles
+connect = ModelingToolkit.connect
+
+@independent_variables t
+D = Differential(t)
+
+@named link1 = Link(; m = 0.2, l = 10, I = 1, g = -9.807)
+@named cart = TranslationalPosition.Mass(; m = 1, s = 0)
+@named fixed = Fixed()
+@named force = Force(use_support = false)
+
+eqs = [connect(link1.TX1, cart.flange)
+       connect(cart.flange, force.flange)
+       connect(link1.TY1, fixed.flange)]
+
+@named model = ODESystem(eqs, t, [], []; systems = [link1, cart, force, fixed])
+lin_outputs = [cart.s, cart.v, link1.A, link1.dA]
+lin_inputs = [force.f.u]
+
+# => nothing to remove extra defaults
+op = Dict(cart.s => 10, cart.v => 0, link1.A => -pi/2, link1.dA => 0, force.f.u => 0, link1.x1 => nothing, link1.y1 => nothing, link1.x2 => nothing, link1.x_cm => nothing)
+G = named_ss(model, lin_inputs, lin_outputs; allow_symbolic = true, op,
+    allow_input_derivatives = true, zero_dummy_der = true)
+G = sminreal(G)
+@info "minreal"
+G = minreal(G)
+@info "poles"
+ps = poles(G)
+
+@test minimum(abs, ps) < 1e-6
+@test minimum(abs, complex(0, 1.3777260367206716) .- ps) < 1e-10
+
+lsys, syss = linearize(model, lin_inputs, lin_outputs, op = op,
+    allow_input_derivatives = true)
+lsyss, sysss = ModelingToolkit.linearize_symbolic(model, lin_inputs, lin_outputs;
+    allow_input_derivatives = true)
+
+dummyder = setdiff(unknowns(sysss), unknowns(model))
+# op2 = merge(ModelingToolkit.guesses(model), op, Dict(x => 0.0 for x in dummyder))
+op2 = merge(ModelingToolkit.defaults(syss), op)
+op2[link1.fy1] = -op2[link1.g] * op2[link1.m]
+op2[cart.f] = 0
+
+@test substitute(lsyss.A, op2) ≈ lsys.A
+# We cannot pivot symbolically, so the part where a linear solve is required
+# is not reliable.
+@test substitute(lsyss.B, op2)[1:6, 1] ≈ lsys.B[1:6, 1]
+@test substitute(lsyss.C, op2) ≈ lsys.C
+@test substitute(lsyss.D, op2) ≈ lsys.D
+
+@test G.nx == 4
+@test G.nu == length(lin_inputs)
+@test G.ny == length(lin_outputs)