Minimize the volume of an ellipsoid.

This will be used in computing the outer-ellipsoid of the ROA/safe set.

compatible_clf_cbf/clf_cbf.py

@@ -346,7 +346,10 @@ def _add_compatibility(
         return poly
 
     def _add_barrier_safe_constraint(
-        self, prog: solvers.MathematicalProgram, b: np.ndarray, phi: List[np.ndarray]
+        self,
+        prog: solvers.MathematicalProgram,
+        b: np.ndarray,
+        lagrangians: List[np.ndarray],
     ) -> np.ndarray:
         """
         Adds the constraint that the 0-superlevel set of the barrier function
@@ -365,17 +368,21 @@ def _add_barrier_safe_constraint(
         Args:
           b: An array of polynomials, b[i] is the barrier function for the i'th
             unsafe region.
-          phi: A array of polynomials, ϕ(x) in the documentation above. phi[i]
-            are the Lagrangian multipliers for the i'th unsafe region.
+          lagrangians: A array of polynomials, ϕ(x) in the documentation above.
+          lagrangians[i] are the Lagrangian multipliers for the i'th unsafe region.
         Returns:
           poly: poly[i] is the polynomial -1-ϕᵢ,₀(x)bᵢ(x) + ∑ⱼϕᵢ,ⱼ(x)pⱼ(x)
         """
         num_unsafe_regions = len(self.unsafe_regions)
         assert b.shape == (num_unsafe_regions,)
-        assert len(phi) == num_unsafe_regions
+        assert len(lagrangians) == num_unsafe_regions
         poly = np.empty((num_unsafe_regions,), dtype=object)
         for i in range(num_unsafe_regions):
-            assert phi[i].shape == (1 + len(self.unsafe_regions[i]),)
-            poly[i] = -1 - phi[i][0] * b[i] + phi[i][1:].dot(self.unsafe_regions[i])
+            assert lagrangians[i].shape == (1 + len(self.unsafe_regions[i]),)
+            poly[i] = (
+                -1
+                - lagrangians[i][0] * b[i]
+                + lagrangians[i][1:].dot(self.unsafe_regions[i])
+            )
             prog.AddSosConstraint(poly[i])
         return poly

compatible_clf_cbf/utils.py

@@ -1,3 +1,4 @@
+import dataclasses
 from typing import Optional, Union
 import numpy as np
 
@@ -54,3 +55,97 @@ def get_polynomial_result(
                 ]
             ).reshape(p.shape)
         return p_result
+
+
+@dataclasses.dataclass
+class LogDetLowerRet:
+    Z: np.ndarray
+    t: np.ndarray
+
+
+def add_log_det_lower(
+    prog: solvers.MathematicalProgram, X: np.ndarray, lower: float
+) -> LogDetLowerRet:
+    """
+    Impose the constraint that log(det(X)) >= lower where X is a psd matrix.
+
+    This can be formulated through semidefinite and exponential cone
+    constraints. We introduce slack variable t, and a lower-diagonal matrix Z,
+    with the constraint
+
+        ⌈X         Z⌉ is positive semidifinite.
+        ⌊Zᵀ  diag(Z)⌋
+
+        log(Z(i, i)) >= t(i)
+        ∑ᵢt(i) >= lower
+
+    TODO(hongkai.dai): move this function into Drake.
+    """
+    if lower < 0:
+        raise Warning(
+            "add_log_det_lower(): lower is negative. You do not need this constraint."
+        )
+    X_rows = X.shape[0]
+    assert X.shape == (X_rows, X_rows)
+    Z_lower = prog.NewContinuousVariables(int(X_rows * (X_rows + 1) / 2))
+    Z = np.empty((X_rows, X_rows), dtype=object)
+    Z_lower_count = 0
+    for i in range(X_rows):
+        for j in range(i + 1):
+            Z[i, j] = Z_lower[Z_lower_count]
+            Z_lower_count += 1
+        for j in range(i + 1, X_rows):
+            Z[i, j] = 0
+    t = prog.NewContinuousVariables(X_rows)
+
+    psd_mat = np.zeros((X_rows * 2, X_rows * 2), dtype=object)
+    psd_mat[:X_rows, :X_rows] = X
+    psd_mat[:X_rows, X_rows:] = Z
+    psd_mat[X_rows:, :X_rows] = Z.T
+    psd_mat[X_rows:, X_rows:] = np.diagonal(Z)
+    prog.AddPositiveSemidefiniteConstraint(psd_mat)
+
+    for i in range(X_rows):
+        prog.AddExponentialConeConstraint(np.array([Z[i, i], 1, t[i]]))
+    prog.AddLinearConstraint(np.ones((1, X_rows)), lower, np.inf, t)
+
+    return LogDetLowerRet(Z=Z, t=t)
+
+
+def add_minimize_ellipsoid_volume(
+    prog: solvers.MathematicalProgram, S: np.ndarray, b: np.ndarray, c: sym.Variable
+) -> sym.Variable:
+    """
+    Minimize the volume of the ellipsoid {x | xᵀSx + bᵀx + c ≤ 0}
+    where S, b, and c are decision variables.
+
+    See doc/minimize_ellipsoid_volume.md for the details (you will need to
+    enable MathJax in your markdown viewer).
+
+    We maximize a lower bound of the volume through convex optimization
+    min r
+    s.t ⌈c+r  bᵀ/2⌉ is psd
+        ⌊b/2     S⌋
+
+        log det(S) >= 0
+
+    Args:
+      S: a symmetric matrix of decision variables. S must have been registered
+      in `prog` already. It is the user's responsibility to impose "S is psd".
+      b: a vector of decision variables. b must have been registered in `prog` already.
+      c: a symbolic Variable. c must have been registered in `prog` already.
+    Returns:
+      r: The slack decision variable.
+    """
+    x_dim = S.shape[0]
+    assert S.shape == (x_dim, x_dim)
+    r = prog.NewContinuousVariables(1, "r")[0]
+    prog.AddLinearCost(r)
+    psd_mat = np.empty((x_dim + 1, x_dim + 1), dtype=object)
+    psd_mat[0, 0] = c + r
+    psd_mat[0, 1:] = b.T / 2
+    psd_mat[1:, 0] = b / 2
+    psd_mat[1:, 1:] = S
+    prog.AddPositiveSemidefiniteConstraint(psd_mat)
+    add_log_det_lower(prog, S, lower=0.0)
+    return r

doc/minimize_ellipsoid_volume.md

@@ -0,0 +1,111 @@
+# Minimizing ellipsoid volume
+
+Say we have an algebraic set $\mathcal{S} = \{x \in\mathbb{R}^n| g(x) < 0\}$, we want to find the smallest ellipsoid containing this set.
+
+## Formulation 1
+
+Let’s consider the ellipsoid parameterized as $\mathcal{E}=\{x | x^TSx+b^Tx+c\leq 0\}$. The constraint that the ellipsoid $\mathcal{E}=\{x | x^TSx+b^Tx+c\le 0\} \supset \mathcal{S}=\{x | g(x)< 0\}$ can be imposed through the p-satz
+
+$$
+\begin{align}
+-1-\phi_0(x)(x^TSx+b^Tx+c) +\phi_1(x)g(x) \text{ is sos}\\
+\phi_0(x), \phi_1(x) \text{ are sos}
+\end{align}
+$$
+
+The volume of this ellipsoid is proportional to
+
+$$
+vol(\mathcal{E})\propto \sqrt{\frac{b^TS^{-1}b/4-c}{\text{det}(S)^{1/n}}}
+$$
+
+Minimizing this volume is equivalent to minimizing
+
+$$
+\begin{align}
+\frac{b^TS^{-1}b/4-c}{\text{det}(S)^{1/n}}
+\end{align}
+$$
+
+How to minimize (3) through convex optimization? Here we try several attempts
+
+### Attempt 1
+
+ Taking the logarithm of (3) we get
+
+$$
+\begin{align}
+\log(b^TS^{-1}b/4-c) - \frac{1}{n}\log\text{det}(S)
+\end{align}
+$$
+
+First note that $\log\text{det}(S)$ is a concave function, hence we can minimize $-\frac{1}{n}\log\text{det}(S)$ through convex optimization.
+
+Second we notice that we can minimize $b^TS^{-1}b/4-c$ through convex optimization. By using Schur complement, we have $b^TS^{-1}b/4-c\le r$ if and only if the following matrix is psd
+
+$$
+\begin{align}
+\begin{bmatrix} c+r & b^T/2\\b/2 & S\end{bmatrix} \succeq 0
+\end{align}
+$$
+
+Hence we can minimize $r$ subject to the convex constraint (5).
+
+Unfortunately we cannot minimize $\log r$ through convex optimization (it is a concave function of $r$). Hence this attempt isn’t successful.
+
+### Attempt 2
+
+Let’s try again. We consider the following optimization program
+
+$$
+\begin{align}
+\min_{S, b, c} b^TS^{-1}b/4-c\\
+\text{s.t } \text{det}(S) \ge 1
+\end{align}
+$$
+
+Note that the constraint (7) is equivalent to
+
+$$
+\begin{align}
+\log \text{det}(S) \ge 0
+\end{align}
+$$
+
+which is a convex constraint. Hence we can solve the objective (6) subject to the constraint (8) through the convex optimization problem
+
+$$
+\begin{align}
+\min_{S, b, c, r} &\;r\\
+\text{s.t }& \begin{bmatrix}c+r & b^T/2\\b/2 & S\end{bmatrix}\succeq 0\\
+&\log\text{det}(S) \ge 0
+\end{align}
+$$
+
+Is this optimization problem (9)-(11) (which is equivalent to (6)(7)) same as minimizing the original objective (3)? First we notice that the optimal cost in (6)(7) is an upper bound of the minimization over (3), because we constrain the denominator $\text{det}(S)\ge 1$. On the other hand, let’s denote an optimal solution of minimizing (3) as $(S^*, b^*, c^*)$, we can see that $(S, b, c) = (S^*, b^*, c^*)/(\frac{1}{n}\text{det}(S^*))$ satisfies (7) and achieves the same cost in (6) as $(S^*, b^*, c^*)$ in (3). Hence we know that the optimization problem (6)(7) is equivalent to minimizing (3).
+
+## Formulation 2
+
+We consider an alternative formulation on the ellipsoid $\mathcal{E} = \{x | \Vert Ax+b\Vert_2\le 1\}$ with $A\succeq 0$. Minimizing the volume of this ellipsoid is equivalent to maximizing $\log\text{det}(A)$. 
+
+To imposing that $\mathcal{E}\supset\mathcal{S}$, we first consider the following relationship from Schur complement
+
+$$
+\begin{align}
+\Vert Ax+b\Vert_2\le 1 \Leftrightarrow \begin{bmatrix}1 & (Ax+b)^T\\Ax+b & I\end{bmatrix}\succeq 0
+\end{align}
+$$
+
+The right hand side of (12) is a sos matrix constraint, which is convex in $A, b$.
+
+The condition $\mathcal{E}\supset\mathcal{S}$ can be imposed as
+
+$$
+\begin{align}
+-\Sigma_0(x)g(x) = \lambda_1(x)\begin{bmatrix}1 & (Ax+b)^T\\Ax+b& I\end{bmatrix} + \Sigma_1(x)\\
+\lambda_1(x)\text{ is sos}\\
+\Sigma_0(x)\succeq 0, \Sigma_1(x)\succeq 0
+\end{align}
+$$
+
+where we need to introduce new sos-matrices $\Sigma_0(x), \Sigma_1(x)$. Note that the sos-matrix constraints (13)-(15) are significantly more complicated than the sos constraint in (1)-(2).

tests/test_utils.py

@@ -4,10 +4,95 @@
 import pytest  # noqa
 
 import pydrake.symbolic as sym
+import pydrake.solvers as solvers
 
 
 def test_check_array_of_polynomials():
     x = sym.MakeVectorContinuousVariable(rows=3, name="x")
     x_set = sym.Variables(x)
     p = np.array([sym.Polynomial(x[0] * x[0]), sym.Polynomial(x[1] + 2)])
     mut.check_array_of_polynomials(p, x_set)
+
+
+def check_psd(X: np.ndarray, tol: float):
+    assert np.all((np.linalg.eig(X)[0] >= -tol))
+
+
+def test_add_log_det_lower():
+    def tester(lower):
+        prog = solvers.MathematicalProgram()
+        X = prog.NewSymmetricContinuousVariables(3, "X")
+        ret = mut.add_log_det_lower(prog, X, lower)
+        result = solvers.Solve(prog)
+        assert result.is_success()
+        X_sol = result.GetSolution(X)
+        check_psd(X_sol, tol=1e-6)
+        assert np.log(np.linalg.det(X_sol)) >= lower - 1e-6
+        t_sol = result.GetSolution(ret.t)
+        assert np.sum(t_sol) >= lower - 1E-6
+
+    tester(2.0)
+    tester(3.0)
+
+
+def check_inscribed_ellipsoid(S_inner: np.ndarray, b_inner: np.ndarray, c_inner: float):
+    """
+    Find the largest ellipsoid {x | xᵀSx + bᵀx + c ≤ 0} contained within the
+    inner ellipsoid {x | xᵀS_inner*x + b_innerᵀx + c_outer ≤ 0}. Obviously the
+    largest ellipsoid should be the inner ellipsoid itself.
+    """
+    assert np.all((np.linalg.eig(S_inner)[0] > 0))
+    x_dim = S_inner.shape[0]
+    prog = solvers.MathematicalProgram()
+    S = prog.NewSymmetricContinuousVariables(x_dim, "S")
+    prog.AddPositiveSemidefiniteConstraint(S)
+    b = prog.NewContinuousVariables(x_dim, "b")
+    c = prog.NewContinuousVariables(1, "c")[0]
+
+    r = mut.add_minimize_ellipsoid_volume(prog, S, b, c)
+
+    # According to s-lemma, xᵀS_inner*x + b_innerᵀx + c_inner <= 0 implies
+    # xᵀSx + bᵀx + c <= 0 iff there exists λ ≥ 0, such that
+    # -(xᵀSx + bᵀx + c) + λ*(xᵀS_inner*x + b_innerᵀx + c_inner) >= 0 for all x.
+    # Namely
+    # ⌈ λS_inner - S   (λb_inner-b)/2⌉ is psd.
+    # ⌊(λb_inner-b)ᵀ/2     λc_inner-c⌋
+    lambda_var = prog.NewContinuousVariables(1, "lambda")[0]
+    prog.AddBoundingBoxConstraint(0, np.inf, lambda_var)
+    psd_mat = np.empty((x_dim + 1, x_dim + 1), dtype=object)
+    psd_mat[:x_dim, :x_dim] = lambda_var * S_inner - S
+    psd_mat[:x_dim, -1] = (lambda_var * b_inner - b) / 2
+    psd_mat[-1, :x_dim] = (lambda_var * b_inner - b).T / 2
+    psd_mat[-1, -1] = lambda_var * c_inner - c
+    prog.AddPositiveSemidefiniteConstraint(psd_mat)
+    result = solvers.Solve(prog)
+    assert result.is_success
+    S_sol = result.GetSolution(S)
+    b_sol = result.GetSolution(b)
+    c_sol = result.GetSolution(c)
+    r_sol = result.GetSolution(r)
+    np.testing.assert_allclose(
+        r_sol, b_sol.dot(np.linalg.solve(S_sol, b_sol)) / 4 - c_sol
+    )
+
+    mat = np.empty((x_dim + 1, x_dim + 1))
+    mat[0, 0] = c_sol + r_sol
+    mat[0, 1:] = b_sol.T / 2
+    mat[1:, 0] = b_sol / 2
+    mat[1:, 1:] = S_sol
+    check_psd(mat, tol=1e-6)
+
+    # Make sure the (S, b, c) is a scaled version of
+    # (S_inner, b_inner, c_inner), namely they correspond to the same
+    # ellipsoid.
+    factor = c_sol / c_inner
+    np.testing.assert_allclose(S_sol, S_inner * factor, atol=1e-7)
+    np.testing.assert_allclose(b_sol, b_inner * factor, atol=1e-7)
+
+
+def test_add_maximize_ellipsoid_volume():
+    check_inscribed_ellipsoid(S_inner=np.eye(2), b_inner=np.zeros(2), c_inner=-1)
+    check_inscribed_ellipsoid(S_inner=np.eye(2), b_inner=np.array([1, 2]), c_inner=-1)
+    check_inscribed_ellipsoid(
+        S_inner=np.array([[1, 2], [2, 9]]), b_inner=np.array([1, 2]), c_inner=-1
+    )