Add more tests for construct_search_clf_cbf_program

tests/test_clf_cbf.py

@@ -9,6 +9,7 @@
 import pydrake.symbolic as sym
 
 import compatible_clf_cbf.utils as utils
+import tests.test_utils as test_utils
 
 
 class TestCompatibleLagrangianDegrees(object):
@@ -480,3 +481,10 @@ def test_search_clf_cbf(self):
         env = {self.x[i]: 0 for i in range(self.nx)}
         assert V_result.Evaluate(env) == 0
         assert sym.Monomial() not in V.monomial_to_coefficient_map()
+        assert test_utils.is_sos(V_result, solvers.ClarabelSolver.id())
+        assert V_result.TotalDegree() == 2
+        rho_result = result.GetSolution(rho)
+        assert rho_result >= 0
+
+        b_result = np.array([result.GetSolution(b[i]) for i in range(b.size)])
+        assert all([b_result[i].TotalDegree() <= 2 for i in range(b.size)])

tests/test_utils.py

@@ -1,5 +1,7 @@
 import compatible_clf_cbf.utils as mut
 
+from typing import Optional
+
 import numpy as np
 import pytest  # noqa
 
@@ -33,3 +35,20 @@ def tester(lower):
 
     tester(2.0)
     tester(3.0)
+
+
+def is_sos(
+    poly: sym.Polynomial,
+    solver_id: Optional[solvers.SolverId] = None,
+    solver_options: Optional[solvers.SolverOptions] = None,
+):
+    prog = solvers.MathematicalProgram()
+    prog.AddIndeterminates(poly.indeterminates())
+    assert poly.decision_variables().empty()
+    prog.AddSosConstraint(poly)
+    if solver_id is None:
+        result = solvers.Solve(prog, None, solver_options)
+    else:
+        solver = solvers.MakeSolver(solver_id)
+        result = solver.Solve(prog, None, solver_options)
+    return result.is_success()