Support Au * u <= bu in CompatibleClfCbf. (#46)

.flake8

@@ -2,3 +2,4 @@
 max-line-length = 88
 exclude =
     venv
+extend-ignore = E203

compatible_clf_cbf/clf_cbf.py

@@ -403,6 +403,16 @@ class CompatibleClfCbf:
 
     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ᵀ * [-∂b/∂x*g(x)] = 0, yᵀ * [ ∂b/∂x*f(x)+κ_b*b(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.
     """  # noqa E501
 
     def __init__(
@@ -485,7 +495,11 @@ def __init__(
         self.bu = bu
         self.with_clf = with_clf
         self.use_y_squared = use_y_squared
-        y_size = len(self.unsafe_regions) + (1 if self.with_clf else 0)
+        y_size = (
+            len(self.unsafe_regions)
+            + (1 if self.with_clf else 0)
+            + (self.Au.shape[0] if self.Au is not None else 0)
+        )
         self.y: np.ndarray = sym.MakeVectorContinuousVariable(y_size, "y")
         self.y_set: sym.Variables = sym.Variables(self.y)
         self.xy_set: sym.Variables = sym.Variables(np.concatenate((self.x, self.y)))
@@ -979,8 +993,10 @@ def _calc_xi_Lambda(
         Compute
         Λ(x) = [-∂b/∂x*g(x)]
                [ ∂V/∂x*g(x)]
+               [        Au ]
         ξ(x) = [ ∂b/∂x*f(x)+κ_b*b(x)]
                [-∂V/∂x*f(x)-κ_V*V(x)]
+               [                 bu ]
 
         Args:
           V: The CLF function. If with_clf is False, then V is None.
@@ -992,13 +1008,17 @@ def _calc_xi_Lambda(
         """
         num_unsafe_regions = len(self.unsafe_regions)
         if self.with_clf:
+            assert V is not None
             assert isinstance(V, sym.Polynomial)
             dVdx = V.Jacobian(self.x)
             xi_rows = num_unsafe_regions + 1
         else:
             assert V is None
             dVdx = None
             xi_rows = num_unsafe_regions
+            assert b.size > 1, "You should use multiple CBF when with_clf is False."
+        if self.Au is not None:
+            xi_rows += self.Au.shape[0]
         assert b.shape == (len(self.unsafe_regions),)
         assert kappa_b.shape == b.shape
         dbdx = np.concatenate(
@@ -1008,12 +1028,15 @@ def _calc_xi_Lambda(
         lambda_mat[:num_unsafe_regions] = -dbdx @ self.g
         xi = np.empty((xi_rows,), dtype=object)
         xi[:num_unsafe_regions] = dbdx @ self.f + kappa_b * b
-        # TODO(hongkai.dai): support input bounds Au * u <= bu
-        assert self.Au is None and self.bu is None
 
         if self.with_clf:
-            lambda_mat[-1] = dVdx @ self.g
-            xi[-1] = -dVdx.dot(self.f) - kappa_V * V
+            assert V is not None
+            assert dVdx is not None
+            lambda_mat[num_unsafe_regions] = dVdx @ self.g
+            xi[num_unsafe_regions] = -dVdx.dot(self.f) - kappa_V * V
+        if self.Au is not None:
+            lambda_mat[-self.Au.shape[0] :] = self.Au
+            xi[-self.Au.shape[0] :] = self.bu
 
         return (xi, lambda_mat)
 

tests/test_clf_cbf.py

@@ -216,6 +216,42 @@ def check_members(cls: mut.CompatibleClfCbf):
         assert dut.y.shape == (len(self.unsafe_regions),)
         check_members(dut)
 
+        # Now construct with Au and bu
+        dut = mut.CompatibleClfCbf(
+            f=self.f,
+            g=self.g,
+            x=self.x,
+            unsafe_regions=self.unsafe_regions,
+            Au=np.array([[-3, -2], [1.0, 4.0], [0.0, 3.0]]),
+            bu=np.array([4, 5.0, 6.0]),
+            with_clf=False,
+            use_y_squared=True,
+        )
+        assert dut.Au is not None
+        assert dut.Au.shape == (3, self.nu)
+        assert dut.bu is not None
+        assert dut.bu.shape == (3,)
+        assert dut.y.shape == (len(self.unsafe_regions) + dut.Au.shape[0],)
+        check_members(dut)
+
+        # Now construct with Au, bu and with_clf=True
+        dut = mut.CompatibleClfCbf(
+            f=self.f,
+            g=self.g,
+            x=self.x,
+            unsafe_regions=self.unsafe_regions,
+            Au=np.array([[-3, -2], [1, 4], [3, -1.0]]),
+            bu=np.array([4, 5.0, 6.0]),
+            with_clf=True,
+            use_y_squared=True,
+        )
+        assert dut.Au is not None
+        assert dut.Au.shape == (3, self.nu)
+        assert dut.bu is not None
+        assert dut.bu.shape == (3,)
+        assert dut.y.shape == (len(self.unsafe_regions) + 1 + dut.Au.shape[0],)
+        check_members(dut)
+
     def test_calc_xi_Lambda_w_clf(self):
         """
         Test _calc_xi_Lambda with CLF.
@@ -297,6 +333,70 @@ def test_calc_xi_Lambda_wo_clf(self):
         lambda_mat_expected = -dbdx @ self.g
         utils.check_polynomial_arrays_equal(lambda_mat, lambda_mat_expected, 1e-8)
 
+    def test_calc_xi_Lambda_w_clf_Aubu(self):
+        """
+        Test _calc_xi_Lambda with CLF and Au * u <= bu
+        """
+        dut = mut.CompatibleClfCbf(
+            f=self.f,
+            g=self.g,
+            x=self.x,
+            unsafe_regions=self.unsafe_regions,
+            Au=np.array([[-3, 2], [1, 4], [3, 8.0]]),
+            bu=np.array([3.0, 5.0, 10.0]),
+            with_clf=True,
+            use_y_squared=True,
+        )
+        V = sym.Polynomial(
+            self.x[0] ** 2 + self.x[1] ** 2 + self.x[2] ** 2 + self.x[0] * 2
+        )
+        b = np.array(
+            [
+                sym.Polynomial(1 - self.x[0] ** 2 - self.x[1] ** 2 - self.x[2] ** 2),
+                sym.Polynomial(2 - self.x[0] ** 4 - self.x[2] ** 2 * self.x[1] ** 2),
+            ]
+        )
+        kappa_V = 0.01
+        kappa_b = np.array([0.02, 0.03])
+        xi, lambda_mat = dut._calc_xi_Lambda(V=V, b=b, kappa_V=kappa_V, kappa_b=kappa_b)
+
+        dVdx = V.Jacobian(self.x)
+        dbdx = np.empty((2, self.nx), dtype=object)
+        dbdx[0] = b[0].Jacobian(self.x)
+        dbdx[1] = b[1].Jacobian(self.x)
+
+        # Check xi
+        assert dut.bu is not None
+        xi_expected = np.empty((b.size + 1 + dut.bu.size,), dtype=object)
+        xi_expected[0] = dbdx[0].dot(self.f) + kappa_b[0] * b[0]
+        xi_expected[1] = dbdx[1].dot(self.f) + kappa_b[1] * b[1]
+        xi_expected[2] = -dVdx.dot(self.f) - kappa_V * V
+        assert dut.Au is not None
+        xi_expected[-dut.Au.shape[0] :] = dut.bu
+
+        assert xi.shape == xi_expected.shape
+        for i in range(xi.size):
+            if isinstance(xi[i], float):
+                np.testing.assert_equal(xi[i], xi_expected[i])
+            else:
+                assert xi[i].CoefficientsAlmostEqual(xi_expected[i], 1e-8)
+
+        # Check Lambda
+        lambda_mat_expected = np.empty((xi_expected.size, self.nu), dtype=object)
+        lambda_mat_expected[0] = -dbdx[0] @ self.g
+        lambda_mat_expected[1] = -dbdx[1] @ self.g
+        lambda_mat_expected[2] = dVdx @ self.g
+        lambda_mat_expected[-dut.Au.shape[0] :] = dut.Au
+        assert lambda_mat.shape == lambda_mat_expected.shape
+        for i in range(lambda_mat.shape[0]):
+            for j in range(lambda_mat.shape[1]):
+                if isinstance(lambda_mat[i, j], float):
+                    np.testing.assert_equal(lambda_mat[i, j], lambda_mat_expected[i, j])
+                else:
+                    assert lambda_mat[i, j].CoefficientsAlmostEqual(
+                        lambda_mat_expected[i, j], 1e-8
+                    )
+
     def test_search_compatible_lagrangians_w_clf_y_squared(self):
         """
         Test search_compatible_lagrangians with CLF and use_y_squared=True