Double Pendulum using Multibody Dynamics Logic

The double pendulum in this code is modelled using formulations from Multibody Dynamics

Both Pendulums are considered as rigid bodies, with reference coordinate systems attached to the centre of masses of each link.

The problem is constructed using what is known as the Augmented Formulation (see Computational Dynamics by Ahmed Shabana). This formulation yields DAEs which can be solved using ode23t.