Allow constraints to be slack

Author: timothy-nunn

src/pyvmcon/vmcon.py

@@ -264,6 +264,8 @@ def solve_qsp(
     lbs: VectorType | None,
     ubs: VectorType | None,
     options: dict[str, Any],
+    *,
+    allow_slack_constraints: bool = False,
 ) -> tuple[VectorType, VectorType, VectorType]:
     """Solves the quadratic programming problem.
 
@@ -313,17 +315,41 @@ def solve_qsp(
             ubs - x if ubs is not None else None,
         ],
     )
-    problem_statement = cp.Minimize(
-        result.f + (0.5 * cp.quad_form(delta, B)) + (delta.T @ result.df),
+
+    num_constraints = problem.num_equality + problem.num_inequality
+    ksi = cp.Variable(num_constraints or 1, bounds=[0.0, 1.0])
+
+    minimise_expression = (
+        result.f + (0.5 * cp.quad_form(delta, B)) + (delta.T @ result.df)
     )
+    if allow_slack_constraints:
+        minimise_expression -= cp.sum(ksi)
 
     constraints = []
-    if problem.has_inequality:
-        constraints.append((result.die @ delta) + result.ie >= 0)
-    if problem.has_equality:
-        constraints.append((result.deq @ delta) + result.eq == 0)
+    for con_idx in range(problem.num_equality):
+        if allow_slack_constraints:
+            constraints.append(
+                (result.deq[con_idx, :] @ delta) + result.eq[con_idx] * ksi[con_idx]
+                == 0
+            )
+        else:
+            constraints.append(
+                (result.deq[con_idx, :] @ delta) + result.eq[con_idx] == 0
+            )
 
-    qsp = cp.Problem(problem_statement, constraints or None)
+    for con_idx in range(problem.num_inequality):
+        if result.ie[con_idx] <= 0 and allow_slack_constraints:
+            constraints.append(
+                (result.die[con_idx, :] @ delta)
+                + result.ie[con_idx] * ksi[problem.num_equality + con_idx]
+                >= 0
+            )
+        else:
+            constraints.append(
+                (result.die[con_idx, :] @ delta) + result.ie[con_idx] >= 0
+            )
+
+    qsp = cp.Problem(cp.Minimize(minimise_expression), constraints or None)
 
     try:
         qsp.solve(**{"solver": cp.OSQP, **options})
@@ -335,18 +361,17 @@ def solve_qsp(
         error_msg = f"QSP failed to solve: {qsp.status}"
         raise _QspSolveException(error_msg)
 
-    lamda_equality = np.array([])
-    lamda_inequality = np.array([])
-
-    if problem.has_inequality and problem.has_equality:
-        lamda_inequality = qsp.constraints[0].dual_value
-        lamda_equality = -qsp.constraints[1].dual_value
-
-    elif problem.has_inequality and not problem.has_equality:
-        lamda_inequality = qsp.constraints[0].dual_value
-
-    elif not problem.has_inequality and problem.has_equality:
-        lamda_equality = -qsp.constraints[0].dual_value
+    lamda_equality = np.array(
+        [-i.dual_value for i in qsp.constraints[: problem.num_equality]]
+    )
+    lamda_inequality = np.array(
+        [
+            i.dual_value
+            for i in qsp.constraints[
+                problem.num_equality : problem.num_equality + problem.num_inequality
+            ]
+        ]
+    )
 
     return delta.value, lamda_equality, lamda_inequality