Find the initial ellipsoid through optimization.

compatible_clf_cbf/clf_cbf.py

@@ -11,6 +11,7 @@
     ContainmentLagrangianDegree,
     check_array_of_polynomials,
     get_polynomial_result,
+    solve_with_id,
 )
 import compatible_clf_cbf.ellipsoid_utils as ellipsoid_utils
 
@@ -700,14 +701,17 @@ def _find_max_inner_ellipsoid(
         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,
+        x_inner_init: np.ndarray,
         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]:
+        """
+        Args:
+          x_inner_init: The initial guess on a point inside V(x) <= rho and
+            b(x) >= 0. The initial ellipsoid will cover this point.
+        """
         prog = solvers.MathematicalProgram()
         dim = self.x_set.size()
 
@@ -736,6 +740,31 @@ def _find_max_inner_ellipsoid(
         for i in range(len(b_contain_lagrangians)):
             b_contain_lagrangians[i].add_constraint(prog, ellipsoid, -b[i])
 
+        # Make sure x_inner_init is inside V(x) <= rho and b(x) >= 0.
+        env_inner_init = {self.x[i]: x_inner_init[i] for i in range(self.nx)}
+        if V is not None:
+            assert V.Evaluate(env_inner_init) <= rho
+        for b_i in b:
+            assert b_i.Evaluate(env_inner_init) >= 0
+
+        # First solve an optimization problem to find an inner ellipsoid.
+        # Add a constraint that the initial ellipsoid contains x_inner_init.
+        x_inner_init_in_ellipsoid = (
+            ellipsoid_utils.add_ellipsoid_contain_pts_constraint(
+                prog,
+                S_ellipsoid,
+                b_ellipsoid,
+                c_ellipsoid,
+                x_inner_init.reshape((1, -1)),
+            )
+        )
+        result_init = solve_with_id(prog, solver_id, None)
+        assert result_init.is_success()
+        S_ellipsoid_init = result_init.GetSolution(S_ellipsoid)
+        b_ellipsoid_init = result_init.GetSolution(b_ellipsoid)
+        c_ellipsoid_init = result_init.GetSolution(c_ellipsoid)
+        prog.RemoveConstraint(x_inner_init_in_ellipsoid)
+
         S_sol, b_sol, c_sol = ellipsoid_utils.maximize_inner_ellipsoid_sequentially(
             prog,
             S_ellipsoid,

compatible_clf_cbf/ellipsoid_utils.py

@@ -132,9 +132,9 @@ def maximize_inner_ellipsoid_sequentially(
       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_init: The initial guess of S.
+      b_init: The initial guess of b.
+      c_init: The initial guess of c.
     """
     S_bar = S_init
     b_bar = b_init
@@ -181,7 +181,6 @@ def volume(S: np.ndarray, b: np.ndarray, c: float) -> float:
         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:
@@ -258,3 +257,27 @@ def is_ellipsoid_contained(
     prog.AddPositiveSemidefiniteConstraint(mat)
     result = solvers.Solve(prog)
     return result.is_success()
+
+
+def add_ellipsoid_contain_pts_constraint(
+    prog: solvers.MathematicalProgram,
+    S: np.ndarray,
+    b: np.ndarray,
+    c: sym.Variable,
+    pts: np.ndarray,
+) -> solvers.Binding[solvers.LinearConstraint]:
+    """
+    Add the constraint that the ellipsoid {x | x'*S*x + b' * x + c <= 0} contains pts[i]
+
+    Args:
+      pts: pts[i] is the i'th point to be contained in the ellipsoid.
+    """
+    dim = S.shape[0]
+    assert pts.shape[1] == dim
+    x = sym.MakeVectorContinuousVariable(dim, "x")
+    poly = sym.Polynomial(x.dot(S @ x) + b.dot(x) + c, sym.Variables(x))
+    linear_coeffs, vars, constant = poly.EvaluateWithAffineCoefficients(x, pts.T)
+    constraint = prog.AddLinearConstraint(
+        linear_coeffs, np.full((pts.shape[0],), -np.inf), -constant, vars
+    )
+    return constraint

compatible_clf_cbf/utils.py

@@ -196,3 +196,16 @@ def to_lower_triangular_columns(mat: np.ndarray) -> np.ndarray:
         ret[count: count + dim - col] = mat[col:, col]
         count += dim - col
     return ret
+
+
+def solve_with_id(
+    prog: solvers.MathematicalProgram,
+    solver_id: Optional[solvers.SolverId] = None,
+    solver_options: Optional[solvers.SolverOptions] = None,
+) -> solvers.MathematicalProgramResult:
+    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

tests/test_clf_cbf.py

@@ -394,9 +394,7 @@ def test_find_max_inner_ellipsoid(self):
             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,
+            x_inner_init=np.linalg.solve(S_ellipsoid, b_ellipsoid) / -2,
             max_iter=10,
             convergence_tol=1e-4,
             trust_region=100,

tests/test_ellipsoid_utils.py

@@ -251,3 +251,26 @@ def test_maximize_inner_ellipsoid_sequentially():
         np.array([0.5, -0.2, 0.3]),
         pydrake.math.RotationMatrix(pydrake.math.RollPitchYaw(0.2, 0.3, 0.5)).matrix(),
     )
+
+
+def test_add_ellipsoid_contain_pts_constraint():
+    prog = solvers.MathematicalProgram()
+    S = prog.NewSymmetricContinuousVariables(3, "S")
+    prog.AddPositiveSemidefiniteConstraint(S)
+    b = prog.NewContinuousVariables(3, "b")
+    c = prog.NewContinuousVariables(1, "c")[0]
+    pts = np.array([[1, 2, 3], [0.5, 1, -2]])
+    constraint = mut.add_ellipsoid_contain_pts_constraint(prog, S, b, c, pts)
+    result = solvers.Solve(prog)
+    assert result.is_success()
+    S_sol = result.GetSolution(S)
+    b_sol = result.GetSolution(b)
+    c_sol = result.GetSolution(c)
+    assert np.all(np.linalg.eigvals(S_sol) >= 0)
+    for i in range(pts.shape[0]):
+        assert pts[i].dot(S_sol @ pts[i]) + b_sol.dot(pts[i]) + c_sol <= 0
+    prog.RemoveConstraint(constraint)
+    assert (
+        len(prog.linear_constraints()) == 0
+        and len(prog.linear_equality_constraints()) == 0
+    )