Fix a bug in the AngularMomentumCost when computing the angular momentum. Now the angular momentum results from NLP does not drift. But the CoM height still changes unexpectedly