From 9af0038a28c711c447e6a58f1a9bfc048d147813 Mon Sep 17 00:00:00 2001 From: Aayush Sabharwal Date: Mon, 30 Dec 2024 00:43:47 +0530 Subject: [PATCH] test: add downstream analysis points tests --- test/downstream/analysis_points.jl | 233 +++++++++++++++++++++++++++++ test/runtests.jl | 1 + 2 files changed, 234 insertions(+) create mode 100644 test/downstream/analysis_points.jl diff --git a/test/downstream/analysis_points.jl b/test/downstream/analysis_points.jl new file mode 100644 index 0000000000..5dc2b8957c --- /dev/null +++ b/test/downstream/analysis_points.jl @@ -0,0 +1,233 @@ +using ModelingToolkit, OrdinaryDiffEq, LinearAlgebra, ControlSystemsBase +using ModelingToolkitStandardLibrary.Mechanical.Rotational +using ModelingToolkitStandardLibrary.Blocks +using ModelingToolkit: connect, AnalysisPoint, t_nounits as t, D_nounits as D +import ControlSystemsBase as CS + +@testset "Complicated model" begin + # Parameters + m1 = 1 + m2 = 1 + k = 1000 # Spring stiffness + c = 10 # Damping coefficient + @named inertia1 = Inertia(; J = m1) + @named inertia2 = Inertia(; J = m2) + @named spring = Spring(; c = k) + @named damper = Damper(; d = c) + @named torque = Torque() + + function SystemModel(u = nothing; name = :model) + eqs = [connect(torque.flange, inertia1.flange_a) + connect(inertia1.flange_b, spring.flange_a, damper.flange_a) + connect(inertia2.flange_a, spring.flange_b, damper.flange_b)] + if u !== nothing + push!(eqs, connect(torque.tau, u.output)) + return ODESystem(eqs, t; + systems = [ + torque, + inertia1, + inertia2, + spring, + damper, + u + ], + name) + end + ODESystem(eqs, t; systems = [torque, inertia1, inertia2, spring, damper], name) + end + + @named r = Step(start_time = 0) + model = SystemModel() + @named pid = PID(k = 100, Ti = 0.5, Td = 1) + @named filt = SecondOrder(d = 0.9, w = 10) + @named sensor = AngleSensor() + @named er = Add(k2 = -1) + + connections = [connect(r.output, :r, filt.input) + connect(filt.output, er.input1) + connect(pid.ctr_output, :u, model.torque.tau) + connect(model.inertia2.flange_b, sensor.flange) + connect(sensor.phi, :y, er.input2) + connect(er.output, :e, pid.err_input)] + + closed_loop = ODESystem(connections, t, systems = [model, pid, filt, sensor, r, er], + name = :closed_loop, defaults = [ + model.inertia1.phi => 0.0, + model.inertia2.phi => 0.0, + model.inertia1.w => 0.0, + model.inertia2.w => 0.0, + filt.x => 0.0, + filt.xd => 0.0 + ]) + + sys = structural_simplify(closed_loop) + prob = ODEProblem(sys, unknowns(sys) .=> 0.0, (0.0, 4.0)) + sol = solve(prob, Rodas5P(), reltol = 1e-6, abstol = 1e-9) + + matrices, ssys = linearize(closed_loop, AnalysisPoint(:r), AnalysisPoint(:y)) + lsys = ss(matrices...) |> sminreal + @test lsys.nx == 8 + + stepres = ControlSystemsBase.step(c2d(lsys, 0.001), 4) + @test Array(stepres.y[:])≈Array(sol(0:0.001:4, idxs = model.inertia2.phi)) rtol=1e-4 + + matrices, ssys = get_sensitivity(closed_loop, :y) + So = ss(matrices...) + + matrices, ssys = get_sensitivity(closed_loop, :u) + Si = ss(matrices...) + + @test tf(So) ≈ tf(Si) +end + +@testset "Analysis points with subsystems" begin + @named P = FirstOrder(k = 1, T = 1) + @named C = Gain(; k = 1) + @named add = Blocks.Add(k2 = -1) + + eqs = [connect(P.output, :plant_output, add.input2) + connect(add.output, C.input) + connect(C.output, :plant_input, P.input)] + + # eqs = [connect(P.output, add.input2) + # connect(add.output, C.input) + # connect(C.output, P.input)] + + sys_inner = ODESystem(eqs, t, systems = [P, C, add], name = :inner) + + @named r = Constant(k = 1) + @named F = FirstOrder(k = 1, T = 3) + + eqs = [connect(r.output, F.input) + connect(F.output, sys_inner.add.input1)] + sys_outer = ODESystem(eqs, t, systems = [F, sys_inner, r], name = :outer) + + # test first that the structural_simplify works correctly + ssys = structural_simplify(sys_outer) + prob = ODEProblem(ssys, Pair[], (0, 10)) + @test_nowarn solve(prob, Rodas5()) + + matrices, _ = get_sensitivity(sys_outer, sys_outer.inner.plant_input) + lsys = sminreal(ss(matrices...)) + @test lsys.A[] == -2 + @test lsys.B[] * lsys.C[] == -1 # either one negative + @test lsys.D[] == 1 + + matrices_So, _ = get_sensitivity(sys_outer, sys_outer.inner.plant_output) + lsyso = sminreal(ss(matrices_So...)) + @test lsys == lsyso || lsys == -1 * lsyso * (-1) # Output and input sensitivities are equal for SISO systems +end + +@testset "multilevel system with loop openings" begin + @named P_inner = FirstOrder(k = 1, T = 1) + @named feedback = Feedback() + @named ref = Step() + @named sys_inner = ODESystem( + [connect(P_inner.output, :y, feedback.input2) + connect(feedback.output, :u, P_inner.input) + connect(ref.output, :r, feedback.input1)], + t, + systems = [P_inner, feedback, ref]) + + P_not_broken, _ = linearize(sys_inner, AnalysisPoint(:u), AnalysisPoint(:y)) + @test P_not_broken.A[] == -2 + P_broken, _ = linearize(sys_inner, AnalysisPoint(:u), AnalysisPoint(:y), + loop_openings = [AnalysisPoint(:u)]) + @test P_broken.A[] == -1 + P_broken, _ = linearize(sys_inner, AnalysisPoint(:u), AnalysisPoint(:y), + loop_openings = [AnalysisPoint(:y)]) + @test P_broken.A[] == -1 + + Sinner = sminreal(ss(get_sensitivity(sys_inner, :u)[1]...)) + + @named sys_inner = ODESystem( + [connect(P_inner.output, :y, feedback.input2) + connect(feedback.output, :u, P_inner.input)], + t, + systems = [P_inner, feedback]) + + @named P_outer = FirstOrder(k = rand(), T = rand()) + + @named sys_outer = ODESystem( + [connect(sys_inner.P_inner.output, :y2, P_outer.input) + connect(P_outer.output, :u2, sys_inner.feedback.input1)], + t, + systems = [P_outer, sys_inner]) + + Souter = sminreal(ss(get_sensitivity(sys_outer, :sys_inner_u)[1]...)) + + Sinner2 = sminreal(ss(get_sensitivity( + sys_outer, :sys_inner_u, loop_openings = [:y2])[1]...)) + + @test Sinner.nx == 1 + @test Sinner == Sinner2 + @test Souter.nx == 2 +end + +@testset "sensitivities in multivariate signals" begin + A = [-0.994 -0.0794; -0.006242 -0.0134] + B = [-0.181 -0.389; 1.1 1.12] + C = [1.74 0.72; -0.33 0.33] + D = [0.0 0.0; 0.0 0.0] + @named P = Blocks.StateSpace(A, B, C, D) + Pss = CS.ss(A, B, C, D) + + A = [-0.097;;] + B = [-0.138 -1.02] + C = [-0.076; 0.09;;] + D = [0.0 0.0; 0.0 0.0] + @named K = Blocks.StateSpace(A, B, C, D) + Kss = CS.ss(A, B, C, D) + + eqs = [connect(P.output, :plant_output, K.input) + connect(K.output, :plant_input, P.input)] + sys = ODESystem(eqs, t, systems = [P, K], name = :hej) + + matrices, _ = Blocks.get_sensitivity(sys, :plant_input) + S = CS.feedback(I(2), Kss * Pss, pos_feedback = true) + + @test CS.tf(CS.ss(matrices...)) ≈ CS.tf(S) + + matrices, _ = Blocks.get_comp_sensitivity(sys, :plant_input) + T = -CS.feedback(Kss * Pss, I(2), pos_feedback = true) + + # bodeplot([ss(matrices...), T]) + @test CS.tf(CS.ss(matrices...)) ≈ CS.tf(T) + + matrices, _ = Blocks.get_looptransfer( + sys, :plant_input) + L = Kss * Pss + @test CS.tf(CS.ss(matrices...)) ≈ CS.tf(L) + + matrices, _ = linearize(sys, :plant_input, :plant_output) + G = CS.feedback(Pss, Kss, pos_feedback = true) + @test CS.tf(CS.ss(matrices...)) ≈ CS.tf(G) +end + +@testset "multiple analysis points" begin + @named P = FirstOrder(k = 1, T = 1) + @named C = Gain(; k = 1) + @named add = Blocks.Add(k2 = -1) + + eqs = [connect(P.output, :plant_output, add.input2) + connect(add.output, C.input) + connect(C.output, :plant_input, P.input)] + + sys_inner = ODESystem(eqs, t, systems = [P, C, add], name = :inner) + + @named r = Constant(k = 1) + @named F = FirstOrder(k = 1, T = 3) + + eqs = [connect(r.output, F.input) + connect(F.output, sys_inner.add.input1)] + sys_outer = ODESystem(eqs, t, systems = [F, sys_inner, r], name = :outer) + + matrices, _ = get_sensitivity(sys_outer, [:, :inner_plant_output]) + + Ps = tf(1, [1, 1]) |> ss + Cs = tf(1) |> ss + + G = CS.ss(matrices...) |> sminreal + Si = CS.feedback(1, Cs * Ps) + @test tf(G[1, 1]) ≈ tf(Si) +end diff --git a/test/runtests.jl b/test/runtests.jl index af83ffccd8..d38aa86bb2 100644 --- a/test/runtests.jl +++ b/test/runtests.jl @@ -111,6 +111,7 @@ end @safetestset "Linearization Tests" include("downstream/linearize.jl") @safetestset "Linearization Dummy Derivative Tests" include("downstream/linearization_dd.jl") @safetestset "Inverse Models Test" include("downstream/inversemodel.jl") + @safetestset "Analysis Points Test" include("downstream/analysis_points.jl") end if GROUP == "All" || GROUP == "Extensions"