Initial implement for Featherstone algorithm.