Find the largest ellipsoid within the ROA and safe region. (#23)

compatible_clf_cbf/clf_cbf.py

@@ -7,7 +7,12 @@
 import pydrake.symbolic as sym
 import pydrake.solvers as solvers
 
-from compatible_clf_cbf.utils import check_array_of_polynomials, get_polynomial_result
+from compatible_clf_cbf.utils import (
+    ContainmentLagrangianDegree,
+    check_array_of_polynomials,
+    get_polynomial_result,
+)
+import compatible_clf_cbf.ellipsoid_utils as ellipsoid_utils
 
 
 @dataclass
@@ -687,3 +692,61 @@ def _construct_search_clf_cbf_program(
         )
 
         return (prog, V, b, rho)
+
+    def _find_max_inner_ellipsoid(
+        self,
+        V: Optional[sym.Polynomial],
+        b: np.ndarray,
+        rho: Optional[float],
+        V_contain_lagrangian_degree: Optional[ContainmentLagrangianDegree],
+        b_contain_lagrangian_degree: List[ContainmentLagrangianDegree],
+        S_ellipsoid_init: np.ndarray,
+        b_ellipsoid_init: np.ndarray,
+        c_ellipsoid_init: float,
+        max_iter: int = 10,
+        convergence_tol: float = 1e-3,
+        solver_id: Optional[solvers.SolverId] = None,
+        trust_region: Optional[float] = None,
+    ) -> Tuple[np.ndarray, np.ndarray, float]:
+        prog = solvers.MathematicalProgram()
+        dim = self.x_set.size()
+
+        S_ellipsoid = prog.NewSymmetricContinuousVariables(dim, "S")
+        prog.AddPositiveSemidefiniteConstraint(S_ellipsoid)
+        b_ellipsoid = prog.NewContinuousVariables(dim, "b")
+        c_ellipsoid = prog.NewContinuousVariables(1, "c")[0]
+
+        ellipsoid = sym.Polynomial(
+            self.x.dot(S_ellipsoid @ self.x) + b_ellipsoid.dot(self.x) + c_ellipsoid,
+            self.x_set,
+        )
+        prog.AddIndeterminates(self.x_set)
+        if V_contain_lagrangian_degree is not None:
+            V_contain_lagrangian = V_contain_lagrangian_degree.construct_lagrangian(
+                prog, self.x_set
+            )
+            assert V is not None
+            assert rho is not None
+            V_contain_lagrangian.add_constraint(prog, ellipsoid, V - rho)
+        b_contain_lagrangians = [
+            degree.construct_lagrangian(prog, self.x_set)
+            for degree in b_contain_lagrangian_degree
+        ]
+
+        for i in range(len(b_contain_lagrangians)):
+            b_contain_lagrangians[i].add_constraint(prog, ellipsoid, -b[i])
+
+        S_sol, b_sol, c_sol = ellipsoid_utils.maximize_inner_ellipsoid_sequentially(
+            prog,
+            S_ellipsoid,
+            b_ellipsoid,
+            c_ellipsoid,
+            S_ellipsoid_init,
+            b_ellipsoid_init,
+            c_ellipsoid_init,
+            max_iter,
+            convergence_tol,
+            solver_id,
+            trust_region,
+        )
+        return (S_sol, b_sol, c_sol)

compatible_clf_cbf/ellipsoid_utils.py

@@ -2,7 +2,7 @@
 The utility function for manipulating ellipsoids
 """
 
-from typing import Tuple
+from typing import Optional, Tuple
 
 import numpy as np
 
@@ -71,6 +71,42 @@ def add_max_volume_linear_cost(
     return cost
 
 
+def _add_ellipsoid_trust_region(
+    prog: solvers.MathematicalProgram,
+    S: np.ndarray,
+    b: np.ndarray,
+    c: sym.Variable,
+    S_bar: np.ndarray,
+    b_bar: np.ndarray,
+    c_bar: float,
+    trust_region,
+):
+    """
+    Add the constraint (S - S_bar)^2 + (b-b_bar)^2 + (c-c_bar)^2 <= trust_region.
+    """
+    dim = S.shape[0]
+    linear_coeff = np.concatenate(
+        (
+            utils.to_lower_triangular_columns(S_bar).reshape((-1,)),
+            b_bar.reshape((-1,)),
+            np.array([c_bar]),
+        )
+    )
+    constraint = prog.AddQuadraticAsRotatedLorentzConeConstraint(
+        np.eye(int((dim + 1) * dim / 2) + dim + 1),
+        -linear_coeff,
+        linear_coeff.dot(linear_coeff) / 2 - trust_region,
+        np.concatenate(
+            (
+                utils.to_lower_triangular_columns(S).reshape((-1,)),
+                b.reshape((-1,)),
+                np.array([c]),
+            )
+        ),
+    )
+    return constraint
+
+
 def maximize_inner_ellipsoid_sequentially(
     prog: solvers.MathematicalProgram,
     S: np.ndarray,
@@ -81,6 +117,8 @@ def maximize_inner_ellipsoid_sequentially(
     c_init: float,
     max_iter: int = 10,
     convergence_tol: float = 1e-3,
+    solver_id: Optional[solvers.SolverId] = None,
+    trust_region: Optional[float] = None,
 ) -> Tuple[np.ndarray, np.ndarray, float]:
     """
     Maximize the inner ellipsoid ℰ={x | xᵀSx+bᵀx+c≤0} through a sequence of
@@ -111,6 +149,7 @@ def volume(S: np.ndarray, b: np.ndarray, c: float) -> float:
     cost = None
     volume_prev = volume(S_init, b_init, c_init)
     assert max_iter >= 1
+    trust_region_constraint = None
     for i in range(max_iter):
         if cost is not None:
             prog.RemoveCost(cost)
@@ -124,12 +163,25 @@ def volume(S: np.ndarray, b: np.ndarray, c: float) -> float:
             ellipsoid_center.dot(S @ ellipsoid_center) + np.dot(b, ellipsoid_center) + c
             <= 0
         )
-        result = solvers.Solve(prog)
+
+        if trust_region_constraint is not None:
+            prog.RemoveConstraint(trust_region_constraint)
+        if trust_region is not None:
+            assert trust_region >= 0
+            trust_region_constraint = _add_ellipsoid_trust_region(
+                prog, S, b, c, S_bar, b_bar, c_bar, trust_region
+            )
+        if solver_id is None:
+            result = solvers.Solve(prog)
+        else:
+            solver = solvers.MakeSolver(solver_id)
+            result = solver.Solve(prog, None, None)
         assert result.is_success()
         S_result = result.GetSolution(S)
         b_result = result.GetSolution(b)
         c_result = result.GetSolution(c)
         volume_result = volume(S_result, b_result, c_result)
+        print(f"{volume_result}")
         if volume_result - volume_prev <= convergence_tol:
             break
         else:
@@ -177,3 +229,32 @@ def add_minimize_ellipsoid_volume(
     prog.AddPositiveSemidefiniteConstraint(psd_mat)
     utils.add_log_det_lower(prog, S, lower=0.0)
     return r
+
+
+def is_ellipsoid_contained(
+    S_inner: np.ndarray,
+    b_inner: np.ndarray,
+    c_inner: float,
+    S_outer: np.ndarray,
+    b_outer: np.ndarray,
+    c_outer: float,
+) -> bool:
+    """
+    Check if {x | x' * S_inner * x + b_inner' * x + c_inner <= 0} is contained
+    in {x | x' * S_outer * x + b_outer' * x + c_outer <= 0}.
+    """
+
+    prog = solvers.MathematicalProgram()
+    gamma = prog.NewContinuousVariables(1, "gamma")[0]
+
+    # -x' * S_outer * x - b_outer' * x - c_outer
+    #  + gamma * (x' * S_inner * x + b_inner' * x + c_inner) is sos.
+    dim = S_inner.shape[0]
+    mat = np.empty((dim + 1, dim + 1), dtype=object)
+    mat[:dim, :dim] = gamma * S_inner - S_outer
+    mat[:dim, -1] = (gamma * b_inner - b_outer) / 2
+    mat[-1, :dim] = mat[:dim, -1]
+    mat[-1, -1] = gamma * c_inner - c_outer
+    prog.AddPositiveSemidefiniteConstraint(mat)
+    result = solvers.Solve(prog)
+    return result.is_success()

compatible_clf_cbf/utils.py

@@ -1,5 +1,5 @@
 import dataclasses
-from typing import Optional, Union
+from typing import Optional, Tuple, Union
 import numpy as np
 
 import pydrake.symbolic as sym
@@ -57,11 +57,20 @@ def get_polynomial_result(
         return p_result
 
 
-def is_sos(p: sym.Polynomial) -> bool:
+def is_sos(
+    poly: sym.Polynomial,
+    solver_id: Optional[solvers.SolverId] = None,
+    solver_options: Optional[solvers.SolverOptions] = None,
+):
     prog = solvers.MathematicalProgram()
-    prog.AddIndeterminates(p.indeterminates())
-    prog.AddSosConstraint(p)
-    result = solvers.Solve(prog)
+    prog.AddIndeterminates(poly.indeterminates())
+    assert poly.decision_variables().empty()
+    prog.AddSosConstraint(poly)
+    if solver_id is None:
+        result = solvers.Solve(prog, None, solver_options)
+    else:
+        solver = solvers.MakeSolver(solver_id)
+        result = solver.Solve(prog, None, solver_options)
     return result.is_success()
 
 
@@ -118,3 +127,72 @@ def add_log_det_lower(
     prog.AddLinearConstraint(np.ones((1, X_rows)), lower, np.inf, t)
 
     return LogDetLowerRet(Z=Z, t=t)
+
+
+@dataclasses.dataclass
+class ContainmentLagrangian:
+    """
+    To certify that an algebraic set { x | f(x) <= 0} is contained in another
+    algebraic set {x | g(x) < 0}, we impose the condition
+    -1 - ϕ₁(x))g(x) + ϕ₂(x)f(x) is sos
+    ϕ₁(x) is sos, ϕ₂(x) is sos
+    """
+
+    # ϕ₂(x) in the documentation above.
+    inner: sym.Polynomial
+    # ϕ₁(x) in the documentation above.
+    outer: sym.Polynomial
+
+    def add_constraint(
+        self, prog, inner_poly: sym.Polynomial, outer_poly: sym.Polynomial
+    ) -> Tuple[sym.Polynomial, np.ndarray]:
+        return prog.AddSosConstraint(
+            -1 - self.outer * outer_poly + self.inner * inner_poly
+        )
+
+
+@dataclasses.dataclass
+class ContainmentLagrangianDegree:
+    """
+    The degree of the polynomials in ContainmentLagrangian
+    If degree < 0, then the Lagrangian polynomial is 1.
+    """
+
+    inner: int = -1
+    outer: int = -1
+
+    def construct_lagrangian(
+        self, prog: solvers.MathematicalProgram, x: sym.Variables
+    ) -> ContainmentLagrangian:
+        if self.inner < 0:
+            inner_lagrangian = sym.Polynomial(1)
+        elif self.inner == 0:
+            inner_lagrangian_var = prog.NewContinuousVariables(1)[0]
+            prog.AddBoundingBoxConstraint(0, np.inf, inner_lagrangian_var)
+            inner_lagrangian = sym.Polynomial(
+                {sym.Monomial(): sym.Expression(inner_lagrangian_var)}
+            )
+        else:
+            inner_lagrangian, _ = prog.NewSosPolynomial(x, self.inner)
+        if self.outer < 0:
+            outer_lagrangian = sym.Polynomial(1)
+        elif self.outer == 0:
+            outer_lagrangian_var = prog.NewContinuousVariables(1)[0]
+            prog.AddBoundingBoxConstraint(0, np.inf, outer_lagrangian_var)
+            outer_lagrangian = sym.Polynomial(
+                {sym.Monomial(): sym.Expression(outer_lagrangian_var)}
+            )
+        else:
+            outer_lagrangian, _ = prog.NewSosPolynomial(x, self.outer)
+        return ContainmentLagrangian(inner=inner_lagrangian, outer=outer_lagrangian)
+
+
+def to_lower_triangular_columns(mat: np.ndarray) -> np.ndarray:
+    assert mat.shape[0] == mat.shape[1]
+    dim = mat.shape[0]
+    ret = np.empty(int((dim + 1) * dim / 2), dtype=mat.dtype)
+    count = 0
+    for col in range(dim):
+        ret[count: count + dim - col] = mat[col:, col]
+        count += dim - col
+    return ret

tests/test_clf_cbf.py

@@ -8,8 +8,8 @@
 import pydrake.solvers as solvers
 import pydrake.symbolic as sym
 
+import compatible_clf_cbf.ellipsoid_utils as ellipsoid_utils
 import compatible_clf_cbf.utils as utils
-import tests.test_utils as test_utils
 
 
 class TestCompatibleLagrangianDegrees(object):
@@ -365,6 +365,50 @@ def test_certify_cbf_unsafe_region(self):
         for i in range(dut.unsafe_regions[0].size):
             assert utils.is_sos(lagrangians.unsafe_region[i])
 
+    def test_find_max_inner_ellipsoid(self):
+        dut = mut.CompatibleClfCbf(
+            f=self.f,
+            g=self.g,
+            x=self.x,
+            unsafe_regions=self.unsafe_regions,
+            Au=None,
+            bu=None,
+            with_clf=True,
+            use_y_squared=True,
+        )
+        S_ellipsoid = np.diag(np.array([1, 2.0, 3.0]))
+        b_ellipsoid = np.array([0.5, 2, 1])
+        c_ellipsoid = b_ellipsoid.dot(np.linalg.solve(S_ellipsoid, b_ellipsoid)) / 4
+        V = sym.Polynomial(
+            self.x.dot(S_ellipsoid @ self.x) + b_ellipsoid.dot(self.x) + c_ellipsoid
+        )
+        b = np.array([sym.Polynomial(1 - self.x.dot(self.x))])
+        rho = 2
+        S_sol, b_sol, c_sol = dut._find_max_inner_ellipsoid(
+            V,
+            b,
+            rho,
+            V_contain_lagrangian_degree=utils.ContainmentLagrangianDegree(
+                inner=-1, outer=0
+            ),
+            b_contain_lagrangian_degree=[
+                utils.ContainmentLagrangianDegree(inner=-1, outer=0)
+            ],
+            S_ellipsoid_init=np.eye(3),
+            b_ellipsoid_init=np.zeros(3),
+            c_ellipsoid_init=-0.5,
+            max_iter=10,
+            convergence_tol=1e-4,
+            trust_region=100,
+        )
+        # Make sure the ellipsoid is within V<= rho and b >= 0
+        assert ellipsoid_utils.is_ellipsoid_contained(
+            S_sol, b_sol, c_sol, S_ellipsoid, b_ellipsoid, c_ellipsoid - rho
+        )
+        assert ellipsoid_utils.is_ellipsoid_contained(
+            S_sol, b_sol, c_sol, np.eye(3), np.zeros(3), -1
+        )
+
 
 class TestClfCbfToy:
     """
@@ -481,7 +525,7 @@ def test_search_clf_cbf(self):
         env = {self.x[i]: 0 for i in range(self.nx)}
         assert V_result.Evaluate(env) == 0
         assert sym.Monomial() not in V.monomial_to_coefficient_map()
-        assert test_utils.is_sos(V_result, solvers.ClarabelSolver.id())
+        assert utils.is_sos(V_result, solvers.ClarabelSolver.id())
         assert V_result.TotalDegree() == 2
         rho_result = result.GetSolution(rho)
         assert rho_result >= 0