RigidBodyDynamics, Munthe-Kaas integrator, step(integrator, t, Δt)


#1

I want to set up a stepwise Munthe-Kass integrator for use in a real-time control system. I do:

t = 0.0; Δt = 1e-4
ringBuff2 = RigidBodyDynamics.OdeIntegrators.RingBufferStorage{Float64}(state, 10000)
butcherTableau = RigidBodyDynamics.OdeIntegrators.runge_kutta_4(Float64)
mk_integrator = RigidBodyDynamics.OdeIntegrators.MuntheKaasIntegrator(state, dynamics!, butcherTableau, ringBuff2)

everything is fine until I do this:

step(mk_integrator, t, Δt)

when I get this:

ERROR: MethodError: no method matching 
step(::RigidBodyDynamics.OdeIntegrators.MuntheKaasIntegrator{4,Float64,typeof(dynamics!),RigidBo
dyDynamics.OdeIntegrators.RingBufferStorage{Float64,SegmentedVector{JointID,Float64,Base.OneTo
{JointID},Array{Float64,1}},SegmentedVector{JointID,Float64,Base.OneTo{JointID},Array{Float64,1}}},
MechanismState{Float64,Float64,Float64,TypeSortedCollections.TypeSortedCollection{Tuple{Array{Joi
nt{Float64,Revolute{Float64}},1}},1}},16,RigidBodyDynamics.OdeIntegrators.MuntheKaasStageCache{
4,Float64,SegmentedVector{JointID,Float64,Base.OneTo{JointID},Array{Float64,1}},SegmentedVector{
JointID,Float64,Base.OneTo{JointID},Array{Float64,1}},Array{Float64,1}}}, ::Float64, ::Float64)

Checking the type:

julia> typeof(mk_integrator) <: RigidBodyDynamics.MuntheKaasIntegrator
true

I can’t see where I have deviated from the documentation. (I am looking forward to reading Twan’s @tkoolen PhD thesis as I hope I will get a better understanding of what I’m doing!)


#2

This is an easy one; step is not exported from RigidBodyDynamics, so you need RigidBodyDynamics.OdeIntegrators.step. Also note that step is exported from Base, so you’ll need to be explicit.

You’ll also need to create a dynamics function that’s compatible with the interface that MuntheKaasIntegrator expects.

The following should work:

using RigidBodyDynamics
using RigidBodyDynamics.OdeIntegrators

passive_dynamics! = function (vd::AbstractArray, sd::AbstractArray, t, state)
    dynamics!(result, state)
    copyto!(vd, result.v̇)
    copyto!(sd, result.ṡ)
    nothing
end

const T = Float64

mechanism = rand_tree_mechanism(T,
    QuaternionFloating{T}, [Revolute{T} for i = 1 : 5]...)
state = MechanismState(mechanism)
result = DynamicsResult(mechanism)

t = 0.0; Δt = 1e-4
ringBuff2 = RingBufferStorage{T}(state, 10000)
butcherTableau = runge_kutta_4(T)
mk_integrator = MuntheKaasIntegrator(
    state, passive_dynamics!, butcherTableau, ringBuff2)

OdeIntegrators.step(mk_integrator, t, Δt)

Do note that RigidBodySim is supposed to be the more user-friendly and full-featured simulator, although it unfortunately doesn’t have a Munthe-Kaas integrator. If your mechanism has e.g. QuaternionFloating or QuaternionSpherical joints, you can use configuration_renormalizer(http://www.juliarobotics.org/RigidBodySim.jl/stable/details/#RigidBodySim.Core.configuration_renormalizer) to stay on the configuration manifold. Although it’s a cruder method, it gets the job done.

I’m afraid this integrator will most likely not be part of my thesis.


#3

I am forever in your debt! Thank you. (And how stupid of me to qualify everything with the exception of the one function I was having trouble with! Mind gone blank again.)

And thank you for writing passive_dynamics! for me. I was trying out my own callbacks but you have saved me a lot of hard thinking.

BTW: I am interested in the Munthe-Kaas integrator because later on I will set the problem up as a Hamiltonian (since you so conveniently provide momentum out of the box) as I would like to end up with some port Hamiltonian systems models and passivity control. PHS are beautifully modular.


#4

If something isn’t covered in the notebooks or documentation, the tests are often a good way to find out how stuff is supposed to work. E.g.

I did dig up a short group presentation I did on integrators for manifolds if you’re interested: https://www.dropbox.com/s/vmtt4ryvcm4rqym/lie-group-methods.pdf?dl=0.


#5

Again, many thanks for the tip! Also thank you for your excellent presentation; I am reading a number of papers, by Iserles, Munthe-Kaas etc. and every new angle helps enormously. I see you are a TikZ/Beamer master as well. Awesome.