From 0a6b4c5f5e82baba65cf3ef5a7022b9ec6800824 Mon Sep 17 00:00:00 2001 From: Hongkai Dai Date: Tue, 17 Sep 2024 21:41:51 -0700 Subject: [PATCH] Support bilinear alternation with input limits using V-rep. (#73) The input limit is parameterized as a V-rep polyhedron. --- compatible_clf_cbf/clf_cbf.py | 175 ++++++++++++++++-------- examples/nonlinear_toy/demo.py | 32 ++--- examples/nonlinear_toy/demo_trigpoly.py | 86 ++++++++---- examples/quadrotor/demo.py | 18 ++- tests/test_clf_cbf.py | 170 ++++++++++++++++++++++- 5 files changed, 369 insertions(+), 112 deletions(-) diff --git a/compatible_clf_cbf/clf_cbf.py b/compatible_clf_cbf/clf_cbf.py index ef801dc..03024d6 100644 --- a/compatible_clf_cbf/clf_cbf.py +++ b/compatible_clf_cbf/clf_cbf.py @@ -269,7 +269,7 @@ def to_lagrangians( @dataclass -class CompatibleWULagrangians: +class CompatibleWVrepLagrangians: # 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. @@ -334,7 +334,7 @@ def get_result( result, self.state_eq_constraints, coefficient_tol ) ) - return CompatibleWULagrangians( + return CompatibleWVrepLagrangians( u_vertices, u_extreme_rays, xi_y, @@ -346,7 +346,7 @@ def get_result( @dataclass -class CompatibleWULagrangianDegrees: +class CompatibleWVrepLagrangianDegrees: u_vertices: Optional[List[XYDegree]] u_extreme_rays: Optional[List[XYDegree]] xi_y: Optional[XYDegree] @@ -369,8 +369,8 @@ def to_lagrangians( 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( + ) -> CompatibleWVrepLagrangians: + return CompatibleWVrepLagrangians( u_vertices=_to_lagrangian_impl( prog, x, @@ -861,8 +861,7 @@ class CompatibleClfCbf: 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 + v¹, v², ..., vⁿ are the extreme rays of the polyhedron, then we need to certify that this set in (1) intersects with the polyhedron 𝒰, which can also be done by Positivstellensatz. """ # noqa E501 @@ -962,8 +961,6 @@ def __init__( ), "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: @@ -1114,12 +1111,17 @@ def construct_search_compatible_lagrangians( h: np.ndarray, kappa_V: Optional[float], kappa_h: np.ndarray, - lagrangian_degrees: CompatibleLagrangianDegrees, + lagrangian_degrees: Union[ + CompatibleLagrangianDegrees, CompatibleWVrepLagrangianDegrees + ], barrier_eps: Optional[np.ndarray], local_clf: bool = True, lagrangian_sos_type=solvers.MathematicalProgram.NonnegativePolynomial.kSos, compatible_sos_type=solvers.MathematicalProgram.NonnegativePolynomial.kSos, - ) -> Tuple[solvers.MathematicalProgram, CompatibleLagrangians]: + ) -> Tuple[ + solvers.MathematicalProgram, + Union[CompatibleLagrangians, CompatibleWVrepLagrangians], + ]: """ Given CLF candidate V and CBF candidate h, construct the optimization program to certify that they are compatible within the region @@ -1149,17 +1151,32 @@ def construct_search_compatible_lagrangians( 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, - xi=xi, - lambda_mat=lambda_mat, - lagrangians=lagrangians, - barrier_eps=barrier_eps, - local_clf=local_clf, - sos_type=compatible_sos_type, - ) + if self.u_vertices is not None or self.u_extreme_rays is not None: + assert isinstance(lagrangians, CompatibleWVrepLagrangians) + self._add_compatibility_w_vrep( + prog=prog, + V=V, + h=h, + xi=xi, + lambda_mat=lambda_mat, + lagrangians=lagrangians, + barrier_eps=barrier_eps, + local_clf=local_clf, + sos_type=compatible_sos_type, + ) + else: + assert isinstance(lagrangians, CompatibleLagrangians) + self._add_compatibility( + prog=prog, + V=V, + h=h, + xi=xi, + lambda_mat=lambda_mat, + lagrangians=lagrangians, + barrier_eps=barrier_eps, + local_clf=local_clf, + sos_type=compatible_sos_type, + ) return (prog, lagrangians) def search_lagrangians_given_clf_cbf( @@ -1169,7 +1186,9 @@ def search_lagrangians_given_clf_cbf( kappa_V: Optional[float], kappa_h: np.ndarray, barrier_eps: np.ndarray, - compatible_lagrangian_degrees: CompatibleLagrangianDegrees, + compatible_lagrangian_degrees: Union[ + CompatibleLagrangianDegrees, CompatibleWVrepLagrangianDegrees + ], safety_set_lagrangian_degrees: SafetySetLagrangianDegrees, solver_id: Optional[solvers.SolverId] = None, solver_options: Optional[solvers.SolverOptions] = None, @@ -1214,8 +1233,12 @@ def search_lagrangians_given_clf_cbf( def search_clf_cbf_given_lagrangian( self, - compatible_lagrangians: CompatibleLagrangians, - compatible_lagrangian_degrees: CompatibleLagrangianDegrees, + compatible_lagrangians: Union[ + CompatibleLagrangians, CompatibleWVrepLagrangians + ], + compatible_lagrangian_degrees: Union[ + CompatibleLagrangianDegrees, CompatibleWVrepLagrangianDegrees + ], safety_sets_lagrangians: SafetySetLagrangians, safety_sets_lagrangian_degrees: SafetySetLagrangianDegrees, clf_degree: Optional[int], @@ -1282,8 +1305,12 @@ def search_clf_cbf_given_lagrangian( def binary_search_clf_cbf( self, - compatible_lagrangians: CompatibleLagrangians, - compatible_lagrangian_degrees: CompatibleLagrangianDegrees, + compatible_lagrangians: Union[ + CompatibleLagrangians, CompatibleWVrepLagrangians + ], + compatible_lagrangian_degrees: Union[ + CompatibleLagrangianDegrees, CompatibleWVrepLagrangianDegrees + ], safety_sets_lagrangians: SafetySetLagrangians, safety_sets_lagrangian_degrees: SafetySetLagrangianDegrees, clf_degree: Optional[int], @@ -1412,7 +1439,9 @@ def bilinear_alternation( self, V_init: Optional[sym.Polynomial], h_init: np.ndarray, - compatible_lagrangian_degrees: CompatibleLagrangianDegrees, + compatible_lagrangian_degrees: Union[ + CompatibleLagrangianDegrees, CompatibleWVrepLagrangianDegrees + ], safety_sets_lagrangian_degrees: SafetySetLagrangianDegrees, kappa_V: Optional[float], kappa_h: np.ndarray, @@ -1800,7 +1829,7 @@ def _add_compatibility( prog.AddSosConstraint(poly, sos_type) return poly - def _add_compatibility_w_u_polyhedron( + def _add_compatibility_w_vrep( self, *, prog: solvers.MathematicalProgram, @@ -1808,8 +1837,9 @@ def _add_compatibility_w_u_polyhedron( h: np.ndarray, xi: np.ndarray, lambda_mat: np.ndarray, - lagrangians: CompatibleWULagrangians, + lagrangians: CompatibleWVrepLagrangians, barrier_eps: Optional[np.ndarray], + local_clf: bool, sos_type=solvers.MathematicalProgram.NonnegativePolynomial.kSos, ) -> sym.Polynomial: """ @@ -1850,12 +1880,15 @@ def _add_compatibility_w_u_polyhedron( y_or_y_squared @ (lambda_mat @ self.u_extreme_rays.T) ) poly -= lagrangians.xi_y * (-xi.dot(y_or_y_squared) - poly_one) + else: + assert lagrangians.u_extreme_rays is None + assert lagrangians.xi_y is None if not self.use_y_squared: assert lagrangians.y is not None poly -= lagrangians.y.dot(self.y_poly) - if self.with_clf: + if self.with_clf and local_clf: assert lagrangians.rho_minus_V is not None assert V is not None poly -= lagrangians.rho_minus_V * (poly_one - V) @@ -1944,8 +1977,12 @@ def _add_barrier_exclude_constraint( def _construct_search_clf_cbf_program( self, - compatible_lagrangians: CompatibleLagrangians, - compatible_lagrangian_degrees: CompatibleLagrangianDegrees, + compatible_lagrangians: Union[ + CompatibleLagrangians, CompatibleWVrepLagrangians + ], + compatible_lagrangian_degrees: Union[ + CompatibleLagrangianDegrees, CompatibleWVrepLagrangianDegrees + ], safety_sets_lagrangians: SafetySetLagrangians, safety_sets_lagrangian_degrees: SafetySetLagrangianDegrees, clf_degree: Optional[int], @@ -2031,32 +2068,62 @@ def _construct_search_clf_cbf_program( # We can search for some compatible Lagrangians as well, including the # Lagrangians for y >= 0 and the state equality constraints, as y>= 0 # and the state equality constraints don't depend on V or h. - compatible_lagrangians_new = compatible_lagrangian_degrees.to_lagrangians( - prog, - self.x_set, - self.y_set, - sos_type=compatible_lagrangian_sos_type, - lambda_y_lagrangian=compatible_lagrangians.lambda_y, - xi_y_lagrangian=compatible_lagrangians.xi_y, - rho_minus_V_lagrangian=compatible_lagrangians.rho_minus_V, - h_plus_eps_lagrangian=compatible_lagrangians.h_plus_eps, - ) + if isinstance(compatible_lagrangian_degrees, CompatibleLagrangianDegrees): + compatible_lagrangians_new = compatible_lagrangian_degrees.to_lagrangians( + prog, + self.x_set, + self.y_set, + sos_type=compatible_lagrangian_sos_type, + lambda_y_lagrangian=compatible_lagrangians.lambda_y, + xi_y_lagrangian=compatible_lagrangians.xi_y, + rho_minus_V_lagrangian=compatible_lagrangians.rho_minus_V, + h_plus_eps_lagrangian=compatible_lagrangians.h_plus_eps, + ) + elif isinstance( + compatible_lagrangian_degrees, CompatibleWVrepLagrangianDegrees + ): + compatible_lagrangians_new = compatible_lagrangian_degrees.to_lagrangians( + prog, + self.x_set, + self.y_set, + sos_type=compatible_lagrangian_sos_type, + u_vertices_lagrangian=compatible_lagrangians.u_vertices, + u_extreme_rays_lagrangian=compatible_lagrangians.u_extreme_rays, + xi_y_lagrangian=compatible_lagrangians.xi_y, + rho_minus_V_lagrangian=compatible_lagrangians.rho_minus_V, + 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, - xi=xi, - lambda_mat=lambda_mat, - lagrangians=compatible_lagrangians_new, - barrier_eps=barrier_eps, - local_clf=local_clf, - sos_type=compatible_sos_type, - ) + if self.u_vertices is not None or self.u_extreme_rays is not None: + assert isinstance(compatible_lagrangians_new, CompatibleWVrepLagrangians) + self._add_compatibility_w_vrep( + prog=prog, + V=V, + h=h, + xi=xi, + lambda_mat=lambda_mat, + lagrangians=compatible_lagrangians_new, + barrier_eps=barrier_eps, + local_clf=local_clf, + sos_type=compatible_sos_type, + ) + else: + assert isinstance(compatible_lagrangians_new, CompatibleLagrangians) + self._add_compatibility( + prog=prog, + V=V, + h=h, + xi=xi, + lambda_mat=lambda_mat, + lagrangians=compatible_lagrangians_new, + barrier_eps=barrier_eps, + local_clf=local_clf, + sos_type=compatible_sos_type, + ) return (prog, V, h) diff --git a/examples/nonlinear_toy/demo.py b/examples/nonlinear_toy/demo.py index fe834c0..4eaadbf 100644 --- a/examples/nonlinear_toy/demo.py +++ b/examples/nonlinear_toy/demo.py @@ -64,25 +64,21 @@ def main(use_y_squared: bool, with_u_bound: bool): compatible_result = solvers.Solve(compatible_prog, None, solver_options) assert compatible_result.is_success() - 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=[], + ) - safety_sets_lagrangians = [ - compatible.certify_cbf_safety_set( - h=h_init, - lagrangian_degrees=safety_sets_lagrangian_degrees[0], - solver_options=None, - ) - ] - assert safety_sets_lagrangians[0] is not None + safety_sets_lagrangians = compatible.certify_cbf_safety_set( + h=h_init, + lagrangian_degrees=safety_sets_lagrangian_degrees, + solver_options=None, + ) + assert safety_sets_lagrangians is not None if __name__ == "__main__": diff --git a/examples/nonlinear_toy/demo_trigpoly.py b/examples/nonlinear_toy/demo_trigpoly.py index 53433a1..c986b9c 100644 --- a/examples/nonlinear_toy/demo_trigpoly.py +++ b/examples/nonlinear_toy/demo_trigpoly.py @@ -124,20 +124,47 @@ def plot_clf_cbf_init( return h_V_init, h_h_init -def search(unit_test_flag: bool = False): +def get_pkl_file_path(use_y_squared): + filename = ( + "nonlinear_toy_clf_cbf.pkl" + if use_y_squared + else "nonlinear_toy_clf_cbf_wo_y_squared.pkl" + ) + path = os.path.join( + os.path.dirname(os.path.abspath(__file__)), "../../data/", filename + ) + return path + + +def search(use_v_rep: bool, unit_test_flag: bool = False): + """ + use_v_rep: If set to True, use the V-rep of the input polytope, otherwise + use H-rep. + """ x = sym.MakeVectorContinuousVariable(3, "x") 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 exclude_sets = [clf_cbf.ExcludeSet(get_unsafe_regions(x))] + + if use_v_rep: + Au = bu = None + u_vertices = np.array([[1], [-1]]) + u_extreme_rays = None + else: + Au = np.array([[1], [-1]]) + bu = np.array([1, 1]) + u_vertices = u_extreme_rays = None compatible = clf_cbf.CompatibleClfCbf( f=f, g=g, x=x, exclude_sets=exclude_sets, within_set=None, - Au=np.array([[1], [-1]]), - bu=np.array([1, 1]), + Au=Au, + bu=bu, + u_vertices=u_vertices, + u_extreme_rays=u_extreme_rays, num_cbf=1, with_clf=True, use_y_squared=use_y_squared, @@ -145,14 +172,33 @@ 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.XYDegree(x=2, y=0)], - xi_y=clf_cbf.XYDegree(x=2, y=0), - y=None, - 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)], - ) + if use_v_rep: + compatible_lagrangian_degrees = clf_cbf.CompatibleWVrepLagrangianDegrees( + u_vertices=[clf_cbf.XYDegree(x=2, y=0) for _ in range(u_vertices.shape[0])], + u_extreme_rays=None, + xi_y=clf_cbf.XYDegree(x=2, y=0), + y=( + None + if use_y_squared + else [clf_cbf.XYDegree(x=4, y=0) for _ in range(compatible.y.size)] + ), + rho_minus_V=clf_cbf.XYDegree(x=2, y=2 if use_y_squared else 0), + h_plus_eps=[clf_cbf.XYDegree(x=2, y=2 if use_y_squared else 0)], + state_eq_constraints=[clf_cbf.XYDegree(x=4, y=2 if use_y_squared else 1)], + ) + else: + compatible_lagrangian_degrees = clf_cbf.CompatibleLagrangianDegrees( + lambda_y=[clf_cbf.XYDegree(x=2, y=0)], + xi_y=clf_cbf.XYDegree(x=2, y=0), + y=( + None + if use_y_squared + else [clf_cbf.XYDegree(x=4, y=0) for _ in range(compatible.y.size)] + ), + rho_minus_V=clf_cbf.XYDegree(x=2, y=2 if use_y_squared else 0), + h_plus_eps=[clf_cbf.XYDegree(x=2, y=2 if use_y_squared else 0)], + state_eq_constraints=[clf_cbf.XYDegree(x=2, y=2 if use_y_squared else 1)], + ) safety_sets_lagrangian_degrees = clf_cbf.SafetySetLagrangianDegrees( exclude=[ clf_cbf.ExcludeRegionLagrangianDegrees( @@ -220,15 +266,7 @@ def search(unit_test_flag: bool = False): x_set = sym.Variables(x) if not unit_test_flag: clf_cbf.save_clf_cbf( - V, - h, - x_set, - kappa_V, - kappa_h, - os.path.join( - os.path.dirname(os.path.abspath(__file__)), - "../../data/nonlinear_toy_clf_cbf.pkl", - ), + V, h, x_set, kappa_V, kappa_h, get_pkl_file_path(use_y_squared) ) return V, h @@ -270,10 +308,7 @@ def plot_incompatible( def visualize(): x = sym.MakeVectorContinuousVariable(3, "x") f, g = toy_system.affine_trig_poly_dynamics(x) - path = os.path.join( - os.path.dirname(os.path.abspath(__file__)), - "../../data/nonlinear_toy_clf_cbf.pkl", - ) + path = get_pkl_file_path(use_y_squared=True) x_set = sym.Variables(x) saved_data = clf_cbf.load_clf_cbf(path, x_set) fig = plt.figure() @@ -328,7 +363,8 @@ def main(): "--unit_test", action="store_true", help="Only turn this on in the unit test." ) args = parser.parse_args() - V, h = search(args.unit_test) + + V, h = search(use_v_rep=False, unit_test_flag=args.unit_test) if not args.unit_test: visualize() diff --git a/examples/quadrotor/demo.py b/examples/quadrotor/demo.py index 413a60f..5cb9a45 100644 --- a/examples/quadrotor/demo.py +++ b/examples/quadrotor/demo.py @@ -111,16 +111,14 @@ def search(use_y_squared: bool, with_u_bound: bool): 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=[ - 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((13,)) candidate_compatible_states_sequences = [] diff --git a/tests/test_clf_cbf.py b/tests/test_clf_cbf.py index 4b0a477..5593db2 100644 --- a/tests/test_clf_cbf.py +++ b/tests/test_clf_cbf.py @@ -518,9 +518,6 @@ def test_add_compatibility_w_clf_y_squared(self): barrier_eps=barrier_eps, local_clf=True, ) - (xi, lambda_mat) = dut._calc_xi_Lambda( - V=V, h=h, kappa_V=kappa_V, kappa_h=kappa_h - ) poly_expected = ( -1 - lagrangians.lambda_y.dot(lambda_mat.T @ dut.y_squared_poly) @@ -530,6 +527,155 @@ def test_add_compatibility_w_clf_y_squared(self): ) assert poly.CoefficientsAlmostEqual(poly_expected, tolerance=1e-5) + def test_add_compatibility_w_vrep1(self): + """ + Test _add_compatibility with CLF and use_y_squared=True + """ + dut = mut.CompatibleClfCbf( + f=self.f, + g=self.g, + x=self.x, + exclude_sets=self.exclude_sets, + within_set=self.within_set, + Au=None, + bu=None, + u_vertices=np.array([[0, 1], [1, 0], [0, 0]]), + u_extreme_rays=np.array([[-1, -1]]), + num_cbf=2, + with_clf=True, + use_y_squared=True, + ) + prog = solvers.MathematicalProgram() + prog.AddIndeterminates(dut.xy_set) + + V = sym.Polynomial(dut.x[0] ** 2 + 4 * dut.x[1] ** 2) / 0.1 + h = np.array( + [ + sym.Polynomial(1 - dut.x[0] ** 2 - dut.x[1] ** 2), + sym.Polynomial(2 - dut.x[0] ** 2 - dut.x[2] ** 2), + ] + ) + kappa_V = 0.01 + kappa_h = np.array([0.02, 0.03]) + + # Set up Lagrangians. + lagrangian_degrees = mut.CompatibleWVrepLagrangianDegrees( + u_vertices=[mut.XYDegree(x=2, y=2) for _ in range(dut.u_vertices.shape[0])], + u_extreme_rays=[ + mut.XYDegree(x=2, y=2) for _ in range(dut.u_extreme_rays.shape[0]) + ], + xi_y=mut.XYDegree(x=2, y=2), + y=None, + rho_minus_V=mut.XYDegree(x=2, y=2), + h_plus_eps=[mut.XYDegree(x=2, y=4) for _ in range(h.size)], + state_eq_constraints=None, + ) + lagrangians = lagrangian_degrees.to_lagrangians(prog, dut.x_set, dut.y_set) + + 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_w_vrep( + prog=prog, + V=V, + h=h, + xi=xi, + lambda_mat=lambda_mat, + lagrangians=lagrangians, + barrier_eps=barrier_eps, + local_clf=True, + ) + + poly_expected = ( + -1 + - lagrangians.u_vertices.dot( + -xi.dot(dut.y_squared_poly) + + dut.y_squared_poly @ (lambda_mat @ dut.u_vertices.T) + - 1 + ) + - lagrangians.u_extreme_rays.dot( + dut.y_squared_poly @ lambda_mat @ dut.u_extreme_rays.T + ) + - lagrangians.xi_y * (-xi.dot(dut.y_squared_poly) - 1) + - lagrangians.rho_minus_V * (1 - V) + - lagrangians.h_plus_eps.dot(h + barrier_eps) + ) + + assert poly.CoefficientsAlmostEqual(poly_expected, tolerance=1e-5) + + def test_add_compatibility_w_vrep2(self): + """ + Test _add_compatibility with CLF and use_y_squared=False + """ + dut = mut.CompatibleClfCbf( + f=self.f, + g=self.g, + x=self.x, + exclude_sets=self.exclude_sets, + within_set=self.within_set, + Au=None, + bu=None, + u_vertices=np.array([[0, 1], [1, 0], [0, 0]]), + u_extreme_rays=np.array([[-1, -1]]), + num_cbf=2, + with_clf=True, + use_y_squared=False, + ) + prog = solvers.MathematicalProgram() + prog.AddIndeterminates(dut.xy_set) + + V = sym.Polynomial(dut.x[0] ** 2 + 4 * dut.x[1] ** 2) / 0.1 + h = np.array( + [ + sym.Polynomial(1 - dut.x[0] ** 2 - dut.x[1] ** 2), + sym.Polynomial(2 - dut.x[0] ** 2 - dut.x[2] ** 2), + ] + ) + kappa_V = 0.01 + kappa_h = np.array([0.02, 0.03]) + + # Set up Lagrangians. + lagrangian_degrees = mut.CompatibleWVrepLagrangianDegrees( + u_vertices=[mut.XYDegree(x=2, y=2) for _ in range(dut.u_vertices.shape[0])], + u_extreme_rays=[ + mut.XYDegree(x=2, y=2) for _ in range(dut.u_extreme_rays.shape[0]) + ], + xi_y=mut.XYDegree(x=2, y=2), + y=[mut.XYDegree(x=2, y=2) for _ in range(dut.y.size)], + rho_minus_V=mut.XYDegree(x=2, y=2), + h_plus_eps=[mut.XYDegree(x=2, y=4) for _ in range(h.size)], + state_eq_constraints=None, + ) + lagrangians = lagrangian_degrees.to_lagrangians(prog, dut.x_set, dut.y_set) + + 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_w_vrep( + prog=prog, + V=V, + h=h, + xi=xi, + lambda_mat=lambda_mat, + lagrangians=lagrangians, + barrier_eps=barrier_eps, + local_clf=True, + ) + + poly_expected = ( + -1 + - lagrangians.u_vertices.dot( + -xi.dot(dut.y_poly) + dut.y_poly @ (lambda_mat @ dut.u_vertices.T) - 1 + ) + - lagrangians.u_extreme_rays.dot( + dut.y_poly @ lambda_mat @ dut.u_extreme_rays.T + ) + - lagrangians.xi_y * (-xi.dot(dut.y_poly) - 1) + - lagrangians.y.dot(dut.y_poly) + - lagrangians.rho_minus_V * (1 - V) + - lagrangians.h_plus_eps.dot(h + barrier_eps) + ) + + assert poly.CoefficientsAlmostEqual(poly_expected, tolerance=1e-5) + def test_add_barrier_exclude_constraint(self): """ Test _add_barrier_exclude_constraint @@ -1365,8 +1511,8 @@ def intersect_tester( 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], + u_vertices_lagrangian_degree=[0] * u_vertices.shape[0], + u_extreme_rays_lagrangian_degree=[0] * 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], ) @@ -1383,6 +1529,7 @@ def test_convexhull_intersect(self): 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]]), + np.array([[1, -0.5], [-0.5, 1], [1, 1]]), ] u_extreme_rays = np.zeros((0, 2)) @@ -1443,3 +1590,16 @@ def test_convexcone_not_intersect(self): self.intersect_tester( lambda_mat, xi, u_vertices, u_extreme_rays, intersect_expected=False ) + + def test_empty_set(self): + """ + The set Λu≤ ξ is empty. + """ + lambda_mat = np.array([[1, 1], [-1, 0], [0, -1]]) + xi = np.array([-1, 0, 0]) + + u_vertices = np.array([[0, 0], [0, 1], [1, 0]]) + u_extreme_rays = np.empty((0, 2)) + self.intersect_tester( + lambda_mat, xi, u_vertices, u_extreme_rays, intersect_expected=False + )