add a CoMMatchConstraint to be used in CoMPlanning
add a CoMPlanning object based on InverseKinTraj. Add the constraint on euler integration. Will need to work on the ContactWrenchConstraint. Also, comddot may not need to be a decision variable