ssm_penalty_function: Calculates the penalized objective function in constrained problems

Description

This function is used internally by essR to calculate the penalized objective function in constrained problems