Reformulate the containment condition. (#30)

The previous psats has -1 constant, which isn't necessary when we must fix some Lagrangians.

compatible_clf_cbf/utils.py

@@ -133,8 +133,8 @@ def add_log_det_lower(
 class ContainmentLagrangian:
     """
     To certify that an algebraic set { x | f(x) <= 0} is contained in another
-    algebraic set {x | g(x) < 0}, we impose the condition
-    -1 - ϕ₁(x))g(x) + ϕ₂(x)f(x) is sos
+    algebraic set {x | g(x) <= 0}, we impose the condition
+    -(1 + ϕ₁(x))g(x) + ϕ₂(x)f(x) is sos
     ϕ₁(x) is sos, ϕ₂(x) is sos
     """
 
@@ -147,7 +147,7 @@ def add_constraint(
         self, prog, inner_poly: sym.Polynomial, outer_poly: sym.Polynomial
     ) -> Tuple[sym.Polynomial, np.ndarray]:
         return prog.AddSosConstraint(
-            -1 - self.outer * outer_poly + self.inner * inner_poly
+            -(1 + self.outer) * outer_poly + self.inner * inner_poly
         )
 
 
@@ -175,7 +175,10 @@ def construct_lagrangian(
         else:
             inner_lagrangian, _ = prog.NewSosPolynomial(x, self.inner)
         if self.outer < 0:
-            outer_lagrangian = sym.Polynomial(1)
+            # The Lagrangian multiply with the outer set is
+            # (1 + outer_lagrangian), hence we can set outer_lagrangian = 0,
+            # such that the multiplied Lagrangian is 1.
+            outer_lagrangian = sym.Polynomial(0)
         elif self.outer == 0:
             outer_lagrangian_var = prog.NewContinuousVariables(1)[0]
             prog.AddBoundingBoxConstraint(0, np.inf, outer_lagrangian_var)

tests/test_clf_cbf.py

@@ -533,8 +533,7 @@ def search_lagrangians(
         )
         solver_options = solvers.SolverOptions()
         solver_options.SetOption(solvers.CommonSolverOption.kPrintToConsole, 1)
-        solver = solvers.ClarabelSolver()
-        compatible_result = solver.Solve(compatible_prog, None, solver_options)
+        compatible_result = solvers.Solve(compatible_prog, None, solver_options)
         assert compatible_result.is_success()
 
         compatible_lagrangians_result = compatible_lagrangians.get_result(

tests/test_utils.py

@@ -38,3 +38,41 @@ def tester(lower):
 def test_to_lower_triangular_columns():
     vec = mut.to_lower_triangular_columns(np.array([[1, 2, 3], [4, 5, 6], [7, 8, 9]]))
     np.testing.assert_equal(vec, np.array([1, 4, 7, 5, 8, 9]))
+
+
+class TestContainmentLagrangian:
+    @pytest.mark.parametrize("inner_degree,outer_degree", [(0, 0), (0, -1), (-1, 0)])
+    def test_add_constraint1(self, inner_degree, outer_degree):
+        x = sym.MakeVectorContinuousVariable(2, "x")
+        x_set = sym.Variables(x)
+        inner_poly = sym.Polynomial(x[0] ** 2 + x[1] ** 2 - 1)
+        outer_poly = sym.Polynomial(x[0] ** 2 + x[1] ** 2 - 1.01)
+        prog = solvers.MathematicalProgram()
+        prog.AddIndeterminates(x_set)
+        containment_lagrangian_degree = mut.ContainmentLagrangianDegree(
+            inner=inner_degree, outer=outer_degree
+        )
+        lagrangian = containment_lagrangian_degree.construct_lagrangian(prog, x_set)
+        dut = mut.ContainmentLagrangian(inner=lagrangian.inner, outer=lagrangian.outer)
+        dut.add_constraint(prog, inner_poly, outer_poly)
+        result = solvers.Solve(prog)
+        assert result.is_success()
+
+    @pytest.mark.parametrize("inner_degree,outer_degree", [(0, 0), (0, -1), (-1, 0)])
+    def test_add_constraint2(self, inner_degree, outer_degree):
+        # The inner_poly sub-level set is larger than the outer_poly sub-level
+        # set. The program should fail.
+        x = sym.MakeVectorContinuousVariable(2, "x")
+        x_set = sym.Variables(x)
+        inner_poly = sym.Polynomial(x[0] ** 2 + x[1] ** 2 - 1.01)
+        outer_poly = sym.Polynomial(x[0] ** 2 + x[1] ** 2 - 1.0)
+        prog = solvers.MathematicalProgram()
+        prog.AddIndeterminates(x_set)
+        containment_lagrangian_degree = mut.ContainmentLagrangianDegree(
+            inner=inner_degree, outer=outer_degree
+        )
+        lagrangian = containment_lagrangian_degree.construct_lagrangian(prog, x_set)
+        dut = mut.ContainmentLagrangian(inner=lagrangian.inner, outer=lagrangian.outer)
+        dut.add_constraint(prog, inner_poly, outer_poly)
+        result = solvers.Solve(prog)
+        assert not result.is_success()