FIx a bug in CoM calculation