diff --git a/docs/Project.toml b/docs/Project.toml index c19fd77..48e803d 100644 --- a/docs/Project.toml +++ b/docs/Project.toml @@ -6,7 +6,8 @@ Documenter = "e30172f5-a6a5-5a46-863b-614d45cd2de4" ModelingToolkit = "961ee093-0014-501f-94e3-6117800e7a78" ModelingToolkitStandardLibrary = "16a59e39-deab-5bd0-87e4-056b12336739" MonteCarloMeasurements = "0987c9cc-fe09-11e8-30f0-b96dd679fdca" -OrdinaryDiffEq = "1dea7af3-3e70-54e6-95c3-0bf5283fa5ed" +OrdinaryDiffEqNonlinearSolve = "127b3ac7-2247-4354-8eb6-78cf4e7c58e8" +OrdinaryDiffEqRosenbrock = "43230ef6-c299-4910-a778-202eb28ce4ce" Plots = "91a5bcdd-55d7-5caf-9e0b-520d859cae80" RobustAndOptimalControl = "21fd56a4-db03-40ee-82ee-a87907bee541" SymbolicControlSystems = "886cb795-8fd3-4b11-92f6-8071e46f71c5" diff --git a/docs/src/batch_linearization.md b/docs/src/batch_linearization.md index 57bcb5e..30feb3c 100644 --- a/docs/src/batch_linearization.md +++ b/docs/src/batch_linearization.md @@ -8,7 +8,6 @@ This example will demonstrate how to linearize a nonlinear ModelingToolkit model The model will be a simple Duffing oscillator: ```@example BATCHLIN using ControlSystemsMTK, ModelingToolkit, MonteCarloMeasurements, ModelingToolkitStandardLibrary.Blocks -using ModelingToolkit: getdefault unsafe_comparisons(true) # Create a model @@ -26,7 +25,7 @@ eqs = [D(x) ~ v y.u ~ x] -@named duffing = ODESystem(eqs, t, systems=[y, u], defaults=[u.u => 0]) +@named duffing = ODESystem(eqs, t, systems=[y, u]) ``` ## Batch linearization @@ -34,7 +33,7 @@ To perform batch linearization, we create a vector of operating points, and then ```@example BATCHLIN N = 16 # Number of samples xs = range(getbounds(x)[1], getbounds(x)[2], length=N) -ops = Dict.(x .=> xs) +ops = Dict.(u.u => 0, x .=> xs) ``` Just like [`ModelingToolkit.linearize`](@ref), [`batch_ss`](@ref) takes the set of inputs and the set of outputs to linearize between. @@ -96,7 +95,7 @@ If you plot the Nyquist curve using the `plotly()` backend rather than the defau Above, we tuned one controller for each operating point, wouldn't it be nice if we had some features to simulate a [gain-scheduled controller](https://en.wikipedia.org/wiki/Gain_scheduling) that interpolates between the different controllers depending on the operating pont? [`GainScheduledStateSpace`](@ref) is such a thing, we show how to use it below. For fun, we simulate some reference step responses for each individual controller in the array `Cs` and end with simulating the gain-scheduled controller. ```@example BATCHLIN -using OrdinaryDiffEq +using OrdinaryDiffEqNonlinearSolve, OrdinaryDiffEqRosenbrock using DataInterpolations # Required to interpolate between the controllers @named fb = Blocks.Add(k2=-1) @named ref = Blocks.Square(frequency=1/6, amplitude=0.5, offset=0.5, start_time=1) @@ -134,7 +133,7 @@ eqs = [ ] @named closed_loop = ODESystem(eqs, t, systems=[duffing, Cgs, fb, ref, F]) prob = ODEProblem(structural_simplify(closed_loop), [F.xd => 0], (0.0, 8.0)) -sol = solve(prob, Rodas5P(), abstol=1e-8, reltol=1e-8, initializealg=NoInit()) +sol = solve(prob, Rodas5P(), abstol=1e-8, reltol=1e-8, initializealg=SciMLBase.NoInit()) plot!(sol, idxs=[duffing.y.u, duffing.u.u], l=(2, :red), lab="Gain scheduled") plot!(sol, idxs=F.output.u, l=(1, :black, :dash, 0.5), lab="Ref") ``` @@ -155,7 +154,7 @@ The generated code starts by defining the interpolation vector `xs`, this variab We can linearize around a trajectory obtained from `solve` using the function [`trajectory_ss`](@ref). We provide it with a vector of time points along the trajectory at which to linearize, and in this case we specify the inputs and outputs to linearize between as analysis points `r` and `y`. ```@example BATCHLIN timepoints = 0:0.01:8 -Ps2, ssys = trajectory_ss(closed_loop, closed_loop.r, closed_loop.y, sol; t=timepoints) +Ps2, ssys = trajectory_ss(closed_loop, closed_loop.r, closed_loop.y, sol; t=timepoints, initialize=true, verbose=true) bodeplot(Ps2, w, legend=false) ``` diff --git a/docs/src/index.md b/docs/src/index.md index 3e67412..3dfd0ad 100644 --- a/docs/src/index.md +++ b/docs/src/index.md @@ -139,7 +139,7 @@ ModelingToolkit has facilities for symbolic linearization that can be used on su We start by building a system mode, we'll use a model of two masses connected by a flexible transmission ```@example LINEAIZE_SYMBOLIC using ControlSystemsMTK, ControlSystemsBase -using ModelingToolkit, OrdinaryDiffEq, LinearAlgebra +using ModelingToolkit, LinearAlgebra using ModelingToolkitStandardLibrary.Mechanical.Rotational using ModelingToolkitStandardLibrary.Blocks: Sine using ModelingToolkit: connect @@ -178,7 +178,8 @@ model = SystemModel() |> complete ### Numeric linearization We can linearize this model numerically using `named_ss`, this produces a `NamedStateSpace{Continuous, Float64}` ```@example LINEAIZE_SYMBOLIC -lsys = named_ss(model, [model.torque.tau.u], [model.inertia1.phi, model.inertia2.phi], op = Dict(model.torque.tau.u => 0)) +op = Dict(model.inertia1.flange_b.phi => 0.0, model.torque.tau.u => 0) +lsys = named_ss(model, [model.torque.tau.u], [model.inertia1.phi, model.inertia2.phi]; op) ``` ### Symbolic linearization If we instead call `linearize_symbolic` and pass the jacobians into `ss`, we get a `StateSpace{Continuous, Num}` diff --git a/src/ode_system.jl b/src/ode_system.jl index 26ff940..346b184 100644 --- a/src/ode_system.jl +++ b/src/ode_system.jl @@ -154,12 +154,14 @@ end """ - RobustAndOptimalControl.named_ss(sys::ModelingToolkit.AbstractTimeDependentSystem, inputs, outputs; kwargs...) + RobustAndOptimalControl.named_ss(sys::ModelingToolkit.AbstractTimeDependentSystem, inputs, outputs; descriptor=true, kwargs...) Convert an `ODESystem` to a `NamedStateSpace` using linearization. `inputs, outputs` are vectors of variables determining the inputs and outputs respectively. See docstring of `ModelingToolkit.linearize` for more info on `kwargs`. -This method automatically converts systems that MTK has failed to produce a proper form for into a proper linear statespace system. Learn more about how that is done here: +If `descriptor = true` (default), this method automatically converts systems that MTK has failed to produce a proper form for into a proper linear statespace system using the method described here: https://juliacontrol.github.io/ControlSystemsMTK.jl/dev/#Internals:-Transformation-of-non-proper-models-to-proper-statespace-form +If `descriptor = false`, the system is instead converted to a statespace realization using `sys[:,uinds] + sys[:,duinds]*tf('s')`, which tends to result in a larger realization on which the user may want to call `minreal(sys, tol)` with a carefully selected tolerance. + See also [`ModelingToolkit.linearize`](@ref) which is the lower-level function called internally. The functions [`get_named_sensitivity`](@ref), [`get_named_comp_sensitivity`](@ref), [`get_named_looptransfer`](@ref) similarily provide convenient ways to compute sensitivity functions while retaining signal names in the same way as `named_ss`. The corresponding lower-level functions `get_sensitivity`, `get_comp_sensitivity` and `get_looptransfer` are available in ModelingToolkitStandardLibrary.Blocks and are documented in [MTKstdlib: Linear analysis](https://docs.sciml.ai/ModelingToolkitStandardLibrary/stable/API/linear_analysis/). """ @@ -167,6 +169,7 @@ function RobustAndOptimalControl.named_ss( sys::ModelingToolkit.AbstractTimeDependentSystem, inputs, outputs; + descriptor = true, kwargs..., ) @@ -206,7 +209,7 @@ function RobustAndOptimalControl.named_ss( # 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) + lsys = causal_simplification(matrices, u2du; descriptor) else lsys = ss(matrices...) end @@ -219,26 +222,33 @@ function RobustAndOptimalControl.named_ss( ) end -function causal_simplification(sys, u2duinds::Vector{Pair{Int, Int}}) +function causal_simplification(sys, u2duinds::Vector{Pair{Int, Int}}; descriptor=true) + fm(x) = convert(Matrix{Float64}, x) 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) + B̄ = sys.B[:, duinds] + D = sys.D[:, 1:nu] + D̄ = sys.D[:, duinds] + iszero(fm(D̄)) || error("Nonzero feedthrough matrix from input derivative not supported") + if descriptor + Iu = u_with_du_inds .== (1:nu)' + E = [I(nx) -B̄; zeros(ndu, nx+ndu)] - 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]) + Ae = cat(sys.A, -I(ndu), dims=(1,2)) + # Ae[1:nx, nx+1:end] .= B + Be = [B; Iu] + Ce = [fm(sys.C) zeros(ny, ndu)] + De = fm(D) + dsys = dss(Ae, E, Be, Ce, De) + return ss(RobustAndOptimalControl.DescriptorSystems.dss2ss(dsys)[1]) + else + ss(sys.A, B, sys.C, D) + ss(sys.A, B̄, sys.C, D̄)*tf('s') + end end for f in [:sensitivity, :comp_sensitivity, :looptransfer] @@ -513,7 +523,11 @@ Linearize `sys` around the trajectory `sol` at times `t`. Returns a vector of `S function trajectory_ss(sys, inputs, outputs, sol; t = _max_100(sol.t), allow_input_derivatives = false, fuzzer = nothing, verbose = true, kwargs...) maximum(t) > maximum(sol.t) && @warn("The maximum time in `t`: $(maximum(t)), is larger than the maximum time in `sol.t`: $(maximum(sol.t)).") minimum(t) < minimum(sol.t) && @warn("The minimum time in `t`: $(minimum(t)), is smaller than the minimum time in `sol.t`: $(minimum(sol.t)).") - lin_fun, ssys = linearization_function(sys, inputs, outputs; kwargs...) + + # NOTE: we call linearization_funciton twice :( The first call is to get x=unknowns(ssys), the second call provides the operating points. + # lin_fun, ssys = linearization_function(sys, inputs, outputs; warn_initialize_determined = false, kwargs...) + lin_fun, ssys = linearization_function(sys, inputs, outputs; warn_initialize_determined = false, kwargs...) + x = unknowns(ssys) defs = ModelingToolkit.defaults(sys) ops = map(t) do ti @@ -526,8 +540,10 @@ function trajectory_ss(sys, inputs, outputs, sol; t = _max_100(sol.t), allow_inp ops = reduce(vcat, opsv) t = repeat(t, inner = length(ops) ÷ length(t)) end + # lin_fun, ssys = linearization_function(sys, inputs, outputs; op=ops[1], initialize, kwargs...) lins = map(zip(ops, t)) do (op, t) linearize(ssys, lin_fun; op, t, allow_input_derivatives) + # linearize(sys, inputs, outputs; op, t, allow_input_derivatives, initialize=false)[1] end (; linsystems = [ss(l...) for l in lins], ssys, ops) end @@ -662,7 +678,7 @@ function GainScheduledStateSpace(systems, vt; interpolator, x = zeros(systems[1] @variables x(t)[1:nx]=x [ description = "State variables of gain-scheduled statespace system $name", ] - @variables v(t) = 0 [ + @variables v(t) [ description = "Scheduling variable of gain-scheduled statespace system $name", ] diff --git a/test/test_ODESystem.jl b/test/test_ODESystem.jl index 955b48a..3e426f0 100644 --- a/test/test_ODESystem.jl +++ b/test/test_ODESystem.jl @@ -1,7 +1,8 @@ using ControlSystemsMTK, - ControlSystemsBase, ModelingToolkit, RobustAndOptimalControl + ControlSystemsBase, ModelingToolkit, RobustAndOptimalControl, Test import ModelingToolkitStandardLibrary.Blocks as Blocks using OrdinaryDiffEqNonlinearSolve, OrdinaryDiffEqRosenbrock +using LinearAlgebra conn = ModelingToolkit.connect connect = ModelingToolkit.connect ## Test SISO (single input, single output) system @@ -40,17 +41,18 @@ sol2 = solve(prob, Rodas5()) isinteractive() && plot(sol2) Pc = complete(P) -Q = ControlSystemsMTK.build_quadratic_cost_matrix(Pc, [Pc.input.u], [Pc.x[1] => 2.0]) +op = Dict(Pc.input.u => 0.0) +Q = ControlSystemsMTK.build_quadratic_cost_matrix(Pc, [Pc.input.u], [Pc.x[1] => 2.0]; op) @test Q[] ≈ 2.0 -Q = ControlSystemsMTK.build_quadratic_cost_matrix(Pc, [Pc.input.u], [Pc.output.u => 2.0]) +Q = ControlSystemsMTK.build_quadratic_cost_matrix(Pc, [Pc.input.u], [Pc.output.u => 2.0]; op) @test Q[] ≈ 2.0 #Mix states and outputs -Q = ControlSystemsMTK.build_quadratic_cost_matrix(Pc, [Pc.input.u], [Pc.x[1] => 2.0, Pc.output.u => 3]) +Q = ControlSystemsMTK.build_quadratic_cost_matrix(Pc, [Pc.input.u], [Pc.x[1] => 2.0, Pc.output.u => 3]; op) @test Q[] ≈ 2.0 + 3 -matrices, ssys = linearize(Pc, [Pc.input.u], [Pc.output.u]) +matrices, ssys = linearize(Pc, [Pc.input.u], [Pc.output.u]; op) Q = ControlSystemsMTK.build_quadratic_cost_matrix(matrices, ssys, [Pc.x[1] => 2.0]) @test Q[] ≈ 2.0 @@ -58,7 +60,7 @@ Q = ControlSystemsMTK.build_quadratic_cost_matrix(matrices, ssys, [Pc.x[1] => 2. Q = ControlSystemsMTK.build_quadratic_cost_matrix(matrices, ssys, [Pc.output.u => 2.0]) @test Q[] ≈ 2.0 -P1 = ss(Pc, [Pc.input.u], [Pc.output.u]) +P1 = ss(Pc, [Pc.input.u], [Pc.output.u]; op) @test P1 == P0 @@ -66,7 +68,7 @@ P1 = ss(Pc, [Pc.input.u], [Pc.output.u]) # === Go the other way, ODESystem -> StateSpace ================================ x = unknowns(P) # I haven't figured out a good way to access states, so this is a bit manual and ugly @unpack input, output = P -P02_named = named_ss(P, [input.u], [output.u]) +P02_named = named_ss(P, [input.u], [output.u]; op) @test P02_named.x == [Symbol("(x(t))[1]")] @test P02_named.u == [Symbol("input₊u(t)")] @test P02_named.y == [Symbol("output₊u(t)")] @@ -76,7 +78,7 @@ P02 = ss(P02_named) # same for controller x = unknowns(C) -@nonamespace C02 = named_ss(C, [C.input], [C.output]) +@nonamespace C02 = named_ss(C, [C.input], [C.output]; op) @test ss(C02) == C0 @@ -216,7 +218,8 @@ end model = SystemModel() |> complete -lsys = named_ss(model, [model.torque.tau.u], [model.inertia1.phi, model.inertia2.phi]) +op = Dict(model.inertia1.flange_b.phi => 0.0, model.torque.tau.u => 0) +lsys = named_ss(model, [model.torque.tau.u], [model.inertia1.phi, model.inertia2.phi]; op) @test -1000 ∈ lsys.A @test -10 ∈ lsys.A @test 1000 ∈ lsys.A @@ -295,7 +298,7 @@ using ModelingToolkitStandardLibrary.Mechanical.TranslationalPosition using Test using ControlSystemsMTK -using ControlSystemsMTK.ControlSystemsBase: sminreal, minreal, poles +using ControlSystemsMTK.ControlSystemsBase: sminreal, minreal, poles, ss, tf connect = ModelingToolkit.connect @independent_variables t @@ -348,3 +351,38 @@ op2[cart.f] = 0 @test G.nx == 4 @test G.nu == length(lin_inputs) @test G.ny == length(lin_outputs) + +## Test difficult `named_ss` simplification +using ControlSystemsMTK, ControlSystemsBase, RobustAndOptimalControl +lsys = (A = [0.0 2.778983834717109e8 1.4122312296634873e6 0.0; 0.0 0.0 0.0 0.037848975765016724; 0.0 24.837541148074962 0.12622006230897712 0.0; -0.0 -4.620724819774693 -0.023481719514324866 -0.6841991610512456], B = [-5.042589978197361e8 0.0; -0.0 0.0; -45.068824982602656 -0.0; 8.384511049369085 54.98555939873381], C = [0.0 0.0 0.954929658551372 0.0], D = [0.0 0.0]) + +# lsys = (A = [-0.0075449237853825925 1.6716817118020731e-6 0.0; 1864.7356343162514 -0.4131578457122937 0.0; 0.011864343330426718 -2.6287085638214332e-6 0.0], B = [0.0 0.0; 0.0 52566.418015009294; 0.0 0.3284546792274811], C = [1.4683007399899215e8 0.0 0.0], D = [-9.157636303058283e7 0.0]) + +G = ControlSystemsMTK.causal_simplification(lsys, [1=>2]) +G2 = ControlSystemsMTK.causal_simplification(lsys, [1=>2], descriptor=false) +G2 = minreal(G2, 1e-12) + +@test dcgain(G, 1e-5)[] ≈ dcgain(G2, 1e-5)[] rtol=1e-3 +@test freqresp(G, 1)[] ≈ freqresp(G2, 1)[] +@test freqresp(G, 10)[] ≈ freqresp(G2, 10)[] + +z = 0.462726166562343204837317130554462562 + +@test minimum(abs, tzeros(G) .- z) < sqrt(eps()) +@test minimum(abs, tzeros(G2) .- z) < sqrt(eps()) + +# using GenericSchur + +Gb = balance_statespace(G)[1] +Gb = minreal(Gb, 1e-8) +@test Gb.nx == 2 +@test minimum(abs, tzeros(Gb) .- z) < sqrt(eps()) + +w = exp10.(LinRange(-12, 2, 2000)) +# ControlSystemsBase.bodeplot([G, G2, minreal(G, 1e-8)], w) + + +## + +# S = schur(A,B) +# V = eigvecs(S) diff --git a/test/test_batchlin.jl b/test/test_batchlin.jl index 4f60818..11804c4 100644 --- a/test/test_batchlin.jl +++ b/test/test_batchlin.jl @@ -1,6 +1,8 @@ using ControlSystemsMTK, ModelingToolkit, RobustAndOptimalControl, MonteCarloMeasurements using OrdinaryDiffEqNonlinearSolve, OrdinaryDiffEqRosenbrock +using ModelingToolkitStandardLibrary.Blocks using ModelingToolkit: getdefault +using Test unsafe_comparisons(true) # Create a model @@ -24,7 +26,7 @@ sample_within_bounds((l, u)) = (u - l) * rand() + l # Create a vector of operating points N = 10 xs = range(getbounds(x)[1], getbounds(x)[2], length=N) -ops = Dict.(x .=> xs) +ops = Dict.(u.u => 0, x .=> xs) inputs, outputs = [u.u], [y.u] Ps, ssys = batch_ss(duffing, inputs, outputs , ops) @@ -55,11 +57,11 @@ import ModelingToolkitStandardLibrary.Blocks closed_loop_eqs = [ - connect(ref.output, :r, F.input) - connect(F.output, fb.input1) - connect(duffing.y, :y, fb.input2) - connect(fb.output, C.input) - connect(C.output, duffing.u) + ModelingToolkit.connect(ref.output, :r, F.input) + ModelingToolkit.connect(F.output, fb.input1) + ModelingToolkit.connect(duffing.y, :y, fb.input2) + ModelingToolkit.connect(fb.output, C.input) + ModelingToolkit.connect(C.output, duffing.u) ] @named closed_loop = ODESystem(closed_loop_eqs, t, systems=[duffing, C, fb, ref, F]) @@ -73,6 +75,7 @@ sol = solve(prob, Rodas5P(), abstol=1e-8, reltol=1e-8) time = 0:0.1:8 inputs, outputs = [duffing.u.u], [duffing.y.u] +op = Dict(u.u => 0) Ps2, ssys = trajectory_ss(closed_loop, closed_loop.r, closed_loop.y, sol; t=time) @test length(Ps2) == length(time) # bodeplot(Ps2)