Fix bug in sign of derivative terms of inverse dynamics ctrl law