Maximize the inner ellipsoid through sequential convex optimization.

compatible_clf_cbf/ellipsoid_utils.py

@@ -2,6 +2,8 @@
 The utility function for manipulating ellipsoids
 """
 
+from typing import Tuple
+
 import numpy as np
 
 import pydrake.solvers as solvers
@@ -69,6 +71,75 @@ def add_max_volume_linear_cost(
     return cost
 
 
+def maximize_inner_ellipsoid_sequentially(
+    prog: solvers.MathematicalProgram,
+    S: np.ndarray,
+    b: np.ndarray,
+    c: sym.Variable,
+    S_init: np.ndarray,
+    b_init: np.ndarray,
+    c_init: float,
+    max_iter: int = 10,
+    convergence_tol: float = 1e-3,
+) -> Tuple[np.ndarray, np.ndarray, float]:
+    """
+    Maximize the inner ellipsoid ℰ={x | xᵀSx+bᵀx+c≤0} through a sequence of
+    convex programs.
+
+    Args:
+      prog: The optimization program that already contains all the constraints
+        that the ellipsoid is inside the set.
+      S: A symmetric matrix of decision variables. S must have been registered
+        in `prog` already.
+      b: A vector of decision variables. b must have been registered in `prog`
+        already.
+      c: A decision variable. c must have been registered in `prog` already.
+      S_init: A symmetric matrix of floats, the initial guess of S.
+      b_init: A vector of floats, the initial guess of b.
+      c_init: A float, the initial guess of c.
+    """
+    S_bar = S_init
+    b_bar = b_init
+    c_bar = c_init
+
+    def volume(S: np.ndarray, b: np.ndarray, c: float) -> float:
+        n = S.shape[0]
+        return (b.dot(np.linalg.solve(S, b)) / 4 - c) ** (n / 2) / np.sqrt(
+            np.linalg.det(S)
+        )
+
+    cost = None
+    volume_prev = volume(S_init, b_init, c_init)
+    assert max_iter >= 1
+    for i in range(max_iter):
+        if cost is not None:
+            prog.RemoveCost(cost)
+        cost = add_max_volume_linear_cost(prog, S, b, c, S_bar, b_bar, c_bar)
+        # The center of the ellipsoid {x | xᵀS_bar*x+b_barᵀx+c_bar≤0}
+        ellipsoid_center = -np.linalg.solve(S_bar, b_bar) / 2
+        # To constrain that {x | xᵀSx+bᵀx+c≤0} is a valid ellipsoid, we want it
+        # to contain at least one point. We choose that contained point to be
+        # ellipsoid_center.
+        prog.AddLinearConstraint(
+            ellipsoid_center.dot(S @ ellipsoid_center) + np.dot(b, ellipsoid_center) + c
+            <= 0
+        )
+        result = solvers.Solve(prog)
+        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)
+        if volume_result - volume_prev <= convergence_tol:
+            break
+        else:
+            volume_prev = volume_result
+            S_bar = S_result
+            b_bar = b_result
+            c_bar = c_result
+    return S_result, b_result, c_result
+
+
 def add_minimize_ellipsoid_volume(
     prog: solvers.MathematicalProgram, S: np.ndarray, b: np.ndarray, c: sym.Variable
 ) -> sym.Variable:

tests/test_ellipsoid_utils.py

@@ -7,6 +7,7 @@
 
 import pydrake.solvers as solvers
 import pydrake.symbolic as sym
+import pydrake.math
 
 
 def eval_max_volume_cost(S: jnp.ndarray, b: jnp.ndarray, c: jnp.ndarray):
@@ -169,3 +170,84 @@ def test_add_minimize_ellipsoid_volume():
     check_outer_ellipsoid(
         S_inner=np.array([[1, 2], [2, 9]]), b_inner=np.array([1, 2]), c_inner=-1
     )
+
+
+def check_maximize_inner_ellipsoid_sequentially(
+    box_size: np.ndarray, box_center: np.ndarray, box_rotation: np.ndarray
+):
+    """
+    We find the maximal ellipsoid contained within a box
+    { x | x = R * x_bar + p, -box_size/2 <= x_bar <= box_size/2}.
+    """
+    dim = box_size.size
+    assert box_center.shape == (dim,)
+    assert box_rotation.shape == (dim, dim)
+
+    # The box can be written as
+    # -box_size/2 <= R.T * (x-p) <= box_size/2
+    # Let's denote this "box constraint" as C * x <= d.
+    # According to s-lemma, an ellipsoid
+    # {x | x.T*S*x + b.T* x + c<= 0} is inside the ellipsoid if and only if
+    # xᵀSx+bᵀx+c − γ(Cᵢx−dᵢ) for some γ>= 0
+    # Namely the matrix
+    # [S             b/2 - γCᵢ] is psd.
+    # [(b/2 - γCᵢ).T   c + γdᵢ]
+    prog = solvers.MathematicalProgram()
+    S = prog.NewSymmetricContinuousVariables(dim, "S")
+    b = prog.NewContinuousVariables(dim, "b")
+    c = prog.NewContinuousVariables(1, "c")[0]
+    prog.AddPositiveSemidefiniteConstraint(S)
+    gamma = prog.NewContinuousVariables(2 * dim)
+    prog.AddBoundingBoxConstraint(0, np.inf, gamma)
+    C = np.concatenate((box_rotation.T, -box_rotation.T), axis=0)
+    d = np.concatenate(
+        (
+            box_size / 2 + box_rotation.T @ box_center,
+            box_size / 2 - box_rotation.T @ box_center,
+        )
+    )
+    for i in range(2 * dim):
+        psd_mat = np.empty((dim + 1, dim + 1), dtype=object)
+        psd_mat[:dim, :dim] = S
+        psd_mat[:dim, -1] = b / 2 - gamma[i] * C[i, :]
+        psd_mat[-1, :dim] = b / 2 - gamma[i] * C[i, :]
+        psd_mat[-1, -1] = c + gamma[i] * d[i]
+        prog.AddPositiveSemidefiniteConstraint(psd_mat)
+
+    S_init = np.eye(dim)
+    b_init = -2 * box_center
+    c_init = box_center.dot(box_center) - 0.99 * box_size.min() ** 2
+
+    S_sol, b_sol, c_sol = mut.maximize_inner_ellipsoid_sequentially(
+        prog, S, b, c, S_init, b_init, c_init, max_iter=10, convergence_tol=1e-5
+    )
+    # The maximal inner ellipsoid is the elliposoid
+    # {R * y + p | y.T * diag(1 / box_size**2) * y <= 1}.
+    S_max = box_rotation.T @ np.diag(1 / box_size**2) @ box_rotation
+    b_max = -2 * S_max @ box_center
+    c_max = box_center.dot(S_max @ box_center) - 1
+
+    ratio = c_sol / c_max
+    np.testing.assert_allclose(S_sol, S_max * ratio, atol=1e-5)
+    np.testing.assert_allclose(b_sol, b_max * ratio, atol=1e-5)
+
+
+def test_maximize_inner_ellipsoid_sequentially():
+    # 2D case, axis aligned box.
+    check_maximize_inner_ellipsoid_sequentially(
+        np.array([1.0, 2.0]), np.array([0.5, 0.6]), np.eye(2)
+    )
+
+    # 2D case, rotated box
+    check_maximize_inner_ellipsoid_sequentially(
+        np.array([2.0, 4.0]),
+        np.array([0.5, 1.0]),
+        np.array([[np.cos(0.2), -np.sin(0.2)], [np.sin(0.2), np.cos(0.2)]]),
+    )
+
+    # 3D case, rotated box.
+    check_maximize_inner_ellipsoid_sequentially(
+        np.array([2, 3, 5.0]),
+        np.array([0.5, -0.2, 0.3]),
+        pydrake.math.RotationMatrix(pydrake.math.RollPitchYaw(0.2, 0.3, 0.5)).matrix(),
+    )