Validation code is fixed in BodyNode.cpp for debug mode.
Use Featherstone forward dynamics algorithm.