Calling a function with inputs and outputs created by `@variables` triggers a `MethodError`

[ferrolho]

Hi! It is my first time using ModelingToolkit.jl. My goal is to be able to evaluate large and sparse Jacobians and Hessians for already-defined functions which are not easy to write algebraically. From what I have read in the docs and from what I understood from this video, I believe the right thing to do is to create symbolic inputs and outputs with @variables, and then send those through my functions to have ModelingToolkit.jl do its thing. I am running into some issues, though.

For now, I am trying to start simple. I am just trying to evaluate a test function f! (which calls TORA.inverse_dynamics_defects!) with symbolic inputs and outputs. While the normal call to the function works (using Float inputs and outputs), the call to the same function with inputs and outputs created by @variables fails. The test code is below, as well as the stacktrace of the error.

using MeshCat
using ModelingToolkit
using TORA

vis = Visualizer()
robot = create_robot_kuka_iiwa_14(vis)

f!(dx, x) = TORA.inverse_dynamics_defects!(dx, robot, x, 1e-2)

n_out = robot.n_q + robot.n_v  # next state (from integration)
n_in = robot.n_q + robot.n_v + robot.n_τ +  # current state + current controls,
       robot.n_q + robot.n_v                # and next state (from discretisation)

let  # works
    x = rand(n_in)
    dx = zeros(n_out)
    f!(dx, x)
end

let  # does not work
    @variables dx[1:n_out] x[1:n_in]
    f!(dx, x)
end

MethodError: no method matching AbstractFloat(::Operation)
Closest candidates are:
  AbstractFloat(::T) where T<:Number at boot.jl:716
  AbstractFloat(!Matched::Bool) at float.jl:258
  AbstractFloat(!Matched::Int8) at float.jl:259
  ...

Stacktrace:
 [1] float(::Operation) at ./float.jl:277
 [2] sincos(::Operation) at ./special/trig.jl:204
 [3] Tuple at /home/henrique/.julia/packages/Rotations/3gsMV/src/angleaxis_types.jl:52 [inlined]
 [4] convert at /home/henrique/.julia/packages/StaticArrays/LJQEe/src/convert.jl:10 [inlined]
 [5] Rotations.RotMatrix{3,Expression,9}(::Rotations.AngleAxis{Expression}) at /home/henrique/.julia/packages/Rotations/3gsMV/src/core_types.jl:83
 [6] convert(::Type{Rotations.RotMatrix{3,Expression,9}}, ::Rotations.AngleAxis{Expression}) at /home/henrique/.julia/packages/Rotations/3gsMV/src/core_types.jl:20
 [7] joint_transform at /home/henrique/.julia/packages/RigidBodyDynamics/2qxSc/src/joint_types/revolute.jl:61 [inlined]
 [8] joint_transform at /home/henrique/.julia/packages/RigidBodyDynamics/2qxSc/src/joint.jl:191 [inlined]
 [9] macro expansion at /home/henrique/.julia/packages/TypeSortedCollections/Z4ytl/src/TypeSortedCollections.jl:173 [inlined]
 [10] map! at /home/henrique/.julia/packages/TypeSortedCollections/Z4ytl/src/TypeSortedCollections.jl:164 [inlined]
 [11] _update_transforms!(::RigidBodyDynamics.MechanismState{Operation,Float64,Expression,TypeSortedCollections.TypeSortedCollection{Tuple{Array{RigidBodyDynamics.Joint{Float64,RigidBodyDynamics.Revolute{Float64}},1}},1}}) at /home/henrique/.julia/packages/RigidBodyDynamics/2qxSc/src/mechanism_state.jl:692
 [12] update_transforms! at /home/henrique/.julia/packages/RigidBodyDynamics/2qxSc/src/mechanism_state.jl:686 [inlined]
 [13] spatial_accelerations!(::RigidBodyDynamics.CustomCollections.IndexDict{RigidBodyDynamics.BodyID,Base.OneTo{RigidBodyDynamics.BodyID},RigidBodyDynamics.Spatial.SpatialAcceleration{Operation}}, ::RigidBodyDynamics.MechanismState{Operation,Float64,Expression,TypeSortedCollections.TypeSortedCollection{Tuple{Array{RigidBodyDynamics.Joint{Float64,RigidBodyDynamics.Revolute{Float64}},1}},1}}, ::RigidBodyDynamics.CustomCollections.SegmentedVector{RigidBodyDynamics.JointID,Operation,Base.OneTo{RigidBodyDynamics.JointID},Array{Operation,1}}) at /home/henrique/.julia/packages/RigidBodyDynamics/2qxSc/src/mechanism_algorithms.jl:388
 [14] inverse_dynamics! at /home/henrique/.julia/packages/RigidBodyDynamics/2qxSc/src/mechanism_algorithms.jl:550 [inlined]
 [15] inverse_dynamics!(::RigidBodyDynamics.CustomCollections.SegmentedVector{RigidBodyDynamics.JointID,Operation,Base.OneTo{RigidBodyDynamics.JointID},Array{Operation,1}}, ::RigidBodyDynamics.CustomCollections.IndexDict{RigidBodyDynamics.BodyID,Base.OneTo{RigidBodyDynamics.BodyID},RigidBodyDynamics.Spatial.Wrench{Operation}}, ::RigidBodyDynamics.CustomCollections.IndexDict{RigidBodyDynamics.BodyID,Base.OneTo{RigidBodyDynamics.BodyID},RigidBodyDynamics.Spatial.SpatialAcceleration{Operation}}, ::RigidBodyDynamics.MechanismState{Operation,Float64,Expression,TypeSortedCollections.TypeSortedCollection{Tuple{Array{RigidBodyDynamics.Joint{Float64,RigidBodyDynamics.Revolute{Float64}},1}},1}}, ::RigidBodyDynamics.CustomCollections.SegmentedVector{RigidBodyDynamics.JointID,Operation,Base.OneTo{RigidBodyDynamics.JointID},Array{Operation,1}}) at /home/henrique/.julia/packages/RigidBodyDynamics/2qxSc/src/mechanism_algorithms.jl:549
 [16] inverse_dynamics_defects!(::Array{Operation,1}, ::Robot, ::Array{Operation,1}, ::Float64) at /home/henrique/git/TORA.jl/src/constraints/dynamics.jl:76
 [17] f!(::Array{Operation,1}, ::Array{Operation,1}) at ./In[4]:1
 [18] top-level scope at In[7]:3
 [19] include_string(::Function, ::Module, ::String, ::String) at ./loading.jl:1091

I wonder if I am doing something wrong... 🤔 And if not, is the problem that some of the dependencies need to be rewritten in order to be compatible with ModelingToolkit.jl? E.g., the stacktrace shows that the error happens somewhere in Rotations.jl; so, does it mean I'd have to make sure the functionality provided by Rotations.jl is written in a way that is compatible with ModelingToolkit.jl?

[YingboMa]

That looks like a very old version of MTK.

[ferrolho]

😮 ModelingToolkit v3.20.1. I'll have a look at what is holding it behind and try again. Thanks!

[ferrolho]

Okay, I am now on the latest version, v5.6.0. Here is the stacktrace of the error on that version:

MethodError: no method matching AbstractFloat(::Num)
Closest candidates are:
  AbstractFloat(::Real, !Matched::RoundingMode) where T<:AbstractFloat at rounding.jl:200
  AbstractFloat(::T) where T<:Number at boot.jl:716
  AbstractFloat(!Matched::Bool) at float.jl:258
  ...

Stacktrace:
 [1] float(::Num) at ./float.jl:277
 [2] sincos(::Num) at ./special/trig.jl:204
 [3] Tuple at /home/henrique/.julia/packages/Rotations/QaTBi/src/angleaxis_types.jl:52 [inlined]
 [4] convert at /home/henrique/.julia/packages/StaticArrays/LJQEe/src/convert.jl:10 [inlined]
 [5] Rotations.RotMatrix{3,Num,9}(::Rotations.AngleAxis{Num}) at /home/henrique/.julia/packages/Rotations/QaTBi/src/core_types.jl:83
 [6] convert(::Type{Rotations.RotMatrix{3,Num,9}}, ::Rotations.AngleAxis{Num}) at /home/henrique/.julia/packages/Rotations/QaTBi/src/core_types.jl:20
 [7] joint_transform at /home/henrique/.julia/packages/RigidBodyDynamics/Mf2gz/src/joint_types/revolute.jl:61 [inlined]
 [8] joint_transform at /home/henrique/.julia/packages/RigidBodyDynamics/Mf2gz/src/joint.jl:191 [inlined]
 [9] macro expansion at /home/henrique/.julia/packages/TypeSortedCollections/Z4ytl/src/TypeSortedCollections.jl:173 [inlined]
 [10] map! at /home/henrique/.julia/packages/TypeSortedCollections/Z4ytl/src/TypeSortedCollections.jl:164 [inlined]
 [11] _update_transforms!(::RigidBodyDynamics.MechanismState{Num,Float64,Num,TypeSortedCollections.TypeSortedCollection{Tuple{Array{RigidBodyDynamics.Joint{Float64,RigidBodyDynamics.Revolute{Float64}},1}},1}}) at /home/henrique/.julia/packages/RigidBodyDynamics/Mf2gz/src/mechanism_state.jl:692
 [12] update_transforms! at /home/henrique/.julia/packages/RigidBodyDynamics/Mf2gz/src/mechanism_state.jl:686 [inlined]
 [13] spatial_accelerations!(::RigidBodyDynamics.CustomCollections.IndexDict{RigidBodyDynamics.BodyID,Base.OneTo{RigidBodyDynamics.BodyID},RigidBodyDynamics.Spatial.SpatialAcceleration{Num}}, ::RigidBodyDynamics.MechanismState{Num,Float64,Num,TypeSortedCollections.TypeSortedCollection{Tuple{Array{RigidBodyDynamics.Joint{Float64,RigidBodyDynamics.Revolute{Float64}},1}},1}}, ::RigidBodyDynamics.CustomCollections.SegmentedVector{RigidBodyDynamics.JointID,Num,Base.OneTo{RigidBodyDynamics.JointID},Array{Num,1}}) at /home/henrique/.julia/packages/RigidBodyDynamics/Mf2gz/src/mechanism_algorithms.jl:388
 [14] inverse_dynamics! at /home/henrique/.julia/packages/RigidBodyDynamics/Mf2gz/src/mechanism_algorithms.jl:550 [inlined]
 [15] inverse_dynamics!(::RigidBodyDynamics.CustomCollections.SegmentedVector{RigidBodyDynamics.JointID,Num,Base.OneTo{RigidBodyDynamics.JointID},Array{Num,1}}, ::RigidBodyDynamics.CustomCollections.IndexDict{RigidBodyDynamics.BodyID,Base.OneTo{RigidBodyDynamics.BodyID},RigidBodyDynamics.Spatial.Wrench{Num}}, ::RigidBodyDynamics.CustomCollections.IndexDict{RigidBodyDynamics.BodyID,Base.OneTo{RigidBodyDynamics.BodyID},RigidBodyDynamics.Spatial.SpatialAcceleration{Num}}, ::RigidBodyDynamics.MechanismState{Num,Float64,Num,TypeSortedCollections.TypeSortedCollection{Tuple{Array{RigidBodyDynamics.Joint{Float64,RigidBodyDynamics.Revolute{Float64}},1}},1}}, ::RigidBodyDynamics.CustomCollections.SegmentedVector{RigidBodyDynamics.JointID,Num,Base.OneTo{RigidBodyDynamics.JointID},Array{Num,1}}) at /home/henrique/.julia/packages/RigidBodyDynamics/Mf2gz/src/mechanism_algorithms.jl:549
 [16] inverse_dynamics_defects!(::Array{Num,1}, ::Robot, ::Array{Num,1}, ::Float64) at /home/henrique/git/TORA.jl/src/constraints/dynamics.jl:76
 [17] f!(::Array{Num,1}, ::Array{Num,1}) at ./In[4]:1
 [18] top-level scope at In[7]:3
 [19] include_string(::Function, ::Module, ::String, ::String) at ./loading.jl:1091

[YingboMa]

Looks like a missing overload. Does

using ModelingToolkit.SymbolicUtils
using SymbolicUtils: Symbolic, Term
Base.sincos(x::Num) = sincos(ModelingToolkit.value(x))
Base.sincos(x::Symbolic) = Term(sincos, [x])

fix it?

Edit: typo correction.

[ferrolho]

Did you mean to write Symbolic instead of Symbolics for the type of argument x in the signature of the last method?

[YingboMa]

Yes, that's a typo.

[ferrolho]

Here's the stacktrace after applying that snippet:

MethodError: no method matching iterate(::Term{Any})
Closest candidates are:
  iterate(!Matched::DataStructures.SparseIntSet, !Matched::Any...) at /home/henrique/.julia/packages/DataStructures/ixwFs/src/sparse_int_set.jl:147
  iterate(!Matched::Base.AsyncGenerator, !Matched::Base.AsyncGeneratorState) at asyncmap.jl:382
  iterate(!Matched::Base.AsyncGenerator) at asyncmap.jl:382
  ...

Stacktrace:
 [1] indexed_iterate(::Term{Any}, ::Int64) at ./tuple.jl:84
 [2] Tuple at /home/henrique/.julia/packages/Rotations/QaTBi/src/angleaxis_types.jl:52 [inlined]
 [3] convert at /home/henrique/.julia/packages/StaticArrays/LJQEe/src/convert.jl:10 [inlined]
 [4] Rotations.RotMatrix{3,Num,9}(::Rotations.AngleAxis{Num}) at /home/henrique/.julia/packages/Rotations/QaTBi/src/core_types.jl:83
 [5] convert at /home/henrique/.julia/packages/Rotations/QaTBi/src/core_types.jl:20 [inlined]
 [6] joint_transform at /home/henrique/.julia/packages/RigidBodyDynamics/Mf2gz/src/joint_types/revolute.jl:61 [inlined]
 [7] joint_transform at /home/henrique/.julia/packages/RigidBodyDynamics/Mf2gz/src/joint.jl:191 [inlined]
 [8] macro expansion at /home/henrique/.julia/packages/TypeSortedCollections/Z4ytl/src/TypeSortedCollections.jl:173 [inlined]
 [9] map! at /home/henrique/.julia/packages/TypeSortedCollections/Z4ytl/src/TypeSortedCollections.jl:164 [inlined]
 [10] _update_transforms!(::RigidBodyDynamics.MechanismState{Num,Float64,Num,TypeSortedCollections.TypeSortedCollection{Tuple{Array{RigidBodyDynamics.Joint{Float64,RigidBodyDynamics.Revolute{Float64}},1}},1}}) at /home/henrique/.julia/packages/RigidBodyDynamics/Mf2gz/src/mechanism_state.jl:692
 [11] update_transforms! at /home/henrique/.julia/packages/RigidBodyDynamics/Mf2gz/src/mechanism_state.jl:686 [inlined]
 [12] spatial_accelerations!(::RigidBodyDynamics.CustomCollections.IndexDict{RigidBodyDynamics.BodyID,Base.OneTo{RigidBodyDynamics.BodyID},RigidBodyDynamics.Spatial.SpatialAcceleration{Num}}, ::RigidBodyDynamics.MechanismState{Num,Float64,Num,TypeSortedCollections.TypeSortedCollection{Tuple{Array{RigidBodyDynamics.Joint{Float64,RigidBodyDynamics.Revolute{Float64}},1}},1}}, ::RigidBodyDynamics.CustomCollections.SegmentedVector{RigidBodyDynamics.JointID,Num,Base.OneTo{RigidBodyDynamics.JointID},Array{Num,1}}) at /home/henrique/.julia/packages/RigidBodyDynamics/Mf2gz/src/mechanism_algorithms.jl:388
 [13] inverse_dynamics! at /home/henrique/.julia/packages/RigidBodyDynamics/Mf2gz/src/mechanism_algorithms.jl:550 [inlined]
 [14] inverse_dynamics!(::RigidBodyDynamics.CustomCollections.SegmentedVector{RigidBodyDynamics.JointID,Num,Base.OneTo{RigidBodyDynamics.JointID},Array{Num,1}}, ::RigidBodyDynamics.CustomCollections.IndexDict{RigidBodyDynamics.BodyID,Base.OneTo{RigidBodyDynamics.BodyID},RigidBodyDynamics.Spatial.Wrench{Num}}, ::RigidBodyDynamics.CustomCollections.IndexDict{RigidBodyDynamics.BodyID,Base.OneTo{RigidBodyDynamics.BodyID},RigidBodyDynamics.Spatial.SpatialAcceleration{Num}}, ::RigidBodyDynamics.MechanismState{Num,Float64,Num,TypeSortedCollections.TypeSortedCollection{Tuple{Array{RigidBodyDynamics.Joint{Float64,RigidBodyDynamics.Revolute{Float64}},1}},1}}, ::RigidBodyDynamics.CustomCollections.SegmentedVector{RigidBodyDynamics.JointID,Num,Base.OneTo{RigidBodyDynamics.JointID},Array{Num,1}}) at /home/henrique/.julia/packages/RigidBodyDynamics/Mf2gz/src/mechanism_algorithms.jl:549
 [15] inverse_dynamics_defects!(::Array{Num,1}, ::Robot, ::Array{Num,1}, ::Float64) at /home/henrique/git/TORA.jl/src/constraints/dynamics.jl:76
 [16] f!(::Array{Num,1}, ::Array{Num,1}) at ./In[4]:1
 [17] top-level scope at In[8]:3
 [18] include_string(::Function, ::Module, ::String, ::String) at ./loading.jl:1091

[YingboMa]

Ah, it should be

using ModelingToolkit.SymbolicUtils
using SymbolicUtils: Symbolic
Base.sincos(x::Union{Symbolic,Num}) = sin(x), cos(x)

[ferrolho]

Hi, @YingboMa. I guess that worked. I cannot tell for sure yet, because it is still running... Is it normal to take this long?

[ferrolho]

Hmm... 🤔 I left it running overnight. It has been more than 12 hours and it has not finished yet.

[ferrolho]

I'm not sure if this is useful or indicative of anything, but here is the stacktrace output from when I stopped the kernel:

InterruptException:

Stacktrace:
 [1] cmp_term_term(::Term{Real}, ::Term{Real}) at /home/henrique/.julia/packages/SymbolicUtils/HntGZ/src/ordering.jl:81
 [2] <ₑ(::Term{Real}, ::Term{Real}) at /home/henrique/.julia/packages/SymbolicUtils/HntGZ/src/ordering.jl:20
 [3] cmp_term_term(::SymbolicUtils.Mul{Real,Float64,Dict{Any,Number}}, ::SymbolicUtils.Mul{Real,Float64,Dict{Any,Number}}) at /home/henrique/.julia/packages/SymbolicUtils/HntGZ/src/ordering.jl:84
 [4] <ₑ(::SymbolicUtils.Mul{Real,Float64,Dict{Any,Number}}, ::SymbolicUtils.Mul{Real,Float64,Dict{Any,Number}}) at /home/henrique/.julia/packages/SymbolicUtils/HntGZ/src/ordering.jl:20
 [5] cmp_term_term(::SymbolicUtils.Add{Real,Int64,Dict{Any,Number}}, ::SymbolicUtils.Add{Real,Int64,Dict{Any,Number}}) at /home/henrique/.julia/packages/SymbolicUtils/HntGZ/src/ordering.jl:82
 ... (the last 4 lines are repeated 1 more time)
 [10] <ₑ(::SymbolicUtils.Add{Real,Int64,Dict{Any,Number}}, ::SymbolicUtils.Add{Real,Int64,Dict{Any,Number}}) at /home/henrique/.julia/packages/SymbolicUtils/HntGZ/src/ordering.jl:20
 [11] cmp_term_term(::SymbolicUtils.Mul{Real,Float64,Dict{Any,Number}}, ::SymbolicUtils.Mul{Real,Float64,Dict{Any,Number}}) at /home/henrique/.julia/packages/SymbolicUtils/HntGZ/src/ordering.jl:82
 ... (the last 2 lines are repeated 1 more time)
 ... (the last 12 lines are repeated 1 more time)
 [26] <ₑ(::SymbolicUtils.Add{Real,Int64,Dict{Any,Number}}, ::SymbolicUtils.Add{Real,Int64,Dict{Any,Number}}) at /home/henrique/.julia/packages/SymbolicUtils/HntGZ/src/ordering.jl:20
 [27] cmp_term_term(::SymbolicUtils.Mul{Real,Int64,Dict{Any,Number}}, ::SymbolicUtils.Mul{Real,Int64,Dict{Any,Number}}) at /home/henrique/.julia/packages/SymbolicUtils/HntGZ/src/ordering.jl:82
 [28] <ₑ(::SymbolicUtils.Mul{Real,Int64,Dict{Any,Number}}, ::SymbolicUtils.Mul{Real,Int64,Dict{Any,Number}}) at /home/henrique/.julia/packages/SymbolicUtils/HntGZ/src/ordering.jl:20
 [29] cmp_term_term(::SymbolicUtils.Add{Real,Int64,Dict{Any,Number}}, ::SymbolicUtils.Add{Real,Int64,Dict{Any,Number}}) at /home/henrique/.julia/packages/SymbolicUtils/HntGZ/src/ordering.jl:84
 ... (the last 2 lines are repeated 1 more time)
 [32] <ₑ(::SymbolicUtils.Mul{Real,Float64,Dict{Any,Number}}, ::SymbolicUtils.Mul{Real,Float64,Dict{Any,Number}}) at /home/henrique/.julia/packages/SymbolicUtils/HntGZ/src/ordering.jl:20
 [33] cmp_term_term(::SymbolicUtils.Add{Real,Int64,Dict{Any,Number}}, ::SymbolicUtils.Add{Real,Int64,Dict{Any,Number}}) at /home/henrique/.julia/packages/SymbolicUtils/HntGZ/src/ordering.jl:82
 ... (the last 4 lines are repeated 1 more time)
 [38] <ₑ(::SymbolicUtils.Add{Real,Int64,Dict{Any,Number}}, ::SymbolicUtils.Add{Real,Int64,Dict{Any,Number}}) at /home/henrique/.julia/packages/SymbolicUtils/HntGZ/src/ordering.jl:20
 [39] cmp_term_term(::SymbolicUtils.Mul{Real,Float64,Dict{Any,Number}}, ::SymbolicUtils.Mul{Real,Float64,Dict{Any,Number}}) at /home/henrique/.julia/packages/SymbolicUtils/HntGZ/src/ordering.jl:82
 ... (the last 2 lines are repeated 4 more times)
 [48] <ₑ(::Term{Real}, ::Term{Real}) at /home/henrique/.julia/packages/SymbolicUtils/HntGZ/src/ordering.jl:20
 [49] cmp_term_term(::SymbolicUtils.Mul{Real,Float64,Dict{Any,Number}}, ::SymbolicUtils.Mul{Real,Float64,Dict{Any,Number}}) at /home/henrique/.julia/packages/SymbolicUtils/HntGZ/src/ordering.jl:84
 [50] <ₑ at /home/henrique/.julia/packages/SymbolicUtils/HntGZ/src/ordering.jl:20 [inlined]
 [51] #1 at ./ordering.jl:73 [inlined]
 [52] lt at ./ordering.jl:60 [inlined]
 [53] sort!(::Array{SymbolicUtils.Mul{Real,Float64,Dict{Any,Number}},1}, ::Int64, ::Int64, ::Base.Sort.InsertionSortAlg, ::Base.Order.Lt{Base.Order.var"#1#3"{typeof(SymbolicUtils.:<ₑ),typeof(identity)}}) at ./sort.jl:498
 [54] sort!(::Array{SymbolicUtils.Mul{Real,Float64,Dict{Any,Number}},1}, ::Int64, ::Int64, ::Base.Sort.MergeSortAlg, ::Base.Order.Lt{Base.Order.var"#1#3"{typeof(SymbolicUtils.:<ₑ),typeof(identity)}}, ::Array{SymbolicUtils.Mul{Real,Float64,Dict{Any,Number}},1}) at ./sort.jl:583
 [55] sort! at ./sort.jl:582 [inlined]
 [56] sort! at ./sort.jl:673 [inlined]
 [57] #sort!#7 at ./sort.jl:733 [inlined]
 [58] arguments(::SymbolicUtils.Add{Real,Int64,Dict{Any,Number}}) at /home/henrique/.julia/packages/SymbolicUtils/HntGZ/src/types.jl:510
 [59] arglength(::SymbolicUtils.Add{Real,Int64,Dict{Any,Number}}) at /home/henrique/.julia/packages/SymbolicUtils/HntGZ/src/ordering.jl:11
 [60] cmp_term_term(::SymbolicUtils.Add{Real,Int64,Dict{Any,Number}}, ::SymbolicUtils.Add{Real,Int64,Dict{Any,Number}}) at /home/henrique/.julia/packages/SymbolicUtils/HntGZ/src/ordering.jl:51
 [61] <ₑ at /home/henrique/.julia/packages/SymbolicUtils/HntGZ/src/ordering.jl:20 [inlined]
 [62] #1 at ./ordering.jl:73 [inlined]
 [63] lt at ./ordering.jl:60 [inlined]
 [64] sort!(::Array{SymbolicUtils.Add{Real,Int64,Dict{Any,Number}},1}, ::Int64, ::Int64, ::Base.Sort.InsertionSortAlg, ::Base.Order.Lt{Base.Order.var"#1#3"{typeof(SymbolicUtils.:<ₑ),typeof(identity)}}) at ./sort.jl:498
 [65] sort!(::Array{SymbolicUtils.Add{Real,Int64,Dict{Any,Number}},1}, ::Int64, ::Int64, ::Base.Sort.MergeSortAlg, ::Base.Order.Lt{Base.Order.var"#1#3"{typeof(SymbolicUtils.:<ₑ),typeof(identity)}}, ::Array{SymbolicUtils.Add{Real,Int64,Dict{Any,Number}},1}) at ./sort.jl:583
 [66] sort! at ./sort.jl:582 [inlined]
 [67] sort! at ./sort.jl:673 [inlined]
 [68] #sort!#7 at ./sort.jl:733 [inlined]
 [69] arguments(::SymbolicUtils.Mul{Real,Int64,Dict{Any,Number}}) at /home/henrique/.julia/packages/SymbolicUtils/HntGZ/src/types.jl:637
 [70] arglength(::SymbolicUtils.Mul{Real,Int64,Dict{Any,Number}}) at /home/henrique/.julia/packages/SymbolicUtils/HntGZ/src/ordering.jl:11
 [71] cmp_term_term(::SymbolicUtils.Mul{Real,Int64,Dict{Any,Number}}, ::SymbolicUtils.Mul{Real,Int64,Dict{Any,Number}}) at /home/henrique/.julia/packages/SymbolicUtils/HntGZ/src/ordering.jl:50
 [72] <ₑ at /home/henrique/.julia/packages/SymbolicUtils/HntGZ/src/ordering.jl:20 [inlined]
 [73] #1 at ./ordering.jl:73 [inlined]
 [74] lt at ./ordering.jl:60 [inlined]
 [75] sort!(::Array{SymbolicUtils.Mul{Real,Int64,Dict{Any,Number}},1}, ::Int64, ::Int64, ::Base.Sort.InsertionSortAlg, ::Base.Order.Lt{Base.Order.var"#1#3"{typeof(SymbolicUtils.:<ₑ),typeof(identity)}}) at ./sort.jl:498
 [76] sort!(::Array{SymbolicUtils.Mul{Real,Int64,Dict{Any,Number}},1}, ::Int64, ::Int64, ::Base.Sort.MergeSortAlg, ::Base.Order.Lt{Base.Order.var"#1#3"{typeof(SymbolicUtils.:<ₑ),typeof(identity)}}, ::Array{SymbolicUtils.Mul{Real,Int64,Dict{Any,Number}},1}) at ./sort.jl:583
 [77] sort!(::Array{SymbolicUtils.Mul{Real,Int64,Dict{Any,Number}},1}, ::Int64, ::Int64, ::Base.Sort.MergeSortAlg, ::Base.Order.Lt{Base.Order.var"#1#3"{typeof(SymbolicUtils.:<ₑ),typeof(identity)}}, ::Array{SymbolicUtils.Mul{Real,Int64,Dict{Any,Number}},1}) at ./sort.jl:588 (repeats 2 times)
 [78] sort! at ./sort.jl:582 [inlined]
 [79] sort! at ./sort.jl:673 [inlined]
 [80] #sort!#7 at ./sort.jl:733 [inlined]
 [81] arguments(::SymbolicUtils.Add{Real,Int64,Dict{Any,Number}}) at /home/henrique/.julia/packages/SymbolicUtils/HntGZ/src/types.jl:510
 [82] isequal(::SymbolicUtils.Add{Real,Int64,Dict{Any,Number}}, ::Term{Real}) at /home/henrique/.julia/packages/SymbolicUtils/HntGZ/src/types.jl:65
 [83] ht_keyindex(::Dict{Any,Number}, ::SymbolicUtils.Add{Real,Int64,Dict{Any,Number}}) at ./dict.jl:289
 [84] get(::Dict{Any,Number}, ::SymbolicUtils.Add{Real,Int64,Dict{Any,Number}}, ::Int64) at ./dict.jl:490
 [85] makemul(::Int64, ::Term{Real}, ::Vararg{Any,N} where N; d::Dict{Any,Number}) at /home/henrique/.julia/packages/SymbolicUtils/HntGZ/src/types.jl:655
 [86] makemul at /home/henrique/.julia/packages/SymbolicUtils/HntGZ/src/types.jl:646 [inlined]
 [87] *(::Term{Real}, ::SymbolicUtils.Add{Real,Int64,Dict{Any,Number}}) at /home/henrique/.julia/packages/SymbolicUtils/HntGZ/src/types.jl:671
 [88] *(::Num, ::Num) at /home/henrique/.julia/packages/SymbolicUtils/HntGZ/src/methods.jl:41
 [89] macro expansion at /home/henrique/.julia/packages/StaticArrays/LJQEe/src/matrix_multiply.jl:50 [inlined]
 [90] _mul at /home/henrique/.julia/packages/StaticArrays/LJQEe/src/matrix_multiply.jl:36 [inlined]
 [91] * at /home/henrique/.julia/packages/StaticArrays/LJQEe/src/matrix_multiply.jl:8 [inlined]
 [92] *(::LinearAlgebra.Transpose{Num,RigidBodyDynamics.Spatial.GeometricJacobian{StaticArrays.SArray{Tuple{3,1},Num,2,3}}}, ::RigidBodyDynamics.Spatial.Wrench{Num}) at /home/henrique/.julia/packages/RigidBodyDynamics/Mf2gz/src/spatial/motion_force_interaction.jl:308
 [93] joint_wrenches_and_torques!(::RigidBodyDynamics.CustomCollections.SegmentedVector{RigidBodyDynamics.JointID,Num,Base.OneTo{RigidBodyDynamics.JointID},Array{Num,1}}, ::RigidBodyDynamics.CustomCollections.IndexDict{RigidBodyDynamics.BodyID,Base.OneTo{RigidBodyDynamics.BodyID},RigidBodyDynamics.Spatial.Wrench{Num}}, ::RigidBodyDynamics.MechanismState{Num,Float64,Num,TypeSortedCollections.TypeSortedCollection{Tuple{Array{RigidBodyDynamics.Joint{Float64,RigidBodyDynamics.Revolute{Float64}},1}},1}}) at /home/henrique/.julia/packages/RigidBodyDynamics/Mf2gz/src/mechanism_algorithms.jl:456
 [94] inverse_dynamics! at /home/henrique/.julia/packages/RigidBodyDynamics/Mf2gz/src/mechanism_algorithms.jl:552 [inlined]
 [95] inverse_dynamics!(::RigidBodyDynamics.CustomCollections.SegmentedVector{RigidBodyDynamics.JointID,Num,Base.OneTo{RigidBodyDynamics.JointID},Array{Num,1}}, ::RigidBodyDynamics.CustomCollections.IndexDict{RigidBodyDynamics.BodyID,Base.OneTo{RigidBodyDynamics.BodyID},RigidBodyDynamics.Spatial.Wrench{Num}}, ::RigidBodyDynamics.CustomCollections.IndexDict{RigidBodyDynamics.BodyID,Base.OneTo{RigidBodyDynamics.BodyID},RigidBodyDynamics.Spatial.SpatialAcceleration{Num}}, ::RigidBodyDynamics.MechanismState{Num,Float64,Num,TypeSortedCollections.TypeSortedCollection{Tuple{Array{RigidBodyDynamics.Joint{Float64,RigidBodyDynamics.Revolute{Float64}},1}},1}}, ::RigidBodyDynamics.CustomCollections.SegmentedVector{RigidBodyDynamics.JointID,Num,Base.OneTo{RigidBodyDynamics.JointID},Array{Num,1}}) at /home/henrique/.julia/packages/RigidBodyDynamics/Mf2gz/src/mechanism_algorithms.jl:549
 [96] inverse_dynamics_defects!(::Array{Num,1}, ::Robot, ::Array{Num,1}, ::Float64) at /home/henrique/git/TORA.jl/src/constraints/dynamics.jl:76
 [97] f!(::Array{Num,1}, ::Array{Num,1}) at ./In[4]:1
 [98] top-level scope at timing.jl:174

[ChrisRackauckas]

Oh you're trying to do a whole RigidBodyDynamics.jl equation? That's a good use case. Can you share your example code so we could look through it?

[ferrolho]

Hi, @ChrisRackauckas! Yes, I am trying to get the Hessian of the Lagrangian for the dynamics of robot systems. I am very happy to share the code. It is pretty much what I shared in the first post of this thread, except now it has that added snippet suggested by Yingbo. So, here's what it looks like:

using MeshCat
using ModelingToolkit
using TORA

vis = Visualizer()
robot = create_robot_kuka_iiwa_14(vis)

f!(dx, x) = TORA.inverse_dynamics_defects!(dx, robot, x, 1e-2)
# f!(dx, x) = TORA.forward_dynamics_defects!(dx, robot, x, 1e-2)  # Could also be forward dynamics

n_out = robot.n_q + robot.n_v  # next state (from integration)
n_in = robot.n_q + robot.n_v + robot.n_τ +  # current state + current controls,
       robot.n_q + robot.n_v                # and next state (from discretisation)

let  # this works
    x = rand(n_in)
    dx = zeros(n_out)
    f!(dx, x)
end

# @YingboMa's snippet
using ModelingToolkit.SymbolicUtils
using SymbolicUtils: Symbolic
Base.sincos(x::Union{Symbolic,Num}) = sin(x), cos(x)

@time let  # I don't know if this works or not, but I know it was still running after 12 hours
    @variables dx[1:n_out] x[1:n_in]
    f!(dx, x)
end

And for a bit of context, I am trying to do this for TORA.jl which, as you noticed, relies heavily on RigidBodyDynamics.jl. And here's a link to the relevant function TORA.inverse_dynamics_defects!.

[YingboMa]

I am not able to use the latest MTK with MeshCat and TORA.