Find CLF and CBF separately for single integrator. (#41)

compatible_clf_cbf/cbf.py

@@ -2,7 +2,7 @@
 Certify and search Control Barrier Function (CBF) through sum-of-squares optimization.
 """
 from dataclasses import dataclass
-from typing import List, Optional, Union
+from typing import List, Optional, Tuple, Union
 from typing_extensions import Self
 
 import numpy as np
@@ -13,9 +13,13 @@
     check_array_of_polynomials,
     get_polynomial_result,
     new_sos_polynomial,
+    solve_with_id,
 )
 
-from compatible_clf_cbf.clf_cbf import UnsafeRegionLagrangians
+from compatible_clf_cbf.clf_cbf import (
+    UnsafeRegionLagrangians,
+    UnsafeRegionLagrangianDegrees,
+)
 
 
 @dataclass
@@ -256,6 +260,61 @@ def __init__(
             check_array_of_polynomials(state_eq_constraints, self.x_set)
         self.state_eq_constraints = state_eq_constraints
 
+    def search_lagrangians_given_cbf(
+        self,
+        b: sym.Polynomial,
+        eps: float,
+        kappa: float,
+        cbf_derivative_lagrangian_degrees: Union[
+            CbfWInputLimitLagrangianDegrees, CbfWoInputLimitLagrangianDegrees
+        ],
+        unsafe_region_lagrangian_degrees: UnsafeRegionLagrangianDegrees,
+        solver_id: Optional[solvers.SolverId] = None,
+        solver_options: Optional[solvers.SolverOptions] = None,
+        lagrangian_coefficient_tol: Optional[float] = None,
+    ) -> Tuple[
+        Optional[Union[CbfWInputLimitLagrangian, CbfWoInputLimitLagrangian]],
+        Optional[UnsafeRegionLagrangians],
+    ]:
+        """
+        For a given CBF candidate, certify the CBF conditions by finding the
+        Lagrangian multipliers.
+        """
+        prog_unsafe = solvers.MathematicalProgram()
+        prog_unsafe.AddIndeterminates(self.x_set)
+        unsafe_lagrangians = unsafe_region_lagrangian_degrees.to_lagrangians(
+            prog_unsafe, self.x_set
+        )
+        self._add_barrier_safe_constraint(prog_unsafe, b, unsafe_lagrangians)
+        result_unsafe = solve_with_id(prog_unsafe, solver_id, solver_options)
+        if result_unsafe.is_success():
+            unsafe_lagrangians_result = unsafe_lagrangians.get_result(
+                result_unsafe, lagrangian_coefficient_tol
+            )
+        else:
+            unsafe_lagrangians_result = None
+
+        prog_cbf_derivative = solvers.MathematicalProgram()
+        prog_cbf_derivative.AddIndeterminates(self.x_set)
+        cbf_derivative_lagrangians = cbf_derivative_lagrangian_degrees.to_lagrangians(
+            prog_cbf_derivative, self.x_set
+        )
+        self._add_cbf_derivative_condition(
+            prog_cbf_derivative, b, cbf_derivative_lagrangians, eps, kappa
+        )
+        result_cbf_derivative = solve_with_id(
+            prog_cbf_derivative, solver_id, solver_options
+        )
+
+        cbf_derivative_lagrangians_result = (
+            cbf_derivative_lagrangians.get_result(
+                result_cbf_derivative, lagrangian_coefficient_tol
+            )
+            if result_cbf_derivative.is_success()
+            else None
+        )
+        return cbf_derivative_lagrangians_result, unsafe_lagrangians_result
+
     def _add_barrier_safe_constraint(
         self,
         prog: solvers.MathematicalProgram,
@@ -292,7 +351,7 @@ def _add_barrier_safe_constraint(
         prog.AddSosConstraint(poly)
         return poly
 
-    def _add_cbf_condition(
+    def _add_cbf_derivative_condition(
         self,
         prog: solvers.MathematicalProgram,
         b: sym.Polynomial,

compatible_clf_cbf/clf.py

@@ -190,7 +190,7 @@ class ControlLyapunov:
     By positivestellasatz, this means that there exists λ₀(x), λ₁(x), λ₂(x)
     satisfying
     -(1+λ₀(x))*(∂V/∂x*f(x)+κ*V(x))−λ₁(x)*∂V/∂x*g(x)−λ₂(x)*(ρ−V(x)) is sos.
-    λ₀(x), λ₁(x), λ₂(x) are sos.
+    λ₀(x), λ₂(x) are sos.
 
     If the set 𝒰 is a polytope with vertices u₁,..., uₙ, then V is an CLF iff
     (V(x)-ρ)xᵀx is always non-negative on the semi-algebraic set
@@ -268,16 +268,20 @@ def search_lagrangian_given_clf(
         solver_id: Optional[solvers.SolverId] = None,
         solver_options: Optional[solvers.SolverOptions] = None,
         coefficient_tol: Optional[float] = None,
-    ) -> Union[ClfWInputLimitLagrangian, ClfWoInputLimitLagrangian]:
+    ) -> Optional[Union[ClfWInputLimitLagrangian, ClfWoInputLimitLagrangian]]:
         prog = solvers.MathematicalProgram()
         prog.AddIndeterminates(self.x_set)
         lagrangians = lagrangian_degrees.to_lagrangians(
             prog, self.x_set, self.x_equilibrium
         )
         self._add_clf_condition(prog, V, lagrangians, rho, kappa)
         result = solve_with_id(prog, solver_id, solver_options)
-        assert result.is_success()
-        return lagrangians.get_result(result, coefficient_tol)
+        lagrangians_result = (
+            lagrangians.get_result(result, coefficient_tol)
+            if result.is_success()
+            else None
+        )
+        return lagrangians_result
 
     def construct_search_clf_given_lagrangian(
         self,
@@ -323,7 +327,7 @@ def _add_clf_condition(
             assert isinstance(lagrangians, ClfWoInputLimitLagrangian)
             sos_poly = (
                 -(1 + lagrangians.dVdx_times_f) * (dVdx_times_f + kappa * V)
-                - dVdx_times_g.squeeze().dot(lagrangians.dVdx_times_g)
+                - dVdx_times_g.reshape((-1,)).dot(lagrangians.dVdx_times_g)
                 - lagrangians.rho_minus_V * (rho - V)
             )
         else:

compatible_clf_cbf/clf_cbf.py

@@ -210,16 +210,20 @@ class UnsafeRegionLagrangians:
     # (such as sin²θ+cos²θ=1)
     state_eq_constraints: Optional[np.ndarray]
 
-    def get_result(self, result: solvers.MathematicalProgramResult) -> Self:
+    def get_result(
+        self,
+        result: solvers.MathematicalProgramResult,
+        coefficient_tol: Optional[float],
+    ) -> Self:
         return UnsafeRegionLagrangians(
-            cbf=result.GetSolution(self.cbf),
-            unsafe_region=np.array(
-                [result.GetSolution(phi) for phi in self.unsafe_region]
+            cbf=get_polynomial_result(result, self.cbf, coefficient_tol),
+            unsafe_region=get_polynomial_result(
+                result, self.unsafe_region, coefficient_tol
             ),
             state_eq_constraints=None
             if self.state_eq_constraints is None
-            else np.array(
-                [result.GetSolution(poly) for poly in self.state_eq_constraints]
+            else get_polynomial_result(
+                result, self.state_eq_constraints, coefficient_tol
             ),
         )
 
@@ -385,6 +389,7 @@ def certify_cbf_unsafe_region(
         cbf: sym.Polynomial,
         lagrangian_degrees: UnsafeRegionLagrangianDegrees,
         solver_options: Optional[solvers.SolverOptions] = None,
+        lagrangian_coefficient_tol: Optional[float] = None,
     ) -> UnsafeRegionLagrangians:
         """
         Certifies that the 0-superlevel set {x | bᵢ(x) >= 0} does not intersect
@@ -408,7 +413,7 @@ def certify_cbf_unsafe_region(
         self._add_barrier_safe_constraint(prog, unsafe_region_index, cbf, lagrangians)
         result = solvers.Solve(prog, None, solver_options)
         assert result.is_success()
-        return lagrangians.get_result(result)
+        return lagrangians.get_result(result, lagrangian_coefficient_tol)
 
     def construct_search_compatible_lagrangians(
         self,

examples/single_integrator/wo_input_limit_demo.py

@@ -0,0 +1,124 @@
+"""
+Find the CLF and CBF jointly, versus finding CLF and CBF separately for a
+2-dimensional single-integrator system xdot = u. I assume the obstacle is a
+sphere.
+"""
+from typing import Tuple
+
+import numpy as np
+
+import pydrake.symbolic as sym
+
+from compatible_clf_cbf.clf import ControlLyapunov, ClfWoInputLimitLagrangianDegrees
+from compatible_clf_cbf.cbf import ControlBarrier, CbfWoInputLimitLagrangianDegrees
+from compatible_clf_cbf.clf_cbf import UnsafeRegionLagrangianDegrees
+
+
+def affine_dynamics() -> Tuple[np.ndarray, np.ndarray]:
+    """
+    Return the dynamics as f(x) + g(x)*u
+    """
+    f = np.array([sym.Polynomial(0), sym.Polynomial(0)])
+    g = np.array(
+        [[sym.Polynomial(1), sym.Polynomial(0)], [sym.Polynomial(0), sym.Polynomial(1)]]
+    )
+    return f, g
+
+
+def find_clf_cbf_separately(
+    x: np.ndarray, obstacle_center: np.ndarray, obstacle_radius: float
+) -> Tuple[sym.Polynomial, sym.Polynomial]:
+    assert x.shape == (2,)
+    clf = sym.Polynomial(x[0] ** 2 + x[1] ** 2)
+    assert obstacle_center.shape == (2,)
+    cbf = sym.Polynomial(
+        (x[0] - obstacle_center[0]) ** 2
+        + (x[1] - obstacle_center[1]) ** 2
+        - obstacle_radius**2
+    )
+    return clf, cbf
+
+
+def certify_clf_cbf_separately(
+    x: np.ndarray,
+    clf: sym.Polynomial,
+    cbf: sym.Polynomial,
+    rho: float,
+    kappa_V: float,
+    kappa_b: float,
+    barrier_eps: float,
+    obstacle_center: np.ndarray,
+    obstacle_radius: float,
+):
+    f, g = affine_dynamics()
+    control_lyapunov = ControlLyapunov(
+        f=f,
+        g=g,
+        x=x,
+        x_equilibrium=np.zeros(2),
+        u_vertices=None,
+        state_eq_constraints=None,
+    )
+    clf_lagrangian = control_lyapunov.search_lagrangian_given_clf(
+        clf,
+        rho,
+        kappa_V,
+        ClfWoInputLimitLagrangianDegrees(
+            dVdx_times_f=0,
+            dVdx_times_g=[1, 1],
+            rho_minus_V=0,
+            state_eq_constraints=None,
+        ),
+    )
+    assert clf_lagrangian is not None
+
+    control_barrier = ControlBarrier(
+        f=f,
+        g=g,
+        x=x,
+        unsafe_region=np.array(
+            [
+                sym.Polynomial(
+                    (x[0] - obstacle_center[0]) ** 2
+                    + (x[1] - obstacle_center[1]) ** 2
+                    - obstacle_radius**2
+                )
+            ]
+        ),
+        u_vertices=None,
+        state_eq_constraints=None,
+    )
+    (
+        cbf_derivative_lagrangians,
+        unsafe_lagrangians,
+    ) = control_barrier.search_lagrangians_given_cbf(
+        cbf,
+        barrier_eps,
+        kappa_b,
+        CbfWoInputLimitLagrangianDegrees(
+            dbdx_times_f=0, dbdx_times_g=[1, 1], b_plus_eps=0, state_eq_constraints=None
+        ),
+        UnsafeRegionLagrangianDegrees(
+            cbf=0, unsafe_region=[0], state_eq_constraints=None
+        ),
+    )
+    assert cbf_derivative_lagrangians is not None
+    assert unsafe_lagrangians is not None
+
+
+def main():
+    x = sym.MakeVectorContinuousVariable(2, "x")
+    obstacle_center = np.array([1.0, 0.0])
+    obstacle_radius = 0.5
+    V, b = find_clf_cbf_separately(x, obstacle_center, obstacle_radius)
+    rho = 10
+    kappa_V = 0.01
+    kappa_b = 0.01
+    barrier_eps = 0.0
+    certify_clf_cbf_separately(
+        x, V, b, rho, kappa_V, kappa_b, barrier_eps, obstacle_center, obstacle_radius
+    )
+
+
+if __name__ == "__main__":
+    main()

tests/test_cbf.py

@@ -89,7 +89,7 @@ def test_add_barrier_safe_constraint(self):
         )
         assert poly.CoefficientsAlmostEqual(poly_expected, 1e-8)
 
-    def test_add_cbf_condition(self):
+    def test_add_cbf_derivative_condition(self):
         dut = mut.ControlBarrier(
             f=self.f,
             g=self.g,
@@ -112,7 +112,9 @@ def test_add_cbf_condition(self):
         lagrangians = lagrangian_degrees.to_lagrangians(prog, dut.x_set)
         eps = 0.01
         kappa = 0.001
-        sos_poly = dut._add_cbf_condition(prog, b_init, lagrangians, eps, kappa)
+        sos_poly = dut._add_cbf_derivative_condition(
+            prog, b_init, lagrangians, eps, kappa
+        )
         result = solvers.Solve(prog)
         assert result.is_success()