From 109c2640d6ddea7a8fe1c4f7530800dd5da9fd84 Mon Sep 17 00:00:00 2001 From: Hongkai Dai Date: Tue, 10 Sep 2024 17:16:51 -0700 Subject: [PATCH] Use ExcludeSet and WithinSet instead of SafetySet. --- compatible_clf_cbf/cbf.py | 63 +++-- compatible_clf_cbf/clf_cbf.py | 234 +++++++++--------- examples/linear_toy/linear_toy_demo.py | 23 +- .../linear_toy_w_input_limits_demo.py | 23 +- examples/nonlinear_toy/demo.py | 19 +- examples/nonlinear_toy/demo_trigpoly.py | 16 +- examples/nonlinear_toy/synthesize_demo.py | 18 +- examples/power_converter/demo.py | 26 +- examples/quadrotor/demo.py | 18 +- examples/quadrotor2d/demo.py | 18 +- examples/quadrotor2d/demo_taylor.py | 18 +- .../single_integrator/wo_input_limit_demo.py | 23 +- tests/test_cbf.py | 47 ++-- tests/test_clf_cbf.py | 144 ++++++----- 14 files changed, 371 insertions(+), 319 deletions(-) diff --git a/compatible_clf_cbf/cbf.py b/compatible_clf_cbf/cbf.py index 390a222..33fc5ae 100644 --- a/compatible_clf_cbf/cbf.py +++ b/compatible_clf_cbf/cbf.py @@ -18,9 +18,10 @@ ) from compatible_clf_cbf.clf_cbf import ( - SafetySet, + ExcludeSet, SafetySetLagrangians, SafetySetLagrangianDegrees, + WithinSet, ) import compatible_clf_cbf.clf_cbf as clf_cbf @@ -215,7 +216,8 @@ def __init__( f: np.ndarray, g: np.ndarray, x: np.ndarray, - safety_set: SafetySet, + exclude_sets: List[ExcludeSet], + within_set: Optional[WithinSet], u_vertices: Optional[np.ndarray] = None, state_eq_constraints: Optional[np.ndarray] = None ): @@ -230,7 +232,11 @@ def __init__( x: np.ndarray An array of symbolic variables representing the state. The shape is (nx,) - safety_set: describes the exclude region and the within region. + exclude_sets: List[ExcludeSet] + A list of "unsafe sets", namely the union of these ExcludeSet is the + unsafe region. + within_set: Optional[WithinSet] + If not None, then the state has to be within this `within_set`. u_vertices: The vertices of the input constraint polytope 𝒰. Each row is a vertex. If u_vertices=None, then the input is unconstrained. state_eq_constraints: An array of polynomials. Some dynamical systems @@ -253,7 +259,8 @@ def __init__( self.x_set: sym.Variables = sym.Variables(x) check_array_of_polynomials(f, self.x_set) check_array_of_polynomials(g, self.x_set) - self.safety_set = safety_set + self.exclude_sets = exclude_sets + self.within_set = within_set if u_vertices is not None: assert u_vertices.shape[1] == self.nu self.u_vertices = u_vertices @@ -281,27 +288,30 @@ def search_lagrangians_given_cbf( For a given CBF candidate, certify the CBF conditions by finding the Lagrangian multipliers. """ - if self.safety_set.exclude is not None: + + safety_set_lagrangians_result = None + exclude_lagrangians_result = [None] * len(self.exclude_sets) + for i in range(len(self.exclude_sets)): prog_exclude = solvers.MathematicalProgram() prog_exclude.AddIndeterminates(self.x_set) - exclude_lagrangians = safety_set_lagrangian_degrees.exclude.to_lagrangians( - prog_exclude, self.x_set + exclude_lagrangians = safety_set_lagrangian_degrees.exclude[ + i + ].to_lagrangians(prog_exclude, self.x_set) + self._add_barrier_exclude_constraint( + prog_exclude, i, h, exclude_lagrangians ) - self._add_barrier_exclude_constraint(prog_exclude, h, exclude_lagrangians) result_exclude = solve_with_id(prog_exclude, solver_id, solver_options) if result_exclude.is_success(): - exclude_lagrangians_result = exclude_lagrangians.get_result( + exclude_lagrangians_result[i] = exclude_lagrangians.get_result( result_exclude, lagrangian_coefficient_tol ) - else: - exclude_lagrangians_result = None - - within_lagrangians_result: Optional[ - List[Optional[clf_cbf.WithinRegionLagrangians]] - ] = None - if self.safety_set.within is not None: - within_lagrangians_result = [None] * (self.safety_set.within.size) - for i in range(self.safety_set.within.size): + + within_lagrangians_result: List[Optional[clf_cbf.WithinRegionLagrangians]] = ( + [None] * len(self.within_set.l) if self.within_set is not None else [] + ) + if self.within_set is not None: + within_lagrangians_result = [None] * (self.within_set.l.size) + for i in range(self.within_set.l.size): prog_within = solvers.MathematicalProgram() prog_within.AddIndeterminates(self.x_set) within_lagrangians = safety_set_lagrangian_degrees.within[ @@ -315,10 +325,10 @@ def search_lagrangians_given_cbf( within_lagrangians_result[i] = within_lagrangians.get_result( result_within, lagrangian_coefficient_tol ) - - safety_set_lagrangians_result = clf_cbf.SafetySetLagrangians( - exclude=exclude_lagrangians_result, within=within_lagrangians_result - ) + if all(exclude_lagrangians_result) and all(within_lagrangians_result): + safety_set_lagrangians_result = clf_cbf.SafetySetLagrangians( + exclude=exclude_lagrangians_result, within=within_lagrangians_result + ) prog_cbf_derivative = solvers.MathematicalProgram() prog_cbf_derivative.AddIndeterminates(self.x_set) @@ -360,9 +370,9 @@ def _add_barrier_within_constraint( Args: safe_region_index: pᵢ(x) = self.safety_set.within[within_index] """ - assert self.safety_set.within is not None + assert self.within_set is not None poly = ( - -(1 + lagrangians.safe_region) * self.safety_set.within[within_index] + -(1 + lagrangians.safe_region) * self.within_set.l[within_index] - lagrangians.cbf * h ) if self.state_eq_constraints is not None: @@ -374,12 +384,13 @@ def _add_barrier_within_constraint( def _add_barrier_exclude_constraint( self, prog: solvers.MathematicalProgram, + exclude_set_index: int, h: sym.Polynomial, lagrangians: clf_cbf.ExcludeRegionLagrangians, ) -> sym.Polynomial: """ Adds the constraint that the 0-superlevel set of the barrier function - does not intersect with the unsafe region. + does not intersect with the unsafe region self.exclude_sets[exclude_set_index]. Since the i'th unsafe regions is defined as the 0-sublevel set of polynomials p(x), we want to certify that the set {x|p(x)≤0, h(x)≥0} is empty. @@ -399,7 +410,7 @@ def _add_barrier_exclude_constraint( poly: poly is the polynomial -(1+ϕ₀(x))hᵢ(x) + ∑ⱼϕⱼ(x)pⱼ(x) """ poly = -(1 + lagrangians.cbf) * h + lagrangians.unsafe_region.dot( - self.safety_set.exclude + self.exclude_sets[exclude_set_index].l ) if self.state_eq_constraints is not None: assert lagrangians.state_eq_constraints is not None diff --git a/compatible_clf_cbf/clf_cbf.py b/compatible_clf_cbf/clf_cbf.py index a6f290e..3cf50b8 100644 --- a/compatible_clf_cbf/clf_cbf.py +++ b/compatible_clf_cbf/clf_cbf.py @@ -387,84 +387,80 @@ def to_lagrangians( @dataclass -class SafetySet: +class ExcludeSet: """ - The safety set is described as the complement of the "exclusion region", - intersecting with the "within region". - Mathematically it is - {x | exclude[i](x) > 0 for some i, and within[j](x) <= 0 for all j} + An exclude set is described as {x | lᵢ(x)<=0 for all i}. Namely it is a + semi-algebraic set. This is the "unsafe set" that the system state should not be in. """ - # An array of polynomials. - exclude: Optional[np.ndarray] - # An array of polynomials. - within: Optional[np.ndarray] + l: np.ndarray + + +@dataclass +class WithinSet: + """ + A within set is described as {x | lᵢ(x)<=0 for all i}. Namely it is a + semi-algebraic set. This is the "safe set" that the system state has to be within. + """ + + l: np.ndarray @dataclass class SafetySetLagrangians: - exclude: Optional[ExcludeRegionLagrangians] - within: Optional[List[WithinRegionLagrangians]] + # exclude[i] is the the i'th exclude set. + exclude: List[ExcludeRegionLagrangians] + # within[i] is for the within_set.l[i] + within: List[WithinRegionLagrangians] + + def contains_none(self) -> bool: + """ + Returns true if self.exclude or self.within contains None + """ + return not (all(self.exclude) and all(self.within)) def get_result( self, result: solvers.MathematicalProgramResult, coefficient_tol: Optional[float], ) -> Self: - exclude = ( - None - if self.exclude is None - else self.exclude.get_result(result, coefficient_tol) - ) - within = ( - None - if self.within is None - else [a.get_result(result, coefficient_tol) for a in self.within] - ) + exclude = [a.get_result(result, coefficient_tol) for a in self.exclude] + within = [a.get_result(result, coefficient_tol) for a in self.within] return SafetySetLagrangians(exclude=exclude, within=within) def get_cbf_lagrangians( self, - ) -> Tuple[Optional[sym.Polynomial], Optional[List[sym.Polynomial]]]: - exclude_cbf = None if self.exclude is None else self.exclude.cbf - within_cbf = ( - None if self.within is None else [within.cbf for within in self.within] - ) + ) -> Tuple[List[sym.Polynomial], List[sym.Polynomial]]: + exclude_cbf = [exclude.cbf for exclude in self.exclude] + within_cbf = [within.cbf for within in self.within] return exclude_cbf, within_cbf @dataclass class SafetySetLagrangianDegrees: - exclude: Optional[ExcludeRegionLagrangianDegrees] - within: Optional[List[WithinRegionLagrangianDegrees]] + exclude: List[ExcludeRegionLagrangianDegrees] + within: List[WithinRegionLagrangianDegrees] def to_lagrangians( self, prog: solvers.MathematicalProgram, x_set: sym.Variables, cbf_lagrangian: Optional[ - Tuple[Optional[sym.Polynomial], Optional[List[sym.Polynomial]]] + Tuple[List[sym.Polynomial], List[sym.Polynomial]] ] = None, ) -> SafetySetLagrangians: - exclude = ( - None - if self.exclude is None - else self.exclude.to_lagrangians( - prog, x_set, (None if cbf_lagrangian is None else cbf_lagrangian[0]) + exclude = [ + self.exclude[i].to_lagrangians( + prog, x_set, (None if cbf_lagrangian is None else cbf_lagrangian[0][i]) ) - ) - within = ( - None - if self.within is None - else [ - self.within[i].to_lagrangians( - prog, - x_set, - (None if cbf_lagrangian is None else cbf_lagrangian[1][i]), - ) - for i in range(len(self.within)) - ] - ) + for i in range(len(self.exclude)) + ] + within = [ + self.within[i].to_lagrangians( + prog, x_set, None if cbf_lagrangian is None else cbf_lagrangian[1][i] + ) + for i in range(len(self.within)) + ] return SafetySetLagrangians(exclude=exclude, within=within) @@ -656,9 +652,11 @@ def __init__( f: np.ndarray, g: np.ndarray, x: np.ndarray, - safety_sets: List[SafetySet], + exclude_sets: List[ExcludeSet], + within_set: Optional[WithinSet], Au: Optional[np.ndarray] = None, bu: Optional[np.ndarray] = None, + num_cbf: int = 1, with_clf: bool = True, use_y_squared: bool = True, state_eq_constraints: Optional[np.ndarray] = None, @@ -674,15 +672,19 @@ def __init__( x: np.ndarray An array of symbolic variables representing the state. The shape is (nx,) - safety_set: List[SafetySet] - A list of safety set descriptions. For each SafetySet object we will - certify/synthesize a corresponding CBF. + exclude_sets: List[ExcludeSet] + A list of "unsafe sets", namely the union of these ExcludeSet is the unsafe region. + within_set: Optional[WithinSet] + If not None, then the state has to be within this `within_set`. Au: Optional[np.ndarray] The set of admissible control is Au * u <= bu. The shape is (Any, nu) bu: Optional[np.ndarray] The set of admissible control is Au * u <= bu. The shape is (Any,) + num_cbf: int + The number of CBF functions. We require these CBF functions to be + compatible in the intersection of their 0-superlevel-set. with_clf: bool Whether to certify or search for CLF. If set to False, then we will certify or search multiple compatible CBFs without CLF. @@ -718,7 +720,8 @@ def __init__( self.x_set: sym.Variables = sym.Variables(x) check_array_of_polynomials(f, self.x_set) check_array_of_polynomials(g, self.x_set) - self.safety_sets = safety_sets + self.exclude_sets = exclude_sets + self.within_set = within_set if Au is not None: assert Au.shape[1] == self.nu assert bu is not None @@ -727,7 +730,7 @@ def __init__( self.bu = bu self.with_clf = with_clf self.use_y_squared = use_y_squared - self.num_cbf = len(self.safety_sets) + self.num_cbf = num_cbf y_size = ( self.num_cbf + (1 if self.with_clf else 0) @@ -751,7 +754,6 @@ def __init__( def certify_cbf_safety_set( self, - safety_set_index: int, cbf: sym.Polynomial, lagrangian_degrees: SafetySetLagrangianDegrees, solver_id: Optional[solvers.SolverId] = None, @@ -759,30 +761,30 @@ def certify_cbf_safety_set( lagrangian_coefficient_tol: Optional[float] = None, ) -> Optional[SafetySetLagrangians]: """ - Certify the 0-super level set of barrier function h(x) is within the - safety set self.safety_set[safety_set_index] + Certify the 0-super level set of the barrier function cbf(x) is within + the safety set. """ - safety_set = self.safety_sets[safety_set_index] - if safety_set.exclude is not None: - assert lagrangian_degrees.exclude is not None - exclude_lagrangians = self.certify_cbf_exclude( - safety_set_index, + assert len(self.exclude_sets) == len(lagrangian_degrees.exclude) + + exclude_lagrangians = [ + self.certify_cbf_exclude( + i, cbf, - lagrangian_degrees.exclude, + lagrangian_degrees.exclude[i], solver_id, solver_options, lagrangian_coefficient_tol, ) - if exclude_lagrangians is None: - return None + for i in range(len(self.exclude_sets)) + ] + if not all(exclude_lagrangians): + return None + if self.within_set is None: + assert len(lagrangian_degrees.within) == 0 + within_lagrangians = [] else: - exclude_lagrangians = None - if safety_set.within is not None: - within_lagrangians = [None] * safety_set.within.size - assert lagrangian_degrees.within is not None - for i in range(safety_set.within.size): - within_lagrangians[i] = self.certify_cbf_within( - safety_set_index, + within_lagrangians = [ + self.certify_cbf_within( i, cbf, lagrangian_degrees.within[i], @@ -790,20 +792,17 @@ def certify_cbf_safety_set( solver_options, lagrangian_coefficient_tol, ) - if within_lagrangians[i] is None: - return None + for i in range(len(self.within_set.l)) + ] + if not all(within_lagrangians): + return None - else: - within_lagrangians = None - lagrangian_result = SafetySetLagrangians( + return SafetySetLagrangians( exclude=exclude_lagrangians, within=within_lagrangians ) - return lagrangian_result - def certify_cbf_within( self, - safety_set_index: int, within_index: int, cbf: sym.Polynomial, lagrangian_degrees: WithinRegionLagrangianDegrees, @@ -813,14 +812,12 @@ def certify_cbf_within( ) -> Optional[WithinRegionLagrangians]: """ Certify the 0-super level set of barrier function h(x) is within the - region self.safety_sets[safety_set_index].within[within_index] + region {x|self.within_set.l[within_index](x) <= 0} """ prog = solvers.MathematicalProgram() prog.AddIndeterminates(self.x_set) lagrangians = lagrangian_degrees.to_lagrangians(prog, self.x_set) - self._add_barrier_within_constraint( - prog, safety_set_index, within_index, cbf, lagrangians - ) + self._add_barrier_within_constraint(prog, within_index, cbf, lagrangians) result = solve_with_id(prog, solver_id, solver_options) lagrangians_result = ( lagrangians.get_result(result, lagrangian_coefficient_tol) @@ -831,7 +828,7 @@ def certify_cbf_within( def certify_cbf_exclude( self, - safety_set_index: int, + exclude_set_index: int, cbf: sym.Polynomial, lagrangian_degrees: ExcludeRegionLagrangianDegrees, solver_id: Optional[solvers.SolverId] = None, @@ -840,7 +837,7 @@ def certify_cbf_exclude( ) -> Optional[ExcludeRegionLagrangians]: """ Certifies that the 0-superlevel set {x | hᵢ(x) >= 0} does not intersect - with the unsafe region self.safety_set[safety_set_index].exclude + with the unsafe region {x | self.exclude_sets[exclude_set_index].l(x) <= 0} If we denote the unsafe region as {x | p(x) <= 0}, then we impose the constraint @@ -849,15 +846,14 @@ def certify_cbf_exclude( ϕᵢ,₀(x), ϕᵢ,ⱼ(x) are sos. Args: - safety_set_index: We certify the CBF for the region - self.safety_sets[safety_set_index].exclude - cbf: hᵢ(x) in the documentation above. The CBF function for - self.safety_set[safety_set_index] + exclude_set_index: We certify the CBF for the region + {x | self.exclude_sets[exclude_set_index].l(x) <= 0} + cbf: hᵢ(x) in the documentation above. """ prog = solvers.MathematicalProgram() prog.AddIndeterminates(self.x_set) lagrangians = lagrangian_degrees.to_lagrangians(prog, self.x_set) - self._add_barrier_exclude_constraint(prog, safety_set_index, cbf, lagrangians) + self._add_barrier_exclude_constraint(prog, exclude_set_index, cbf, lagrangians) result = solve_with_id(prog, solver_id, solver_options) lagrangians_result = ( lagrangians.get_result(result, lagrangian_coefficient_tol) @@ -962,7 +958,6 @@ def search_lagrangians_given_clf_cbf( ] * self.num_cbf for i in range(self.num_cbf): safety_set_lagrangians_result[i] = self.certify_cbf_safety_set( - i, h[i], safety_set_lagrangian_degrees[i], solver_id, @@ -1222,13 +1217,13 @@ def bilinear_alternation( iteration = 0 clf = V_init - assert len(h_init) == len(self.safety_sets) + assert len(h_init) == self.num_cbf cbf = h_init compatible_lagrangians = None - safety_sets_lagrangians: List[Optional[SafetySetLagrangians]] = [None] * len( - self.safety_sets - ) + safety_sets_lagrangians: List[Optional[SafetySetLagrangians]] = [ + None + ] * self.num_cbf def evaluate_compatible_states(clf_fun, cbf_funs, x_val): if clf_fun is not None: @@ -1557,7 +1552,6 @@ def _add_compatibility( def _add_barrier_within_constraint( self, prog: solvers.MathematicalProgram, - safety_set_index: int, within_index: int, h: sym.Polynomial, lagrangians: WithinRegionLagrangians, @@ -1572,13 +1566,11 @@ def _add_barrier_within_constraint( ϕ₁(x) is sos. Args: - safe_region_index: pᵢ(x) = - self.safety_sets[safety_set_index].within[within_index] + within_index: pᵢ(x) = self.within_set.l[within_index] """ - assert self.safety_sets[safety_set_index].within is not None + assert self.within_set is not None poly = ( - -(1 + lagrangians.safe_region) - * self.safety_sets[safety_set_index].within[within_index] + -(1 + lagrangians.safe_region) * self.within_set.l[within_index] - lagrangians.cbf * h ) if self.state_eq_constraints is not None: @@ -1590,7 +1582,7 @@ def _add_barrier_within_constraint( def _add_barrier_exclude_constraint( self, prog: solvers.MathematicalProgram, - safety_set_index: int, + exclude_set_index: int, h: sym.Polynomial, lagrangians: ExcludeRegionLagrangians, ) -> sym.Polynomial: @@ -1609,21 +1601,20 @@ def _add_barrier_exclude_constraint( It doesn't add the constraint ϕᵢ,₀(x), ϕᵢ,ⱼ(x) are sos. Args: - safety_set_index: We certify that the 0-superlevel set of the + exclude_set_index: We certify that the 0-superlevel set of the barrier function doesn't intersect with the exclude region - self.safety_sets[safety_set_index].exclude + self.exclude_sets[exclude_set_index].l h: a polynomial, h is the barrier function for the - exclude region self.safety_sets[safety_set_index]. + exclude region self.exclude_sets[exclude_set_index]. lagrangians: A array of polynomials, ϕᵢ(x) in the documentation above. Returns: poly: poly is the polynomial -(1+ϕᵢ,₀(x))hᵢ(x) + ∑ⱼϕᵢ,ⱼ(x)pⱼ(x) """ - assert self.safety_sets[safety_set_index].exclude is not None assert lagrangians.unsafe_region.size == len( - self.safety_sets[safety_set_index].exclude + self.exclude_sets[exclude_set_index].l ) poly = -(1 + lagrangians.cbf) * h + lagrangians.unsafe_region.dot( - self.safety_sets[safety_set_index].exclude + self.exclude_sets[exclude_set_index].l ) if self.state_eq_constraints is not None: assert lagrangians.state_eq_constraints is not None @@ -1659,13 +1650,13 @@ def _construct_search_clf_cbf_program( compatible_lagrangians: The Lagrangian polynomials. Result from solving construct_search_compatible_lagrangians(). safety_sets_lagrangians: The Lagrangians certifying that the 0-super - level set of the i'th CBF is in the i'th safety set self.safety_sets[i]. + level set of the i'th CBF is in the safety set. clf_degree: if not None, the total degree of CLF. cbf_degrees: cbf_degrees[i] is the total degree of the i'th CBF. x_equilibrium: if not None, the equilibrium state. """ - assert len(safety_sets_lagrangians) == len(self.safety_sets) - assert len(cbf_degrees) == len(self.safety_sets) + assert len(safety_sets_lagrangians) == self.num_cbf + assert len(cbf_degrees) == self.num_cbf prog = solvers.MathematicalProgram() prog.AddIndeterminates(self.xy_set) @@ -1700,23 +1691,24 @@ def _construct_search_clf_cbf_program( ) # We can search for the Lagrangians for the safety set as well, since # the safety set is fixed. - safety_sets_lagrangians_new: List[SafetySetLagrangians] = [None] * len( - self.safety_sets - ) - for i in range(len(self.safety_sets)): + safety_sets_lagrangians_new: List[SafetySetLagrangians] = [None] * self.num_cbf + for i in range(self.num_cbf): cbf_lagrangian = safety_sets_lagrangians[i].get_cbf_lagrangians() safety_sets_lagrangians_new[i] = safety_sets_lagrangian_degrees[ i ].to_lagrangians(prog, self.x_set, cbf_lagrangian) - if self.safety_sets[i].exclude is not None: - assert safety_sets_lagrangians_new[i].exclude is not None + assert len(safety_sets_lagrangians_new[i].exclude) == len(self.exclude_sets) + for exclude_set_index in range(len(self.exclude_sets)): self._add_barrier_exclude_constraint( - prog, i, h[i], safety_sets_lagrangians_new[i].exclude + prog, + exclude_set_index, + h[i], + safety_sets_lagrangians_new[i].exclude[exclude_set_index], ) - if self.safety_sets[i].within is not None: - for j in range(self.safety_sets[i].within.size): + if self.within_set is not None: + for j in range(self.within_set.l.size): self._add_barrier_within_constraint( - prog, i, j, h[i], safety_sets_lagrangians_new[i].within[j] + prog, j, h[i], safety_sets_lagrangians_new[i].within[j] ) # We can search for some compatible Lagrangians as well, including the diff --git a/examples/linear_toy/linear_toy_demo.py b/examples/linear_toy/linear_toy_demo.py index 147e3a9..c2cdec6 100644 --- a/examples/linear_toy/linear_toy_demo.py +++ b/examples/linear_toy/linear_toy_demo.py @@ -55,13 +55,15 @@ def search_barrier_safe_lagrangians( ) -> List[clf_cbf.SafetySetLagrangians]: lagrangian_degrees = [ clf_cbf.SafetySetLagrangianDegrees( - exclude=clf_cbf.ExcludeRegionLagrangianDegrees( - cbf=2, unsafe_region=[2], state_eq_constraints=None - ), - within=None, + exclude=[ + clf_cbf.ExcludeRegionLagrangianDegrees( + cbf=2, unsafe_region=[2], state_eq_constraints=None + ) + ], + within=[], ) ] - lagrangians = dut.certify_cbf_safety_set(0, h[0], lagrangian_degrees[0]) + lagrangians = dut.certify_cbf_safety_set(h[0], lagrangian_degrees[0]) assert lagrangians is not None return [lagrangians] @@ -99,10 +101,9 @@ def search(use_y_squared: bool): # Use an arbitrary unsafe region alpha = 0.5 - safety_sets = [ - clf_cbf.SafetySet( - exclude=np.array([1.1 * alpha - sym.Polynomial(x.dot(S_lqr @ x))]), - within=None, + exclude_sets = [ + clf_cbf.ExcludeSet( + np.array([1.1 * alpha - sym.Polynomial(x.dot(S_lqr @ x))]), ) ] @@ -110,9 +111,11 @@ def search(use_y_squared: bool): f=f, g=g, x=x, - safety_sets=safety_sets, + exclude_sets=exclude_sets, + within_set=None, Au=None, bu=None, + num_cbf=1, with_clf=True, use_y_squared=use_y_squared, ) diff --git a/examples/linear_toy/linear_toy_w_input_limits_demo.py b/examples/linear_toy/linear_toy_w_input_limits_demo.py index cfbed42..986aa33 100644 --- a/examples/linear_toy/linear_toy_w_input_limits_demo.py +++ b/examples/linear_toy/linear_toy_w_input_limits_demo.py @@ -56,13 +56,15 @@ def search_barrier_safe_lagrangians( ) -> List[clf_cbf.SafetySetLagrangians]: lagrangian_degrees = [ clf_cbf.SafetySetLagrangianDegrees( - exclude=clf_cbf.ExcludeRegionLagrangianDegrees( - cbf=2, unsafe_region=[2], state_eq_constraints=None - ), - within=None, + exclude=[ + clf_cbf.ExcludeRegionLagrangianDegrees( + cbf=2, unsafe_region=[2], state_eq_constraints=None + ) + ], + within=[], ) ] - lagrangians = dut.certify_cbf_safety_set(0, h[0], lagrangian_degrees[0]) + lagrangians = dut.certify_cbf_safety_set(h[0], lagrangian_degrees[0]) assert lagrangians is not None return [lagrangians] @@ -102,10 +104,9 @@ def search(): # Use an arbitrary unsafe region alpha = 0.5 - safety_sets = [ - clf_cbf.SafetySet( - exclude=np.array([1.1 * alpha - sym.Polynomial(x.dot(S_lqr @ x))]), - within=None, + exclude_sets = [ + clf_cbf.ExcludeSet( + np.array([1.1 * alpha - sym.Polynomial(x.dot(S_lqr @ x))]), ) ] @@ -116,9 +117,11 @@ def search(): f=f, g=g, x=x, - safety_sets=safety_sets, + exclude_sets=exclude_sets, + within_set=None, Au=Au, bu=bu, + num_cbf=1, with_clf=True, use_y_squared=True, ) diff --git a/examples/nonlinear_toy/demo.py b/examples/nonlinear_toy/demo.py index 2811744..3ef3481 100644 --- a/examples/nonlinear_toy/demo.py +++ b/examples/nonlinear_toy/demo.py @@ -22,16 +22,16 @@ def main(use_y_squared: bool, with_u_bound: bool): else: Au = None bu = None - safety_sets = [ - clf_cbf.SafetySet(exclude=np.array([sym.Polynomial(x[0] + 10)]), within=None) - ] + exclude_sets = [clf_cbf.ExcludeSet(np.array([sym.Polynomial(x[0] + 10)]))] compatible = clf_cbf.CompatibleClfCbf( f=f, g=g, x=x, - safety_sets=safety_sets, + exclude_sets=exclude_sets, + within_set=None, Au=Au, bu=bu, + num_cbf=1, with_clf=True, use_y_squared=use_y_squared, ) @@ -69,16 +69,17 @@ def main(use_y_squared: bool, with_u_bound: bool): safety_sets_lagrangian_degrees = [ clf_cbf.SafetySetLagrangianDegrees( - exclude=clf_cbf.ExcludeRegionLagrangianDegrees( - cbf=0, unsafe_region=[0], state_eq_constraints=None - ), - within=None, + exclude=[ + clf_cbf.ExcludeRegionLagrangianDegrees( + cbf=0, unsafe_region=[0], state_eq_constraints=None + ) + ], + within=[], ) ] safety_sets_lagrangians = [ compatible.certify_cbf_safety_set( - safety_set_index=0, cbf=h_init[0], lagrangian_degrees=safety_sets_lagrangian_degrees[0], solver_options=None, diff --git a/examples/nonlinear_toy/demo_trigpoly.py b/examples/nonlinear_toy/demo_trigpoly.py index 3ff33e8..6913e14 100644 --- a/examples/nonlinear_toy/demo_trigpoly.py +++ b/examples/nonlinear_toy/demo_trigpoly.py @@ -129,14 +129,16 @@ def search(unit_test_flag: bool = False): f, g = toy_system.affine_trig_poly_dynamics(x) state_eq_constraints = np.array([toy_system.affine_trig_poly_state_constraints(x)]) use_y_squared = True - safety_sets = [clf_cbf.SafetySet(exclude=get_unsafe_regions(x), within=None)] + exclude_sets = [clf_cbf.ExcludeSet(get_unsafe_regions(x))] compatible = clf_cbf.CompatibleClfCbf( f=f, g=g, x=x, - safety_sets=safety_sets, + exclude_sets=exclude_sets, + within_set=None, Au=np.array([[1], [-1]]), bu=np.array([1, 1]), + num_cbf=1, with_clf=True, use_y_squared=use_y_squared, state_eq_constraints=state_eq_constraints, @@ -153,10 +155,12 @@ def search(unit_test_flag: bool = False): ) safety_sets_lagrangian_degrees = [ clf_cbf.SafetySetLagrangianDegrees( - exclude=clf_cbf.ExcludeRegionLagrangianDegrees( - cbf=0, unsafe_region=[0], state_eq_constraints=[0] - ), - within=None, + exclude=[ + clf_cbf.ExcludeRegionLagrangianDegrees( + cbf=0, unsafe_region=[0], state_eq_constraints=[0] + ) + ], + within=[], ) ] kappa_V = 0.1 diff --git a/examples/nonlinear_toy/synthesize_demo.py b/examples/nonlinear_toy/synthesize_demo.py index 51085d0..917ef65 100644 --- a/examples/nonlinear_toy/synthesize_demo.py +++ b/examples/nonlinear_toy/synthesize_demo.py @@ -22,16 +22,16 @@ def main(with_u_bound: bool): else: Au = None bu = None - safety_sets = [ - clf_cbf.SafetySet(exclude=np.array([sym.Polynomial(x[0] + 5)]), within=None) - ] + exclude_sets = [clf_cbf.ExcludeSet(np.array([sym.Polynomial(x[0] + 5)]))] compatible = clf_cbf.CompatibleClfCbf( f=f, g=g, x=x, - safety_sets=safety_sets, + exclude_sets=exclude_sets, + within_set=None, Au=Au, bu=bu, + num_cbf=1, with_clf=True, use_y_squared=True, ) @@ -51,10 +51,12 @@ def main(with_u_bound: bool): ) safety_sets_lagrangian_degrees = [ clf_cbf.SafetySetLagrangianDegrees( - exclude=clf_cbf.ExcludeRegionLagrangianDegrees( - cbf=0, unsafe_region=[0], state_eq_constraints=None - ), - within=None, + exclude=[ + clf_cbf.ExcludeRegionLagrangianDegrees( + cbf=0, unsafe_region=[0], state_eq_constraints=None + ) + ], + within=[], ) ] x_equilibrium = np.array([0.0, 0.0]) diff --git a/examples/power_converter/demo.py b/examples/power_converter/demo.py index d2a2f7c..0a2a262 100644 --- a/examples/power_converter/demo.py +++ b/examples/power_converter/demo.py @@ -69,26 +69,26 @@ def search(use_y_squared: bool): x = sym.MakeVectorContinuousVariable(3, "x") f, g = plant.affine_dynamics(x) - safety_sets = [ - clf_cbf.SafetySet( - exclude=None, - within=np.array( - [ - sym.Polynomial(x[0] - 0.2), - sym.Polynomial(-x[0] - 0.8), - sym.Polynomial(((x[1] - 0.001) ** 2) + x[2] ** 2 - 1.2**2), - ] - ), + exclude_sets = [] + within_set = clf_cbf.WithinSet( + np.array( + [ + sym.Polynomial(x[0] - 0.2), + sym.Polynomial(-x[0] - 0.8), + sym.Polynomial(((x[1] - 0.001) ** 2) + x[2] ** 2 - 1.2**2), + ] ) - ] + ) compatible = clf_cbf.CompatibleClfCbf( f=f, g=g, x=x, - safety_sets=safety_sets, + exclude_sets=exclude_sets, + within_set=within_set, Au=None, bu=None, + num_cbf=1, with_clf=True, use_y_squared=use_y_squared, ) @@ -122,7 +122,7 @@ def search(use_y_squared: bool): safety_sets_lagrangian_degrees = [ clf_cbf.SafetySetLagrangianDegrees( - exclude=None, + exclude=[], within=[ clf_cbf.WithinRegionLagrangianDegrees( cbf=0, safe_region=0, state_eq_constraints=None diff --git a/examples/quadrotor/demo.py b/examples/quadrotor/demo.py index 7e82ff0..9aa8ac5 100644 --- a/examples/quadrotor/demo.py +++ b/examples/quadrotor/demo.py @@ -49,17 +49,17 @@ def search(use_y_squared: bool, with_u_bound: bool): Au, bu = None, None # Ground as the unsafe region. - safety_sets = [ - clf_cbf.SafetySet(exclude=np.array([sym.Polynomial(x[6] + 0.5)]), within=None) - ] + exclude_sets = [clf_cbf.ExcludeSet(np.array([sym.Polynomial(x[6] + 0.5)]))] state_eq_constraints = quadrotor.equality_constraint(x) compatible = clf_cbf.CompatibleClfCbf( f=f, g=g, x=x, - safety_sets=safety_sets, + exclude_sets=exclude_sets, + within_set=None, Au=Au, bu=bu, + num_cbf=1, with_clf=True, use_y_squared=use_y_squared, state_eq_constraints=state_eq_constraints, @@ -121,10 +121,12 @@ def search(use_y_squared: bool, with_u_bound: bool): ) safety_sets_lagrangian_degrees = [ clf_cbf.SafetySetLagrangianDegrees( - exclude=clf_cbf.ExcludeRegionLagrangianDegrees( - cbf=0, unsafe_region=[0], state_eq_constraints=[0] - ), - within=None, + exclude=[ + clf_cbf.ExcludeRegionLagrangianDegrees( + cbf=0, unsafe_region=[0], state_eq_constraints=[0] + ) + ], + within=[], ) ] barrier_eps = np.array([0.000]) diff --git a/examples/quadrotor2d/demo.py b/examples/quadrotor2d/demo.py index a83657a..e23ab2d 100644 --- a/examples/quadrotor2d/demo.py +++ b/examples/quadrotor2d/demo.py @@ -30,17 +30,17 @@ def main(use_y_squared: bool, with_u_bound: bool): Au, bu = None, None # Ground as the unsafe region. - safety_sets = [ - clf_cbf.SafetySet(exclude=np.array([sym.Polynomial(x[1] + 0.5)]), within=None) - ] + exclude_sets = [clf_cbf.ExcludeSet(np.array([sym.Polynomial(x[1] + 0.5)]))] state_eq_constraints = quadrotor.equality_constraint(x) compatible = clf_cbf.CompatibleClfCbf( f=f, g=g, x=x, - safety_sets=safety_sets, + exclude_sets=exclude_sets, + within_set=None, Au=Au, bu=bu, + num_cbf=1, with_clf=True, use_y_squared=use_y_squared, state_eq_constraints=state_eq_constraints, @@ -77,10 +77,12 @@ def main(use_y_squared: bool, with_u_bound: bool): ) safety_sets_lagrangian_degrees = [ clf_cbf.SafetySetLagrangianDegrees( - exclude=clf_cbf.ExcludeRegionLagrangianDegrees( - cbf=0, unsafe_region=[0], state_eq_constraints=[0] - ), - within=None, + exclude=[ + clf_cbf.ExcludeRegionLagrangianDegrees( + cbf=0, unsafe_region=[0], state_eq_constraints=[0] + ) + ], + within=[], ) ] barrier_eps = np.array([0.000]) diff --git a/examples/quadrotor2d/demo_taylor.py b/examples/quadrotor2d/demo_taylor.py index 90846a3..5476d9f 100644 --- a/examples/quadrotor2d/demo_taylor.py +++ b/examples/quadrotor2d/demo_taylor.py @@ -56,16 +56,16 @@ def search_clf_cbf( kappa_h = np.array([kappa_V]) # Ground as the unsafe region. - safety_sets = [ - clf_cbf.SafetySet(exclude=np.array([sym.Polynomial(x[1] + 0.5)]), within=None) - ] + exclude_sets = [clf_cbf.ExcludeSet(np.array([sym.Polynomial(x[1] + 0.5)]))] compatible = clf_cbf.CompatibleClfCbf( f=f, g=g, x=x, - safety_sets=safety_sets, + exclude_sets=exclude_sets, + within_set=None, Au=Au, bu=bu, + num_cbf=1, with_clf=True, use_y_squared=use_y_squared, state_eq_constraints=None, @@ -91,10 +91,12 @@ def search_clf_cbf( safety_sets_lagrangian_degrees = [ clf_cbf.SafetySetLagrangianDegrees( - exclude=clf_cbf.ExcludeRegionLagrangianDegrees( - cbf=0, unsafe_region=[2], state_eq_constraints=None - ), - within=None, + exclude=[ + clf_cbf.ExcludeRegionLagrangianDegrees( + cbf=0, unsafe_region=[2], state_eq_constraints=None + ) + ], + within=[], ) ] solver_options = solvers.SolverOptions() diff --git a/examples/single_integrator/wo_input_limit_demo.py b/examples/single_integrator/wo_input_limit_demo.py index cd3b836..da134be 100644 --- a/examples/single_integrator/wo_input_limit_demo.py +++ b/examples/single_integrator/wo_input_limit_demo.py @@ -86,15 +86,16 @@ def certify_clf_cbf_separately( ), ) assert clf_lagrangian is not None - safety_set = clf_cbf.SafetySet( - exclude=get_unsafe_region(x, obstacle_center, obstacle_radius), within=None - ) + exclude_sets = [ + clf_cbf.ExcludeSet(get_unsafe_region(x, obstacle_center, obstacle_radius)) + ] control_barrier = ControlBarrier( f=f, g=g, x=x, - safety_set=safety_set, + exclude_sets=exclude_sets, + within_set=None, u_vertices=None, state_eq_constraints=None, ) @@ -109,10 +110,12 @@ def certify_clf_cbf_separately( dhdx_times_f=0, dhdx_times_g=[1, 1], h_plus_eps=0, state_eq_constraints=None ), clf_cbf.SafetySetLagrangianDegrees( - exclude=clf_cbf.ExcludeRegionLagrangianDegrees( - cbf=0, unsafe_region=[0], state_eq_constraints=None - ), - within=None, + exclude=[ + clf_cbf.ExcludeRegionLagrangianDegrees( + cbf=0, unsafe_region=[0], state_eq_constraints=None + ) + ], + within=[], ), ) assert cbf_derivative_lagrangians is not None @@ -122,9 +125,11 @@ def certify_clf_cbf_separately( f=f, g=g, x=x, - safety_sets=[safety_set], + exclude_sets=exclude_sets, + within_set=None, Au=None, bu=None, + num_cbf=1, with_clf=True, use_y_squared=True, state_eq_constraints=None, diff --git a/tests/test_cbf.py b/tests/test_cbf.py index 1da5c0e..8f44026 100644 --- a/tests/test_cbf.py +++ b/tests/test_cbf.py @@ -63,15 +63,17 @@ def test_add_barrier_safe_constraint(self): f=self.f, g=self.g, x=self.x, - safety_set=clf_cbf.SafetySet( - exclude=np.array( - [ - sym.Polynomial(self.x[0] + self.x[1] + self.x[2] + 2), - sym.Polynomial(2 - self.x[0] - self.x[1] - self.x[2]), - ] - ), - within=None, - ), + exclude_sets=[ + clf_cbf.ExcludeSet( + np.array( + [ + sym.Polynomial(self.x[0] + self.x[1] + self.x[2] + 2), + sym.Polynomial(2 - self.x[0] - self.x[1] - self.x[2]), + ] + ), + ) + ], + within_set=None, u_vertices=None, state_eq_constraints=None, ) @@ -87,9 +89,12 @@ def test_add_barrier_safe_constraint(self): state_eq_constraints=None, ) - poly = dut._add_barrier_exclude_constraint(prog, h, lagrangians) + exclude_set_index = 0 + poly = dut._add_barrier_exclude_constraint( + prog, exclude_set_index, h, lagrangians + ) poly_expected = -(1 + lagrangians.cbf) * h + lagrangians.unsafe_region.dot( - dut.safety_set.exclude + dut.exclude_sets[exclude_set_index].l ) assert poly.CoefficientsAlmostEqual(poly_expected, 1e-8) @@ -98,15 +103,17 @@ def test_add_cbf_derivative_condition(self): f=self.f, g=self.g, x=self.x, - safety_set=clf_cbf.SafetySet( - exclude=np.array( - [ - sym.Polynomial(self.x[0] + self.x[1] + self.x[2] + 2), - sym.Polynomial(2 - self.x[0] - self.x[1] - self.x[2]), - ] - ), - within=None, - ), + exclude_sets=[ + clf_cbf.ExcludeSet( + np.array( + [ + sym.Polynomial(self.x[0] + self.x[1] + self.x[2] + 2), + sym.Polynomial(2 - self.x[0] - self.x[1] - self.x[2]), + ] + ) + ) + ], + within_set=None, u_vertices=None, state_eq_constraints=None, ) diff --git a/tests/test_clf_cbf.py b/tests/test_clf_cbf.py index cdf7567..03cf839 100644 --- a/tests/test_clf_cbf.py +++ b/tests/test_clf_cbf.py @@ -162,17 +162,13 @@ def setup_class(cls): [sym.Polynomial(cls.x[0] * cls.x[2]), sym.Polynomial(cls.x[1])], ] ) - cls.safety_sets = [ - mut.SafetySet( - exclude=np.array([sym.Polynomial(cls.x[0] + 1)]), within=None - ), - mut.SafetySet( - exclude=np.array( - [sym.Polynomial(1 - cls.x[1]), sym.Polynomial(1 - cls.x[0])] - ), - within=None, + cls.exclude_sets = [ + mut.ExcludeSet(np.array([sym.Polynomial(cls.x[0] + 1)])), + mut.ExcludeSet( + np.array([sym.Polynomial(1 - cls.x[1]), sym.Polynomial(1 - cls.x[0])]) ), ] + cls.within_set = None def linearize(self) -> Tuple[np.ndarray, np.ndarray]: """ @@ -195,9 +191,11 @@ def test_constructor(self): f=self.f, g=self.g, x=self.x, - safety_sets=self.safety_sets, + exclude_sets=self.exclude_sets, + within_set=self.within_set, Au=None, bu=None, + num_cbf=1, with_clf=True, use_y_squared=True, ) @@ -210,7 +208,7 @@ def check_members(cls: mut.CompatibleClfCbf): for y_squared_poly_i, y_i in zip(cls.y_squared_poly.flat, cls.y.flat): assert y_squared_poly_i.EqualTo(sym.Polynomial(y_i**2)) - assert dut.y.shape == (len(self.safety_sets) + 1,) + assert dut.y.shape == (dut.num_cbf + 1,) check_members(dut) # Now construct with with_clf=False @@ -218,13 +216,15 @@ def check_members(cls: mut.CompatibleClfCbf): f=self.f, g=self.g, x=self.x, - safety_sets=self.safety_sets, + exclude_sets=self.exclude_sets, + within_set=self.within_set, Au=None, bu=None, + num_cbf=1, with_clf=False, use_y_squared=True, ) - assert dut.y.shape == (len(self.safety_sets),) + assert dut.y.shape == (dut.num_cbf,) check_members(dut) # Now construct with Au and bu @@ -232,9 +232,11 @@ def check_members(cls: mut.CompatibleClfCbf): f=self.f, g=self.g, x=self.x, - safety_sets=self.safety_sets, + exclude_sets=self.exclude_sets, + within_set=self.within_set, Au=np.array([[-3, -2], [1.0, 4.0], [0.0, 3.0]]), bu=np.array([4, 5.0, 6.0]), + num_cbf=2, with_clf=False, use_y_squared=True, ) @@ -242,7 +244,7 @@ def check_members(cls: mut.CompatibleClfCbf): assert dut.Au.shape == (3, self.nu) assert dut.bu is not None assert dut.bu.shape == (3,) - assert dut.y.shape == (len(self.safety_sets) + dut.Au.shape[0],) + assert dut.y.shape == (dut.num_cbf + dut.Au.shape[0],) check_members(dut) # Now construct with Au, bu and with_clf=True @@ -250,9 +252,11 @@ def check_members(cls: mut.CompatibleClfCbf): f=self.f, g=self.g, x=self.x, - safety_sets=self.safety_sets, + exclude_sets=self.exclude_sets, + within_set=self.within_set, Au=np.array([[-3, -2], [1, 4], [3, -1.0]]), bu=np.array([4, 5.0, 6.0]), + num_cbf=1, with_clf=True, use_y_squared=True, ) @@ -260,7 +264,7 @@ def check_members(cls: mut.CompatibleClfCbf): assert dut.Au.shape == (3, self.nu) assert dut.bu is not None assert dut.bu.shape == (3,) - assert dut.y.shape == (len(self.safety_sets) + 1 + dut.Au.shape[0],) + assert dut.y.shape == (dut.num_cbf + 1 + dut.Au.shape[0],) check_members(dut) def test_calc_xi_Lambda_w_clf(self): @@ -271,9 +275,11 @@ def test_calc_xi_Lambda_w_clf(self): f=self.f, g=self.g, x=self.x, - safety_sets=self.safety_sets, + exclude_sets=self.exclude_sets, + within_set=self.within_set, Au=None, bu=None, + num_cbf=2, with_clf=True, use_y_squared=True, ) @@ -289,8 +295,8 @@ def test_calc_xi_Lambda_w_clf(self): kappa_V = 0.01 kappa_h = np.array([0.02, 0.03]) xi, lambda_mat = dut._calc_xi_Lambda(V=V, h=h, kappa_V=kappa_V, kappa_h=kappa_h) - assert xi.shape == (1 + len(self.safety_sets),) - assert lambda_mat.shape == (1 + len(self.safety_sets), dut.nu) + assert xi.shape == (1 + dut.num_cbf,) + assert lambda_mat.shape == (1 + dut.num_cbf, dut.nu) dhdx = np.empty((2, 3), dtype=object) dhdx[0] = h[0].Jacobian(self.x) dhdx[1] = h[1].Jacobian(self.x) @@ -314,9 +320,11 @@ def test_calc_xi_Lambda_wo_clf(self): f=self.f, g=self.g, x=self.x, - safety_sets=self.safety_sets, + exclude_sets=self.exclude_sets, + within_set=self.within_set, Au=None, bu=None, + num_cbf=2, with_clf=False, use_y_squared=True, ) @@ -330,8 +338,8 @@ def test_calc_xi_Lambda_wo_clf(self): kappa_V = None kappa_h = np.array([0.02, 0.03]) xi, lambda_mat = dut._calc_xi_Lambda(V=V, h=h, kappa_V=kappa_V, kappa_h=kappa_h) - assert xi.shape == (len(self.safety_sets),) - assert lambda_mat.shape == (len(self.safety_sets), dut.nu) + assert xi.shape == (dut.num_cbf,) + assert lambda_mat.shape == (dut.num_cbf, dut.nu) dhdx = np.empty((2, 3), dtype=object) dhdx[0] = h[0].Jacobian(self.x) dhdx[1] = h[1].Jacobian(self.x) @@ -352,9 +360,11 @@ def test_calc_xi_Lambda_w_clf_Aubu(self): f=self.f, g=self.g, x=self.x, - safety_sets=self.safety_sets, + exclude_sets=self.exclude_sets, + within_set=self.within_set, Au=np.array([[-3, 2], [1, 4], [3, 8.0]]), bu=np.array([3.0, 5.0, 10.0]), + num_cbf=2, with_clf=True, use_y_squared=True, ) @@ -416,9 +426,11 @@ def test_search_compatible_lagrangians_w_clf_y_squared(self): f=self.f, g=self.g, x=self.x, - safety_sets=self.safety_sets, + exclude_sets=self.exclude_sets, + within_set=self.within_set, Au=None, bu=None, + num_cbf=2, with_clf=True, use_y_squared=True, ) @@ -440,7 +452,7 @@ def test_search_compatible_lagrangians_w_clf_y_squared(self): rho_minus_V=mut.CompatibleLagrangianDegrees.Degree(x=4, y=2), h_plus_eps=[ mut.CompatibleLagrangianDegrees.Degree(x=4, y=2) - for _ in range(len(self.safety_sets)) + for _ in range(dut.num_cbf) ], state_eq_constraints=None, ) @@ -458,9 +470,11 @@ def test_add_compatibility_w_clf_y_squared(self): f=self.f, g=self.g, x=self.x, - safety_sets=self.safety_sets, + exclude_sets=self.exclude_sets, + within_set=self.within_set, Au=None, bu=None, + num_cbf=2, with_clf=True, use_y_squared=True, ) @@ -485,10 +499,7 @@ def test_add_compatibility_w_clf_y_squared(self): y_lagrangian = None rho_minus_V_lagrangian, _ = prog.NewSosPolynomial(dut.xy_set, degree=2) h_plus_eps_lagrangian = np.array( - [ - prog.NewSosPolynomial(dut.xy_set, degree=2)[0] - for _ in range(len(dut.safety_sets)) - ] + [prog.NewSosPolynomial(dut.xy_set, degree=2)[0] for _ in range(dut.num_cbf)] ) lagrangians = mut.CompatibleLagrangians( lambda_y=lambda_y_lagrangian, @@ -530,9 +541,11 @@ def test_add_barrier_exclude_constraint(self): f=self.f, g=self.g, x=self.x, - safety_sets=self.safety_sets, + exclude_sets=self.exclude_sets, + within_set=self.within_set, Au=None, bu=None, + num_cbf=2, with_clf=True, use_y_squared=True, ) @@ -540,7 +553,7 @@ def test_add_barrier_exclude_constraint(self): prog = solvers.MathematicalProgram() prog.AddIndeterminates(self.x) - safety_set_index = 0 + exclude_set_index = 0 h = sym.Polynomial(1 + 2 * self.x[0] * self.x[1]) lagrangians = mut.ExcludeRegionLagrangians( cbf=sym.Polynomial(1 + self.x[0]), @@ -549,10 +562,10 @@ def test_add_barrier_exclude_constraint(self): ) poly = dut._add_barrier_exclude_constraint( - prog, safety_set_index, h, lagrangians + prog, exclude_set_index, h, lagrangians ) poly_expected = -(1 + lagrangians.cbf) * h + lagrangians.unsafe_region.dot( - dut.safety_sets[safety_set_index].exclude + dut.exclude_sets[exclude_set_index].l ) assert poly.CoefficientsAlmostEqual(poly_expected, 1e-8) @@ -561,9 +574,11 @@ def test_certify_cbf_exclude(self): f=self.f, g=self.g, x=self.x, - safety_sets=self.safety_sets, + exclude_sets=self.exclude_sets, + within_set=self.within_set, Au=None, bu=None, + num_cbf=2, with_clf=True, use_y_squared=True, ) @@ -579,7 +594,7 @@ def test_certify_cbf_exclude(self): ) assert lagrangians is not None assert utils.is_sos(lagrangians.cbf) - for i in range(dut.safety_sets[0].exclude.size): + for i in range(dut.exclude_sets[0].l.size): assert utils.is_sos(lagrangians.unsafe_region[i]) def test_find_max_inner_ellipsoid(self): @@ -587,9 +602,11 @@ def test_find_max_inner_ellipsoid(self): f=self.f, g=self.g, x=self.x, - safety_sets=self.safety_sets, + exclude_sets=self.exclude_sets, + within_set=self.within_set, Au=None, bu=None, + num_cbf=1, with_clf=True, use_y_squared=True, ) @@ -645,9 +662,11 @@ def test_add_ellipsoid_in_compatible_region_constraint(self): f=self.f, g=self.g, x=self.x, - safety_sets=self.safety_sets, + exclude_sets=self.exclude_sets, + within_set=self.within_set, Au=None, bu=None, + num_cbf=1, with_clf=True, use_y_squared=True, ) @@ -691,11 +710,8 @@ def setup_class(cls): ) cls.g = np.array([[sym.Polynomial(1)], [sym.Polynomial(-1)]]) - cls.safety_sets = [ - mut.SafetySet( - exclude=np.array([sym.Polynomial(cls.x[0] + 10)]), within=None - ) - ] + cls.exclude_sets = [mut.ExcludeSet(np.array([sym.Polynomial(cls.x[0] + 10)]))] + cls.within_set = None cls.kappa_V = 0.001 cls.kappa_h = np.array([cls.kappa_V]) @@ -704,7 +720,7 @@ def setup_class(cls): def check_unsafe_region_by_sample(self, h: np.ndarray, x_samples): # Sample many points, make sure that {x | h[i] >= 0} doesn't intersect # with the i'th unsafe region. - for i, safety_set in enumerate(self.safety_sets): + for i, exclude_set in enumerate(self.exclude_sets): unsafe_flag = np.all( np.concatenate( [ @@ -712,7 +728,7 @@ def check_unsafe_region_by_sample(self, h: np.ndarray, x_samples): unsafe_region_j.EvaluateIndeterminates(self.x, x_samples.T) <= 0 ).reshape((-1, 1)) - for unsafe_region_j in safety_set.exclude + for unsafe_region_j in exclude_set.l ], axis=1, ), @@ -737,9 +753,11 @@ def search_lagrangians( f=self.f, g=self.g, x=self.x, - safety_sets=self.safety_sets, + exclude_sets=self.exclude_sets, + within_set=self.within_set, Au=None, bu=None, + num_cbf=1, with_clf=True, use_y_squared=use_y_squared, ) @@ -778,12 +796,11 @@ def search_lagrangians( ) safety_sets_lagrangian_degrees = [ mut.SafetySetLagrangianDegrees( - exclude=exclude_region_lagrangian_degrees, within=None + exclude=[exclude_region_lagrangian_degrees], within=[] ) ] safety_sets_lagrangians = dut.certify_cbf_safety_set( - safety_set_index=0, cbf=h_init[0], lagrangian_degrees=safety_sets_lagrangian_degrees[0], solver_options=None, @@ -1062,12 +1079,12 @@ def setup_class(cls): [sym.Polynomial(-1)], ] ) - cls.safety_sets = [ - mut.SafetySet( - exclude=np.array([sym.Polynomial(cls.x[0] + cls.x[1] + cls.x[2] + 3)]), - within=None, + cls.exclude_sets = [ + mut.ExcludeSet( + np.array([sym.Polynomial(cls.x[0] + cls.x[1] + cls.x[2] + 3)]) ) ] + cls.within_set = None cls.state_eq_constraints = np.array( [sym.Polynomial(cls.x[0] ** 2 + cls.x[1] ** 2 + 2 * cls.x[1])] ) @@ -1089,9 +1106,11 @@ def search_lagrangians(self, check_result=False) -> Tuple[ f=self.f, g=self.g, x=self.x, - safety_sets=self.safety_sets, + exclude_sets=self.exclude_sets, + within_set=self.within_set, Au=None, bu=None, + num_cbf=1, with_clf=True, use_y_squared=use_y_squared, state_eq_constraints=self.state_eq_constraints, @@ -1134,13 +1153,12 @@ def search_lagrangians(self, check_result=False) -> Tuple[ ) safety_sets_lagrangian_degrees = [ mut.SafetySetLagrangianDegrees( - exclude=exclude_region_lagrangian_degrees, within=None + exclude=[exclude_region_lagrangian_degrees], within=[] ) ] safety_sets_lagrangians = [ dut.certify_cbf_safety_set( - safety_set_index=0, cbf=h_init[0], lagrangian_degrees=safety_sets_lagrangian_degrees[0], solver_options=None, @@ -1149,13 +1167,13 @@ def search_lagrangians(self, check_result=False) -> Tuple[ assert safety_sets_lagrangians[0] is not None if check_result: assert utils.is_sos( - -(1 + safety_sets_lagrangians[0].exclude.cbf) * h_init[0] - + safety_sets_lagrangians[0].exclude.unsafe_region.dot( - self.safety_sets[0].exclude - ) - - safety_sets_lagrangians[0].exclude.state_eq_constraints.dot( - self.state_eq_constraints - ) + -(1 + safety_sets_lagrangians[0].exclude[0].cbf) * h_init[0] + + safety_sets_lagrangians[0] + .exclude[0] + .unsafe_region.dot(self.exclude_sets[0].l) + - safety_sets_lagrangians[0] + .exclude[0] + .state_eq_constraints.dot(self.state_eq_constraints) ) return ( dut,