diff --git a/compatible_clf_cbf/clf_cbf.py b/compatible_clf_cbf/clf_cbf.py index c9902b5..ef801dc 100644 --- a/compatible_clf_cbf/clf_cbf.py +++ b/compatible_clf_cbf/clf_cbf.py @@ -12,7 +12,7 @@ import os import os.path import pickle -from typing import List, Optional, Tuple +from typing import List, Optional, Tuple, Union from typing_extensions import Self import numpy as np @@ -56,7 +56,7 @@ class CompatibleLagrangians: rho_minus_V: Optional[sym.Polynomial] # The Lagrangian polynomials multiplies with h(x)+ε. Should be an array of SOS # polynomials. - h_plus_eps: Optional[np.ndarray] + h_plus_eps: np.ndarray # The free Lagrangian polynomials multiplying the state equality # constraints. state_eq_constraints: Optional[np.ndarray] @@ -81,10 +81,8 @@ def get_result( if self.rho_minus_V is not None else None ) - h_plus_eps_result = ( - get_polynomial_result(result, self.h_plus_eps, coefficient_tol) - if self.h_plus_eps is not None - else None + h_plus_eps_result = get_polynomial_result( + result, self.h_plus_eps, coefficient_tol ) state_eq_constraints_result = ( get_polynomial_result(result, self.state_eq_constraints, coefficient_tol) @@ -102,51 +100,101 @@ def get_result( @dataclass -class CompatibleLagrangianDegrees: +class XYDegree: """ - The degree of the Lagrangian multipliers in CompatibleLagrangians. + The degree of each Lagrangian polynomial in indeterminates x and y. For + example, if we have a polynomial x₀²x₁y₂ + 3x₀y₁y₂³, its degree in x is + 3 (from x₀²x₁), and its degree in y is 4 (from y₁y₂³) """ - @dataclass - class Degree: + x: int + y: int + + def construct_polynomial( + self, + prog: solvers.MathematicalProgram, + x: sym.Variables, + y: sym.Variables, + is_sos: bool, + sos_type=solvers.MathematicalProgram.NonnegativePolynomial.kSos, + ) -> sym.Polynomial: """ - The degree of each Lagrangian polynomial in indeterminates x and y. For - example, if we have a polynomial x₀²x₁y₂ + 3x₀y₁y₂³, its degree in x is - 3 (from x₀²x₁), and its degree in y is 4 (from y₁y₂³) + Args: + is_sos: whether the constructed polynomial is sos or not. """ + if is_sos: + basis = sym.MonomialBasis( + {x: int(np.floor(self.x / 2)), y: int(np.floor(self.y / 2))} + ) + poly, _ = prog.NewSosPolynomial(basis, type=sos_type) + else: + basis = sym.MonomialBasis({x: self.x, y: self.y}) + coeffs = prog.NewContinuousVariables(basis.size) + poly = sym.Polynomial({basis[i]: coeffs[i] for i in range(basis.size)}) + return poly + - x: int - y: int - - def construct_polynomial( - self, - prog: solvers.MathematicalProgram, - x: sym.Variables, - y: sym.Variables, - is_sos: bool, - sos_type=solvers.MathematicalProgram.NonnegativePolynomial.kSos, - ) -> sym.Polynomial: - """ - Args: - is_sos: whether the constructed polynomial is sos or not. - """ - if is_sos: - basis = sym.MonomialBasis( - {x: int(np.floor(self.x / 2)), y: int(np.floor(self.y / 2))} +def _to_lagrangian_impl( + prog: solvers.MathematicalProgram, + x: sym.Variables, + y: sym.Variables, + sos_type, + is_sos: bool, + degree: Union[ + Optional[List[XYDegree]], + Optional[XYDegree], + ], + lagrangian: Union[Optional[np.ndarray], Optional[sym.Polynomial]], +) -> Union[Optional[np.ndarray], Optional[sym.Polynomial]]: + """ + Convert a XYDegree (or an array of XYDegree) to Lagrangians, if `lagrangian` + is not None; otherwise just return `lagrangian`. + """ + if lagrangian is not None: + return lagrangian + else: + if degree is None: + return None + else: + if isinstance(degree, XYDegree): + return degree.construct_polynomial( + prog, x, y, is_sos=is_sos, sos_type=sos_type + ) + elif isinstance(degree, List): + return np.array( + [ + d.construct_polynomial( + prog, x, y, is_sos=is_sos, sos_type=sos_type + ) + for d in degree + ] ) - poly, _ = prog.NewSosPolynomial(basis, type=sos_type) else: - basis = sym.MonomialBasis({x: self.x, y: self.y}) - coeffs = prog.NewContinuousVariables(basis.size) - poly = sym.Polynomial({basis[i]: coeffs[i] for i in range(basis.size)}) - return poly - - lambda_y: List[Degree] - xi_y: Degree - y: Optional[List[Degree]] - rho_minus_V: Optional[Degree] - h_plus_eps: Optional[List[Degree]] - state_eq_constraints: Optional[List[Degree]] + raise Exception() + + +@dataclass +class CompatibleLagrangianDegrees: + """ + The degree of the Lagrangian multipliers in CompatibleLagrangians. + """ + + @dataclass + class Degree: + def __init__(self, *args, **kwargs): + from warnings import warn + + warn( + "CompatibleLagrangianDegrees.Degree is deprecated, use XYDegree instead" + ) + return XYDegree(*args, **kwargs) + + lambda_y: List[XYDegree] + xi_y: XYDegree + y: Optional[List[XYDegree]] + rho_minus_V: Optional[XYDegree] + h_plus_eps: List[XYDegree] + state_eq_constraints: Optional[List[XYDegree]] def to_lagrangians( self, @@ -162,82 +210,233 @@ def to_lagrangians( h_plus_eps_lagrangian: Optional[np.ndarray] = None, state_eq_constraints_lagrangian: Optional[np.ndarray] = None, ) -> CompatibleLagrangians: - if lambda_y_lagrangian is None: - lambda_y = np.array( - [ - lambda_y_i.construct_polynomial(prog, x, y, is_sos=False) - for lambda_y_i in self.lambda_y - ] - ) - else: - lambda_y = lambda_y_lagrangian - if xi_y_lagrangian is None: - xi_y = self.xi_y.construct_polynomial(prog, x, y, is_sos=False) - else: - xi_y = xi_y_lagrangian - if y_lagrangian is None: - y_lagrangian = ( - None - if self.y is None - else np.array( - [ - y_i.construct_polynomial( - prog, x, y, is_sos=True, sos_type=sos_type - ) - for y_i in self.y - ] - ) - ) - if rho_minus_V_lagrangian is None: - rho_minus_V = ( - None - if self.rho_minus_V is None - else self.rho_minus_V.construct_polynomial( - prog, x, y, is_sos=True, sos_type=sos_type - ) - ) - else: - rho_minus_V = rho_minus_V_lagrangian - if h_plus_eps_lagrangian is None: - h_plus_eps = ( - None - if self.h_plus_eps is None - else np.array( - [ - h_plus_eps_i.construct_polynomial( - prog, x, y, is_sos=True, sos_type=sos_type - ) - for h_plus_eps_i in self.h_plus_eps - ] - ) - ) - else: - h_plus_eps = h_plus_eps_lagrangian - if state_eq_constraints_lagrangian is None: - state_eq_constraints = ( - None - if self.state_eq_constraints is None - else np.array( - [ - state_eq_constraints_i.construct_polynomial( - prog, x, y, is_sos=False - ) - for state_eq_constraints_i in self.state_eq_constraints - ] - ) - ) - else: - state_eq_constraints = state_eq_constraints_lagrangian + lambda_y = _to_lagrangian_impl( + prog, + x, + y, + sos_type, + is_sos=False, + degree=self.lambda_y, + lagrangian=lambda_y_lagrangian, + ) + xi_y = _to_lagrangian_impl( + prog, + x, + y, + sos_type, + is_sos=False, + degree=self.xi_y, + lagrangian=xi_y_lagrangian, + ) + y_lagrangian_new = _to_lagrangian_impl( + prog, x, y, sos_type, is_sos=True, degree=self.y, lagrangian=y_lagrangian + ) + rho_minus_V = _to_lagrangian_impl( + prog, + x, + y, + sos_type, + is_sos=True, + degree=self.rho_minus_V, + lagrangian=rho_minus_V_lagrangian, + ) + h_plus_eps = _to_lagrangian_impl( + prog, + x, + y, + sos_type, + is_sos=True, + degree=self.h_plus_eps, + lagrangian=h_plus_eps_lagrangian, + ) + state_eq_constraints = _to_lagrangian_impl( + prog, + x, + y, + sos_type, + is_sos=False, + degree=self.state_eq_constraints, + lagrangian=state_eq_constraints_lagrangian, + ) return CompatibleLagrangians( lambda_y=lambda_y, xi_y=xi_y, - y=y_lagrangian, + y=y_lagrangian_new, rho_minus_V=rho_minus_V, h_plus_eps=h_plus_eps, state_eq_constraints=state_eq_constraints, ) +@dataclass +class CompatibleWULagrangians: + # The Lagrangian multiplier multiplies with −ξᵀy+yᵀΛuⁱ−1 + # where uⁱ is the i'th vertex of the admissible control set. + # Each polynomial should be SOS. + # The size is (num_u_vertices,) + u_vertices: Optional[np.ndarray] + # The Lagrangian multiplier multiplies with yᵀΛvʲ + # where vʲ is the j'th extreme ray of the admissible control set. + # Each polynomial should be SOS. + # The size is (num_u_extreme_rays,) + u_extreme_rays: Optional[np.ndarray] + # The SOS Lagrangian multiplier multiplies with −ξᵀy + # when there is no extreme ray in the admissible control set, this + # Lagrangian multiplier is None. + xi_y: Optional[sym.Polynomial] + # The SOS Lagrangian multiplier multiplies with y. When use_y_squared=True, + # this is None. + # Size is (num_y,) + y: Optional[np.ndarray] + # The SOS lagrangian multiplier multiplies with (1-V). If we don't search + # for CLF, then this multiplier is None. + rho_minus_V: Optional[sym.Polynomial] + # The SOS lagrangian multiplier multiplies with h + eps. + h_plus_eps: np.ndarray + # The free Lagrangian multiplier multiplies with state equality constraints. + state_eq_constraints: Optional[np.ndarray] + + def get_result( + self, + result: solvers.MathematicalProgramResult, + coefficient_tol: Optional[float], + ) -> Self: + u_vertices = ( + None + if self.u_vertices is None + else get_polynomial_result(result, self.u_vertices, coefficient_tol) + ) + u_extreme_rays = ( + None + if self.u_extreme_rays is None + else get_polynomial_result(result, self.u_extreme_rays, coefficient_tol) + ) + xi_y = ( + None + if self.xi_y is None + else get_polynomial_result(result, self.xi_y, coefficient_tol) + ) + y = ( + None + if self.y is None + else get_polynomial_result(result, self.y, coefficient_tol) + ) + rho_minus_V = ( + None + if self.rho_minus_V is None + else get_polynomial_result(result, self.rho_minus_V, coefficient_tol) + ) + h_plus_eps = get_polynomial_result(result, self.h_plus_eps, coefficient_tol) + state_eq_constraints = ( + None + if self.state_eq_constraints is None + else get_polynomial_result( + result, self.state_eq_constraints, coefficient_tol + ) + ) + return CompatibleWULagrangians( + u_vertices, + u_extreme_rays, + xi_y, + y, + rho_minus_V, + h_plus_eps, + state_eq_constraints, + ) + + +@dataclass +class CompatibleWULagrangianDegrees: + u_vertices: Optional[List[XYDegree]] + u_extreme_rays: Optional[List[XYDegree]] + xi_y: Optional[XYDegree] + y: Optional[List[XYDegree]] + rho_minus_V: Optional[XYDegree] + h_plus_eps: List[XYDegree] + state_eq_constraints: Optional[List[XYDegree]] + + def to_lagrangians( + self, + prog: solvers.MathematicalProgram, + x: sym.Variables, + y: sym.Variables, + *, + sos_type=solvers.MathematicalProgram.NonnegativePolynomial.kSos, + u_vertices_lagrangian: Optional[np.ndarray] = None, + u_extreme_rays_lagrangian: Optional[np.ndarray] = None, + xi_y_lagrangian: Optional[sym.Polynomial] = None, + y_lagrangian: Optional[np.ndarray] = None, + rho_minus_V_lagrangian: Optional[sym.Polynomial] = None, + h_plus_eps_lagrangian: Optional[np.ndarray] = None, + state_eq_constraints_lagrangian: Optional[np.ndarray] = None, + ) -> CompatibleWULagrangians: + return CompatibleWULagrangians( + u_vertices=_to_lagrangian_impl( + prog, + x, + y, + sos_type, + is_sos=True, + degree=self.u_vertices, + lagrangian=u_vertices_lagrangian, + ), + u_extreme_rays=_to_lagrangian_impl( + prog, + x, + y, + sos_type, + is_sos=True, + degree=self.u_extreme_rays, + lagrangian=u_extreme_rays_lagrangian, + ), + xi_y=_to_lagrangian_impl( + prog, + x, + y, + sos_type, + is_sos=True, + degree=self.xi_y, + lagrangian=xi_y_lagrangian, + ), + y=_to_lagrangian_impl( + prog, + x, + y, + sos_type, + is_sos=True, + degree=self.y, + lagrangian=y_lagrangian, + ), + rho_minus_V=_to_lagrangian_impl( + prog, + x, + y, + sos_type, + is_sos=True, + degree=self.rho_minus_V, + lagrangian=rho_minus_V_lagrangian, + ), + h_plus_eps=_to_lagrangian_impl( + prog, + x, + y, + sos_type, + is_sos=True, + degree=self.h_plus_eps, + lagrangian=h_plus_eps_lagrangian, + ), + state_eq_constraints=_to_lagrangian_impl( + prog, + x, + y, + sos_type, + is_sos=False, + degree=self.state_eq_constraints, + lagrangian=state_eq_constraints_lagrangian, + ), + ) + + @dataclass class WithinRegionLagrangians: """ @@ -640,22 +839,32 @@ class CompatibleClfCbf: By Farkas lemma, this is equivalent to the following set being empty {(x, y) | [y(0)]ᵀ*[-∂h/∂x*g(x)] = 0, [y(0)]ᵀ*[ ∂h/∂x*f(x)+κ_h*h(x)] = -1, y>=0} (1) - [y(1)] [ ∂V/∂x*g(x)] [y(1)] [-∂V/∂x*f(x)-κ_V*V(x)] + [y(1)] [ ∂V/∂x*g(x)] [y(1)] [-∂V/∂x*f(x)-κ_V*V(x)] (1) We can then use Positivstellensatz to certify the emptiness of this set. The same math applies to multiple CBFs, or when u is constrained within a polyhedron. - If u is constrained within a polytope {u | Au * u <= bu}, we know that there exists - u in the polytope satisfying the CLF and CBF condition, iff the following set is - empty - - {(x, y) | yᵀ * [-∂h/∂x*g(x)] = 0, yᵀ * [ ∂h/∂x*f(x)+κ_h*h(x)] = -1 } (2) - [ ∂V/∂x*g(x)] [-∂V/∂x*f(x)-κ_V*V(x)] - [ Au] [ bu ] - Namely we increase the dimensionality of y and append the equality condition in (1) - with Au and bu. + When u is constrained in a polyhedron, we consider two different formulations: + 1. Using the H-representation of the polyhedron + If the polyhedron is parameterized as {u | Au * u <= bu}, we know that + there exists u in the polyhedron satisfying the CLF and CBF condition, + iff the following set is empty + + {(x, y) | yᵀ * [-∂h/∂x*g(x)] = 0, yᵀ * [ ∂h/∂x*f(x)+κ_h*h(x)] = -1 } (2) + [ ∂V/∂x*g(x)] [-∂V/∂x*f(x)-κ_V*V(x)] + [ Au] [ bu ] + Namely we increase the dimensionality of y and append the equality + condition in (1) with Au and bu. + 2. Using the V-representation of the polyhedron + If the polyhedron is parameterized as + u ∈ 𝒰 = ConvexHull(u¹, u², ..., uᵐ) ⊕ ConvexCone(v¹, v², ..., vⁿ) + where u¹, u², ..., uᵐ are the vertices of the polyhedron, and + v¹, v², ..., vⁿ are the extreme rays of the polyhedron, then in addition + to certifying the set {(x, y)} in (1) is non-empty, we also need to + certify that this set in (1) intersects with the polyhedron 𝒰, which can + also be done by Positivstellensatz. """ # noqa E501 def __init__( @@ -668,6 +877,8 @@ def __init__( within_set: Optional[WithinSet], Au: Optional[np.ndarray] = None, bu: Optional[np.ndarray] = None, + u_vertices: Optional[np.ndarray] = None, + u_extreme_rays: Optional[np.ndarray] = None, num_cbf: int = 1, with_clf: bool = True, use_y_squared: bool = True, @@ -689,11 +900,18 @@ def __init__( 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,) + Au and bu describe the set of admissible control as an H-rep polyhedron {u | Au * u <= bu} + The shape of Au (Any, nu), the shape of bu is (Any,) + u_vertices: Optional[np.ndarray] + u_extreme_rays: Optional[np.ndarray] + u_vertices and u_extreme_rays describe the set of admissible + control as a V-rep polyhedron. + The set of admissible control is + u ∈𝒰 = ConvexHull(u_vertices[0], u_vertices[1], ..., u_vertices[-1]) + ⊕ ConvexCone(u_extreme_rays[0], u_extreme_rays[1], ..., u_extreme_rays[-1]) + Note that we cannot use both the H-rep and V-rep for the admissible + control simultaneously. num_cbf: int The number of CBF functions. We require these CBF functions to be compatible in the intersection of their 0-superlevel-set. @@ -734,12 +952,28 @@ def __init__( check_array_of_polynomials(g, self.x_set) self.exclude_sets = exclude_sets self.within_set = within_set + assert (Au is None) == (bu is None) if Au is not None: assert Au.shape[1] == self.nu assert bu is not None assert bu.shape == (Au.shape[0],) + assert ( + u_vertices is None and u_extreme_rays is None + ), "Cannot use both (Au, bu) and (u_vertices, u_extreme_rays)" self.Au = Au self.bu = bu + if u_vertices is not None or u_extreme_rays is not None: + raise NotImplementedError + if u_vertices is not None: + assert u_vertices.shape[1] == self.nu + if u_extreme_rays is not None: + assert u_extreme_rays.shape[1] == self.nu + if u_vertices is not None or u_extreme_rays is not None: + assert ( + Au is None and bu is None + ), "Cannot use both (Au, bu) and (u_vertices, u_extreme_rays)" + self.u_vertices = u_vertices + self.u_extreme_rays = u_extreme_rays self.with_clf = with_clf self.use_y_squared = use_y_squared self.num_cbf = num_cbf @@ -911,12 +1145,16 @@ def construct_search_compatible_lagrangians( lagrangians = lagrangian_degrees.to_lagrangians( prog, self.x_set, self.y_set, sos_type=lagrangian_sos_type ) + + xi, lambda_mat = self._calc_xi_Lambda( + V=V, h=h, kappa_V=kappa_V, kappa_h=kappa_h + ) self._add_compatibility( prog=prog, V=V, h=h, - kappa_V=kappa_V, - kappa_h=kappa_h, + xi=xi, + lambda_mat=lambda_mat, lagrangians=lagrangians, barrier_eps=barrier_eps, local_clf=local_clf, @@ -1483,8 +1721,8 @@ def _add_compatibility( prog: solvers.MathematicalProgram, V: Optional[sym.Polynomial], h: np.ndarray, - kappa_V: Optional[float], - kappa_h: np.ndarray, + xi: np.ndarray, + lambda_mat: np.ndarray, lagrangians: CompatibleLagrangians, barrier_eps: Optional[np.ndarray], local_clf: bool, @@ -1520,9 +1758,6 @@ def _add_compatibility( Returns: poly: The polynomial on the left hand side of equation (3) or (4). """ # noqa: E501 - xi, lambda_mat = self._calc_xi_Lambda( - V=V, h=h, kappa_V=kappa_V, kappa_h=kappa_h - ) # This is just polynomial 1. poly_one = sym.Polynomial(sym.Monomial()) @@ -1565,6 +1800,75 @@ def _add_compatibility( prog.AddSosConstraint(poly, sos_type) return poly + def _add_compatibility_w_u_polyhedron( + self, + *, + prog: solvers.MathematicalProgram, + V: Optional[sym.Polynomial], + h: np.ndarray, + xi: np.ndarray, + lambda_mat: np.ndarray, + lagrangians: CompatibleWULagrangians, + barrier_eps: Optional[np.ndarray], + sos_type=solvers.MathematicalProgram.NonnegativePolynomial.kSos, + ) -> sym.Polynomial: + """ + In order to prove that the polyhedron {u | Λ(x)u≤ ξ(x)} intersects with + u∈𝒰 = ConvexHull(u⁽¹⁾,...,u⁽ᵐ⁾) ⊕ ConvexCone(v⁽¹⁾, ..., v⁽ⁿ⁾), we want + to prove that a separating plane between the two polyhedron doesn't + exist. This is the same as proving that the following set is empty + {y | y≥0, −ξ(x)ᵀy + yᵀΛ(x)u⁽ⁱ⁾−1≥0 , yᵀΛ(x)v⁽ʲ⁾≥0, −ξ(x)ᵀy-1≥0, i=1,..,m, j=1,...,n, V(x)≤1, h(x)≥ −ε} + Using S-procedure, a sufficient condition for this set being empty is + -1 - t₁(x,y)ᵀ(−ξᵀy + yᵀΛu−1) - t₂(x,y)ᵀyᵀΛ(x)v - t₃(x,y)(−ξ(x)ᵀy-1) - t₄(x,y)ᵀy - t₅(x,y)(1−V(x)) − t₆(x, y)(h(x)+ε) is sos. + + If the convex cone ConvexCone(v⁽¹⁾, ..., v⁽ⁿ⁾) is empty, then we don't need the term - t₂(x,y)ᵀyᵀΛ(x)v-t₃(x,y)(−ξ(x)ᵀy-1). + + Alternatively, we could use y² to replace y in the sos polynomial, and + remove the term t₄(x,y)ᵀy. Namely the following polynomial should be sos + -1 - t₁(x,y)ᵀ(−ξᵀy² + (y²)ᵀΛu−1) - t₂(x,y)ᵀ(y²)ᵀΛ(x)v - t₃(x,y)(−ξ(x)ᵀy²-1) - t₅(x,y)(1−V(x)) − t₆(x, y)(h(x)+ε) is sos. + """ # noqa E501 + assert self.u_vertices is not None or self.u_extreme_rays is not None + # This is just polynomial 1. + poly_one = sym.Polynomial(sym.Monomial()) + + poly = -poly_one + + y_or_y_squared = self.y_squared_poly if self.use_y_squared else self.y_poly + + if self.u_vertices is not None: + assert lagrangians.u_vertices is not None + poly -= lagrangians.u_vertices.dot( + -xi.dot(y_or_y_squared) + + y_or_y_squared @ (lambda_mat @ self.u_vertices.T) + - poly_one + ) + + if self.u_extreme_rays is not None: + assert lagrangians.u_extreme_rays is not None + assert lagrangians.xi_y is not None + poly -= lagrangians.u_extreme_rays.dot( + y_or_y_squared @ (lambda_mat @ self.u_extreme_rays.T) + ) + poly -= lagrangians.xi_y * (-xi.dot(y_or_y_squared) - poly_one) + + if not self.use_y_squared: + assert lagrangians.y is not None + poly -= lagrangians.y.dot(self.y_poly) + + if self.with_clf: + assert lagrangians.rho_minus_V is not None + assert V is not None + poly -= lagrangians.rho_minus_V * (poly_one - V) + + poly -= lagrangians.h_plus_eps.dot(h + barrier_eps) + + 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) + + prog.AddSosConstraint(poly, sos_type) + return poly + def _add_barrier_within_constraint( self, prog: solvers.MathematicalProgram, @@ -1738,12 +2042,16 @@ def _construct_search_clf_cbf_program( h_plus_eps_lagrangian=compatible_lagrangians.h_plus_eps, ) + xi, lambda_mat = self._calc_xi_Lambda( + V=V, h=h, kappa_V=kappa_V, kappa_h=kappa_h + ) + self._add_compatibility( prog=prog, V=V, h=h, - kappa_V=kappa_V, - kappa_h=kappa_h, + xi=xi, + lambda_mat=lambda_mat, lagrangians=compatible_lagrangians_new, barrier_eps=barrier_eps, local_clf=local_clf, diff --git a/examples/linear_toy/linear_toy_demo.py b/examples/linear_toy/linear_toy_demo.py index 57d5ee2..b88a86c 100644 --- a/examples/linear_toy/linear_toy_demo.py +++ b/examples/linear_toy/linear_toy_demo.py @@ -24,20 +24,15 @@ def search_compatible_lagrangians( # Search for the lagrangian_degrees = clf_cbf.CompatibleLagrangianDegrees( - lambda_y=[ - clf_cbf.CompatibleLagrangianDegrees.Degree(x=2, y=0) for _ in range(dut.nu) - ], - xi_y=clf_cbf.CompatibleLagrangianDegrees.Degree(x=2, y=0), + lambda_y=[clf_cbf.XYDegree(x=2, y=0) for _ in range(dut.nu)], + xi_y=clf_cbf.XYDegree(x=2, y=0), y=( None if dut.use_y_squared - else [ - clf_cbf.CompatibleLagrangianDegrees.Degree(x=2, y=0) - for _ in range(y_size) - ] + else [clf_cbf.XYDegree(x=2, y=0) for _ in range(y_size)] ), rho_minus_V=None, - h_plus_eps=None, + h_plus_eps=[clf_cbf.XYDegree(x=2, y=0)], state_eq_constraints=None, ) prog, lagrangians = dut.construct_search_compatible_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 95a0084..54dd680 100644 --- a/examples/linear_toy/linear_toy_w_input_limits_demo.py +++ b/examples/linear_toy/linear_toy_w_input_limits_demo.py @@ -25,20 +25,15 @@ def search_compatible_lagrangians( y_size = dut.y.size lagrangian_degrees = clf_cbf.CompatibleLagrangianDegrees( - lambda_y=[ - clf_cbf.CompatibleLagrangianDegrees.Degree(x=2, y=0) for _ in range(dut.nu) - ], - xi_y=clf_cbf.CompatibleLagrangianDegrees.Degree(x=2, y=0), + lambda_y=[clf_cbf.XYDegree(x=2, y=0) for _ in range(dut.nu)], + xi_y=clf_cbf.XYDegree(x=2, y=0), y=( None if dut.use_y_squared - else [ - clf_cbf.CompatibleLagrangianDegrees.Degree(x=2, y=0) - for _ in range(y_size) - ] + else [clf_cbf.XYDegree(x=2, y=0) for _ in range(y_size)] ), - rho_minus_V=clf_cbf.CompatibleLagrangianDegrees.Degree(x=2, y=2), - h_plus_eps=[clf_cbf.CompatibleLagrangianDegrees.Degree(x=2, y=2)], + rho_minus_V=clf_cbf.XYDegree(x=2, y=2), + h_plus_eps=[clf_cbf.XYDegree(x=2, y=2)], state_eq_constraints=None, ) prog, lagrangians = dut.construct_search_compatible_lagrangians( diff --git a/examples/nonlinear_toy/demo.py b/examples/nonlinear_toy/demo.py index d4855a2..fe834c0 100644 --- a/examples/nonlinear_toy/demo.py +++ b/examples/nonlinear_toy/demo.py @@ -41,18 +41,15 @@ def main(use_y_squared: bool, with_u_bound: bool): kappa_h = np.array([kappa_V]) lagrangian_degrees = clf_cbf.CompatibleLagrangianDegrees( - lambda_y=[clf_cbf.CompatibleLagrangianDegrees.Degree(x=3, y=0)], - xi_y=clf_cbf.CompatibleLagrangianDegrees.Degree(x=2, y=0), + lambda_y=[clf_cbf.XYDegree(x=3, y=0)], + xi_y=clf_cbf.XYDegree(x=2, y=0), y=( None if use_y_squared - else [ - clf_cbf.CompatibleLagrangianDegrees.Degree(x=4, y=0) - for _ in range(compatible.y.size) - ] + else [clf_cbf.XYDegree(x=4, y=0) for _ in range(compatible.y.size)] ), - rho_minus_V=clf_cbf.CompatibleLagrangianDegrees.Degree(x=2, y=2), - h_plus_eps=[clf_cbf.CompatibleLagrangianDegrees.Degree(x=2, y=2)], + rho_minus_V=clf_cbf.XYDegree(x=2, y=2), + h_plus_eps=[clf_cbf.XYDegree(x=2, y=2)], state_eq_constraints=None, ) barrier_eps = np.array([0.0001]) diff --git a/examples/nonlinear_toy/demo_trigpoly.py b/examples/nonlinear_toy/demo_trigpoly.py index 65791bb..53433a1 100644 --- a/examples/nonlinear_toy/demo_trigpoly.py +++ b/examples/nonlinear_toy/demo_trigpoly.py @@ -146,12 +146,12 @@ def search(unit_test_flag: bool = False): V_init, h_init = get_clf_cbf_init(x) compatible_lagrangian_degrees = clf_cbf.CompatibleLagrangianDegrees( - lambda_y=[clf_cbf.CompatibleLagrangianDegrees.Degree(x=2, y=0)], - xi_y=clf_cbf.CompatibleLagrangianDegrees.Degree(x=2, y=0), + lambda_y=[clf_cbf.XYDegree(x=2, y=0)], + xi_y=clf_cbf.XYDegree(x=2, y=0), y=None, - rho_minus_V=clf_cbf.CompatibleLagrangianDegrees.Degree(x=2, y=2), - h_plus_eps=[clf_cbf.CompatibleLagrangianDegrees.Degree(x=2, y=2)], - state_eq_constraints=[clf_cbf.CompatibleLagrangianDegrees.Degree(x=2, y=2)], + rho_minus_V=clf_cbf.XYDegree(x=2, y=2), + h_plus_eps=[clf_cbf.XYDegree(x=2, y=2)], + state_eq_constraints=[clf_cbf.XYDegree(x=2, y=2)], ) safety_sets_lagrangian_degrees = clf_cbf.SafetySetLagrangianDegrees( exclude=[ diff --git a/examples/nonlinear_toy/synthesize_demo.py b/examples/nonlinear_toy/synthesize_demo.py index 80155a7..97aa1d0 100644 --- a/examples/nonlinear_toy/synthesize_demo.py +++ b/examples/nonlinear_toy/synthesize_demo.py @@ -42,11 +42,11 @@ def main(with_u_bound: bool): barrier_eps = np.array([0.0001]) compatible_lagrangian_degrees = clf_cbf.CompatibleLagrangianDegrees( - lambda_y=[clf_cbf.CompatibleLagrangianDegrees.Degree(x=3, y=0)], - xi_y=clf_cbf.CompatibleLagrangianDegrees.Degree(x=2, y=0), + lambda_y=[clf_cbf.XYDegree(x=3, y=0)], + xi_y=clf_cbf.XYDegree(x=2, y=0), y=None, - rho_minus_V=clf_cbf.CompatibleLagrangianDegrees.Degree(x=2, y=2), - h_plus_eps=[clf_cbf.CompatibleLagrangianDegrees.Degree(x=2, y=2)], + rho_minus_V=clf_cbf.XYDegree(x=2, y=2), + h_plus_eps=[clf_cbf.XYDegree(x=2, y=2)], state_eq_constraints=None, ) safety_sets_lagrangian_degrees = clf_cbf.SafetySetLagrangianDegrees( diff --git a/examples/power_converter/demo.py b/examples/power_converter/demo.py index a440a98..8ff829f 100644 --- a/examples/power_converter/demo.py +++ b/examples/power_converter/demo.py @@ -100,23 +100,15 @@ def search(use_y_squared: bool): kappa_h = np.array([kappa_V]) compatible_lagrangian_degrees = clf_cbf.CompatibleLagrangianDegrees( - lambda_y=[ - clf_cbf.CompatibleLagrangianDegrees.Degree(x=2, y=0 if use_y_squared else 1) - for _ in range(2) - ], - xi_y=clf_cbf.CompatibleLagrangianDegrees.Degree( - x=2, y=0 if use_y_squared else 1 - ), + lambda_y=[clf_cbf.XYDegree(x=2, y=0 if use_y_squared else 1) for _ in range(2)], + xi_y=clf_cbf.XYDegree(x=2, y=0 if use_y_squared else 1), y=( None if use_y_squared - else [ - clf_cbf.CompatibleLagrangianDegrees.Degree(x=4, y=0) - for _ in range(compatible.y.size) - ] + else [clf_cbf.XYDegree(x=4, y=0) for _ in range(compatible.y.size)] ), - rho_minus_V=clf_cbf.CompatibleLagrangianDegrees.Degree(x=4, y=2), - h_plus_eps=[clf_cbf.CompatibleLagrangianDegrees.Degree(x=4, y=2)], + rho_minus_V=clf_cbf.XYDegree(x=4, y=2), + h_plus_eps=[clf_cbf.XYDegree(x=4, y=2)], state_eq_constraints=None, ) diff --git a/examples/quadrotor/demo.py b/examples/quadrotor/demo.py index 9aa8ac5..413a60f 100644 --- a/examples/quadrotor/demo.py +++ b/examples/quadrotor/demo.py @@ -100,24 +100,16 @@ def search(use_y_squared: bool, with_u_bound: bool): kappa_h_sequences = [np.array([0.2]) for _ in range(len(kappa_V_sequences))] compatible_lagrangian_degrees = clf_cbf.CompatibleLagrangianDegrees( - lambda_y=[ - clf_cbf.CompatibleLagrangianDegrees.Degree(x=1, y=0 if use_y_squared else 1) - for _ in range(4) - ], - xi_y=clf_cbf.CompatibleLagrangianDegrees.Degree( - x=1, y=0 if use_y_squared else 1 - ), + lambda_y=[clf_cbf.XYDegree(x=1, y=0 if use_y_squared else 1) for _ in range(4)], + xi_y=clf_cbf.XYDegree(x=1, y=0 if use_y_squared else 1), y=( None if use_y_squared - else [ - clf_cbf.CompatibleLagrangianDegrees.Degree(x=4, y=0) - for _ in range(compatible.y.size) - ] + else [clf_cbf.XYDegree(x=4, y=0) for _ in range(compatible.y.size)] ), - rho_minus_V=clf_cbf.CompatibleLagrangianDegrees.Degree(x=2, y=2), - h_plus_eps=[clf_cbf.CompatibleLagrangianDegrees.Degree(x=2, y=2)], - state_eq_constraints=[clf_cbf.CompatibleLagrangianDegrees.Degree(x=2, y=2)], + rho_minus_V=clf_cbf.XYDegree(x=2, y=2), + h_plus_eps=[clf_cbf.XYDegree(x=2, y=2)], + state_eq_constraints=[clf_cbf.XYDegree(x=2, y=2)], ) safety_sets_lagrangian_degrees = [ clf_cbf.SafetySetLagrangianDegrees( diff --git a/examples/quadrotor2d/demo.py b/examples/quadrotor2d/demo.py index d0c7f7a..7e21bfd 100644 --- a/examples/quadrotor2d/demo.py +++ b/examples/quadrotor2d/demo.py @@ -56,24 +56,16 @@ def main(use_y_squared: bool, with_u_bound: bool): solver_options.SetOption(solvers.CommonSolverOption.kPrintToConsole, True) compatible_lagrangian_degrees = clf_cbf.CompatibleLagrangianDegrees( - lambda_y=[ - clf_cbf.CompatibleLagrangianDegrees.Degree(x=1, y=0 if use_y_squared else 1) - for _ in range(2) - ], - xi_y=clf_cbf.CompatibleLagrangianDegrees.Degree( - x=2, y=0 if use_y_squared else 1 - ), + lambda_y=[clf_cbf.XYDegree(x=1, y=0 if use_y_squared else 1) for _ in range(2)], + xi_y=clf_cbf.XYDegree(x=2, y=0 if use_y_squared else 1), y=( None if use_y_squared - else [ - clf_cbf.CompatibleLagrangianDegrees.Degree(x=4, y=0) - for _ in range(compatible.y.size) - ] + else [clf_cbf.XYDegree(x=4, y=0) for _ in range(compatible.y.size)] ), - rho_minus_V=clf_cbf.CompatibleLagrangianDegrees.Degree(x=2, y=2), - h_plus_eps=[clf_cbf.CompatibleLagrangianDegrees.Degree(x=2, y=2)], - state_eq_constraints=[clf_cbf.CompatibleLagrangianDegrees.Degree(x=2, y=2)], + rho_minus_V=clf_cbf.XYDegree(x=2, y=2), + h_plus_eps=[clf_cbf.XYDegree(x=2, y=2)], + state_eq_constraints=[clf_cbf.XYDegree(x=2, y=2)], ) safety_sets_lagrangian_degrees = clf_cbf.SafetySetLagrangianDegrees( exclude=[ diff --git a/examples/quadrotor2d/demo_taylor.py b/examples/quadrotor2d/demo_taylor.py index b3f1bfe..f482644 100644 --- a/examples/quadrotor2d/demo_taylor.py +++ b/examples/quadrotor2d/demo_taylor.py @@ -71,20 +71,15 @@ def search_clf_cbf( state_eq_constraints=None, ) lagrangian_degrees = clf_cbf.CompatibleLagrangianDegrees( - lambda_y=[ - clf_cbf.CompatibleLagrangianDegrees.Degree(x=2, y=0) for _ in range(2) - ], - xi_y=clf_cbf.CompatibleLagrangianDegrees.Degree(x=4, y=0), + lambda_y=[clf_cbf.XYDegree(x=2, y=0) for _ in range(2)], + xi_y=clf_cbf.XYDegree(x=4, y=0), y=( None if use_y_squared - else [ - clf_cbf.CompatibleLagrangianDegrees.Degree(x=4, y=0) - for _ in range(compatible.y.size) - ] + else [clf_cbf.XYDegree(x=4, y=0) for _ in range(compatible.y.size)] ), - rho_minus_V=clf_cbf.CompatibleLagrangianDegrees.Degree(x=4, y=2), - h_plus_eps=[clf_cbf.CompatibleLagrangianDegrees.Degree(x=4, y=2)], + rho_minus_V=clf_cbf.XYDegree(x=4, y=2), + h_plus_eps=[clf_cbf.XYDegree(x=4, y=2)], state_eq_constraints=None, ) barrier_eps = np.array([0.0]) diff --git a/tests/test_clf_cbf.py b/tests/test_clf_cbf.py index 607be2a..4b0a477 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 Tuple +from typing import List, Optional, Tuple import numpy as np import pytest # noqa @@ -14,7 +14,7 @@ class TestCompatibleLagrangianDegrees(object): def test_construct_polynomial(self): - degree = mut.CompatibleLagrangianDegrees.Degree(x=3, y=2) + degree = mut.XYDegree(x=3, y=2) prog = solvers.MathematicalProgram() x = prog.NewIndeterminates(3, "x") y = prog.NewIndeterminates(2, "y") @@ -444,16 +444,11 @@ def test_search_compatible_lagrangians_w_clf_y_squared(self): kappa_V = 0.01 kappa_h = np.array([0.02, 0.03]) lagrangian_degrees = mut.CompatibleLagrangianDegrees( - lambda_y=[ - mut.CompatibleLagrangianDegrees.Degree(x=2, y=0) for _ in range(self.nu) - ], - xi_y=mut.CompatibleLagrangianDegrees.Degree(x=2, y=0), + lambda_y=[mut.XYDegree(x=2, y=0) for _ in range(self.nu)], + xi_y=mut.XYDegree(x=2, y=0), y=None, - rho_minus_V=mut.CompatibleLagrangianDegrees.Degree(x=4, y=2), - h_plus_eps=[ - mut.CompatibleLagrangianDegrees.Degree(x=4, y=2) - for _ in range(dut.num_cbf) - ], + rho_minus_V=mut.XYDegree(x=4, y=2), + h_plus_eps=[mut.XYDegree(x=4, y=2) for _ in range(dut.num_cbf)], state_eq_constraints=None, ) barrier_eps = np.array([0.01, 0.02]) @@ -510,13 +505,15 @@ def test_add_compatibility_w_clf_y_squared(self): state_eq_constraints=None, ) + xi, lambda_mat = dut._calc_xi_Lambda(V=V, h=h, kappa_V=kappa_V, kappa_h=kappa_h) + barrier_eps = np.array([0.01, 0.02]) poly = dut._add_compatibility( prog=prog, V=V, h=h, - kappa_V=kappa_V, - kappa_h=kappa_h, + xi=xi, + lambda_mat=lambda_mat, lagrangians=lagrangians, barrier_eps=barrier_eps, local_clf=True, @@ -765,11 +762,11 @@ def search_lagrangians( h_init = np.array([sym.Polynomial(0.001 - self.x[0] ** 2 - self.x[1] ** 2)]) lagrangian_degrees = mut.CompatibleLagrangianDegrees( - lambda_y=[mut.CompatibleLagrangianDegrees.Degree(x=3, y=0)], - xi_y=mut.CompatibleLagrangianDegrees.Degree(x=2, y=0), + lambda_y=[mut.XYDegree(x=3, y=0)], + xi_y=mut.XYDegree(x=2, y=0), y=None, - rho_minus_V=mut.CompatibleLagrangianDegrees.Degree(x=2, y=0), - h_plus_eps=[mut.CompatibleLagrangianDegrees.Degree(x=2, y=0)], + rho_minus_V=mut.XYDegree(x=2, y=0), + h_plus_eps=[mut.XYDegree(x=2, y=0)], state_eq_constraints=None, ) @@ -1119,12 +1116,12 @@ def search_lagrangians(self, check_result=False) -> Tuple[ ) lagrangian_degrees = mut.CompatibleLagrangianDegrees( - lambda_y=[mut.CompatibleLagrangianDegrees.Degree(x=2, y=0)], - xi_y=mut.CompatibleLagrangianDegrees.Degree(x=2, y=0), + lambda_y=[mut.XYDegree(x=2, y=0)], + xi_y=mut.XYDegree(x=2, y=0), y=None, - rho_minus_V=mut.CompatibleLagrangianDegrees.Degree(x=2, y=2), - h_plus_eps=[mut.CompatibleLagrangianDegrees.Degree(x=2, y=2)], - state_eq_constraints=[mut.CompatibleLagrangianDegrees.Degree(x=2, y=2)], + rho_minus_V=mut.XYDegree(x=2, y=2), + h_plus_eps=[mut.XYDegree(x=2, y=2)], + state_eq_constraints=[mut.XYDegree(x=2, y=2)], ) ( @@ -1235,3 +1232,214 @@ def test_search_clf_cbf(self): assert V is not None assert sym.Monomial() not in V.monomial_to_coefficient_map().keys() assert h is not None + + +class TestCompatibleWithU: + """ + Test the math that the polyhedron {u | Λu≤ ξ} intersects with + u∈𝒰 = ConvexHull(u⁽¹⁾,...,u⁽ᵐ⁾) ⊕ ConvexCone(v⁽¹⁾, ..., v⁽ⁿ⁾) + """ + + def intersect( + self, + lambda_mat: np.ndarray, + xi: np.ndarray, + u_vertices: np.ndarray, + u_extreme_rays: np.ndarray, + ) -> bool: + u_dim = lambda_mat.shape[1] + prog = solvers.MathematicalProgram() + u = prog.NewContinuousVariables(u_dim) + prog.AddLinearConstraint(lambda_mat, np.full_like(xi, -np.inf), xi, u) + vertices_weight = prog.NewContinuousVariables(u_vertices.shape[0]) + if vertices_weight.size > 0: + prog.AddBoundingBoxConstraint(0, 1, vertices_weight) + prog.AddLinearEqualityConstraint( + np.ones((u_vertices.shape[0],)), 1, vertices_weight + ) + extreme_rays_weight = prog.NewContinuousVariables(u_extreme_rays.shape[0]) + if extreme_rays_weight.size > 0: + prog.AddBoundingBoxConstraint(0, np.inf, extreme_rays_weight) + prog.AddLinearEqualityConstraint( + u - vertices_weight @ u_vertices - extreme_rays_weight @ u_extreme_rays, + np.zeros((u_dim,)), + ) + result = solvers.Solve(prog) + return result.is_success() + + def intersect_by_sos( + self, + lambda_mat: np.ndarray, + xi: np.ndarray, + u_vertices: np.ndarray, + u_extreme_rays: np.ndarray, + use_y_squared: bool, + u_vertices_lagrangian_degree: List[int], + u_extreme_rays_lagrangian_degree: List[int], + xi_y_lagrangian_degree: Optional[int], + y_lagrangian_degree: Optional[List[int]], + ) -> bool: + prog = solvers.MathematicalProgram() + y = prog.NewIndeterminates(lambda_mat.shape[0], "y") + y_set = sym.Variables(y) + + if use_y_squared: + y_or_y_squared = np.array( + [sym.Polynomial(sym.Monomial(y[i], 2)) for i in range(y.size)] + ) + else: + y_poly = np.array([sym.Polynomial(y[i]) for i in range(y.size)]) + y_or_y_squared = y_poly + + poly_one = sym.Polynomial(sym.Monomial()) + + poly = -poly_one + + if u_vertices.shape[0] > 0: + u_vertices_lagrangian = np.array( + [ + prog.NewSosPolynomial(y_set, degree)[0] + for degree in u_vertices_lagrangian_degree + ] + ) + poly -= u_vertices_lagrangian.dot( + -xi.dot(y_or_y_squared) + + y_or_y_squared @ (lambda_mat @ u_vertices.T) + - poly_one + ) + + if u_extreme_rays.shape[0] > 0: + u_extreme_rays_lagrangian = np.array( + [ + prog.NewSosPolynomial(y_set, degree)[0] + for degree in u_extreme_rays_lagrangian_degree + ] + ) + + poly -= u_extreme_rays_lagrangian.dot( + y_or_y_squared @ (lambda_mat @ u_extreme_rays.T) + ) + assert xi_y_lagrangian_degree is not None + xi_y_lagrangian = prog.NewSosPolynomial(y_set, xi_y_lagrangian_degree)[0] + poly -= xi_y_lagrangian * (-xi.dot(y_or_y_squared) - poly_one) + + if not use_y_squared: + assert y_lagrangian_degree is not None + y_lagrangian = np.array( + [ + prog.NewSosPolynomial(y_set, degree)[0] + for degree in y_lagrangian_degree + ] + ) + poly -= y_lagrangian.dot(y_poly) + + prog.AddSosConstraint(poly) + result = solvers.Solve(prog) + return result.is_success() + + def intersect_tester( + self, lambda_mat, xi, u_vertices, u_extreme_rays, intersect_expected + ): + assert ( + self.intersect(lambda_mat, xi, u_vertices, u_extreme_rays) + == intersect_expected + ) + assert ( + self.intersect_by_sos( + lambda_mat, + xi, + u_vertices, + u_extreme_rays, + use_y_squared=True, + u_vertices_lagrangian_degree=[2] * u_vertices.shape[0], + u_extreme_rays_lagrangian_degree=[2] * u_extreme_rays.shape[0], + xi_y_lagrangian_degree=None if u_extreme_rays.shape[0] == 0 else 0, + y_lagrangian_degree=None, + ) + == intersect_expected + ) + assert ( + self.intersect_by_sos( + lambda_mat, + xi, + u_vertices, + u_extreme_rays, + use_y_squared=False, + u_vertices_lagrangian_degree=[2] * u_vertices.shape[0], + u_extreme_rays_lagrangian_degree=[2] * u_extreme_rays.shape[0], + xi_y_lagrangian_degree=None if u_extreme_rays.shape[0] == 0 else 0, + y_lagrangian_degree=[0, 0, 0], + ) + == intersect_expected + ) + + def test_convexhull_intersect(self): + """ + The convex hull of u_vertices intersects with the polyhedron Λu≤ ξ + """ + lambda_mat = np.array([[1, 1], [-1, 0], [0, -1]]) + xi = np.array([1, 0, 0]) + + u_vertices_sequences = [ + np.array([[0.4, 0.4], [0.4, 2], [2, 0.2]]), + np.array([[-1, -1], [0.5, 1], [1, 0.5]]), + ] + u_extreme_rays = np.zeros((0, 2)) + + intersect_expected = True + for u_vertices in u_vertices_sequences: + self.intersect_tester( + lambda_mat, xi, u_vertices, u_extreme_rays, intersect_expected + ) + + def test_convexhull_not_intersect(self): + """ + The convex hull of u_vertices does not intersect with the polyhedron Λu≤ ξ + """ + lambda_mat = np.array([[1, 1], [-1, 0], [0, -1]]) + xi = np.array([1, 0, 0]) + + u_vertices_sequences = [ + np.array([[0.55, 0.55], [0.6, 2], [2, 0.6]]), + np.array([[-1, -1], [-0.5, -0.1], [-0.1, -0.5]]), + ] + u_extreme_rays = np.zeros((0, 2)) + + intersect_expected = False + for u_vertices in u_vertices_sequences: + self.intersect_tester( + lambda_mat, xi, u_vertices, u_extreme_rays, intersect_expected + ) + + def test_convexcone_intersect(self): + """ + The convex cone of u_extreme_rays intersects with the polyhedron Λu≤ ξ + """ + lambda_mat = np.array([[1, 1], [-1, 0], [0, -1]]) + xi = np.array([1, 0, 0]) + u_vertices = np.zeros((0, 2)) + + u_extreme_rays = np.array([[0.1, 1], [1, 0.1]]) + + self.intersect_tester( + lambda_mat, xi, u_vertices, u_extreme_rays, intersect_expected=True + ) + + u_vertices = np.array([[-1, -1]]) + u_extreme_rays = np.array([[1, 1.1], [1.1, -1]]) + self.intersect_tester( + lambda_mat, xi, u_vertices, u_extreme_rays, intersect_expected=True + ) + + def test_convexcone_not_intersect(self): + """ + The convex cone of u_extreme_rays doesn't intersects with the polyhedron Λu≤ ξ + """ + lambda_mat = np.array([[1, 1], [-1, 0], [0, -1]]) + xi = np.array([1, -0.1, -0.1]) + + u_vertices = np.empty((0, 2)) + u_extreme_rays = np.array([[-1, 0], [0, -1]]) + self.intersect_tester( + lambda_mat, xi, u_vertices, u_extreme_rays, intersect_expected=False + )