RigidBodyDynamics.jl could actually also be useful for the ‘maximal coordinates’ type of approach used in the paper you referenced. Continuing the cart-pole example:
urdf = download("https://raw.githubusercontent.com/bulletphysics/bullet3/0e1dce41eab75fd210ec73a52adbf249710c8edf/data/cartpole.urdf", "cartpole.urdf")
using RigidBodyDynamics
cartpole = parse_urdf(Float64, urdf)
cartpole_maxcoord, _ = maximal_coordinates(cartpole)
which results in a version of the cart-pole with a flat tree structure (each body has a quaternion-parameterized floating joint connecting it directly to the world) and a bunch of non-tree joints that restrict the motion between the bodies:
Spanning tree:
Vertex: world (root)
Vertex: slideBar, Edge: slideBar
Vertex: cart, Edge: cart
Vertex: pole, Edge: pole
Non-tree joints:
slideBar_to_world, predecessor: world, successor: slideBar
slider_to_cart, predecessor: slideBar, successor: cart
cart_to_pole, predecessor: cart, successor: pole
after which it’s pretty easy to compute the constraint Jacobian needed in the approach from your paper:
state = MechanismState(cartpole_maxcoord)
rand!(state)
result = DynamicsResult(cartpole_maxcoord)
RigidBodyDynamics.constraint_jacobian!(result, state) # currently not exported, but could be
This updates the constraintjacobian field of DynamicsResult, which is (at least part of) the J you need. constraint_jacobian! is reasonably fast and it doesn’t allocate. It’s automatically used internally in the dynamics! function for Mechanisms with non-tree (loop) joints.
Just a suggestion of course, I understand if you want to write it yourself.