Change API of bilinear_alternation.

Use InnerEllipsoidOptions to group the optional arguments for finding inscribed ellipsoid.

compatible_clf_cbf/clf.py

@@ -9,6 +9,7 @@
 import numpy as np
 import pydrake.solvers as solvers
 import pydrake.symbolic as sym
+import pydrake.autodiffutils
 
 from compatible_clf_cbf.utils import (
     check_array_of_polynomials,
@@ -342,3 +343,129 @@ def _add_clf_condition(
             sos_poly -= lagrangians.state_eq_constraints.dot(self.state_eq_constraints)
         prog.AddSosConstraint(sos_poly)
         return sos_poly
+
+
+class FindClfWoInputLimitsCounterExample:
+    """
+    For a candidate CLF function (without input limits), find the counter-example
+    (that violates the CLF condition) through nonlinear optimization.
+    Namely we solve this problem:
+    max ∂V/∂x*f(x)+κ*V
+    s.t ∂V/∂x*g(x) = 0
+        V(x) <= ρ
+
+    This class constructs the optimization program above (but doesn't solve it).
+    You can access the constructed program through self.prog.
+    """
+
+    def __init__(
+        self,
+        x: np.ndarray,
+        f: np.ndarray,
+        g: np.ndarray,
+        V: sym.Polynomial,
+        rho: float,
+        kappa: float,
+        state_eq_constraints: Optional[np.ndarray],
+    ):
+        self.x = x
+        self.nx = x.size
+        self.f = f
+        assert f.shape == (self.nx,)
+        self.g = g
+        assert g.shape[0] == self.nx
+        self.nu = g.shape[1]
+        self.V = V
+        self.dVdx = V.Jacobian(self.x)
+        self.dVdx_times_f: sym.Polynomial = self.dVdx.dot(f)
+        self.J_dVdx_times_f = self.dVdx_times_f.Jacobian(self.x)
+        self.dVdx_times_g = (self.dVdx.reshape((1, -1)) @ self.g).reshape((-1,))
+        self.J_dVdx_times_g = np.array(
+            [self.dVdx_times_g[i].Jacobian(self.x) for i in range(self.nu)]
+        )
+        self.rho = rho
+        self.kappa = kappa
+        self.h = state_eq_constraints
+        self.h_size = 0 if self.h is None else self.h.size
+        self.J_h = (
+            np.array([self.h[i].Jacobian(x) for i in range(self.h.size)])
+            if self.h is not None
+            else None
+        )
+        self.cost_poly = -(self.dVdx_times_f + self.kappa * V)
+        self.J_cost_poly = -(self.J_dVdx_times_f + self.kappa * self.dVdx)
+
+        def constraint(x_eval):
+            if x_eval.dtype == object:
+                x_val = pydrake.autodiffutils.ExtractValue(x_eval)
+                x_grad = pydrake.autodiffutils.ExtractGradient(x_eval)
+            else:
+                x_val = x_eval
+            env = {x[i]: x_val[i] for i in range(self.nx)}
+            constraint_val = np.empty((1 + self.nu + self.h_size,))
+            constraint_val[: self.nu] = np.array(
+                [self.dVdx_times_g[i].Evaluate(env) for i in range(self.nu)]
+            )
+            constraint_val[self.nu] = self.V.Evaluate(env)
+            if self.h is not None:
+                constraint_val[-self.h_size :] = np.array(
+                    [self.h[i].Evaluate(env) for i in range(self.h_size)]
+                )
+            # Now evaluate the gradient.
+            if x_eval.dtype == object:
+                dconstraint_dx = np.zeros((constraint_val.size, self.nx))
+                dconstraint_dx[: self.nu, :] = np.array(
+                    [
+                        [
+                            self.J_dVdx_times_g[i, j].Evaluate(env)
+                            for j in range(self.nx)
+                        ]
+                        for i in range(self.nu)
+                    ]
+                )
+                dconstraint_dx[self.nu, :] = np.array(
+                    [self.dVdx[i].Evaluate(env) for i in range(self.nx)]
+                )
+                if self.h is not None:
+                    dconstraint_dx[-self.h_size :, :] = np.array(
+                        [
+                            [self.J_h[i, j].Evaluate(env) for j in range(self.nx)]
+                            for i in range(self.h_size)
+                        ]
+                    )
+                d_constraint = dconstraint_dx @ x_grad
+                return pydrake.autodiffutils.InitializeAutoDiff(
+                    constraint_val, d_constraint
+                )
+            else:
+                return constraint_val
+
+        def cost(x_eval):
+            if x_eval.dtype == object:
+                x_val = pydrake.autodiffutils.ExtractValue(x_eval)
+                x_grad = pydrake.autodiffutils.ExtractGradient(x_eval)
+            else:
+                x_val = x_eval
+            env = {x[i]: x_val[i] for i in range(self.nx)}
+            cost_val = self.cost_poly.Evaluate(env)
+
+            if x_eval.dtype == object:
+                cost_grad = np.array(
+                    [self.J_cost_poly[i].Evaluate(env) for i in range(self.nx)]
+                )
+                d_cost = (cost_grad.reshape((1, -1)) @ x_grad).reshape((-1,))
+                return pydrake.autodiffutils.AutoDiffXd(cost_val, d_cost)
+            else:
+                return cost_val
+
+        self.prog = solvers.MathematicalProgram()
+        self.prog.AddDecisionVariables(x)
+        self.prog.AddCost(cost, x)
+        constraint_lb = np.concatenate(
+            (np.zeros((self.nu,)), np.array([-np.inf]), np.zeros((self.h_size,)))
+        )
+        constraint_ub = np.concatenate(
+            (np.zeros((self.nu,)), np.array([self.rho]), np.zeros((self.h_size,)))
+        )
+
+        self.prog.AddConstraint(constraint, constraint_lb, constraint_ub, x)

compatible_clf_cbf/clf_cbf.py

@@ -266,6 +266,33 @@ def to_lagrangians(
         )
 
 
+@dataclass
+class InnerEllipsoidOptions:
+    """
+    This option is used to encourage the compatible region to cover an inscribed
+    ellipsoid.
+    """
+
+    # A state that should be contained in the inscribed ellipsoid
+    x_inner: np.ndarray
+    # when we search for the ellipsoid, we put a trust region constraint. This
+    # is the squared radius of that trust region.
+    ellipsoid_trust_region: float
+    # We enlarge the inner ellipsoid through a sequence of SDPs. This is the max
+    # number of iterations in that sequence.
+    find_inner_ellipsoid_max_iter: int
+
+    def __init__(
+        self,
+        x_inner: np.ndarray,
+        ellipsoid_trust_region: float = 100.0,
+        find_inner_ellipsoid_max_iter: int = 3,
+    ):
+        self.x_inner = x_inner
+        self.ellipsoid_trust_region = ellipsoid_trust_region
+        self.find_inner_ellipsoid_max_iter = find_inner_ellipsoid_max_iter
+
+
 @dataclass
 class CompatibleStatesOptions:
     """
@@ -837,13 +864,11 @@ def bilinear_alternation(
         cbf_degrees: List[int],
         max_iter: int,
         *,
-        x_inner: np.ndarray,
         solver_id: Optional[solvers.SolverId] = None,
         solver_options: Optional[solvers.SolverOptions] = None,
         lagrangian_coefficient_tol: Optional[float] = None,
-        ellipsoid_trust_region: Optional[float] = 100,
+        inner_ellipsoid_options: Optional[InnerEllipsoidOptions] = None,
         binary_search_scale_options: Optional[BinarySearchOptions] = None,
-        find_inner_ellipsoid_max_iter: int = 3,
         compatible_states_options: Optional[CompatibleStatesOptions] = None,
         backoff_scale: Optional[float] = None,
     ) -> Tuple[Optional[sym.Polynomial], np.ndarray]:
@@ -860,16 +885,18 @@ def bilinear_alternation(
         - Expand the compatible region to cover some candidate states.
 
         Args:
-          x_inner: Each row of x_inner is a state that should be contained in
-            the compatible region.
           max_iter: The maximal number of bilinear alternation iterations.
           lagrangian_coefficient_tol: We remove the coefficients whose absolute
             value is smaller than this tolerance in the Lagrangian polynomials.
             Use None to preserve all coefficients.
-          ellipsoid_trust_region: when we search for the ellipsoid, we put a
-            trust region constraint. This is the squared radius of that trust
-            region.
         """
+
+        # One and only one of inner_ellipsoid_options and compatible_states_options is None.
+        assert (
+            inner_ellipsoid_options is not None and compatible_states_options is None
+        ) or (inner_ellipsoid_options is None and compatible_states_options is not None)
+        if inner_ellipsoid_options is not None:
+            assert binary_search_scale_options is not None
         assert isinstance(binary_search_scale_options, Optional[BinarySearchOptions])
         assert isinstance(compatible_states_options, Optional[CompatibleStatesOptions])
 
@@ -904,7 +931,7 @@ def bilinear_alternation(
             assert compatible_lagrangians is not None
             assert all(unsafe_lagrangians)
 
-            if find_inner_ellipsoid_max_iter > 0:
+            if inner_ellipsoid_options is not None:
                 # We use the heuristics to grow the inner ellipsoid.
                 assert compatible_states_options is None
                 # Search for the inner ellipsoid.
@@ -923,10 +950,11 @@ def bilinear_alternation(
                     cbf,
                     V_contain_ellipsoid_lagrangian_degree,
                     b_contain_ellipsoid_lagrangian_degree,
-                    x_inner,
+                    inner_ellipsoid_options.x_inner,
                     solver_id=solver_id,
-                    max_iter=find_inner_ellipsoid_max_iter,
-                    trust_region=ellipsoid_trust_region,
+                    solver_options=solver_options,
+                    max_iter=inner_ellipsoid_options.find_inner_ellipsoid_max_iter,
+                    trust_region=inner_ellipsoid_options.ellipsoid_trust_region,
                 )
 
                 assert binary_search_scale_options is not None
@@ -1144,6 +1172,7 @@ def _add_compatibility(
         xi, lambda_mat = self._calc_xi_Lambda(
             V=V, b=b, kappa_V=kappa_V, kappa_b=kappa_b
         )
+        # This is just polynomial 1.
         poly_one = sym.Polynomial(sym.Monomial())
 
         poly = -poly_one
@@ -1155,7 +1184,6 @@ def _add_compatibility(
         poly -= lagrangians.lambda_y.dot(lambda_y)
 
         # Compute s₁(x, y)(ξ(x)ᵀy+1)
-        # This is just polynomial 1.
         if self.use_y_squared:
             xi_y = xi.dot(self.y_squared_poly) + poly_one
         else:
@@ -1321,6 +1349,7 @@ def _find_max_inner_ellipsoid(
         max_iter: int = 10,
         convergence_tol: float = 1e-3,
         solver_id: Optional[solvers.SolverId] = None,
+        solver_options: Optional[solvers.SolverOptions] = None,
         trust_region: Optional[float] = None,
     ) -> Tuple[np.ndarray, np.ndarray, float]:
         """
@@ -1401,6 +1430,7 @@ def _find_max_inner_ellipsoid(
             max_iter,
             convergence_tol,
             solver_id,
+            solver_options,
             trust_region,
         )
         return (S_sol, b_sol, c_sol)

compatible_clf_cbf/ellipsoid_utils.py

@@ -120,6 +120,7 @@ def maximize_inner_ellipsoid_sequentially(
     max_iter: int = 10,
     convergence_tol: float = 1e-3,
     solver_id: Optional[solvers.SolverId] = None,
+    solver_options: Optional[solvers.SolverOptions] = None,
     trust_region: Optional[float] = None,
 ) -> Tuple[np.ndarray, np.ndarray, float]:
     """
@@ -173,8 +174,10 @@ def volume(S: np.ndarray, b: np.ndarray, c: float) -> float:
             trust_region_constraint = _add_ellipsoid_trust_region(
                 prog, S, b, c, S_bar, b_bar, c_bar, trust_region
             )
-        result = utils.solve_with_id(prog, solver_id, solver_options=None)
-        assert result.is_success()
+        result = utils.solve_with_id(prog, solver_id, solver_options)
+        assert (
+            result.is_success()
+        ), f"Fails in iter={i} with {result.get_solution_result()}"
         S_result = result.GetSolution(S)
         b_result = result.GetSolution(b)
         c_result = result.GetSolution(c)

examples/nonlinear_toy/synthesize_demo.py

@@ -67,6 +67,7 @@ def main(with_u_bound: bool):
     solver_options = solvers.SolverOptions()
     solver_options.SetOption(solvers.CommonSolverOption.kPrintToConsole, 0)
     solver_options.SetOption(solvers.ClarabelSolver.id(), "max_iter", 10000)
+    inner_ellipsoid_options = None
 
     V, b = compatible.bilinear_alternation(
         V_init,
@@ -80,9 +81,8 @@ def main(with_u_bound: bool):
         clf_degree,
         cbf_degrees,
         max_iter,
-        x_inner=x_equilibrium,
+        inner_ellipsoid_options=None,
         binary_search_scale_options=None,
-        find_inner_ellipsoid_max_iter=0,
         compatible_states_options=compatible_states_options,
         solver_options=solver_options,
         backoff_scale=0.02,

examples/nonlinear_toy/wo_input_limits_trigpoly_demo.py

@@ -64,12 +64,14 @@ def main(grow_heuristics: GrowHeuristics):
 
     if grow_heuristics == GrowHeuristics.kInnerEllipsoid:
         binary_search_scale_options = utils.BinarySearchOptions(min=1, max=2, tol=0.1)
-        find_inner_ellipsoid_max_iter = 1
+        inner_ellipsoid_options = clf_cbf.InnerEllipsoidOptions(
+            x_inner=x_equilibrium, find_inner_ellipsoid_max_iter=1
+        )
         compatible_states_options = None
         solver_options = None
     elif grow_heuristics == GrowHeuristics.kCompatibleStates:
         binary_search_scale_options = None
-        find_inner_ellipsoid_max_iter = 0
+        inner_ellipsoid_options = None
         compatible_states_options = clf_cbf.CompatibleStatesOptions(
             candidate_compatible_states=np.array(
                 [
@@ -97,9 +99,8 @@ def main(grow_heuristics: GrowHeuristics):
         clf_degree,
         cbf_degrees,
         max_iter,
-        x_inner=x_equilibrium,
+        inner_ellipsoid_options=inner_ellipsoid_options,
         binary_search_scale_options=binary_search_scale_options,
-        find_inner_ellipsoid_max_iter=find_inner_ellipsoid_max_iter,
         compatible_states_options=compatible_states_options,
         solver_options=solver_options,
     )