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

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 
JointID,Float64,Base.OneTo{JointID},Array{Float64,1}},Array{Float64,1}}}, ::Float64, ::Float64)

Checking the type:

julia> typeof(mk_integrator) <: RigidBodyDynamics.MuntheKaasIntegrator

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

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.ṡ)

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.

1 Like

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.

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.

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.