Fix bug getCOM bug 

The getCOM returned the position of the com always in world frame, thx @EnricoMingo for catching this bug.