Move add_minimize_ellipsoid_volume to ellipsoid_utils.py (#19)

compatible_clf_cbf/ellipsoid_utils.py

@@ -7,6 +7,8 @@
 import pydrake.solvers as solvers
 import pydrake.symbolic as sym
 
+import compatible_clf_cbf.utils as utils
+
 
 def add_max_volume_linear_cost(
     prog: solvers.MathematicalProgram,
@@ -65,3 +67,42 @@ def add_max_volume_linear_cost(
     )
     cost = prog.AddLinearCost(-cost_expr)
     return cost
+
+
+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 minimize the volume through the convex program
+    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)
+    utils.add_log_det_lower(prog, S, lower=0.0)
+    return r

compatible_clf_cbf/utils.py

@@ -118,42 +118,3 @@ def add_log_det_lower(
     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 minimize the volume through the convex program
-    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

tests/test_ellipsoid_utils.py

@@ -20,6 +20,10 @@ def eval_max_volume_cost(S: jnp.ndarray, b: jnp.ndarray, c: jnp.ndarray):
     )
 
 
+def check_psd(X: np.ndarray, tol: float):
+    assert np.all((np.linalg.eig(X)[0] >= -tol))
+
+
 def check_add_max_volume_linear_cost(
     cost: solvers.Binding[solvers.LinearCost],
     S: np.ndarray,
@@ -102,3 +106,66 @@ def test_add_max_volume_linear_cost2():
     check_add_max_volume_linear_cost(
         cost, S, b, c, S_val, b_val, c_val, S_bar, b_bar, c_bar
     )
+
+
+def check_outer_ellipsoid(S_inner: np.ndarray, b_inner: np.ndarray, c_inner: float):
+    """
+    Find the smallest ellipsoid {x | xᵀSx + bᵀx + c ≤ 0} containing the
+    inner ellipsoid {x | xᵀS_inner*x + b_innerᵀx + c_inner ≤ 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_minimize_ellipsoid_volume():
+    check_outer_ellipsoid(S_inner=np.eye(2), b_inner=np.zeros(2), c_inner=-1)
+    check_outer_ellipsoid(S_inner=np.eye(2), b_inner=np.array([1, 2]), c_inner=-1)
+    check_outer_ellipsoid(
+        S_inner=np.array([[1, 2], [2, 9]]), b_inner=np.array([1, 2]), c_inner=-1
+    )

tests/test_utils.py

@@ -33,66 +33,3 @@ def tester(lower):
 
     tester(2.0)
     tester(3.0)
-
-
-def check_outer_ellipsoid(S_inner: np.ndarray, b_inner: np.ndarray, c_inner: float):
-    """
-    Find the smallest ellipsoid {x | xᵀSx + bᵀx + c ≤ 0} containing the
-    inner ellipsoid {x | xᵀS_inner*x + b_innerᵀx + c_inner ≤ 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_minimize_ellipsoid_volume():
-    check_outer_ellipsoid(S_inner=np.eye(2), b_inner=np.zeros(2), c_inner=-1)
-    check_outer_ellipsoid(S_inner=np.eye(2), b_inner=np.array([1, 2]), c_inner=-1)
-    check_outer_ellipsoid(
-        S_inner=np.array([[1, 2], [2, 9]]), b_inner=np.array([1, 2]), c_inner=-1
-    )