Add a SimpleCentroidalTorqueConstraint to enforce the total torque at CoM being zero for the CoM model