WIP: search CLF and CBF given lagrangians, binary search the largest inner ellipsoid.

compatible_clf_cbf/clf_cbf.py

@@ -447,6 +447,16 @@ def search_clf_cbf_given_lagrangian(
         Optional[float],
         solvers.MathematicalProgramResult,
     ]:
+        """
+        Given the Lagrangian multipliers and an inner ellipsoid, find the clf
+        and cbf, such that the compatible region contains that inner ellipsoid.
+
+        Returns: (V, b, rho, result)
+          V: The CLF.
+          b: The CBF.
+          rho: The certified ROA is {x | V(x) <= rho}.
+          result: The result of the optimization program.
+        """
         prog, V, b, rho = self._construct_search_clf_cbf_program(
             compatible_lagrangians,
             unsafe_regions_lagrangians,
@@ -473,6 +483,134 @@ def search_clf_cbf_given_lagrangian(
             rho_sol = None
         return V_sol, b_sol, rho_sol, result
 
+    def binary_search_clf_cbf(
+        self,
+        compatible_lagrangians: CompatibleLagrangians,
+        unsafe_regions_lagrangians: List[UnsafeRegionLagrangians],
+        clf_degree: Optional[int],
+        cbf_degrees: List[int],
+        x_equilibrium: Optional[np.ndarray],
+        kappa_V: Optional[float],
+        kappa_b: np.ndarray,
+        barrier_eps: np.ndarray,
+        S_ellipsoid_inner: np.ndarray,
+        b_ellipsoid_inner: np.ndarray,
+        c_ellipsoid_inner: float,
+        scale_min: float,
+        scale_max: float,
+        scale_tol: float,
+        solver_id: Optional[solvers.SolverId] = None,
+        solver_options: Optional[solvers.SolverOptions] = None,
+    ) -> Tuple[Optional[sym.Polynomial], np.ndarray, Optional[float]]:
+        """
+        Given the Lagrangian multipliers, find the compatible CLF and CBFs,
+        with the goal to enlarge the compatible region.
+
+        We measure the size of the compatible region through binary searching
+        the inner ellipsoid. We scale the inner ellipsoid about its center,
+        and binary search on the scaling factor.
+
+        Args:
+          scale_min: The minimum of the ellipsoid scaling factor.
+          scale_max: The maximal of the ellipsoid scaling factor.
+          scale_tol: Terminate the binary search when the difference between
+            the max/min scaling factor is below this tolerance.
+
+        Return: (V, b, rho)
+        """
+
+        def search(
+            scale,
+        ) -> Tuple[
+            Optional[sym.Polynomial],
+            Optional[np.ndarray],
+            Optional[float],
+            solvers.MathematicalProgramResult,
+        ]:
+            c_new = ellipsoid_utils.scale_ellipsoid(
+                S_ellipsoid_inner, b_ellipsoid_inner, c_ellipsoid_inner, scale
+            )
+            V, b, rho, result = self.search_clf_cbf_given_lagrangian(
+                compatible_lagrangians,
+                unsafe_regions_lagrangians,
+                clf_degree,
+                cbf_degrees,
+                x_equilibrium,
+                kappa_V,
+                kappa_b,
+                barrier_eps,
+                S_ellipsoid_inner,
+                b_ellipsoid_inner,
+                c_new,
+                solver_id,
+                solver_options,
+            )
+            return V, b, rho, result
+
+        assert scale_max >= scale_min
+        assert scale_tol > 0
+        V, b, rho, result = search(scale_max)
+        if result.is_success():
+            print(f"binary_search_clf_cbf: scale={scale_max} is feasible.")
+            assert b is not None
+            return V, b, rho
+
+        V_success, b_success, rho_success, result = search(scale_min)
+        assert (
+            result.is_success()
+        ), f"binary_search_clf_cbf: scale_min={scale_min} is not feasible."
+        assert b_success is not None
+
+        while scale_max - scale_min > scale_tol:
+            scale = (scale_max + scale_min) / 2
+            V, b, rho, result = search(scale)
+            if result.is_success():
+                print(f"binary_search_clf_cbf: scale={scale} is feasible.")
+                scale_min = scale
+                V_success = V
+                assert b is not None
+                b_success = b
+                rho_success = rho
+            else:
+                print(f"binary_search_clf_cbf: scale={scale} is not feasible.")
+                scale_max = scale
+
+        return V_success, b_success, rho_success
+
+    def in_compatible_region(
+        self,
+        V: Optional[sym.Polynomial],
+        b: np.ndarray,
+        rho: Optional[float],
+        x_samples: np.ndarray,
+    ) -> np.ndarray:
+        """
+        Returns if x_samples[i] is in the compatible region
+        {x | V(x) <= rho, b(x) >= 0}.
+
+        Return:
+        in_compatible_flag: in_compatible_flag[i] is True iff x_samples[i] is
+          in the compatible region.
+        """
+        in_b = np.all(
+            np.concatenate(
+                [
+                    (b_i.EvaluateIndeterminates(self.x, x_samples.T) >= 0).reshape(
+                        (-1, 1)
+                    )
+                    for b_i in b
+                ],
+                axis=1,
+            ),
+            axis=1,
+        )
+        if V is not None:
+            assert rho is not None
+            in_V = V.EvaluateIndeterminates(self.x, x_samples.T) <= rho
+            return np.logical_and(in_b, in_V)
+        else:
+            return in_b
+
     def _calc_xi_Lambda(
         self,
         *,

tests/test_clf_cbf.py

@@ -451,15 +451,7 @@ def test_add_ellipsoid_in_compatible_region_constraint(self):
         V_sol = result.GetSolution(V)
         rho_sol = result.GetSolution(rho)
         b_sol = np.array([result.GetSolution(b_i) for b_i in b])
-        in_V = V_sol.EvaluateIndeterminates(self.x, x_samples.T) <= rho_sol
-        in_b = np.concatenate(
-            [
-                b_i.EvaluateIndeterminates(self.x, x_samples.T >= 0).reshape((1, -1))
-                for b_i in b_sol
-            ],
-            axis=0,
-        ).T
-        in_compatible = np.logical_and(np.all(in_b, axis=1), in_V)
+        in_compatible = dut.in_compatible_region(V_sol, b_sol, rho_sol, x_samples)
         assert np.all(in_compatible[in_ellipsoid])
 
 
@@ -489,12 +481,35 @@ def setup_class(cls):
         cls.kappa_b = np.array([cls.kappa_V])
         cls.barrier_eps = np.array([0.01])
 
+    def check_unsafe_region_by_sample(self, b: np.ndarray, x_samples):
+        # Sample many points, make sure that {x | b[i] >= 0} doesn't intersect
+        # with the i'th unsafe region.
+        for i, unsafe_region in enumerate(self.unsafe_regions):
+            unsafe_flag = np.all(
+                np.concatenate(
+                    [
+                        (
+                            unsafe_region_j.EvaluateIndeterminates(self.x, x_samples.T)
+                            <= 0
+                        ).reshape((-1, 1))
+                        for unsafe_region_j in unsafe_region
+                    ],
+                    axis=1,
+                ),
+                axis=1,
+            )
+            in_b = b[i].EvaluateIndeterminates(self.x, x_samples.T) >= 0
+            assert np.all(np.logical_not(unsafe_flag[in_b]))
+
     def search_lagrangians(
         self,
     ) -> Tuple[
         mut.CompatibleClfCbf,
         mut.CompatibleLagrangians,
         List[mut.UnsafeRegionLagrangians],
+        sym.Polynomial,
+        np.ndarray,
+        float,
     ]:
         use_y_squared = True
         dut = mut.CompatibleClfCbf(
@@ -512,7 +527,7 @@ def search_lagrangians(
 
         lagrangian_degrees = mut.CompatibleLagrangianDegrees(
             lambda_y=[mut.CompatibleLagrangianDegrees.Degree(x=3, y=0)],
-            xi_y=mut.CompatibleLagrangianDegrees.Degree(x=0, y=0),
+            xi_y=mut.CompatibleLagrangianDegrees.Degree(x=2, y=0),
             y=None,
             rho_minus_V=mut.CompatibleLagrangianDegrees.Degree(x=2, y=0),
             b_plus_eps=[mut.CompatibleLagrangianDegrees.Degree(x=2, y=0)],
@@ -532,7 +547,6 @@ def search_lagrangians(
             self.barrier_eps,
         )
         solver_options = solvers.SolverOptions()
-        solver_options.SetOption(solvers.CommonSolverOption.kPrintToConsole, 1)
         compatible_result = solvers.Solve(compatible_prog, None, solver_options)
         assert compatible_result.is_success()
 
@@ -550,13 +564,23 @@ def search_lagrangians(
             )
         ]
 
-        return dut, compatible_lagrangians_result, unsafe_lagrangians
+        return (
+            dut,
+            compatible_lagrangians_result,
+            unsafe_lagrangians,
+            V_init,
+            b_init,
+            rho,
+        )
 
-    def test_search_clf_cbf(self):
+    def test_construct_search_clf_cbf_program(self):
         (
             dut,
             compatible_lagrangians,
             unsafe_lagrangians,
+            _,
+            _,
+            _,
         ) = self.search_lagrangians()
         prog, V, b, rho = dut._construct_search_clf_cbf_program(
             compatible_lagrangians,
@@ -569,18 +593,136 @@ def test_search_clf_cbf(self):
             barrier_eps=self.barrier_eps,
         )
         solver_options = solvers.SolverOptions()
-        solver_options.SetOption(solvers.CommonSolverOption.kPrintToConsole, 1)
-        solver = solvers.ClarabelSolver()
-        result = solver.Solve(prog, None, solver_options)
+        solver_options.SetOption(solvers.CommonSolverOption.kPrintToConsole, 0)
+        result = solvers.Solve(prog, None, solver_options)
         assert result.is_success()
         V_result = result.GetSolution(V)
         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 utils.is_sos(V_result, solvers.ClarabelSolver.id())
+        assert utils.is_sos(V_result)
         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)])
+
+        # Sample many points, make sure that {x | b[i] >= 0} doesn't intersect
+        # with the i'th unsafe region.
+        x_samples = 10 * np.random.randn(1000, 2) - np.array([[10, 0]])
+        self.check_unsafe_region_by_sample(b_result, x_samples)
+
+    def test_search_clf_cbf_given_lagrangian(self):
+        (
+            dut,
+            compatible_lagrangians,
+            unsafe_lagrangians,
+            V,
+            b,
+            rho,
+        ) = self.search_lagrangians()
+
+        # Find the large ellipsoid inside the compatible region.
+        x_equilibrium = np.array([0.0, 0.0])
+        (
+            S_ellipsoid_inner,
+            b_ellipsoid_inner,
+            c_ellipsoid_inner,
+        ) = dut._find_max_inner_ellipsoid(
+            V,
+            b,
+            rho,
+            V_contain_lagrangian_degree=mut.ContainmentLagrangianDegree(
+                inner=-1, outer=0
+            ),
+            b_contain_lagrangian_degree=[
+                mut.ContainmentLagrangianDegree(inner=-1, outer=0)
+            ],
+            x_inner_init=x_equilibrium,
+            max_iter=10,
+            convergence_tol=1e-4,
+            trust_region=1000,
+        )
+
+        V_new, b_new, rho_new, result = dut.search_clf_cbf_given_lagrangian(
+            compatible_lagrangians,
+            unsafe_lagrangians,
+            clf_degree=2,
+            cbf_degrees=[2],
+            x_equilibrium=x_equilibrium,
+            kappa_V=self.kappa_V,
+            kappa_b=self.kappa_b,
+            barrier_eps=self.barrier_eps,
+            S_ellipsoid_inner=S_ellipsoid_inner,
+            b_ellipsoid_inner=b_ellipsoid_inner,
+            c_ellipsoid_inner=c_ellipsoid_inner,
+        )
+        assert result.is_success()
+        # Check that the compatible region contains the inner_ellipsoid.
+        x_samples = np.random.randn(100, 2)
+        in_ellipsoid = ellipsoid_utils.in_ellipsoid(
+            S_ellipsoid_inner, b_ellipsoid_inner, c_ellipsoid_inner, x_samples
+        )
+        assert b_new is not None
+        in_compatible = dut.in_compatible_region(V_new, b_new, rho_new, x_samples)
+        assert np.all(in_compatible[in_ellipsoid])
+
+    def test_binary_search_clf_cbf_given_lagrangian(self):
+        (
+            dut,
+            compatible_lagrangians,
+            unsafe_lagrangians,
+            V_init,
+            b_init,
+            rho_init,
+        ) = self.search_lagrangians()
+
+        x_equilibrium = np.array([0.0, 0.0])
+        (
+            S_ellipsoid_inner,
+            b_ellipsoid_inner,
+            c_ellipsoid_inner,
+        ) = dut._find_max_inner_ellipsoid(
+            V_init,
+            b_init,
+            rho_init,
+            V_contain_lagrangian_degree=mut.ContainmentLagrangianDegree(
+                inner=-1, outer=0
+            ),
+            b_contain_lagrangian_degree=[
+                mut.ContainmentLagrangianDegree(inner=-1, outer=0)
+            ],
+            x_inner_init=x_equilibrium,
+            max_iter=10,
+            convergence_tol=1e-4,
+            trust_region=1000,
+        )
+
+        solver_options = solvers.SolverOptions()
+        solver_options.SetOption(solvers.CommonSolverOption.kPrintToConsole, 0)
+
+        V, b, rho = dut.binary_search_clf_cbf(
+            compatible_lagrangians,
+            unsafe_lagrangians,
+            clf_degree=2,
+            cbf_degrees=[2],
+            x_equilibrium=x_equilibrium,
+            kappa_V=self.kappa_V,
+            kappa_b=self.kappa_b,
+            barrier_eps=self.barrier_eps,
+            S_ellipsoid_inner=S_ellipsoid_inner,
+            b_ellipsoid_inner=b_ellipsoid_inner,
+            c_ellipsoid_inner=c_ellipsoid_inner,
+            scale_min=1,
+            scale_max=50,
+            scale_tol=0.1,
+            solver_options=solver_options,
+        )
+        assert V is not None
+        assert b is not None
+        assert rho is not None
+        # Sample many points, make sure that {x | b[i] >= 0} doesn't intersect
+        # with the i'th unsafe region.
+        x_samples = 5 * np.random.randn(1000, 2) - np.array([[5, 0]])
+        self.check_unsafe_region_by_sample(b, x_samples)