P step is numerically correct. Need to add some kinematic constraint on the distance between CoM and the contact points, so that the contact position makes sense