factor out causal simplification (#71)

* factor out causal simplification

* add tests

* import fix

* more import

Author: baggepinnen

Project.toml

@@ -20,16 +20,16 @@ DataInterpolations = "3, 4, 5, 6, 7"
 ModelingToolkit = "9.61"
 ModelingToolkitStandardLibrary = "2"
 MonteCarloMeasurements = "1.1"
-OrdinaryDiffEq = "6"
 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"
+OrdinaryDiffEqNonlinearSolve = "127b3ac7-2247-4354-8eb6-78cf4e7c58e8"
+OrdinaryDiffEqRosenbrock = "43230ef6-c299-4910-a778-202eb28ce4ce"
 Test = "8dfed614-e22c-5e08-85e1-65c5234f0b40"
 
 [targets]
-test = ["Test", "ControlSystems", "OrdinaryDiffEq"]
+test = ["Test", "ControlSystems", "OrdinaryDiffEqRosenbrock", "OrdinaryDiffEqNonlinearSolve"]

src/ode_system.jl

@@ -199,29 +199,14 @@ function RobustAndOptimalControl.named_ss(
             out
         end
     end
-    ny = length(outputs)
     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
-        nx = size(matrices.A, 1)
-         # This indicates that input derivatives are present
-        duinds = findall(any(!iszero, eachcol(matrices.B[:, nu+1:end])))
-        B̄ = matrices.B[:, duinds .+ nu]
-        ndu = length(duinds)
-        B = matrices.B[:, 1:nu]
-        Iu = duinds .== (1:nu)'
-        E = [I(nx) -B̄; zeros(ndu, nx+ndu)]
-
-        Ae = cat(matrices.A, -I(ndu), dims=(1,2))
-        Be = [B; Iu]
-        Ce = [fm(matrices.C) zeros(ny, ndu)]
-        De = fm(matrices.D[:, 1:nu])
-        dsys = dss(Ae, E, Be, Ce, De)
-        lsys = ss(RobustAndOptimalControl.DescriptorSystems.dss2ss(dsys)[1])
-        # unames = [unames; Symbol.("der_" .* string.(unames))]
-        # sys = ss(matrices...)
+        # This indicates that input derivatives are present
+        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
         lsys = ss(matrices...)
     end
@@ -234,6 +219,28 @@ function RobustAndOptimalControl.named_ss(
     )
 end
 
+function causal_simplification(sys, u2duinds::Vector{Pair{Int, Int}})
+    nx = size(sys.A, 1)
+    ny = size(sys.C, 1)
+    ndu = length(u2duinds)
+    nu = size(sys.B, 2) - ndu
+    u_with_du_inds = first.(u2duinds)
+    duinds = last.(u2duinds)
+    B̄ = sys.B[:, duinds]
+    B = sys.B[:, 1:nu]
+    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)]
+    De = fm(sys.D[:, 1:nu])
+    dsys = dss(Ae, E, Be, Ce, De)
+    ss(RobustAndOptimalControl.DescriptorSystems.dss2ss(dsys)[1])
+end
+
 for f in [:sensitivity, :comp_sensitivity, :looptransfer]
     fnn = Symbol("get_named_$f")
     fn = Symbol("get_$f")

test/test_ODESystem.jl

@@ -1,6 +1,7 @@
 using ControlSystemsMTK,
-    ControlSystemsBase, ModelingToolkit, OrdinaryDiffEq, RobustAndOptimalControl
+    ControlSystemsBase, ModelingToolkit, RobustAndOptimalControl
 import ModelingToolkitStandardLibrary.Blocks as Blocks
+using OrdinaryDiffEqNonlinearSolve, OrdinaryDiffEqRosenbrock
 conn = ModelingToolkit.connect
 connect = ModelingToolkit.connect
 ## Test SISO (single input, single output) system
@@ -165,7 +166,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 +178,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 +281,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 = TranslationalPosition.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)

test/test_batchlin.jl

@@ -1,4 +1,5 @@
 using ControlSystemsMTK, ModelingToolkit, RobustAndOptimalControl, MonteCarloMeasurements
+using OrdinaryDiffEqNonlinearSolve, OrdinaryDiffEqRosenbrock
 using ModelingToolkit: getdefault
 unsafe_comparisons(true)
 
@@ -44,7 +45,7 @@ using DataInterpolations
 # code = SymbolicControlSystems.print_c_array(stdout, Ps, xs, "gain_scheduled_controller", struct_name="hej", struct_type="kaj")
 
 ## Simulate
-using OrdinaryDiffEq, ControlSystemsBase
+using OrdinaryDiffEqRosenbrock, ControlSystemsBase
 import ModelingToolkitStandardLibrary.Blocks
 
 @named fb = Blocks.Add(k2=-1)