Add the cross term y[i]*y[j]>=0 to P-satz. (#76)

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

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,

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,

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,

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)],

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,

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,

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)],

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)],

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,

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)],