Initial work for Featherstone forward dynamics.