Updated mobile robot controller to use inverse dynamics