diff --git a/compatible_clf_cbf/clf_cbf.py b/compatible_clf_cbf/clf_cbf.py index 03024d6..f0ce863 100644 --- a/compatible_clf_cbf/clf_cbf.py +++ b/compatible_clf_cbf/clf_cbf.py @@ -50,6 +50,10 @@ class CompatibleLagrangians: # The Lagrangian polynomial multiplies with y if use_y_squared = False. # This multiplier is an array of SOS polynomials. y: Optional[np.ndarray] + # The Lagrangian polynomial multiplies with cross terms of y + # (namely y[i]*y[j]) if use_y_squared = False. This multiplier is an array + # of SOS poynomials. + y_cross: Optional[np.ndarray] # The Lagrangian polynomial multiplies with ρ − V when with_clf = True, and # we search for an CLF with a region-of-attraction {x | V(x) <= ρ}. # Should be a SOS polynomial. @@ -76,6 +80,11 @@ def get_result( if self.y is not None else None ) + y_cross_result = ( + get_polynomial_result(result, self.y_cross, coefficient_tol) + if self.y_cross is not None + else None + ) rho_minus_V_result = ( get_polynomial_result(result, self.rho_minus_V, coefficient_tol) if self.rho_minus_V is not None @@ -93,6 +102,7 @@ def get_result( lambda_y=lambda_y_result, xi_y=xi_y_result, y=y_result, + y_cross=y_cross_result, rho_minus_V=rho_minus_V_result, h_plus_eps=h_plus_eps_result, state_eq_constraints=state_eq_constraints_result, @@ -192,6 +202,7 @@ def __init__(self, *args, **kwargs): lambda_y: List[XYDegree] xi_y: XYDegree y: Optional[List[XYDegree]] + y_cross: Optional[List[XYDegree]] rho_minus_V: Optional[XYDegree] h_plus_eps: List[XYDegree] state_eq_constraints: Optional[List[XYDegree]] @@ -206,6 +217,7 @@ def to_lagrangians( lambda_y_lagrangian: Optional[np.ndarray] = None, xi_y_lagrangian: Optional[sym.Polynomial] = None, y_lagrangian: Optional[np.ndarray] = None, + y_cross_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, @@ -231,6 +243,15 @@ def to_lagrangians( y_lagrangian_new = _to_lagrangian_impl( prog, x, y, sos_type, is_sos=True, degree=self.y, lagrangian=y_lagrangian ) + y_cross_lagrangian_new = _to_lagrangian_impl( + prog, + x, + y, + sos_type, + is_sos=True, + degree=self.y_cross, + lagrangian=y_cross_lagrangian, + ) rho_minus_V = _to_lagrangian_impl( prog, x, @@ -262,6 +283,7 @@ def to_lagrangians( lambda_y=lambda_y, xi_y=xi_y, y=y_lagrangian_new, + y_cross=y_cross_lagrangian_new, rho_minus_V=rho_minus_V, h_plus_eps=h_plus_eps, state_eq_constraints=state_eq_constraints, @@ -288,6 +310,10 @@ class CompatibleWVrepLagrangians: # this is None. # Size is (num_y,) y: Optional[np.ndarray] + # The SOS Lagrangian multiplier multiplies with cross terms of y + # (namely y[i] * y[j]). When use_y_square=True, this is None. Size is + # (num_y * (num_y-1)/2,) + y_cross: 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] @@ -321,6 +347,11 @@ def get_result( if self.y is None else get_polynomial_result(result, self.y, coefficient_tol) ) + y_cross = ( + None + if self.y_cross is None + else get_polynomial_result(result, self.y_cross, coefficient_tol) + ) rho_minus_V = ( None if self.rho_minus_V is None @@ -339,6 +370,7 @@ def get_result( u_extreme_rays, xi_y, y, + y_cross, rho_minus_V, h_plus_eps, state_eq_constraints, @@ -351,6 +383,7 @@ class CompatibleWVrepLagrangianDegrees: u_extreme_rays: Optional[List[XYDegree]] xi_y: Optional[XYDegree] y: Optional[List[XYDegree]] + y_cross: Optional[List[XYDegree]] rho_minus_V: Optional[XYDegree] h_plus_eps: List[XYDegree] state_eq_constraints: Optional[List[XYDegree]] @@ -366,6 +399,7 @@ def to_lagrangians( u_extreme_rays_lagrangian: Optional[np.ndarray] = None, xi_y_lagrangian: Optional[sym.Polynomial] = None, y_lagrangian: Optional[np.ndarray] = None, + y_cross_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, @@ -407,6 +441,15 @@ def to_lagrangians( degree=self.y, lagrangian=y_lagrangian, ), + y_cross=_to_lagrangian_impl( + prog, + x, + y, + sos_type, + is_sos=True, + degree=self.y_cross, + lagrangian=y_cross_lagrangian, + ), rho_minus_V=_to_lagrangian_impl( prog, x, @@ -987,6 +1030,17 @@ def __init__( self.y_poly = np.array( [sym.Polynomial(sym.Monomial(self.y[i], 1)) for i in range(y_size)] ) + # y_cross_poly contains the cross terms y[i]*y[j]. + self.y_cross_poly = np.empty( + int(self.y.size * (self.y.size - 1) / 2), dtype=object + ) + y_cross_count = 0 + for i in range(self.y.size): + for j in range(i + 1, self.y.size): + self.y_cross_poly[y_cross_count] = sym.Polynomial( + sym.Monomial({self.y[i]: 1, self.y[j]: 1}) + ) + y_cross_count += 1 # y_squared_poly[i] is just the polynomial y[i]**2. self.y_squared_poly = np.array( [sym.Polynomial(sym.Monomial(self.y[i], 2)) for i in range(y_size)] @@ -1809,6 +1863,8 @@ def _add_compatibility( if not self.use_y_squared: assert lagrangians.y is not None poly -= lagrangians.y.dot(self.y_poly) + if lagrangians.y_cross is not None: + poly -= lagrangians.y_cross.dot(self.y_cross_poly) # Compute s₃(x, y)(1 − V) if self.with_clf and local_clf: @@ -1887,6 +1943,8 @@ def _add_compatibility_w_vrep( if not self.use_y_squared: assert lagrangians.y is not None poly -= lagrangians.y.dot(self.y_poly) + if lagrangians.y_cross is not None: + poly -= lagrangians.y_cross.dot(self.y_cross_poly) if self.with_clf and local_clf: assert lagrangians.rho_minus_V is not None diff --git a/examples/linear_toy/linear_toy_demo.py b/examples/linear_toy/linear_toy_demo.py index b88a86c..3b01029 100644 --- a/examples/linear_toy/linear_toy_demo.py +++ b/examples/linear_toy/linear_toy_demo.py @@ -31,6 +31,7 @@ def search_compatible_lagrangians( if dut.use_y_squared else [clf_cbf.XYDegree(x=2, y=0) for _ in range(y_size)] ), + y_cross=None, rho_minus_V=None, h_plus_eps=[clf_cbf.XYDegree(x=2, y=0)], state_eq_constraints=None, 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 54dd680..fb02cfa 100644 --- a/examples/linear_toy/linear_toy_w_input_limits_demo.py +++ b/examples/linear_toy/linear_toy_w_input_limits_demo.py @@ -32,6 +32,7 @@ def search_compatible_lagrangians( if dut.use_y_squared else [clf_cbf.XYDegree(x=2, y=0) for _ in range(y_size)] ), + y_cross=None, rho_minus_V=clf_cbf.XYDegree(x=2, y=2), h_plus_eps=[clf_cbf.XYDegree(x=2, y=2)], state_eq_constraints=None, diff --git a/examples/nonlinear_toy/demo.py b/examples/nonlinear_toy/demo.py index 4eaadbf..cec94c0 100644 --- a/examples/nonlinear_toy/demo.py +++ b/examples/nonlinear_toy/demo.py @@ -48,6 +48,7 @@ def main(use_y_squared: bool, with_u_bound: bool): if use_y_squared else [clf_cbf.XYDegree(x=4, y=0) for _ in range(compatible.y.size)] ), + y_cross=None, rho_minus_V=clf_cbf.XYDegree(x=2, y=2), h_plus_eps=[clf_cbf.XYDegree(x=2, y=2)], state_eq_constraints=None, diff --git a/examples/nonlinear_toy/demo_trigpoly.py b/examples/nonlinear_toy/demo_trigpoly.py index c986b9c..90d1090 100644 --- a/examples/nonlinear_toy/demo_trigpoly.py +++ b/examples/nonlinear_toy/demo_trigpoly.py @@ -182,6 +182,7 @@ def search(use_v_rep: bool, unit_test_flag: bool = False): if use_y_squared else [clf_cbf.XYDegree(x=4, y=0) for _ in range(compatible.y.size)] ), + y_cross=None, 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)], @@ -195,6 +196,7 @@ def search(use_v_rep: bool, unit_test_flag: bool = False): if use_y_squared else [clf_cbf.XYDegree(x=4, y=0) for _ in range(compatible.y.size)] ), + y_cross=None, 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)], diff --git a/examples/nonlinear_toy/synthesize_demo.py b/examples/nonlinear_toy/synthesize_demo.py index 97aa1d0..9542b60 100644 --- a/examples/nonlinear_toy/synthesize_demo.py +++ b/examples/nonlinear_toy/synthesize_demo.py @@ -45,6 +45,7 @@ def main(with_u_bound: bool): lambda_y=[clf_cbf.XYDegree(x=3, y=0)], xi_y=clf_cbf.XYDegree(x=2, y=0), y=None, + y_cross=None, rho_minus_V=clf_cbf.XYDegree(x=2, y=2), h_plus_eps=[clf_cbf.XYDegree(x=2, y=2)], state_eq_constraints=None, diff --git a/examples/power_converter/demo.py b/examples/power_converter/demo.py index 8ff829f..9830e1c 100644 --- a/examples/power_converter/demo.py +++ b/examples/power_converter/demo.py @@ -107,6 +107,7 @@ def search(use_y_squared: bool): if use_y_squared else [clf_cbf.XYDegree(x=4, y=0) for _ in range(compatible.y.size)] ), + y_cross=None, 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 5cb9a45..fb8284a 100644 --- a/examples/quadrotor/demo.py +++ b/examples/quadrotor/demo.py @@ -107,6 +107,7 @@ def search(use_y_squared: bool, with_u_bound: bool): if use_y_squared else [clf_cbf.XYDegree(x=4, y=0) for _ in range(compatible.y.size)] ), + y_cross=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)], diff --git a/examples/quadrotor2d/demo.py b/examples/quadrotor2d/demo.py index a2f114b..7ee8200 100644 --- a/examples/quadrotor2d/demo.py +++ b/examples/quadrotor2d/demo.py @@ -82,6 +82,7 @@ def main(use_y_squared: bool, with_u_bound: bool, use_v_rep: bool): if use_y_squared else [clf_cbf.XYDegree(x=4, y=2) for _ in range(compatible.y.size)] ), + y_cross=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=4, y=2)], @@ -97,6 +98,7 @@ def main(use_y_squared: bool, with_u_bound: bool, use_v_rep: bool): if use_y_squared else [clf_cbf.XYDegree(x=4, y=0) for _ in range(compatible.y.size)] ), + y_cross=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)], diff --git a/examples/quadrotor2d/demo_taylor.py b/examples/quadrotor2d/demo_taylor.py index 97dbe30..23e3a5c 100644 --- a/examples/quadrotor2d/demo_taylor.py +++ b/examples/quadrotor2d/demo_taylor.py @@ -93,6 +93,7 @@ def search_clf_cbf( if use_y_squared else [clf_cbf.XYDegree(x=4, y=0) for _ in range(compatible.y.size)] ), + y_cross=None, rho_minus_V=clf_cbf.XYDegree(x=4, y=2), h_plus_eps=[clf_cbf.XYDegree(x=4, y=2)], state_eq_constraints=None, @@ -106,6 +107,7 @@ def search_clf_cbf( if use_y_squared else [clf_cbf.XYDegree(x=4, y=0) for _ in range(compatible.y.size)] ), + y_cross=None, 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/tests/test_clf_cbf.py b/tests/test_clf_cbf.py index 5593db2..ba87fae 100644 --- a/tests/test_clf_cbf.py +++ b/tests/test_clf_cbf.py @@ -207,6 +207,12 @@ def check_members(cls: mut.CompatibleClfCbf): assert cls.y_squared_poly.shape == cls.y.shape 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 cls.y_cross_poly.size == cls.y.size * (cls.y.size - 1) / 2 + for y_cross in cls.y_cross_poly: + assert y_cross.TotalDegree() == 2 + assert len(y_cross.monomial_to_coefficient_map()) == 1 + for v in y_cross.indeterminates(): + assert y_cross.Degree(v) == 1 assert dut.y.shape == (dut.num_cbf + 1,) check_members(dut) @@ -447,6 +453,7 @@ def test_search_compatible_lagrangians_w_clf_y_squared(self): lambda_y=[mut.XYDegree(x=2, y=0) for _ in range(self.nu)], xi_y=mut.XYDegree(x=2, y=0), y=None, + y_cross=None, 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, @@ -492,6 +499,7 @@ def test_add_compatibility_w_clf_y_squared(self): ) xi_y_lagrangian = prog.NewFreePolynomial(dut.xy_set, deg=2) y_lagrangian = None + y_cross_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(dut.num_cbf)] @@ -500,6 +508,7 @@ def test_add_compatibility_w_clf_y_squared(self): lambda_y=lambda_y_lagrangian, xi_y=xi_y_lagrangian, y=y_lagrangian, + y_cross=y_cross_lagrangian, rho_minus_V=rho_minus_V_lagrangian, h_plus_eps=h_plus_eps_lagrangian, state_eq_constraints=None, @@ -566,6 +575,7 @@ def test_add_compatibility_w_vrep1(self): ], xi_y=mut.XYDegree(x=2, y=2), y=None, + y_cross=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, @@ -641,6 +651,7 @@ def test_add_compatibility_w_vrep2(self): ], xi_y=mut.XYDegree(x=2, y=2), y=[mut.XYDegree(x=2, y=2) for _ in range(dut.y.size)], + y_cross=[mut.XYDegree(x=2, y=0) for _ in range(dut.y_cross_poly.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, @@ -670,6 +681,7 @@ def test_add_compatibility_w_vrep2(self): ) - lagrangians.xi_y * (-xi.dot(dut.y_poly) - 1) - lagrangians.y.dot(dut.y_poly) + - lagrangians.y_cross.dot(dut.y_cross_poly) - lagrangians.rho_minus_V * (1 - V) - lagrangians.h_plus_eps.dot(h + barrier_eps) ) @@ -911,6 +923,7 @@ def search_lagrangians( lambda_y=[mut.XYDegree(x=3, y=0)], xi_y=mut.XYDegree(x=2, y=0), y=None, + y_cross=None, rho_minus_V=mut.XYDegree(x=2, y=0), h_plus_eps=[mut.XYDegree(x=2, y=0)], state_eq_constraints=None, @@ -1265,6 +1278,7 @@ def search_lagrangians(self, check_result=False) -> Tuple[ lambda_y=[mut.XYDegree(x=2, y=0)], xi_y=mut.XYDegree(x=2, y=0), y=None, + y_cross=None, 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)],