diff --git a/compatible_clf_cbf/cbf.py b/compatible_clf_cbf/cbf.py index 33fc5ae..4e090f2 100644 --- a/compatible_clf_cbf/cbf.py +++ b/compatible_clf_cbf/cbf.py @@ -361,19 +361,19 @@ def _add_barrier_within_constraint( """ Adds the constraint that the 0-super level set of the barrier function is in the safe region {x | pᵢ(x) <= 0}. - −(1+ϕ₀(x))pᵢ(x) − ϕ₁(x)h(x) is sos. + −(1+ϕ₀(x))pᵢ(x) − ∑ᵢψᵢ(x)h(x) is sos. Note it doesn't add the constraints - ϕ₀(x) is sos - ϕ₁(x) is sos. + ϕ₀(x) is sos, ψᵢ(x) is sos. Args: safe_region_index: pᵢ(x) = self.safety_set.within[within_index] """ assert self.within_set is not None + assert lagrangians.cbf.size == 1 poly = ( -(1 + lagrangians.safe_region) * self.within_set.l[within_index] - - lagrangians.cbf * h + - lagrangians.cbf[0] * h ) if self.state_eq_constraints is not None: assert lagrangians.state_eq_constraints is not None @@ -395,12 +395,12 @@ def _add_barrier_exclude_constraint( polynomials p(x), we want to certify that the set {x|p(x)≤0, h(x)≥0} is empty. The emptiness of the set can be certified by the constraint - -(1+ϕ₀(x))h(x) +∑ⱼϕⱼ(x)pⱼ(x) is sos - ϕ₀(x), ϕⱼ(x) are sos. + -∑ᵢϕᵢ(x))*hᵢ(x) +∑ⱼψⱼ(x)pⱼ(x)-1 is sos + ϕᵢ(x), ψⱼ(x) are sos. Note that this function only adds the constraint - -(1+ϕ₀(x))*hᵢ(x) +∑ⱼϕⱼ(x)pⱼ(x) is sos - It doesn't add the constraint ϕ₀(x), ϕⱼ(x) are sos. + -(1+ϕᵢ,₀(x))*hᵢ(x) +∑ⱼϕᵢ,ⱼ(x)pⱼ(x) is sos + It doesn't add the constraint ϕᵢ,₀(x), ϕᵢ,ⱼ(x) are sos. Args: h: a polynomial, h is the barrier function for the @@ -409,7 +409,8 @@ def _add_barrier_exclude_constraint( Returns: poly: poly is the polynomial -(1+ϕ₀(x))hᵢ(x) + ∑ⱼϕⱼ(x)pⱼ(x) """ - poly = -(1 + lagrangians.cbf) * h + lagrangians.unsafe_region.dot( + assert lagrangians.cbf.size == 1 + poly = -(1 + lagrangians.cbf[0]) * h + lagrangians.unsafe_region.dot( self.exclude_sets[exclude_set_index].l ) if self.state_eq_constraints is not None: diff --git a/compatible_clf_cbf/clf_cbf.py b/compatible_clf_cbf/clf_cbf.py index 9dfd37c..c9902b5 100644 --- a/compatible_clf_cbf/clf_cbf.py +++ b/compatible_clf_cbf/clf_cbf.py @@ -244,14 +244,14 @@ class WithinRegionLagrangians: The Lagrangians for certifying that the 0-super level set of a CBF is a subset of the safe region. - for a CBF h(x), to prove that the 0-super level set {x | h(x) >= 0} is a - subset of the safe set {x | p(x) <= 0}, we can impose the constraint - −(1+ϕ₀(x))p(x) − ϕ₁(x)h(x) is sos. + for a CBF h(x), to prove that the 0-super level set {x | hᵢ(x) >= 0, ∀i} is + a subset of the safe set {x | p(x) <= 0}, we can impose the constraint + −(1+ϕ₀(x))p(x) − ∑ᵢψᵢ(x)hᵢ(x) is sos. ϕ₀(x) is sos - ϕ₁(x) is sos. + ψᵢ(x) is sos. """ - cbf: sym.Polynomial + cbf: np.ndarray safe_region: sym.Polynomial state_eq_constraints: Optional[np.ndarray] @@ -277,7 +277,7 @@ def get_result( @dataclass class WithinRegionLagrangianDegrees: - cbf: int + cbf: List[int] safe_region: int state_eq_constraints: Optional[List[int]] @@ -285,10 +285,12 @@ def to_lagrangians( self, prog: solvers.MathematicalProgram, x_set: sym.Variables, - cbf_lagrangian: Optional[sym.Polynomial] = None, + cbf_lagrangian: Optional[np.ndarray] = None, ) -> WithinRegionLagrangians: if cbf_lagrangian is None: - cbf, _ = new_sos_polynomial(prog, x_set, self.cbf) + cbf = np.array( + [new_sos_polynomial(prog, x_set, degree)[0] for degree in self.cbf] + ) else: cbf = cbf_lagrangian @@ -318,18 +320,18 @@ class ExcludeRegionLagrangians: intersect with an unsafe region. For a CBF function hᵢ(x), to prove that the 0-super level set - {x |hᵢ(x) >= 0} doesn't intersect with an unsafe set + {x |hᵢ(x) >= 0, ∀i} doesn't intersect with an unsafe set {x | pⱼ(x) <= 0 for all j}, we impose the condition: - -(1+ϕᵢ,₀(x))*hᵢ(x) +∑ⱼϕᵢ,ⱼ(x)pⱼ(x) is sos - ϕᵢ,₀(x), ϕᵢ,ⱼ(x) are sos. + -∑ᵢϕᵢ(x))*hᵢ(x) +∑ⱼψⱼ(x)pⱼ(x)-1 is sos + ϕᵢ(x), ψⱼ(x) are sos. """ # The Lagrangian that multiplies with CBF function. - # ϕᵢ,₀(x) in the documentation above. - cbf: sym.Polynomial + # ϕᵢ(x) in the documentation above. + cbf: np.ndarray # An array of sym.Polynomial. The Lagrangians that multiply the unsafe region - # polynomials. ϕᵢ,ⱼ(x) in the documentation above. + # polynomials. ψⱼ(x) in the documentation above. unsafe_region: np.ndarray # The free Lagrangian that multiplies with the state equality constraints # (such as sin²θ+cos²θ=1) @@ -357,7 +359,7 @@ def get_result( @dataclass class ExcludeRegionLagrangianDegrees: - cbf: int + cbf: List[int] unsafe_region: List[int] state_eq_constraints: Optional[List[int]] @@ -365,10 +367,12 @@ def to_lagrangians( self, prog: solvers.MathematicalProgram, x_set: sym.Variables, - cbf_lagrangian: Optional[sym.Polynomial] = None, + cbf_lagrangian: Optional[np.ndarray] = None, ) -> ExcludeRegionLagrangians: if cbf_lagrangian is None: - cbf, _ = new_sos_polynomial(prog, x_set, self.cbf) + cbf = np.array( + [new_sos_polynomial(prog, x_set, degree)[0] for degree in self.cbf] + ) else: cbf = cbf_lagrangian @@ -440,7 +444,7 @@ def get_result( def get_cbf_lagrangians( self, - ) -> Tuple[List[sym.Polynomial], List[sym.Polynomial]]: + ) -> Tuple[List[np.ndarray], List[np.ndarray]]: exclude_cbf = [exclude.cbf for exclude in self.exclude] within_cbf = [within.cbf for within in self.within] return exclude_cbf, within_cbf @@ -455,9 +459,7 @@ def to_lagrangians( self, prog: solvers.MathematicalProgram, x_set: sym.Variables, - cbf_lagrangian: Optional[ - Tuple[List[sym.Polynomial], List[sym.Polynomial]] - ] = None, + cbf_lagrangian: Optional[Tuple[List[np.ndarray], List[np.ndarray]]] = None, ) -> SafetySetLagrangians: exclude = [ self.exclude[i].to_lagrangians( @@ -764,7 +766,7 @@ def __init__( def certify_cbf_safety_set( self, - cbf: sym.Polynomial, + h: np.ndarray, lagrangian_degrees: SafetySetLagrangianDegrees, solver_id: Optional[solvers.SolverId] = None, solver_options: Optional[solvers.SolverOptions] = None, @@ -779,7 +781,7 @@ def certify_cbf_safety_set( exclude_lagrangians = [ self.certify_cbf_exclude( i, - cbf, + h, lagrangian_degrees.exclude[i], solver_id, solver_options, @@ -796,7 +798,7 @@ def certify_cbf_safety_set( within_lagrangians = [ self.certify_cbf_within( i, - cbf, + h, lagrangian_degrees.within[i], solver_id, solver_options, @@ -814,7 +816,7 @@ def certify_cbf_safety_set( def certify_cbf_within( self, within_index: int, - cbf: sym.Polynomial, + h: np.ndarray, lagrangian_degrees: WithinRegionLagrangianDegrees, solver_id: Optional[solvers.SolverId] = None, solver_options: Optional[solvers.SolverOptions] = None, @@ -827,7 +829,7 @@ def certify_cbf_within( prog = solvers.MathematicalProgram() prog.AddIndeterminates(self.x_set) lagrangians = lagrangian_degrees.to_lagrangians(prog, self.x_set) - self._add_barrier_within_constraint(prog, within_index, cbf, lagrangians) + self._add_barrier_within_constraint(prog, within_index, h, lagrangians) result = solve_with_id(prog, solver_id, solver_options) lagrangians_result = ( lagrangians.get_result(result, lagrangian_coefficient_tol) @@ -839,7 +841,7 @@ def certify_cbf_within( def certify_cbf_exclude( self, exclude_set_index: int, - cbf: sym.Polynomial, + h: np.ndarray, lagrangian_degrees: ExcludeRegionLagrangianDegrees, solver_id: Optional[solvers.SolverId] = None, solver_options: Optional[solvers.SolverOptions] = None, @@ -863,7 +865,7 @@ def certify_cbf_exclude( prog = solvers.MathematicalProgram() prog.AddIndeterminates(self.x_set) lagrangians = lagrangian_degrees.to_lagrangians(prog, self.x_set) - self._add_barrier_exclude_constraint(prog, exclude_set_index, cbf, lagrangians) + self._add_barrier_exclude_constraint(prog, exclude_set_index, h, lagrangians) result = solve_with_id(prog, solver_id, solver_options) lagrangians_result = ( lagrangians.get_result(result, lagrangian_coefficient_tol) @@ -930,7 +932,7 @@ def search_lagrangians_given_clf_cbf( kappa_h: np.ndarray, barrier_eps: np.ndarray, compatible_lagrangian_degrees: CompatibleLagrangianDegrees, - safety_set_lagrangian_degrees: List[SafetySetLagrangianDegrees], + safety_set_lagrangian_degrees: SafetySetLagrangianDegrees, solver_id: Optional[solvers.SolverId] = None, solver_options: Optional[solvers.SolverOptions] = None, lagrangian_coefficient_tol: Optional[float] = None, @@ -938,7 +940,7 @@ def search_lagrangians_given_clf_cbf( compatible_sos_type=solvers.MathematicalProgram.NonnegativePolynomial.kSos, ) -> Tuple[ Optional[CompatibleLagrangians], - List[Optional[SafetySetLagrangians]], + Optional[SafetySetLagrangians], ]: ( prog_compatible, @@ -963,25 +965,21 @@ def search_lagrangians_given_clf_cbf( else None ) - safety_set_lagrangians_result: List[Optional[SafetySetLagrangians]] = [ - None - ] * self.num_cbf - for i in range(self.num_cbf): - safety_set_lagrangians_result[i] = self.certify_cbf_safety_set( - h[i], - safety_set_lagrangian_degrees[i], - solver_id, - solver_options, - lagrangian_coefficient_tol, - ) + safety_set_lagrangians_result = self.certify_cbf_safety_set( + h, + safety_set_lagrangian_degrees, + solver_id, + solver_options, + lagrangian_coefficient_tol, + ) return compatible_lagrangians_result, safety_set_lagrangians_result def search_clf_cbf_given_lagrangian( self, compatible_lagrangians: CompatibleLagrangians, compatible_lagrangian_degrees: CompatibleLagrangianDegrees, - safety_sets_lagrangians: List[SafetySetLagrangians], - safety_sets_lagrangian_degrees: List[SafetySetLagrangianDegrees], + safety_sets_lagrangians: SafetySetLagrangians, + safety_sets_lagrangian_degrees: SafetySetLagrangianDegrees, clf_degree: Optional[int], cbf_degrees: List[int], x_equilibrium: Optional[np.ndarray], @@ -1048,8 +1046,8 @@ def binary_search_clf_cbf( self, compatible_lagrangians: CompatibleLagrangians, compatible_lagrangian_degrees: CompatibleLagrangianDegrees, - safety_sets_lagrangians: List[SafetySetLagrangians], - safety_sets_lagrangian_degrees: List[SafetySetLagrangianDegrees], + safety_sets_lagrangians: SafetySetLagrangians, + safety_sets_lagrangian_degrees: SafetySetLagrangianDegrees, clf_degree: Optional[int], cbf_degrees: List[int], x_equilibrium: Optional[np.ndarray], @@ -1177,7 +1175,7 @@ def bilinear_alternation( V_init: Optional[sym.Polynomial], h_init: np.ndarray, compatible_lagrangian_degrees: CompatibleLagrangianDegrees, - safety_sets_lagrangian_degrees: List[SafetySetLagrangianDegrees], + safety_sets_lagrangian_degrees: SafetySetLagrangianDegrees, kappa_V: Optional[float], kappa_h: np.ndarray, barrier_eps: np.ndarray, @@ -1240,9 +1238,7 @@ def bilinear_alternation( cbf = h_init compatible_lagrangians = None - safety_sets_lagrangians: List[Optional[SafetySetLagrangians]] = [ - None - ] * self.num_cbf + safety_sets_lagrangians = None def evaluate_compatible_states(clf_fun, cbf_funs, x_val): if clf_fun is not None: @@ -1283,7 +1279,7 @@ def evaluate_compatible_states(clf_fun, cbf_funs, x_val): compatible_sos_type=compatible_sos_type, ) assert compatible_lagrangians is not None - assert all(safety_sets_lagrangians) + assert safety_sets_lagrangians is not None if inner_ellipsoid_options is not None: # We use the heuristics to grow the inner ellipsoid. @@ -1316,6 +1312,7 @@ def evaluate_compatible_states(clf_fun, cbf_funs, x_val): compatible_lagrangians, compatible_lagrangian_degrees, safety_sets_lagrangians, + safety_sets_lagrangian_degrees, clf_degree, cbf_degrees, x_equilibrium, @@ -1572,26 +1569,24 @@ def _add_barrier_within_constraint( self, prog: solvers.MathematicalProgram, within_index: int, - h: sym.Polynomial, + h: np.ndarray, lagrangians: WithinRegionLagrangians, ) -> sym.Polynomial: """ Adds the constraint that the 0-super level set of the barrier function is in the safe region {x | pᵢ(x) <= 0}. - −(1+ϕ₀(x))pᵢ(x) − ϕ₁(x)h(x) is sos. + −(1+ϕ₀(x))pᵢ(x) − ∑ᵢψᵢ(x)h(x) is sos. Note it doesn't add the constraints - ϕ₀(x) is sos - ϕ₁(x) is sos. + ϕ₀(x) is sos, ψᵢ(x) is sos. Args: within_index: pᵢ(x) = self.within_set.l[within_index] """ assert self.within_set is not None - poly = ( - -(1 + lagrangians.safe_region) * self.within_set.l[within_index] - - lagrangians.cbf * h - ) + poly = -(1 + lagrangians.safe_region) * self.within_set.l[ + within_index + ] - lagrangians.cbf.dot(h) if self.state_eq_constraints is not None: assert lagrangians.state_eq_constraints is not None poly -= lagrangians.state_eq_constraints.dot(self.state_eq_constraints) @@ -1602,18 +1597,18 @@ def _add_barrier_exclude_constraint( self, prog: solvers.MathematicalProgram, exclude_set_index: int, - h: sym.Polynomial, + h: np.ndarray, lagrangians: ExcludeRegionLagrangians, ) -> sym.Polynomial: """ Adds the constraint that the 0-superlevel set of the barrier function does not intersect with the exclude region. 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} + polynomials p(x), we want to certify that the set {x|p(x)≤0, hᵢ(x)≥0, ∀i} is empty. The emptiness of the set can be certified by the constraint - -(1+ϕᵢ,₀(x))hᵢ(x) +∑ⱼϕᵢ,ⱼ(x)pⱼ(x) is sos - ϕᵢ,₀(x), ϕᵢ,ⱼ(x) are sos. + -∑ᵢϕᵢ(x))*hᵢ(x) +∑ⱼψⱼ(x)pⱼ(x)-1 is sos + ϕᵢ(x), ψⱼ(x) are sos. Note that this function only adds the constraint -(1+ϕᵢ,₀(x))*hᵢ(x) +∑ⱼϕᵢ,ⱼ(x)pⱼ(x) is sos @@ -1627,14 +1622,16 @@ def _add_barrier_exclude_constraint( 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) + poly: poly is the polynomial -∑ᵢϕᵢ(x))*hᵢ(x) +∑ⱼψⱼ(x)pⱼ(x)-1 """ assert lagrangians.unsafe_region.size == len( self.exclude_sets[exclude_set_index].l ) - poly = -(1 + lagrangians.cbf) * h + lagrangians.unsafe_region.dot( - self.exclude_sets[exclude_set_index].l - ) + if self.num_cbf == 1: + poly = -(1 + lagrangians.cbf[0]) * h[0] + else: + poly = -1 - lagrangians.cbf.dot(h) + poly += lagrangians.unsafe_region.dot(self.exclude_sets[exclude_set_index].l) if self.state_eq_constraints is not None: assert lagrangians.state_eq_constraints is not None poly -= lagrangians.state_eq_constraints.dot(self.state_eq_constraints) @@ -1645,8 +1642,8 @@ def _construct_search_clf_cbf_program( self, compatible_lagrangians: CompatibleLagrangians, compatible_lagrangian_degrees: CompatibleLagrangianDegrees, - safety_sets_lagrangians: List[SafetySetLagrangians], - safety_sets_lagrangian_degrees: List[SafetySetLagrangianDegrees], + safety_sets_lagrangians: SafetySetLagrangians, + safety_sets_lagrangian_degrees: SafetySetLagrangianDegrees, clf_degree: Optional[int], cbf_degrees: List[int], x_equilibrium: Optional[np.ndarray], @@ -1668,13 +1665,12 @@ def _construct_search_clf_cbf_program( Args: 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 safety set. + safety_sets_lagrangian: The Lagrangians certifying that the 0-super + level set of the 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) == self.num_cbf assert len(cbf_degrees) == self.num_cbf prog = solvers.MathematicalProgram() prog.AddIndeterminates(self.xy_set) @@ -1710,25 +1706,23 @@ 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] * 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) - 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, - exclude_set_index, - h[i], - safety_sets_lagrangians_new[i].exclude[exclude_set_index], + cbf_lagrangian = safety_sets_lagrangians.get_cbf_lagrangians() + safety_sets_lagrangians_new = safety_sets_lagrangian_degrees.to_lagrangians( + prog, self.x_set, cbf_lagrangian + ) + assert len(safety_sets_lagrangians_new.exclude) == len(self.exclude_sets) + for exclude_set_index in range(len(self.exclude_sets)): + self._add_barrier_exclude_constraint( + prog, + exclude_set_index, + h, + safety_sets_lagrangians_new.exclude[exclude_set_index], + ) + if self.within_set is not None: + for j in range(self.within_set.l.size): + self._add_barrier_within_constraint( + prog, j, h, safety_sets_lagrangians_new.within[j] ) - if self.within_set is not None: - for j in range(self.within_set.l.size): - self._add_barrier_within_constraint( - prog, j, h[i], safety_sets_lagrangians_new[i].within[j] - ) # We can search for some compatible Lagrangians as well, including the # Lagrangians for y >= 0 and the state equality constraints, as y>= 0 diff --git a/examples/linear_toy/linear_toy_demo.py b/examples/linear_toy/linear_toy_demo.py index c2cdec6..57d5ee2 100644 --- a/examples/linear_toy/linear_toy_demo.py +++ b/examples/linear_toy/linear_toy_demo.py @@ -57,13 +57,13 @@ def search_barrier_safe_lagrangians( clf_cbf.SafetySetLagrangianDegrees( exclude=[ clf_cbf.ExcludeRegionLagrangianDegrees( - cbf=2, unsafe_region=[2], state_eq_constraints=None + cbf=[2], unsafe_region=[2], state_eq_constraints=None ) ], within=[], ) ] - lagrangians = dut.certify_cbf_safety_set(h[0], lagrangian_degrees[0]) + lagrangians = dut.certify_cbf_safety_set(h, lagrangian_degrees[0]) assert lagrangians is not None return [lagrangians] 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 986aa33..95a0084 100644 --- a/examples/linear_toy/linear_toy_w_input_limits_demo.py +++ b/examples/linear_toy/linear_toy_w_input_limits_demo.py @@ -58,13 +58,13 @@ def search_barrier_safe_lagrangians( clf_cbf.SafetySetLagrangianDegrees( exclude=[ clf_cbf.ExcludeRegionLagrangianDegrees( - cbf=2, unsafe_region=[2], state_eq_constraints=None + cbf=[2], unsafe_region=[2], state_eq_constraints=None ) ], within=[], ) ] - lagrangians = dut.certify_cbf_safety_set(h[0], lagrangian_degrees[0]) + lagrangians = dut.certify_cbf_safety_set(h, lagrangian_degrees[0]) assert lagrangians is not None return [lagrangians] diff --git a/examples/nonlinear_toy/demo.py b/examples/nonlinear_toy/demo.py index 3ef3481..d4855a2 100644 --- a/examples/nonlinear_toy/demo.py +++ b/examples/nonlinear_toy/demo.py @@ -71,7 +71,7 @@ def main(use_y_squared: bool, with_u_bound: bool): clf_cbf.SafetySetLagrangianDegrees( exclude=[ clf_cbf.ExcludeRegionLagrangianDegrees( - cbf=0, unsafe_region=[0], state_eq_constraints=None + cbf=[0], unsafe_region=[0], state_eq_constraints=None ) ], within=[], @@ -80,7 +80,7 @@ def main(use_y_squared: bool, with_u_bound: bool): safety_sets_lagrangians = [ compatible.certify_cbf_safety_set( - cbf=h_init[0], + h=h_init, 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 6913e14..65791bb 100644 --- a/examples/nonlinear_toy/demo_trigpoly.py +++ b/examples/nonlinear_toy/demo_trigpoly.py @@ -153,16 +153,15 @@ def search(unit_test_flag: bool = False): h_plus_eps=[clf_cbf.CompatibleLagrangianDegrees.Degree(x=2, y=2)], state_eq_constraints=[clf_cbf.CompatibleLagrangianDegrees.Degree(x=2, y=2)], ) - safety_sets_lagrangian_degrees = [ - clf_cbf.SafetySetLagrangianDegrees( - exclude=[ - clf_cbf.ExcludeRegionLagrangianDegrees( - cbf=0, unsafe_region=[0], state_eq_constraints=[0] - ) - ], - within=[], - ) - ] + safety_sets_lagrangian_degrees = clf_cbf.SafetySetLagrangianDegrees( + exclude=[ + clf_cbf.ExcludeRegionLagrangianDegrees( + cbf=[0], unsafe_region=[0], state_eq_constraints=[0] + ) + ], + within=[], + ) + kappa_V = 0.1 kappa_h = np.array([0.1]) barrier_eps = np.array([0.001]) diff --git a/examples/nonlinear_toy/synthesize_demo.py b/examples/nonlinear_toy/synthesize_demo.py index 917ef65..80155a7 100644 --- a/examples/nonlinear_toy/synthesize_demo.py +++ b/examples/nonlinear_toy/synthesize_demo.py @@ -49,16 +49,15 @@ def main(with_u_bound: bool): h_plus_eps=[clf_cbf.CompatibleLagrangianDegrees.Degree(x=2, y=2)], state_eq_constraints=None, ) - safety_sets_lagrangian_degrees = [ - clf_cbf.SafetySetLagrangianDegrees( - exclude=[ - clf_cbf.ExcludeRegionLagrangianDegrees( - cbf=0, unsafe_region=[0], state_eq_constraints=None - ) - ], - within=[], - ) - ] + safety_sets_lagrangian_degrees = clf_cbf.SafetySetLagrangianDegrees( + exclude=[ + clf_cbf.ExcludeRegionLagrangianDegrees( + cbf=[0], unsafe_region=[0], state_eq_constraints=None + ) + ], + within=[], + ) + x_equilibrium = np.array([0.0, 0.0]) clf_degree = 2 diff --git a/examples/power_converter/demo.py b/examples/power_converter/demo.py index 0a2a262..a440a98 100644 --- a/examples/power_converter/demo.py +++ b/examples/power_converter/demo.py @@ -120,17 +120,16 @@ def search(use_y_squared: bool): state_eq_constraints=None, ) - safety_sets_lagrangian_degrees = [ - clf_cbf.SafetySetLagrangianDegrees( - exclude=[], - within=[ - clf_cbf.WithinRegionLagrangianDegrees( - cbf=0, safe_region=0, state_eq_constraints=None - ) - for _ in range(3) - ], - ) - ] + safety_sets_lagrangian_degrees = clf_cbf.SafetySetLagrangianDegrees( + exclude=[], + within=[ + clf_cbf.WithinRegionLagrangianDegrees( + cbf=[0], safe_region=0, state_eq_constraints=None + ) + for _ in range(3) + ], + ) + barrier_eps = np.array([0.0]) x_equilibrium = np.zeros((3,)) diff --git a/examples/quadrotor2d/demo.py b/examples/quadrotor2d/demo.py index e23ab2d..d0c7f7a 100644 --- a/examples/quadrotor2d/demo.py +++ b/examples/quadrotor2d/demo.py @@ -75,16 +75,15 @@ def main(use_y_squared: bool, with_u_bound: bool): h_plus_eps=[clf_cbf.CompatibleLagrangianDegrees.Degree(x=2, y=2)], state_eq_constraints=[clf_cbf.CompatibleLagrangianDegrees.Degree(x=2, y=2)], ) - safety_sets_lagrangian_degrees = [ - clf_cbf.SafetySetLagrangianDegrees( - exclude=[ - clf_cbf.ExcludeRegionLagrangianDegrees( - cbf=0, unsafe_region=[0], state_eq_constraints=[0] - ) - ], - within=[], - ) - ] + safety_sets_lagrangian_degrees = clf_cbf.SafetySetLagrangianDegrees( + exclude=[ + clf_cbf.ExcludeRegionLagrangianDegrees( + cbf=[0], unsafe_region=[0], state_eq_constraints=[0] + ) + ], + within=[], + ) + barrier_eps = np.array([0.000]) x_equilibrium = np.zeros((7,)) compatible_states_options = clf_cbf.CompatibleStatesOptions( diff --git a/examples/quadrotor2d/demo_taylor.py b/examples/quadrotor2d/demo_taylor.py index 5476d9f..b3f1bfe 100644 --- a/examples/quadrotor2d/demo_taylor.py +++ b/examples/quadrotor2d/demo_taylor.py @@ -89,16 +89,15 @@ def search_clf_cbf( ) barrier_eps = np.array([0.0]) - safety_sets_lagrangian_degrees = [ - clf_cbf.SafetySetLagrangianDegrees( - exclude=[ - clf_cbf.ExcludeRegionLagrangianDegrees( - cbf=0, unsafe_region=[2], state_eq_constraints=None - ) - ], - within=[], - ) - ] + safety_sets_lagrangian_degrees = clf_cbf.SafetySetLagrangianDegrees( + exclude=[ + clf_cbf.ExcludeRegionLagrangianDegrees( + cbf=[0], unsafe_region=[2], state_eq_constraints=None + ) + ], + within=[], + ) + solver_options = solvers.SolverOptions() solver_options.SetOption(solvers.CommonSolverOption.kPrintToConsole, True) if grow_heuristics == GrowHeuristics.kInnerEllipsoid: diff --git a/examples/single_integrator/wo_input_limit_demo.py b/examples/single_integrator/wo_input_limit_demo.py index da134be..5a68e60 100644 --- a/examples/single_integrator/wo_input_limit_demo.py +++ b/examples/single_integrator/wo_input_limit_demo.py @@ -112,7 +112,7 @@ def certify_clf_cbf_separately( clf_cbf.SafetySetLagrangianDegrees( exclude=[ clf_cbf.ExcludeRegionLagrangianDegrees( - cbf=0, unsafe_region=[0], state_eq_constraints=None + cbf=[0], unsafe_region=[0], state_eq_constraints=None ) ], within=[], diff --git a/tests/test_cbf.py b/tests/test_cbf.py index 8f44026..6a4b986 100644 --- a/tests/test_cbf.py +++ b/tests/test_cbf.py @@ -82,7 +82,7 @@ def test_add_barrier_safe_constraint(self): prog.AddIndeterminates(dut.x_set) lagrangians = ExcludeRegionLagrangians( - cbf=sym.Polynomial(1 + self.x[0]), + cbf=np.array([sym.Polynomial(1 + self.x[0])]), unsafe_region=np.array( [sym.Polynomial(2 + self.x[0]), sym.Polynomial(3 - self.x[1])] ), @@ -93,7 +93,7 @@ def test_add_barrier_safe_constraint(self): poly = dut._add_barrier_exclude_constraint( prog, exclude_set_index, h, lagrangians ) - poly_expected = -(1 + lagrangians.cbf) * h + lagrangians.unsafe_region.dot( + poly_expected = -(1 + lagrangians.cbf[0]) * h + lagrangians.unsafe_region.dot( dut.exclude_sets[exclude_set_index].l ) assert poly.CoefficientsAlmostEqual(poly_expected, 1e-8) diff --git a/tests/test_clf_cbf.py b/tests/test_clf_cbf.py index 03cf839..607be2a 100644 --- a/tests/test_clf_cbf.py +++ b/tests/test_clf_cbf.py @@ -1,6 +1,6 @@ import compatible_clf_cbf.clf_cbf as mut -from typing import List, Tuple +from typing import Tuple import numpy as np import pytest # noqa @@ -554,9 +554,9 @@ def test_add_barrier_exclude_constraint(self): prog.AddIndeterminates(self.x) exclude_set_index = 0 - h = sym.Polynomial(1 + 2 * self.x[0] * self.x[1]) + h = np.array([sym.Polynomial(1 + 2 * self.x[0] * self.x[1])]) lagrangians = mut.ExcludeRegionLagrangians( - cbf=sym.Polynomial(1 + self.x[0]), + cbf=np.array([sym.Polynomial(1 + self.x[0])]), unsafe_region=np.array([sym.Polynomial(2 + self.x[0])]), state_eq_constraints=None, ) @@ -564,9 +564,9 @@ def test_add_barrier_exclude_constraint(self): poly = dut._add_barrier_exclude_constraint( prog, exclude_set_index, h, lagrangians ) - poly_expected = -(1 + lagrangians.cbf) * h + lagrangians.unsafe_region.dot( - dut.exclude_sets[exclude_set_index].l - ) + poly_expected = -( + 1 + lagrangians.cbf[0] * h[0] + ) + lagrangians.unsafe_region.dot(dut.exclude_sets[exclude_set_index].l) assert poly.CoefficientsAlmostEqual(poly_expected, 1e-8) def test_certify_cbf_exclude(self): @@ -578,22 +578,22 @@ def test_certify_cbf_exclude(self): within_set=self.within_set, Au=None, bu=None, - num_cbf=2, + num_cbf=1, with_clf=True, use_y_squared=True, ) cbf = sym.Polynomial(1 - self.x.dot(self.x)) exclude_region_lagrangian_degrees = mut.ExcludeRegionLagrangianDegrees( - cbf=2, unsafe_region=[2], state_eq_constraints=None + cbf=[2], unsafe_region=[2], state_eq_constraints=None ) lagrangians = dut.certify_cbf_exclude( 0, - cbf, + np.array([cbf]), exclude_region_lagrangian_degrees, ) assert lagrangians is not None - assert utils.is_sos(lagrangians.cbf) + assert utils.is_sos(lagrangians.cbf[0]) for i in range(dut.exclude_sets[0].l.size): assert utils.is_sos(lagrangians.unsafe_region[i]) @@ -743,8 +743,8 @@ def search_lagrangians( mut.CompatibleClfCbf, mut.CompatibleLagrangians, mut.CompatibleLagrangianDegrees, - List[mut.SafetySetLagrangians], - List[mut.SafetySetLagrangianDegrees], + mut.SafetySetLagrangians, + mut.SafetySetLagrangianDegrees, sym.Polynomial, np.ndarray, ]: @@ -792,17 +792,15 @@ def search_lagrangians( compatible_result, coefficient_tol=1e-5 ) exclude_region_lagrangian_degrees = mut.ExcludeRegionLagrangianDegrees( - cbf=0, unsafe_region=[0], state_eq_constraints=None + cbf=[0], unsafe_region=[0], state_eq_constraints=None + ) + safety_sets_lagrangian_degrees = mut.SafetySetLagrangianDegrees( + exclude=[exclude_region_lagrangian_degrees], within=[] ) - safety_sets_lagrangian_degrees = [ - mut.SafetySetLagrangianDegrees( - exclude=[exclude_region_lagrangian_degrees], within=[] - ) - ] safety_sets_lagrangians = dut.certify_cbf_safety_set( - cbf=h_init[0], - lagrangian_degrees=safety_sets_lagrangian_degrees[0], + h=h_init, + lagrangian_degrees=safety_sets_lagrangian_degrees, solver_options=None, ) @@ -812,7 +810,7 @@ def search_lagrangians( dut, compatible_lagrangians_result, lagrangian_degrees, - [safety_sets_lagrangians], + safety_sets_lagrangians, safety_sets_lagrangian_degrees, V_init, h_init, @@ -1096,8 +1094,8 @@ def search_lagrangians(self, check_result=False) -> Tuple[ mut.CompatibleClfCbf, mut.CompatibleLagrangians, mut.CompatibleLagrangianDegrees, - List[mut.SafetySetLagrangians], - List[mut.SafetySetLagrangianDegrees], + mut.SafetySetLagrangians, + mut.SafetySetLagrangianDegrees, sym.Polynomial, np.ndarray, ]: @@ -1149,31 +1147,29 @@ def search_lagrangians(self, check_result=False) -> Tuple[ ) exclude_region_lagrangian_degrees = mut.ExcludeRegionLagrangianDegrees( - cbf=0, unsafe_region=[0], state_eq_constraints=[0] + cbf=[0], unsafe_region=[0], state_eq_constraints=[0] + ) + safety_sets_lagrangian_degrees = mut.SafetySetLagrangianDegrees( + exclude=[exclude_region_lagrangian_degrees], within=[] ) - safety_sets_lagrangian_degrees = [ - mut.SafetySetLagrangianDegrees( - exclude=[exclude_region_lagrangian_degrees], within=[] - ) - ] - safety_sets_lagrangians = [ - dut.certify_cbf_safety_set( - cbf=h_init[0], - lagrangian_degrees=safety_sets_lagrangian_degrees[0], - solver_options=None, - ) - ] - assert safety_sets_lagrangians[0] is not None + safety_sets_lagrangians = dut.certify_cbf_safety_set( + h=h_init, + lagrangian_degrees=safety_sets_lagrangian_degrees, + solver_options=None, + ) + + assert safety_sets_lagrangians is not None if check_result: assert utils.is_sos( - -(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) + -1 + - safety_sets_lagrangians.exclude[0].cbf.dot(h_init) + + safety_sets_lagrangians.exclude[0].unsafe_region.dot( + self.exclude_sets[0].l + ) + - safety_sets_lagrangians.exclude[0].state_eq_constraints.dot( + self.state_eq_constraints + ) ) return ( dut,