Implement PositiveSemiDefinite constraint for CVXOPT

1 files changed, 23 insertions, 1 deletions

diff --git a/sumofsquares/solver/cvxopt.py b/sumofsquares/solver/cvxopt.py
index 4534a8c..a608546 100644
--- a/sumofsquares/solver/cvxopt.py
+++ b/sumofsquares/solver/cvxopt.py
@@ -88,7 +88,29 @@ def solve_sos_cone(prob: Problem, verbose: bool = False,
     x = poly.v_stack(prob.polynomial_variables)
     for c in prob.constraints:
         if isinstance(c, PositiveSemiDefinite):
-            raise NotImplementedError
+            constr = poly.to_affine(c.expression.read(prob.state))
+
+            nrows, ncols = constr.shape
+            if nrows != ncols:
+                raise ValueError(f"PSD constraint cannot contain non-square matrix of shape ({nrows, ncols})!")
+
+            s_dims.append(nrows)
+
+            # constant term
+            c_coeff = constr.affine_coefficient(MonomialIndex.constant())
+            c_stacked = c_coeff.T.reshape((nrows * ncols,))
+
+            # linear terms
+            l_stacked_rows = []
+            for v in variable_indices:
+                v_coeff = constr.affine_coefficient(v)
+                v_stacked = v_coeff.T.reshape((nrows * ncols,))
+                l_stacked_rows.append(v_stacked)
+
+            l_stacked = np.vstack(l_stacked_rows)
+
+            h_rows.append(c_stacked)
+            G_rows.append(-1 * l_stacked)
 
         elif isinstance(c, ExponentialCone):
             raise NotSupportedBySolver("CVXOpt cannot solve problems with the exponential cone.")