Skip to content

Commit

Permalink
fix: fix unknowns not present in initialization system in linearization
Browse files Browse the repository at this point in the history
  • Loading branch information
AayushSabharwal committed Jun 17, 2024
1 parent 22fb377 commit f97dae6
Show file tree
Hide file tree
Showing 2 changed files with 72 additions and 0 deletions.
9 changes: 9 additions & 0 deletions src/systems/abstractsystem.jl
Original file line number Diff line number Diff line change
Expand Up @@ -1888,6 +1888,15 @@ function linearization_function(sys::AbstractSystem, inputs,
generate_initializesystem(
sys, guesses = guesses(sys), algebraic_only = true),
fully_determined = false)
# HACK: some unknowns may not be involved in any initialization equations, and are
# thus removed from the system during `structural_simplify`.
# This causes `getu(initsys, unknowns(sys))` to fail, so we add them back as parameters
# for now.
missing_unknowns = setdiff(unknowns(sys), unknowns(initsys))
new_parameters = [parameters(initsys); missing_unknowns]
@set! initsys.ps = new_parameters
initsys = complete(initsys)

if p isa SciMLBase.NullParameters
p = Dict()
else
Expand Down
63 changes: 63 additions & 0 deletions test/downstream/linearize.jl
Original file line number Diff line number Diff line change
Expand Up @@ -195,3 +195,66 @@ lsys, ssys = linearize(sat, [u], [y]; op = Dict(u => 2))
@test isempty(lsys.B)
@test isempty(lsys.C)
@test lsys.D[] == 0

# Test case when unknowns in system do not have equations in initialization system
using ModelingToolkit, OrdinaryDiffEq, LinearAlgebra
using ModelingToolkitStandardLibrary.Mechanical.Rotational
using ModelingToolkitStandardLibrary.Blocks: Add, Sine, PID, SecondOrder, Step, RealOutput
using ModelingToolkit: connect

# 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
])

@test_nowarn linearize(closed_loop, :r, :y)

0 comments on commit f97dae6

Please sign in to comment.