diff --git a/compatible_clf_cbf/clf_cbf.py b/compatible_clf_cbf/clf_cbf.py index d7acd22..9d8e63f 100644 --- a/compatible_clf_cbf/clf_cbf.py +++ b/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, *, diff --git a/tests/test_clf_cbf.py b/tests/test_clf_cbf.py index 8c2b526..ac470d6 100644 --- a/tests/test_clf_cbf.py +++ b/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)