Skip to content

Commit 71095f1

Browse files
AayushSabharwalAayushSabharwal
committed
test: add downstream analysis points tests
1 parent 9d88423 commit 71095f1

File tree

2 files changed

+234
-0
lines changed

2 files changed

+234
-0
lines changed

‎test/downstream/analysis_points.jl

+233
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,233 @@
1+
using ModelingToolkit, OrdinaryDiffEq, LinearAlgebra, ControlSystemsBase
2+
using ModelingToolkitStandardLibrary.Mechanical.Rotational
3+
using ModelingToolkitStandardLibrary.Blocks
4+
using ModelingToolkit: connect, AnalysisPoint, t_nounits as t, D_nounits as D
5+
import ControlSystemsBase as CS
6+
7+
@testset "Complicated model" begin
8+
# Parameters
9+
m1 = 1
10+
m2 = 1
11+
k = 1000 # Spring stiffness
12+
c = 10 # Damping coefficient
13+
@named inertia1 = Inertia(; J = m1)
14+
@named inertia2 = Inertia(; J = m2)
15+
@named spring = Spring(; c = k)
16+
@named damper = Damper(; d = c)
17+
@named torque = Torque()
18+
19+
function SystemModel(u = nothing; name = :model)
20+
eqs = [connect(torque.flange, inertia1.flange_a)
21+
connect(inertia1.flange_b, spring.flange_a, damper.flange_a)
22+
connect(inertia2.flange_a, spring.flange_b, damper.flange_b)]
23+
if u !== nothing
24+
push!(eqs, connect(torque.tau, u.output))
25+
return ODESystem(eqs, t;
26+
systems = [
27+
torque,
28+
inertia1,
29+
inertia2,
30+
spring,
31+
damper,
32+
u
33+
],
34+
name)
35+
end
36+
ODESystem(eqs, t; systems = [torque, inertia1, inertia2, spring, damper], name)
37+
end
38+
39+
@named r = Step(start_time = 0)
40+
model = SystemModel()
41+
@named pid = PID(k = 100, Ti = 0.5, Td = 1)
42+
@named filt = SecondOrder(d = 0.9, w = 10)
43+
@named sensor = AngleSensor()
44+
@named er = Add(k2 = -1)
45+
46+
connections = [connect(r.output, :r, filt.input)
47+
connect(filt.output, er.input1)
48+
connect(pid.ctr_output, :u, model.torque.tau)
49+
connect(model.inertia2.flange_b, sensor.flange)
50+
connect(sensor.phi, :y, er.input2)
51+
connect(er.output, :e, pid.err_input)]
52+
53+
closed_loop = ODESystem(connections, t, systems = [model, pid, filt, sensor, r, er],
54+
name = :closed_loop, defaults = [
55+
model.inertia1.phi => 0.0,
56+
model.inertia2.phi => 0.0,
57+
model.inertia1.w => 0.0,
58+
model.inertia2.w => 0.0,
59+
filt.x => 0.0,
60+
filt.xd => 0.0
61+
])
62+
63+
sys = structural_simplify(closed_loop)
64+
prob = ODEProblem(sys, unknowns(sys) .=> 0.0, (0.0, 4.0))
65+
sol = solve(prob, Rodas5P(), reltol = 1e-6, abstol = 1e-9)
66+
67+
matrices, ssys = linearize(closed_loop, AnalysisPoint(:r), AnalysisPoint(:y))
68+
lsys = ss(matrices...) |> sminreal
69+
@test lsys.nx == 8
70+
71+
stepres = ControlSystemsBase.step(c2d(lsys, 0.001), 4)
72+
@test Array(stepres.y[:])≈Array(sol(0:0.001:4, idxs = model.inertia2.phi)) rtol=1e-4
73+
74+
matrices, ssys = get_sensitivity(closed_loop, :y)
75+
So = ss(matrices...)
76+
77+
matrices, ssys = get_sensitivity(closed_loop, :u)
78+
Si = ss(matrices...)
79+
80+
@test tf(So) ≈ tf(Si)
81+
end
82+
83+
@testset "Analysis points with subsystems" begin
84+
@named P = FirstOrder(k = 1, T = 1)
85+
@named C = Gain(; k = 1)
86+
@named add = Blocks.Add(k2 = -1)
87+
88+
eqs = [connect(P.output, :plant_output, add.input2)
89+
connect(add.output, C.input)
90+
connect(C.output, :plant_input, P.input)]
91+
92+
# eqs = [connect(P.output, add.input2)
93+
# connect(add.output, C.input)
94+
# connect(C.output, P.input)]
95+
96+
sys_inner = ODESystem(eqs, t, systems = [P, C, add], name = :inner)
97+
98+
@named r = Constant(k = 1)
99+
@named F = FirstOrder(k = 1, T = 3)
100+
101+
eqs = [connect(r.output, F.input)
102+
connect(F.output, sys_inner.add.input1)]
103+
sys_outer = ODESystem(eqs, t, systems = [F, sys_inner, r], name = :outer)
104+
105+
# test first that the structural_simplify works correctly
106+
ssys = structural_simplify(sys_outer)
107+
prob = ODEProblem(ssys, Pair[], (0, 10))
108+
@test_nowarn solve(prob, Rodas5())
109+
110+
matrices, _ = get_sensitivity(sys_outer, sys_outer.inner.plant_input)
111+
lsys = sminreal(ss(matrices...))
112+
@test lsys.A[] == -2
113+
@test lsys.B[] * lsys.C[] == -1 # either one negative
114+
@test lsys.D[] == 1
115+
116+
matrices_So, _ = get_sensitivity(sys_outer, sys_outer.inner.plant_output)
117+
lsyso = sminreal(ss(matrices_So...))
118+
@test lsys == lsyso || lsys == -1 * lsyso * (-1) # Output and input sensitivities are equal for SISO systems
119+
end
120+
121+
@testset "multilevel system with loop openings" begin
122+
@named P_inner = FirstOrder(k = 1, T = 1)
123+
@named feedback = Feedback()
124+
@named ref = Step()
125+
@named sys_inner = ODESystem(
126+
[connect(P_inner.output, :y, feedback.input2)
127+
connect(feedback.output, :u, P_inner.input)
128+
connect(ref.output, :r, feedback.input1)],
129+
t,
130+
systems = [P_inner, feedback, ref])
131+
132+
P_not_broken, _ = linearize(sys_inner, AnalysisPoint(:u), AnalysisPoint(:y))
133+
@test P_not_broken.A[] == -2
134+
P_broken, _ = linearize(sys_inner, AnalysisPoint(:u), AnalysisPoint(:y),
135+
loop_openings = [AnalysisPoint(:u)])
136+
@test P_broken.A[] == -1
137+
P_broken, _ = linearize(sys_inner, AnalysisPoint(:u), AnalysisPoint(:y),
138+
loop_openings = [AnalysisPoint(:y)])
139+
@test P_broken.A[] == -1
140+
141+
Sinner = sminreal(ss(get_sensitivity(sys_inner, :u)[1]...))
142+
143+
@named sys_inner = ODESystem(
144+
[connect(P_inner.output, :y, feedback.input2)
145+
connect(feedback.output, :u, P_inner.input)],
146+
t,
147+
systems = [P_inner, feedback])
148+
149+
@named P_outer = FirstOrder(k = rand(), T = rand())
150+
151+
@named sys_outer = ODESystem(
152+
[connect(sys_inner.P_inner.output, :y2, P_outer.input)
153+
connect(P_outer.output, :u2, sys_inner.feedback.input1)],
154+
t,
155+
systems = [P_outer, sys_inner])
156+
157+
Souter = sminreal(ss(get_sensitivity(sys_outer, :sys_inner_u)[1]...))
158+
159+
Sinner2 = sminreal(ss(get_sensitivity(
160+
sys_outer, :sys_inner_u, loop_openings = [:y2])[1]...))
161+
162+
@test Sinner.nx == 1
163+
@test Sinner == Sinner2
164+
@test Souter.nx == 2
165+
end
166+
167+
@testset "sensitivities in multivariate signals" begin
168+
A = [-0.994 -0.0794; -0.006242 -0.0134]
169+
B = [-0.181 -0.389; 1.1 1.12]
170+
C = [1.74 0.72; -0.33 0.33]
171+
D = [0.0 0.0; 0.0 0.0]
172+
@named P = Blocks.StateSpace(A, B, C, D)
173+
Pss = CS.ss(A, B, C, D)
174+
175+
A = [-0.097;;]
176+
B = [-0.138 -1.02]
177+
C = [-0.076; 0.09;;]
178+
D = [0.0 0.0; 0.0 0.0]
179+
@named K = Blocks.StateSpace(A, B, C, D)
180+
Kss = CS.ss(A, B, C, D)
181+
182+
eqs = [connect(P.output, :plant_output, K.input)
183+
connect(K.output, :plant_input, P.input)]
184+
sys = ODESystem(eqs, t, systems = [P, K], name = :hej)
185+
186+
matrices, _ = Blocks.get_sensitivity(sys, :plant_input)
187+
S = CS.feedback(I(2), Kss * Pss, pos_feedback = true)
188+
189+
@test CS.tf(CS.ss(matrices...)) ≈ CS.tf(S)
190+
191+
matrices, _ = Blocks.get_comp_sensitivity(sys, :plant_input)
192+
T = -CS.feedback(Kss * Pss, I(2), pos_feedback = true)
193+
194+
# bodeplot([ss(matrices...), T])
195+
@test CS.tf(CS.ss(matrices...)) ≈ CS.tf(T)
196+
197+
matrices, _ = Blocks.get_looptransfer(
198+
sys, :plant_input)
199+
L = Kss * Pss
200+
@test CS.tf(CS.ss(matrices...)) ≈ CS.tf(L)
201+
202+
matrices, _ = linearize(sys, :plant_input, :plant_output)
203+
G = CS.feedback(Pss, Kss, pos_feedback = true)
204+
@test CS.tf(CS.ss(matrices...)) ≈ CS.tf(G)
205+
end
206+
207+
@testset "multiple analysis points" begin
208+
@named P = FirstOrder(k = 1, T = 1)
209+
@named C = Gain(; k = 1)
210+
@named add = Blocks.Add(k2 = -1)
211+
212+
eqs = [connect(P.output, :plant_output, add.input2)
213+
connect(add.output, C.input)
214+
connect(C.output, :plant_input, P.input)]
215+
216+
sys_inner = ODESystem(eqs, t, systems = [P, C, add], name = :inner)
217+
218+
@named r = Constant(k = 1)
219+
@named F = FirstOrder(k = 1, T = 3)
220+
221+
eqs = [connect(r.output, F.input)
222+
connect(F.output, sys_inner.add.input1)]
223+
sys_outer = ODESystem(eqs, t, systems = [F, sys_inner, r], name = :outer)
224+
225+
matrices, _ = get_sensitivity(sys_outer, [:, :inner_plant_output])
226+
227+
Ps = tf(1, [1, 1]) |> ss
228+
Cs = tf(1) |> ss
229+
230+
G = CS.ss(matrices...) |> sminreal
231+
Si = CS.feedback(1, Cs * Ps)
232+
@test tf(G[1, 1]) ≈ tf(Si)
233+
end

‎test/runtests.jl

+1
Original file line numberDiff line numberDiff line change
@@ -110,6 +110,7 @@ end
110110
@safetestset "Linearization Tests" include("downstream/linearize.jl")
111111
@safetestset "Linearization Dummy Derivative Tests" include("downstream/linearization_dd.jl")
112112
@safetestset "Inverse Models Test" include("downstream/inversemodel.jl")
113+
@safetestset "Analysis Points Test" include("downstream/analysis_points.jl")
113114
end
114115

115116
if GROUP == "All" || GROUP == "Extensions"

0 commit comments

Comments
 (0)