1.add ContactWrenchConstraint into CoMPlanning.
2.add CentroidalTorque in ContactWrenchConstraint to compute torque about the center of mass