From eccba5941060f624de342c6b01cef82821228176 Mon Sep 17 00:00:00 2001 From: Hongkai Dai Date: Wed, 13 Dec 2023 21:08:48 -0800 Subject: [PATCH] Find the initial ellipsoid through optimization. Solve an SDP to find an initial ellipsoid within the compatible region and contains a certain point. --- compatible_clf_cbf/clf_cbf.py | 35 ++++++++++++++++++++++++--- compatible_clf_cbf/ellipsoid_utils.py | 31 +++++++++++++++++++++--- compatible_clf_cbf/utils.py | 13 ++++++++++ tests/test_clf_cbf.py | 4 +-- tests/test_ellipsoid_utils.py | 23 ++++++++++++++++++ 5 files changed, 96 insertions(+), 10 deletions(-) diff --git a/compatible_clf_cbf/clf_cbf.py b/compatible_clf_cbf/clf_cbf.py index 9c183d6..685fa96 100644 --- a/compatible_clf_cbf/clf_cbf.py +++ b/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, diff --git a/compatible_clf_cbf/ellipsoid_utils.py b/compatible_clf_cbf/ellipsoid_utils.py index 1bfe3c1..f351fcc 100644 --- a/compatible_clf_cbf/ellipsoid_utils.py +++ b/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 diff --git a/compatible_clf_cbf/utils.py b/compatible_clf_cbf/utils.py index fb62f20..fcb2d5f 100644 --- a/compatible_clf_cbf/utils.py +++ b/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 diff --git a/tests/test_clf_cbf.py b/tests/test_clf_cbf.py index 2a52dee..911e069 100644 --- a/tests/test_clf_cbf.py +++ b/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, diff --git a/tests/test_ellipsoid_utils.py b/tests/test_ellipsoid_utils.py index 3053038..db0c3f3 100644 --- a/tests/test_ellipsoid_utils.py +++ b/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 + )