From 55107fc71577b3f8a662741cdd317a3ada7fd187 Mon Sep 17 00:00:00 2001 From: Fredrik Bagge Carlson Date: Fri, 3 Jul 2026 09:11:19 +0000 Subject: [PATCH 1/2] Default all linearizations to inline linear-SCC reassembly Add DEFAULT_LINEARIZE_KWARGS passing reassemble_alg = DefaultReassembleAlgorithm(; inline_linear_sccs = true, analytical_linear_scc_limit = 1) to every linearization-based function (named_ss, get_named_sensitivity and friends, batch_ss/batch_linearize, trajectory_ss, build_quadratic_cost_matrix). The numerical linear-SCC fallback in generated code can produce silently rank-deficient linearizations on models with large linear SCCs (observed on a planar two-joint robot, where the two torque-input columns of the loop transfer came out numerically identical, collapsing the input complementary sensitivity to a rank-1 projector instead of I at low frequency). Caller-supplied keyword arguments take precedence, so the ModelingToolkit default remains reachable. Co-Authored-By: Claude Fable 5 Claude-Session: https://claude.ai/code/session_01W4JdEEqr6ifdSbp6MvbrU8 --- src/ControlSystemsMTK.jl | 17 +++++++++++++++++ src/ode_system.jl | 10 +++++----- 2 files changed, 22 insertions(+), 5 deletions(-) diff --git a/src/ControlSystemsMTK.jl b/src/ControlSystemsMTK.jl index 1e987e9..e7e1b62 100644 --- a/src/ControlSystemsMTK.jl +++ b/src/ControlSystemsMTK.jl @@ -35,6 +35,23 @@ export build_quadratic_cost_matrix export get_named_sensitivity, get_named_comp_sensitivity, get_named_looptransfer +""" +Default keyword arguments forwarded to ModelingToolkit's linearization (and through it to +`mtkcompile`) by every linearization-based function in this package (`named_ss`, +`get_named_sensitivity` and friends, `batch_ss`, `trajectory_ss`, +`build_quadratic_cost_matrix`). `inline_linear_sccs` makes tearing solve linear +strongly-connected components inline (symbolically for size ≤ `analytical_linear_scc_limit`) +rather than through numerical linear-solve calls embedded in the generated code. This is +required for correct linearization of models with large linear SCCs (e.g. multibody +mechanisms). Keyword arguments passed by the caller take precedence, so pass +`reassemble_alg = ModelingToolkit.StructuralTransformations.DefaultReassembleAlgorithm()` to +restore the ModelingToolkit default. +""" +const DEFAULT_LINEARIZE_KWARGS = (; + reassemble_alg = ModelingToolkit.StructuralTransformations.DefaultReassembleAlgorithm(; + inline_linear_sccs = true, analytical_linear_scc_limit = 1), +) + include("ode_system.jl") # include("symbolic_optimization.jl") diff --git a/src/ode_system.jl b/src/ode_system.jl index a8fad8f..3221db9 100644 --- a/src/ode_system.jl +++ b/src/ode_system.jl @@ -110,7 +110,7 @@ function RobustAndOptimalControl.named_ss( out end end - matrices, ssys, xpt = ModelingToolkit.linearize(sys, inputs, outputs; kwargs...) + matrices, ssys, xpt = ModelingToolkit.linearize(sys, inputs, outputs; DEFAULT_LINEARIZE_KWARGS..., kwargs...) unames = symstr.(inputs) if nu > 0 && size(matrices.B, 2) == 2nu # This indicates that input derivatives are present @@ -301,7 +301,7 @@ function named_sensitivity_function( end end nu = length(inputs) - matrices, ssys = fun(sys, inputs, args...; kwargs...) + matrices, ssys = fun(sys, inputs, args...; DEFAULT_LINEARIZE_KWARGS..., kwargs...) symstr(x) = Symbol(x isa AnalysisPoint ? x.name : string(x)) unames = symstr.(inputs) fm(x) = convert(Matrix{Float64}, x) @@ -384,7 +384,7 @@ The second problem above, the ordering of the states, can be worked around using - `costs`: A vector of pairs. """ function build_quadratic_cost_matrix(sys::System, inputs::AbstractVector, costs::AbstractVector{<:Pair}; kwargs...) - matrices, ssys, extras = ModelingToolkit.linearize(sys, inputs, first.(costs); kwargs...) + matrices, ssys, extras = ModelingToolkit.linearize(sys, inputs, first.(costs); DEFAULT_LINEARIZE_KWARGS..., kwargs...) x = ModelingToolkit.unknowns(ssys) y = ModelingToolkit.outputs(ssys) nx = length(x) @@ -406,7 +406,7 @@ end function batch_linearize(sys, inputs, outputs, ops::AbstractVector{<:AbstractDict}; t = 0.0, allow_input_derivatives = false, kwargs...) - lin_fun, ssys = linearization_function(sys, inputs, outputs; op=ops[1], kwargs...) + lin_fun, ssys = linearization_function(sys, inputs, outputs; op=ops[1], DEFAULT_LINEARIZE_KWARGS..., kwargs...) lins_ops = map(ops) do op linearize(ssys, lin_fun; op, t, allow_input_derivatives) end @@ -552,7 +552,7 @@ 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], kwargs...)#, initialization_abstol=1e-1, initialization_reltol=1e-1, kwargs...) # initializealg=ModelingToolkit.SciMLBase.NoInit() + lin_fun, ssys = linearization_function(sys, inputs, outputs; op=ops[1], DEFAULT_LINEARIZE_KWARGS..., kwargs...)#, initialization_abstol=1e-1, initialization_reltol=1e-1, kwargs...) # initializealg=ModelingToolkit.SciMLBase.NoInit() # Main.lin_fun = lin_fun # Main.op1 = ops[1] # Main.ops = ops From fa5e62e85a98eed7bf6540eaae0a40cf7b4ef00f Mon Sep 17 00:00:00 2001 From: Fredrik Bagge Carlson Date: Fri, 3 Jul 2026 09:42:38 +0000 Subject: [PATCH 2/2] Fix input-derivative column pairing in causal simplification call sites MTK's linearize with allow_input_derivatives = true returns B with 2nu columns where column nu + i is the derivative of input i. The pairing passed to causal_simplification was derived by broadcasting 1:nu against the set of NONZERO derivative columns, which silently pairs every input to the same derivative channel when exactly one column is nonzero (and errors for other partial subsets). This produced wrong linearizations whenever inputs enter retained algebraic equations, e.g. through the numerical (non-inlined) linear-SCC reassembly: on a planar two-joint robot the input complementary sensitivity collapsed to a rank-1 projector instead of I at low frequency. The pairing is now positional over all inputs; all-zero derivative columns contribute nothing. Co-Authored-By: Claude Fable 5 Claude-Session: https://claude.ai/code/session_01W4JdEEqr6ifdSbp6MvbrU8 --- src/ode_system.jl | 24 ++++++++++++++++++------ 1 file changed, 18 insertions(+), 6 deletions(-) diff --git a/src/ode_system.jl b/src/ode_system.jl index 3221db9..7eee5b5 100644 --- a/src/ode_system.jl +++ b/src/ode_system.jl @@ -114,8 +114,12 @@ function RobustAndOptimalControl.named_ss( unames = symstr.(inputs) if nu > 0 && size(matrices.B, 2) == 2nu # 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 + # Derivative column nu + i is the derivative of input i (see the MTK docs for + # `linearize` with `allow_input_derivatives = true`), so the pairing is positional. + # All-zero derivative columns contribute nothing and are included for simplicity; + # a pairing derived from the set of nonzero columns mispairs inputs when only a + # subset of the derivative columns is nonzero. + u2du = [i => i + nu for i in 1:nu] # This maps inputs to their derivatives lsys = causal_simplification(matrices, u2du; descriptor, simple_infeigs, big, balance) else lsys = ss(matrices...) @@ -154,8 +158,12 @@ function RobustAndOptimalControl.named_ss( unames = symstr.(inputs) if nu > 0 && size(matrices.B, 2) == 2nu # 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 + # Derivative column nu + i is the derivative of input i (see the MTK docs for + # `linearize` with `allow_input_derivatives = true`), so the pairing is positional. + # All-zero derivative columns contribute nothing and are included for simplicity; + # a pairing derived from the set of nonzero columns mispairs inputs when only a + # subset of the derivative columns is nonzero. + u2du = [i => i + nu for i in 1:nu] # This maps inputs to their derivatives lsys = causal_simplification(matrices, u2du; descriptor, simple_infeigs, big, balance, verbose=false) else lsys = ss(matrices...) @@ -307,8 +315,12 @@ function named_sensitivity_function( fm(x) = convert(Matrix{Float64}, x) if nu > 0 && size(matrices.B, 2) == 2nu # 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 + # Derivative column nu + i is the derivative of input i (see the MTK docs for + # `linearize` with `allow_input_derivatives = true`), so the pairing is positional. + # All-zero derivative columns contribute nothing and are included for simplicity; + # a pairing derived from the set of nonzero columns mispairs inputs when only a + # subset of the derivative columns is nonzero. + u2du = [i => i + nu for i in 1:nu] # This maps inputs to their derivatives lsys = causal_simplification(matrices, u2du; descriptor, simple_infeigs, big, balance) else lsys = ss(matrices...)