Removed joint anchor COM subtraction, seems to work now