How fast does a model has to be for NMPC

It may be a bug in NonlinearSolve.jl. I looked at runtest.jl in SeeToDee.jl and there is test of differentiation with ForwardDiff.jl on a simple pendulum model discretized with SimpleColloc. All the tests pass on my side. Do you have any ideas @baggepinnen ?

I’m not sure what code you are running, but the error might come from f being on the in-place form f(dx, x, u, p, t) while SimpleColloc is only setup to work with the out-of-place form f(x, u, p, t)

I am using a control function generated by generate_control_function from a modelingtoolkit to create an f_ip function.

f_ip, dvs, psym, io_sys = ModelingToolkit.generate_control_function(IRSystem(model), inputs)

I convert this to an f_oop function and discretize using SimpleAlloc, so I am not using f_ip directly in SimpleAlloc.

function f_oop(x, u, par, t)
    dx = Vector{Any}(undef, nx)
    f_ip(dx, x, u, par, t)
    return dx
end
f_disc = SimpleColloc(f_oop, Ts, nx, 0, nu; n = 5, abstol = 1e-12, solver=NewtonRaphson(), residual=false)
function f(x, u, _, _)
    return f_disc(x, u, par, 1.0)
end

This should be more performant

dx = Vector{Base.promote_type(typeof(x), typeof(u)))}(undef, nx)

In that case I am unfortunately not sure what is wrong, the stack trace is not enough for me to figure it out :confused: Somewhere an array is being assigned with an unexpected type, but I can’t tell which array it is.

I think it is a bug, because this works fine:

    f_disc = SeeToDee.Rk4(f_oop, Ts; supersample = 1)

While this does not work:

    f_disc = SimpleColloc(f_oop, Ts, nx, 0, nu; n = 2, abstol = 1e-8, solver=NewtonRaphson(), residual=false)

Here is a minimum reproducable example. Linearizing works with Rk4 but doesn’t work with SimpleColloc.

using ModelPredictiveControl
using ModelingToolkit
using ModelingToolkit: D_nounits as D, t_nounits as t, varmap_to_vars
using JuliaSimCompiler
using Plots
using SeeToDee

const JSC = JuliaSimCompiler

function get_control_function(model, inputs)
    f_ip, dvs, psym, io_sys = ModelingToolkit.generate_control_function(IRSystem(model), inputs)
    return (f_ip, dvs, psym, io_sys)
end

function generate_f_h(io_sys, inputs, outputs, f_ip, dvs, psym, Ts)
    h_ = JuliaSimCompiler.build_explicit_observed_function(io_sys, outputs; inputs = inputs, target = JuliaSimCompiler.JuliaTarget())
    nu = length(inputs)
    ny = length(outputs)
    nx = length(dvs)
    vu = string.(inputs)
    vy = string.(outputs)
    vx = string.(dvs)
    par = JuliaSimCompiler.initial_conditions(io_sys, defaults(io_sys), psym)[2]
    function f_oop(x, u, par, t)
        dx = Vector{Any}(undef, nx)
        f_ip(dx, x, u, par, t)
        return dx
    end
    # fails when using SimpleColloc, but works when using Rk4
    # f_disc = SeeToDee.Rk4(f_oop, Ts; supersample = 1)
    f_disc = SimpleColloc(f_oop, Ts, nx, 0, nu)
    SeeToDee.linearize(f_disc, [0.0, 0.0], [0.0], par, 1)
    function f(x, u, _, _)
        return f_disc(x, u, par, 1.0)
    end
    function h!(y, x, _, _)
        h_(y, x, fill(nothing, length(inputs)), par, 1.0)
        nothing
    end
    return f, (h!, nu, ny, nx, vu, vy, vx) # TODO: check on mwe.jl
end

@mtkmodel Pendulum begin
    @parameters begin
        g = 9.8
        L = 0.4
        K = 1.2
        m = 0.3
    end
    @variables begin
        θ(t) = 0.0 # state
        ω(t) = 0.0 # state
        τ(t) = 0.0 # input
        y(t) = 0.0 # output
    end
    @equations begin
        D(θ)    ~ ω
        D(ω)    ~ -g/L*sin(θ) - K/m*ω + τ/m/L^2
        y       ~ θ * 180 / π
    end
end
@named mtk_model = Pendulum()
mtk_model = complete(mtk_model)
inputs, outputs = [mtk_model.τ], [mtk_model.y]

Ts = 0.1
N = 35
(f_ip, dvs, psym, io_sys) = get_control_function(mtk_model, inputs)
f, (h!, nu, ny, nx, vu, vy, vx) = generate_f_h(io_sys, inputs, outputs, f_ip, dvs, psym, Ts)
model = setname!(NonLinModel(f, h!, Ts, nu, nx, ny, solver=nothing); u=vu, x=vx, y=vy)

println("sanity check")
u = [0.5]
@time res = sim!(model, N, u, x_0 = [0, 0])
display(plot(res, plotu=false))

println("linearize using MPC.jl")
ModelPredictiveControl.linearize(model)
nothing

Are there any good alternatives to SeeToDee.jl, maybe a fast non-allocating integrator in DifferentialEquations.jl?

Something appears to have changed in NonlinearSolve, the tests I have for ForwardDiff in the test suite also fails with NonlinearSolve instead of SimpleNonlinearSolve. Unfortunately, SimpleNonlinearSolve isn’t an option in this case since it’s terribly slow to differentiate through, it’s probably not setup with the required chain rules for implicit differentiation to make this fast.

Unfortunately, I don’t have the time to look further into this at the moment, I opened this issue so that I can get back to it eventually

2 Likes

Thank you! I will try to use OrdinaryDiffEq.jl with QNDF or some other solver in the meantime.

MWE of this?

Both NonlinearSolve and SimpleNonlinearSolve are setup with implicit differentiation:

And tested in:

If you can share a full stack trace that would help figure out if it’s missing the implicit diff dispatches or if it’s something inside of the dispatch.

SimpleNonlinearSolve appears to work well now, it used to be slow once upon a time so I wrote the docs to recommend using NonlinearSolve for speed. The error with NonlinearSolve is some array-conversion error

using SeeToDee
using Test
using StaticArrays
using ForwardDiff
using NonlinearSolve
using FastGaussQuadrature

function cartesian_pendulum(U, inp, p, t)
    x,y,u,v,τ = U
    SA[u;
    v;
    - τ*x;
    - (τ*y - 1);
    x^2 + y^2 - 1]
end

n = 5
N = 100
Ts = 30/N
θ0 = π/3
x = [sin(θ0), -cos(θ0), 0, 0, 0.1]

discrete_dynamics = SimpleColloc(cartesian_pendulum, Ts, 4, 1, 0; n, abstol=1e-9, nodetype=gausslobatto, solver=NonlinearSolve.NewtonRaphson()) 

A = ForwardDiff.jacobian(x -> discrete_dynamics(x, 0, 0, 0), x) # This errors with the same error as  posted above
julia> ForwardDiff.jacobian(x -> discrete_dynamics(x, 0, 0, 0), x)
ERROR: MethodError: Cannot `convert` an object of type Vector{ForwardDiff.Dual{ForwardDiff.Tag{var"#25#26", Float64}, Float64, 5}} to an object of type Base.ReinterpretArray{ForwardDiff.Dual{ForwardDiff.Tag{var"#25#26", Float64}, Float64, 5}, 1, Float64, SubArray{Float64, 1, Vector{Float64}, Tuple{UnitRange{Int64}}, true}, false}

Closest candidates are:
  convert(::Type{T}, ::T) where T
   @ Base Base.jl:84
  convert(::Type{T}, ::LinearAlgebra.Factorization) where T<:AbstractArray
   @ LinearAlgebra ~/.julia/juliaup/julia-1.10.5+0.x64.linux.gnu/share/julia/stdlib/v1.10/LinearAlgebra/src/factorization.jl:108
  convert(::Type{T}, ::T) where T<:AbstractArray
   @ Base abstractarray.jl:16
  ...

Stacktrace:
  [1] setproperty!(x::NonlinearSolve.GeneralizedFirstOrderAlgorithmCache{…}, f::Symbol, v::Vector{…})
    @ Base ./Base.jl:40
  [2] set_u!(cache::NonlinearSolve.GeneralizedFirstOrderAlgorithmCache{…}, u::Vector{…})
    @ NonlinearSolve ~/.julia/packages/NonlinearSolve/sBl1H/src/abstract_types.jl:240
  [3] update_from_termination_cache!(tc_cache::DiffEqBase.NonlinearTerminationModeCache{…}, cache::NonlinearSolve.GeneralizedFirstOrderAlgorithmCache{…}, ::AbsSafeBestTerminationMode{…}, u::Base.ReinterpretArray{…})
    @ NonlinearSolve ~/.julia/packages/NonlinearSolve/sBl1H/src/internal/termination.jl:57
  [4] update_from_termination_cache!(tc_cache::DiffEqBase.NonlinearTerminationModeCache{…}, cache::NonlinearSolve.GeneralizedFirstOrderAlgorithmCache{…}, u::Base.ReinterpretArray{…})
    @ NonlinearSolve ~/.julia/packages/NonlinearSolve/sBl1H/src/internal/termination.jl:43
  [5] solve!(cache::NonlinearSolve.GeneralizedFirstOrderAlgorithmCache{…})
    @ NonlinearSolve ~/.julia/packages/NonlinearSolve/sBl1H/src/core/generic.jl:22
  [6] __solve(::NonlinearProblem{…}, ::GeneralizedFirstOrderAlgorithm{…}; stats::SciMLBase.NLStats, kwargs::@Kwargs{…})
    @ NonlinearSolve ~/.julia/packages/NonlinearSolve/sBl1H/src/core/generic.jl:4
  [7] solve_call(_prob::NonlinearProblem{…}, args::GeneralizedFirstOrderAlgorithm{…}; merge_callbacks::Bool, kwargshandle::Nothing, kwargs::@Kwargs{…})
    @ DiffEqBase ~/.julia/packages/DiffEqBase/gFJ8H/src/solve.jl:612
  [8] solve_up(prob::NonlinearProblem{…}, sensealg::Nothing, u0::Base.ReinterpretArray{…}, p::Tuple{…}, args::GeneralizedFirstOrderAlgorithm{…}; kwargs::@Kwargs{…})
    @ DiffEqBase ~/.julia/packages/DiffEqBase/gFJ8H/src/solve.jl:1084
  [9] solve(prob::NonlinearProblem{…}, args::GeneralizedFirstOrderAlgorithm{…}; sensealg::Nothing, u0::Nothing, p::Nothing, wrap::Val{…}, kwargs::@Kwargs{…})
    @ DiffEqBase ~/.julia/packages/DiffEqBase/gFJ8H/src/solve.jl:1072
 [10] (::SimpleColloc{…})(x0::Vector{…}, u::Int64, p::Int64, t::Int64; abstol::Float64)
    @ SeeToDee ~/.julia/dev/SeeToDee/src/SeeToDee.jl:259
 [11] (::SimpleColloc{…})(x0::Vector{…}, u::Int64, p::Int64, t::Int64)
    @ SeeToDee ~/.julia/dev/SeeToDee/src/SeeToDee.jl:249
 [12] (::var"#25#26")(x::Vector{ForwardDiff.Dual{ForwardDiff.Tag{var"#25#26", Float64}, Float64, 5}})
    @ Main ./REPL[29]:1
 [13] vector_mode_dual_eval!
    @ ~/.julia/packages/ForwardDiff/PcZ48/src/apiutils.jl:24 [inlined]
 [14] vector_mode_jacobian(f::var"#25#26", x::Vector{…}, cfg::ForwardDiff.JacobianConfig{…})
    @ ForwardDiff ~/.julia/packages/ForwardDiff/PcZ48/src/jacobian.jl:125
 [15] jacobian(f::Function, x::Vector{…}, cfg::ForwardDiff.JacobianConfig{…}, ::Val{…})
    @ ForwardDiff ~/.julia/packages/ForwardDiff/PcZ48/src/jacobian.jl:21
 [16] jacobian(f::Function, x::Vector{Float64}, cfg::ForwardDiff.JacobianConfig{ForwardDiff.Tag{…}, Float64, 5, Vector{…}})
    @ ForwardDiff ~/.julia/packages/ForwardDiff/PcZ48/src/jacobian.jl:19
 [17] top-level scope
    @ REPL[29]:1
Some type information was truncated. Use `show(err)` to see complete types.

@avikpal

FYI, I will work on adding some other simple ODE solver other than RungeKutta(4). Probably something inspired by SimpleColloc but easier to integrate with ModelPredictiveControl.jl. It won’t be in the near term since I will be out for one month and I need to finish custom nonlinear constraints first.

2 Likes

Part of the issue here is that p is not a AbstractArray{<:Dual} or <:Dual so our dispatches don’t get hit. I am not sure how to handle the case for non-abstract array p’s given we need to call jacobian later on

There is no differentiation w.r.t. p here, why does the type of this argument matter?

1 Like

With just u0 being duals, the Jacobian is zeros, right? Currently we use IFT only when the u0 and p are Dual/AbstractArray of Duals

1 Like

Right, I actually do pass some duals into one of the p elements, sorry for the confusion.

Now I understand what the issue is, maybe I could stick the rest of the tuple entries elsewhere and leave p to be an array

1 Like

I see what’s going on now. That’s what we have the SciMLStructures interface for… but we haven’t completely rolled out the docs yet.

2 Likes

But this still does not work, even with SimpleNonlinearSolve.

Stacktrace:

ERROR: LoadError: MethodError: no method matching Float64(::ForwardDiff.Dual{ForwardDiff.Tag{SeeToDee.var"#2#4"{…}, Float64}, Float64, 1})

Closest candidates are:
  (::Type{T})(::Real, ::RoundingMode) where T<:AbstractFloat
   @ Base rounding.jl:207
  (::Type{T})(::T) where T<:Number
   @ Core boot.jl:792
  Float64(::IrrationalConstants.Log2π)
   @ IrrationalConstants ~/.julia/packages/IrrationalConstants/vp5v4/src/macro.jl:112
  ...

Stacktrace:
  [1] convert(::Type{Float64}, x::ForwardDiff.Dual{ForwardDiff.Tag{SeeToDee.var"#2#4"{…}, Float64}, Float64, 1})
    @ Base ./number.jl:7
  [2] setindex!(A::Vector{Float64}, x::ForwardDiff.Dual{ForwardDiff.Tag{…}, Float64, 1}, i1::Int64)
    @ Base ./array.jl:1021
  [3] setindex!
    @ ./subarray.jl:355 [inlined]
  [4] macro expansion
    @ ./broadcast.jl:1004 [inlined]
  [5] macro expansion
    @ ./simdloop.jl:77 [inlined]
  [6] copyto!
    @ ./broadcast.jl:1003 [inlined]
  [7] copyto!
    @ ./broadcast.jl:956 [inlined]
  [8] materialize!
    @ ./broadcast.jl:914 [inlined]
  [9] materialize!
    @ ./broadcast.jl:911 [inlined]
 [10] coldyn(xv::Vector{Float64}, ::Tuple{SimpleColloc{…}, Vector{…}, Vector{…}, Vector{…}, Int64})
    @ SeeToDee ~/.julia/packages/SeeToDee/t9mPJ/src/SeeToDee.jl:239
 [11] NonlinearFunction
    @ ~/.julia/packages/SciMLBase/tEuIM/src/scimlfunctions.jl:2337 [inlined]
 [12] _get_fx
    @ ~/.julia/packages/SimpleNonlinearSolve/YQl3A/src/utils.jl:138 [inlined]
 [13] _get_fx
    @ ~/.julia/packages/SimpleNonlinearSolve/YQl3A/src/utils.jl:126 [inlined]
 [14] __solve(::SimpleNonlinearSolve.ImmutableNonlinearProblem{…}, ::SimpleNonlinearSolve.SimpleNewtonRaphson{…}; abstol::Float64, reltol::Nothing, maxiters::Int64, termination_condition::Nothing, alias_u0::Bool, kwargs::@Kwargs{})
    @ SimpleNonlinearSolve ~/.julia/packages/SimpleNonlinearSolve/YQl3A/src/nlsolve/raphson.jl:31
 [15] __solve
    @ ~/.julia/packages/SimpleNonlinearSolve/YQl3A/src/nlsolve/raphson.jl:26 [inlined]
 [16] #__internal_solve_up#117
    @ ~/.julia/packages/SimpleNonlinearSolve/YQl3A/src/SimpleNonlinearSolve.jl:99 [inlined]
 [17] __internal_solve_up
    @ ~/.julia/packages/SimpleNonlinearSolve/YQl3A/src/SimpleNonlinearSolve.jl:96 [inlined]
 [18] #solve#115
    @ ~/.julia/packages/SimpleNonlinearSolve/YQl3A/src/SimpleNonlinearSolve.jl:80 [inlined]
 [19] solve
    @ ~/.julia/packages/SimpleNonlinearSolve/YQl3A/src/SimpleNonlinearSolve.jl:72 [inlined]
 [20] (::SimpleColloc{…})(x0::Vector{…}, u::Vector{…}, p::Vector{…}, t::Int64; abstol::Float64)
    @ SeeToDee ~/.julia/packages/SeeToDee/t9mPJ/src/SeeToDee.jl:259
 [21] SimpleColloc
    @ ~/.julia/packages/SeeToDee/t9mPJ/src/SeeToDee.jl:249 [inlined]
 [22] #2
    @ ~/.julia/packages/SeeToDee/t9mPJ/src/SeeToDee.jl:18 [inlined]
 [23] vector_mode_dual_eval!
    @ ~/.julia/packages/ForwardDiff/PcZ48/src/apiutils.jl:24 [inlined]
 [24] vector_mode_jacobian(f::SeeToDee.var"#2#4"{…}, x::Vector{…}, cfg::ForwardDiff.JacobianConfig{…})
    @ ForwardDiff ~/.julia/packages/ForwardDiff/PcZ48/src/jacobian.jl:125
 [25] jacobian(f::Function, x::Vector{…}, cfg::ForwardDiff.JacobianConfig{…}, ::Val{…})
    @ ForwardDiff ~/.julia/packages/ForwardDiff/PcZ48/src/jacobian.jl:21
 [26] jacobian(f::Function, x::Vector{Float64}, cfg::ForwardDiff.JacobianConfig{ForwardDiff.Tag{…}, Float64, 1, Vector{…}})
    @ ForwardDiff ~/.julia/packages/ForwardDiff/PcZ48/src/jacobian.jl:19
 [27] linearize
    @ ~/.julia/packages/SeeToDee/t9mPJ/src/SeeToDee.jl:18 [inlined]
 [28] generate_f_h(io_sys::JuliaSimCompiler.ScheduledSystem{…}, inputs::Vector{…}, outputs::Vector{…}, f_ip::RuntimeGeneratedFunctions.RuntimeGeneratedFunction{…}, dvs::Vector{…}, psym::Vector{…}, Ts::Float64)
    @ Main ~/Code/KitePredictiveControl.jl/src/mwe.jl:32
 [29] top-level scope
    @ ~/Code/KitePredictiveControl.jl/src/mwe.jl:69
 [30] include(fname::String)
    @ Base.MainInclude ./client.jl:489
 [31] top-level scope
    @ REPL[1]:1
in expression starting at /home/bart/Code/KitePredictiveControl.jl/src/mwe.jl:69

Something has to be fixed here? Can I get some tips so I can try to fix it myself @baggepinnen?

Try this PR branch with the default nonlinear solver


Edit: The PR has now been merged and a new release is out, it should resolve the error MethodError: no method matching Float64(::ForwardDiff.Dual{ForwardDiff.Tag{SeeToDee.var"#2#4"{…}, Float64}, Float64, 1})

2 Likes

I ended up using Modelingtoolkit.linearize as it is very fast.

lin_fun, simple_sys = ModelingToolkit.linearization_function(kite_model, inputs, outputs)
@time (; A, B, C, D) = ModelingToolkit.linearize(simple_sys, lin_fun; t=1.0, op = defaults(kite_model))
css = ss(A, B, C, D)
dss = c2d(css, Ts)
model = LinModel(dss.A, dss.B, dss.C, dss.B[:, end+1:end], dss.D[:, end+1:end], Ts)

Linearizing takes 0.005405 seconds (27.35 k allocations: 2.698 MiB) for a system with 70 differentiable variables.

2 Likes