Add soft constraints penalty function (not fully tested)