search

SciML / ModelingToolkit.jl

An acausal modeling framework for automatically parallelized scientific machine learning (SciML) in Julia. A computer algebra system for integrated symbolics for physics-informed machine learning and automated transformations of differential equations
https://mtk.sciml.ai/dev/
Other
1.43k stars 207 forks source link

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

Closed ferrolho closed 3 years ago

ferrolho commented 3 years ago

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... :thinking: 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 commented 3 years ago

That looks like a very old version of MTK.

ferrolho commented 3 years ago

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

ferrolho commented 3 years ago

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 commented 3 years ago

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 commented 3 years ago

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

YingboMa commented 3 years ago

Yes, that's a typo.

ferrolho commented 3 years ago

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 commented 3 years ago

Ah, it should be

using ModelingToolkit.SymbolicUtils
using SymbolicUtils: Symbolic
Base.sincos(x::Union{Symbolic,Num}) = sin(x), cos(x)
ferrolho commented 3 years ago

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 commented 3 years ago

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

image

ferrolho commented 3 years ago

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 commented 3 years ago

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 commented 3 years ago

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 commented 3 years ago

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

ferrolho commented 3 years ago

Ah, yes... Sorry about that. The dependencies are being held because of a PR that needs to be merged to MeshCatMechanisms.jl. Can you try to add the fork below? The remaining dependencies should then resolve to their latest versions.

add https://github.com/ferrolho/MeshCatMechanisms.jl

Edit: Actually, I think the latest TORA.jl release does not resolve MT.jl to the latest version. Please use the branch hf/exact-hessians.

ferrolho commented 3 years ago

Just a quick update on my previous comment: that PR I mentioned has now been merged, so ]add MeshCatMechanisms should work again. Please continue to use the branch hf/exact-hessians for TORA.jl, though.

ChrisRackauckas commented 3 years ago

What's the status on this? What can be run and how?

ferrolho commented 3 years ago

Hi, Chris! The status did not change. I still get an error message with the latest MTK.jl (v5.12.1).

How to reproduce this:

pkg> add MeshCat ModelingToolkit SymbolicUtils
pkg> add https://github.com/JuliaRobotics/TORA.jl#hf/exact-hessians
julia> using MeshCat
       using ModelingToolkit
       using TORA

julia> vis = Visualizer()
       robot = create_robot_kuka_iiwa_14(vis)

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

julia> 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)

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

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

Here is the output:

ERROR: MethodError: no method matching AbstractFloat(::Num)
Closest candidates are:
  (::Type{T})(::Real, ::RoundingMode) where T<:AbstractFloat at rounding.jl:200
  (::Type{T})(::T) where T<:Number at boot.jl:760
  (::Type{T})(::AbstractChar) where T<:Union{AbstractChar, Number} at char.jl:50
  ...
Stacktrace:
  [1] float(x::Num)
    @ Base ./float.jl:206
  [2] sincos(x::Num)
    @ Base.Math ./special/trig.jl:203
  [3] Tuple
    @ ~/.julia/packages/Rotations/QaTBi/src/angleaxis_types.jl:52 [inlined]
  [4] convert
    @ ~/.julia/packages/StaticArrays/LJQEe/src/convert.jl:10 [inlined]
  [5] Rotations.RotMatrix3{Num}(x::Rotations.AngleAxis{Num})
    @ Rotations ~/.julia/packages/Rotations/QaTBi/src/core_types.jl:83
  [6] convert(#unused#::Type{Rotations.RotMatrix3{Num}}, rot::Rotations.AngleAxis{Num})
    @ Rotations ~/.julia/packages/Rotations/QaTBi/src/core_types.jl:20
  [7] joint_transform
    @ ~/.julia/packages/RigidBodyDynamics/FM89V/src/joint_types/revolute.jl:61 [inlined]
  [8] joint_transform
    @ ~/.julia/packages/RigidBodyDynamics/FM89V/src/joint.jl:191 [inlined]
  [9] macro expansion
    @ ~/.julia/packages/TypeSortedCollections/Z4ytl/src/TypeSortedCollections.jl:173 [inlined]
 [10] map!
    @ ~/.julia/packages/TypeSortedCollections/Z4ytl/src/TypeSortedCollections.jl:164 [inlined]
 [11] _update_transforms!(state::RigidBodyDynamics.MechanismState{Num, Float64, Num, TypeSortedCollections.TypeSortedCollection{Tuple{Vector{RigidBodyDynamics.Joint{Float64, RigidBodyDynamics.Revolute{Float64}}}}, 1}})
    @ RigidBodyDynamics ~/.julia/packages/RigidBodyDynamics/FM89V/src/mechanism_state.jl:692
 [12] update_transforms!
    @ ~/.julia/packages/RigidBodyDynamics/FM89V/src/mechanism_state.jl:686 [inlined]
 [13] spatial_accelerations!(out::RigidBodyDynamics.CustomCollections.IndexDict{RigidBodyDynamics.BodyID, Base.OneTo{RigidBodyDynamics.BodyID}, RigidBodyDynamics.Spatial.SpatialAcceleration{Num}}, state::RigidBodyDynamics.MechanismState{Num, Float64, Num, TypeSortedCollections.TypeSortedCollection{Tuple{Vector{RigidBodyDynamics.Joint{Float64, RigidBodyDynamics.Revolute{Float64}}}}, 1}}, v̇::RigidBodyDynamics.CustomCollections.SegmentedVector{RigidBodyDynamics.JointID, Num, Base.OneTo{RigidBodyDynamics.JointID}, Vector{Num}})
    @ RigidBodyDynamics ~/.julia/packages/RigidBodyDynamics/FM89V/src/mechanism_algorithms.jl:388
 [14] inverse_dynamics!
    @ ~/.julia/packages/RigidBodyDynamics/FM89V/src/mechanism_algorithms.jl:550 [inlined]
 [15] inverse_dynamics!(torquesout::RigidBodyDynamics.CustomCollections.SegmentedVector{RigidBodyDynamics.JointID, Num, Base.OneTo{RigidBodyDynamics.JointID}, Vector{Num}}, jointwrenchesout::RigidBodyDynamics.CustomCollections.IndexDict{RigidBodyDynamics.BodyID, Base.OneTo{RigidBodyDynamics.BodyID}, RigidBodyDynamics.Spatial.Wrench{Num}}, accelerations::RigidBodyDynamics.CustomCollections.IndexDict{RigidBodyDynamics.BodyID, Base.OneTo{RigidBodyDynamics.BodyID}, RigidBodyDynamics.Spatial.SpatialAcceleration{Num}}, state::RigidBodyDynamics.MechanismState{Num, Float64, Num, TypeSortedCollections.TypeSortedCollection{Tuple{Vector{RigidBodyDynamics.Joint{Float64, RigidBodyDynamics.Revolute{Float64}}}}, 1}}, v̇::RigidBodyDynamics.CustomCollections.SegmentedVector{RigidBodyDynamics.JointID, Num, Base.OneTo{RigidBodyDynamics.JointID}, Vector{Num}})
    @ RigidBodyDynamics ~/.julia/packages/RigidBodyDynamics/FM89V/src/mechanism_algorithms.jl:549
 [16] inverse_dynamics_defects!(defects::Vector{Num}, robot::Robot, x::Vector{Num}, h::Float64)
    @ TORA ~/.julia/packages/TORA/zDLko/src/constraints/dynamics.jl:76
 [17] f!(dx::Vector{Num}, x::Vector{Num})
    @ Main ./REPL[9]:1
 [18] top-level scope
    @ REPL[13]:3

Then, if we run the snippet suggested by Yingbo,

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

and give it another try,

julia> let
           @variables dx[1:n_out] x[1:n_in]
           f!(dx, x)
       end

it does not error, but it seems to get stuck doing something (as in, the function does not terminate).

Let me know if more info is needed.

YingboMa commented 3 years ago

I think it's just because the function is too complex, so the size of the expression blows up. You should consider to register some functions so that it doesn't trace into those functions.

baggepinnen commented 3 years ago

If so, I guess the number of links of the robot can be reduced until the code works? It would be nice if working with the symbolic expressions for 6-7DOF robots was doable, sympy takes about a weekend for 6DOF so there's a benchmark to beat 😊

ferrolho commented 3 years ago

Hi! Following the advice above, I was testing robots with fewer degrees of freedom. However, I just noticed that this is not an issue anymore, at least after upgrading ModelingToolkit.jl from v5.12.1 to v5.13.3. I was able to run the original snippet (without even defining Base.sincos(x::Union{Symbolic,Num}) = sin(x), cos(x)) and it just works.

Here is a table of times (from @btime) for degrees of freedom from 1 to 7:

1 DoF:   2.627 ms (   20817 allocations: 815.45 KiB)
2 DoF:  18.248 ms (  119680 allocations:   3.98 MiB)
3 DoF: 102.982 ms (  732693 allocations:  22.75 MiB)
4 DoF: 237.223 ms ( 1307762 allocations:  38.01 MiB)
5 DoF: 848.787 ms ( 5206459 allocations: 152.01 MiB)
6 DoF:  10.259 s  (85000591 allocations:   2.46 GiB)
7 DoF:   7.792 s  (45706270 allocations:   1.27 GiB)
baggepinnen commented 3 years ago

Those are mighty impressive timings :+1: The reduction in time from 6 to 7 DOF is interesting..

ferrolho commented 3 years ago

How can I generate the function that evaluates the Hessian of f!? I suppose it is done with generate_hessian, but the examples in the documentation start by defining the equations of the system, e.g.,

eqs = [D(x) ~ σ*(y-x),
       D(y) ~ x*(ρ-z)-y,
       D(z) ~ x*y - β*z]

whereas I have an already-defined function f! (which we are now able to convert to a symbolic representation by having it called with arguments created by @variables).

YingboMa commented 3 years ago

This issue is fixed.

ferrolho commented 3 years ago

Hi, @YingboMa. Yes, the original issue is fixed─thanks! :smiley: Can you help me with my last comment above?

YingboMa commented 3 years ago

Is Hessian defined for a R^n -> R^n function?

ChrisRackauckas commented 3 years ago

I mean, there's a second derivative 3-tensor, but it's not a "Hessian". We can overload it to mean that anyways.

ferrolho commented 3 years ago

I believe the name for what I am looking for is the Jacobian of the extended gradient. Apologies for the confusion. Perhaps looking at the function hessian in the first post of this Discourse thread will clarify what output I am looking for.