Fix forward dynamics algorithm bug in BodyNode::updateAcceleration