Add a toy 2D system with nonlinear dynamics.

We can certify the CLF/CBF are compatible for this toy system.

.github/workflows/python-app.yml

@@ -23,3 +23,5 @@ jobs:
         run: pytest tests
       - name: run linear_toy_demo
         run: python examples/linear_toy/linear_toy_demo.py
+      - name: run nonlinear_toy_demo
+        run: python examples/nonlinear_toy/wo_input_limits_demo.py

compatible_clf_cbf/clf_cbf.py

@@ -341,7 +341,7 @@ def certify_cbf_unsafe_region(
         Certifies that the 0-superlevel set {x | bᵢ(x) >= 0} does not intersect
         with the unsafe region self.unsafe_regions[unsafe_region_index].
 
-        If we denote the unsafe region as {x | q(x) <= 0}, then we impose the constraint
+        If we denote the unsafe region as {x | p(x) <= 0}, then we impose the constraint
 
         We impose the constraint
         -(1+ϕᵢ,₀(x))*bᵢ(x) +∑ⱼϕᵢ,ⱼ(x)pⱼ(x) is sos
@@ -546,6 +546,7 @@ def _add_compatibility(
 
         # Compute s₄(x, y)ᵀ(b(x)+ε)
         if barrier_eps is not None:
+            assert np.all(barrier_eps >= 0)
             assert lagrangians.b_plus_eps is not None
             poly -= lagrangians.b_plus_eps.dot(barrier_eps + b)
 

examples/nonlinear_toy/README.md

@@ -0,0 +1,11 @@
+We search for the compatible CLF and CBF function of a nonlinear dynamical system with 2 states
+
+$$
+\begin{align*}
+\dot{x}_0 =& u\\
+\dot{x}_1 =& -x_0 + \frac{1}{6}x_0^3 - u
+\end{align*}
+$$
+We will consider the case with or without the input limits on $u$.
+
+This system is introduced in *Searching for control Lyapunov functions using sums-of-squares programming* by Weehong Tan and Andrew Packard.

examples/nonlinear_toy/toy_system.py

@@ -0,0 +1,34 @@
+from typing import Tuple
+
+import numpy as np
+import pydrake.symbolic as sym
+
+
+def affine_dynamics(x: np.ndarray) -> Tuple[np.ndarray, np.ndarray]:
+    """
+    Compute the dynamics
+    ẋ₀ = u
+    ẋ₁ = −x₀ + 1/6x₀³−u
+    as ẋ = f(x) + g(x)*u
+
+    return f and g.
+    """
+    assert x.shape == (2,)
+    if x.dtype == object:
+        f = np.array(
+            [
+                sym.Polynomial(),
+                sym.Polynomial(
+                    {
+                        sym.Monomial(x[0]): sym.Expression(-1),
+                        sym.Monomial(x[0], 3): sym.Expression(-1.0 / 6.0),
+                    }
+                ),
+            ]
+        )
+        g = np.array([[sym.Polynomial(1)], [sym.Polynomial(-1)]])
+    else:
+        f = np.array([0, -x[0] + x[0] ** 3 / 6])
+        g = np.array([[1], [-1]])
+
+    return (f, g)

examples/nonlinear_toy/wo_input_limits_demo.py

@@ -0,0 +1,65 @@
+"""
+Find the compatible CLF/CBF with no input limits.
+"""
+import numpy as np
+import pydrake.solvers as solvers
+import pydrake.symbolic as sym
+
+from compatible_clf_cbf import clf_cbf
+from examples.nonlinear_toy import toy_system
+
+
+def main(use_y_squared: bool):
+    x = sym.MakeVectorContinuousVariable(2, "x")
+    f, g = toy_system.affine_dynamics(x)
+    use_y_squared = True
+    compatible = clf_cbf.CompatibleClfCbf(
+        f=f,
+        g=g,
+        x=x,
+        unsafe_regions=[np.array([sym.Polynomial(x[0] + 10)])],
+        Au=None,
+        bu=None,
+        with_clf=True,
+        use_y_squared=use_y_squared,
+    )
+    V_init = sym.Polynomial(x[0] ** 2 + x[1] ** 2)
+    b_init = np.array([sym.Polynomial(0.001 - x[0] ** 2 - x[1] ** 2)])
+    kappa_V = 1e-3
+    kappa_b = np.array([kappa_V])
+
+    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),
+        y=None
+        if use_y_squared
+        else [
+            clf_cbf.CompatibleLagrangianDegrees.Degree(x=4, y=0)
+            for _ in range(compatible.y.size)
+        ],
+        rho_minus_V=clf_cbf.CompatibleLagrangianDegrees.Degree(x=2, y=0),
+        b_plus_eps=[clf_cbf.CompatibleLagrangianDegrees.Degree(x=2, y=0)],
+    )
+    rho = 0.01
+    barrier_eps = np.array([0.0001])
+    (
+        compatible_prog,
+        compatible_lagrangians,
+    ) = compatible.construct_search_compatible_lagrangians(
+        V_init, b_init, kappa_V, kappa_b, lagrangian_degrees, rho, barrier_eps
+    )
+    compatible_result = solvers.Solve(compatible_prog)
+    assert compatible_result.is_success()
+
+    compatible.certify_cbf_unsafe_region(
+        unsafe_region_index=0,
+        cbf=b_init[0],
+        cbf_lagrangian_degree=0,
+        unsafe_region_lagrangian_degrees=[0],
+        solver_options=None,
+    )
+
+
+if __name__ == "__main__":
+    main(use_y_squared=True)
+    main(use_y_squared=False)