Add constraint on the end position of CoM