Add AngularMomentumCost class to compute the PD residue cost as a nonlinear function from force and CoM/foot_pos. This class is going to be used to solve the NLP version of dynamics planning with linear constraints only