Fix SoftBodyNode::updateBodyForce()
Update testEquationOfMotion.cpp
- Passed unit test for calculating of M, M^{-1}, C, C+g comparing recursive method and inverse dynamics method