Add newton law into CoMPlanning. Need to add friction cone constraint later