Featherstone, Roy: Rigid Body Dynamics Algorithms