Fixed bug in updateForceDynamics