Based on definitions in chapter 2 of the book: Featherstone, R. (2008). Rigid body dynamics algorithms. Springer. This work is sponsored by Carnegie Wave Energy Ltd