From 4f2b9892b5f875de13fb59064ac1d3c0848c9901 Mon Sep 17 00:00:00 2001 From: Hongkai Dai Date: Mon, 11 Dec 2023 22:48:03 -0800 Subject: [PATCH] Find the largest ellipsoid within the ROA and safe region. --- compatible_clf_cbf/clf_cbf.py | 65 +++++++++++++++++++- compatible_clf_cbf/ellipsoid_utils.py | 85 +++++++++++++++++++++++++- compatible_clf_cbf/utils.py | 88 +++++++++++++++++++++++++-- tests/test_clf_cbf.py | 48 ++++++++++++++- tests/test_utils.py | 20 +----- 5 files changed, 279 insertions(+), 27 deletions(-) diff --git a/compatible_clf_cbf/clf_cbf.py b/compatible_clf_cbf/clf_cbf.py index 5521d84..9c183d6 100644 --- a/compatible_clf_cbf/clf_cbf.py +++ b/compatible_clf_cbf/clf_cbf.py @@ -7,7 +7,12 @@ import pydrake.symbolic as sym import pydrake.solvers as solvers -from compatible_clf_cbf.utils import check_array_of_polynomials, get_polynomial_result +from compatible_clf_cbf.utils import ( + ContainmentLagrangianDegree, + check_array_of_polynomials, + get_polynomial_result, +) +import compatible_clf_cbf.ellipsoid_utils as ellipsoid_utils @dataclass @@ -687,3 +692,61 @@ def _construct_search_clf_cbf_program( ) return (prog, V, b, rho) + + def _find_max_inner_ellipsoid( + self, + V: Optional[sym.Polynomial], + b: np.ndarray, + 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, + 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]: + prog = solvers.MathematicalProgram() + dim = self.x_set.size() + + S_ellipsoid = prog.NewSymmetricContinuousVariables(dim, "S") + prog.AddPositiveSemidefiniteConstraint(S_ellipsoid) + b_ellipsoid = prog.NewContinuousVariables(dim, "b") + c_ellipsoid = prog.NewContinuousVariables(1, "c")[0] + + ellipsoid = sym.Polynomial( + self.x.dot(S_ellipsoid @ self.x) + b_ellipsoid.dot(self.x) + c_ellipsoid, + self.x_set, + ) + prog.AddIndeterminates(self.x_set) + if V_contain_lagrangian_degree is not None: + V_contain_lagrangian = V_contain_lagrangian_degree.construct_lagrangian( + prog, self.x_set + ) + assert V is not None + assert rho is not None + V_contain_lagrangian.add_constraint(prog, ellipsoid, V - rho) + b_contain_lagrangians = [ + degree.construct_lagrangian(prog, self.x_set) + for degree in b_contain_lagrangian_degree + ] + + for i in range(len(b_contain_lagrangians)): + b_contain_lagrangians[i].add_constraint(prog, ellipsoid, -b[i]) + + S_sol, b_sol, c_sol = ellipsoid_utils.maximize_inner_ellipsoid_sequentially( + prog, + S_ellipsoid, + b_ellipsoid, + c_ellipsoid, + S_ellipsoid_init, + b_ellipsoid_init, + c_ellipsoid_init, + max_iter, + convergence_tol, + solver_id, + trust_region, + ) + return (S_sol, b_sol, c_sol) diff --git a/compatible_clf_cbf/ellipsoid_utils.py b/compatible_clf_cbf/ellipsoid_utils.py index ae578e5..1bfe3c1 100644 --- a/compatible_clf_cbf/ellipsoid_utils.py +++ b/compatible_clf_cbf/ellipsoid_utils.py @@ -2,7 +2,7 @@ The utility function for manipulating ellipsoids """ -from typing import Tuple +from typing import Optional, Tuple import numpy as np @@ -71,6 +71,42 @@ def add_max_volume_linear_cost( return cost +def _add_ellipsoid_trust_region( + prog: solvers.MathematicalProgram, + S: np.ndarray, + b: np.ndarray, + c: sym.Variable, + S_bar: np.ndarray, + b_bar: np.ndarray, + c_bar: float, + trust_region, +): + """ + Add the constraint (S - S_bar)^2 + (b-b_bar)^2 + (c-c_bar)^2 <= trust_region. + """ + dim = S.shape[0] + linear_coeff = np.concatenate( + ( + utils.to_lower_triangular_columns(S_bar).reshape((-1,)), + b_bar.reshape((-1,)), + np.array([c_bar]), + ) + ) + constraint = prog.AddQuadraticAsRotatedLorentzConeConstraint( + np.eye(int((dim + 1) * dim / 2) + dim + 1), + -linear_coeff, + linear_coeff.dot(linear_coeff) / 2 - trust_region, + np.concatenate( + ( + utils.to_lower_triangular_columns(S).reshape((-1,)), + b.reshape((-1,)), + np.array([c]), + ) + ), + ) + return constraint + + def maximize_inner_ellipsoid_sequentially( prog: solvers.MathematicalProgram, S: np.ndarray, @@ -81,6 +117,8 @@ def maximize_inner_ellipsoid_sequentially( c_init: float, 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]: """ Maximize the inner ellipsoid ℰ={x | xᵀSx+bᵀx+c≤0} through a sequence of @@ -111,6 +149,7 @@ def volume(S: np.ndarray, b: np.ndarray, c: float) -> float: cost = None volume_prev = volume(S_init, b_init, c_init) assert max_iter >= 1 + trust_region_constraint = None for i in range(max_iter): if cost is not None: prog.RemoveCost(cost) @@ -124,12 +163,25 @@ def volume(S: np.ndarray, b: np.ndarray, c: float) -> float: ellipsoid_center.dot(S @ ellipsoid_center) + np.dot(b, ellipsoid_center) + c <= 0 ) - result = solvers.Solve(prog) + + if trust_region_constraint is not None: + prog.RemoveConstraint(trust_region_constraint) + if trust_region is not None: + assert trust_region >= 0 + trust_region_constraint = _add_ellipsoid_trust_region( + prog, S, b, c, S_bar, b_bar, c_bar, trust_region + ) + if solver_id is None: + result = solvers.Solve(prog) + else: + solver = solvers.MakeSolver(solver_id) + result = solver.Solve(prog, None, None) 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) + print(f"{volume_result}") if volume_result - volume_prev <= convergence_tol: break else: @@ -177,3 +229,32 @@ def add_minimize_ellipsoid_volume( prog.AddPositiveSemidefiniteConstraint(psd_mat) utils.add_log_det_lower(prog, S, lower=0.0) return r + + +def is_ellipsoid_contained( + S_inner: np.ndarray, + b_inner: np.ndarray, + c_inner: float, + S_outer: np.ndarray, + b_outer: np.ndarray, + c_outer: float, +) -> bool: + """ + Check if {x | x' * S_inner * x + b_inner' * x + c_inner <= 0} is contained + in {x | x' * S_outer * x + b_outer' * x + c_outer <= 0}. + """ + + prog = solvers.MathematicalProgram() + gamma = prog.NewContinuousVariables(1, "gamma")[0] + + # -x' * S_outer * x - b_outer' * x - c_outer + # + gamma * (x' * S_inner * x + b_inner' * x + c_inner) is sos. + dim = S_inner.shape[0] + mat = np.empty((dim + 1, dim + 1), dtype=object) + mat[:dim, :dim] = gamma * S_inner - S_outer + mat[:dim, -1] = (gamma * b_inner - b_outer) / 2 + mat[-1, :dim] = mat[:dim, -1] + mat[-1, -1] = gamma * c_inner - c_outer + prog.AddPositiveSemidefiniteConstraint(mat) + result = solvers.Solve(prog) + return result.is_success() diff --git a/compatible_clf_cbf/utils.py b/compatible_clf_cbf/utils.py index 35d840d..fb62f20 100644 --- a/compatible_clf_cbf/utils.py +++ b/compatible_clf_cbf/utils.py @@ -1,5 +1,5 @@ import dataclasses -from typing import Optional, Union +from typing import Optional, Tuple, Union import numpy as np import pydrake.symbolic as sym @@ -57,11 +57,20 @@ def get_polynomial_result( return p_result -def is_sos(p: sym.Polynomial) -> bool: +def is_sos( + poly: sym.Polynomial, + solver_id: Optional[solvers.SolverId] = None, + solver_options: Optional[solvers.SolverOptions] = None, +): prog = solvers.MathematicalProgram() - prog.AddIndeterminates(p.indeterminates()) - prog.AddSosConstraint(p) - result = solvers.Solve(prog) + prog.AddIndeterminates(poly.indeterminates()) + assert poly.decision_variables().empty() + prog.AddSosConstraint(poly) + 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.is_success() @@ -118,3 +127,72 @@ def add_log_det_lower( prog.AddLinearConstraint(np.ones((1, X_rows)), lower, np.inf, t) return LogDetLowerRet(Z=Z, t=t) + + +@dataclasses.dataclass +class ContainmentLagrangian: + """ + To certify that an algebraic set { x | f(x) <= 0} is contained in another + algebraic set {x | g(x) < 0}, we impose the condition + -1 - ϕ₁(x))g(x) + ϕ₂(x)f(x) is sos + ϕ₁(x) is sos, ϕ₂(x) is sos + """ + + # ϕ₂(x) in the documentation above. + inner: sym.Polynomial + # ϕ₁(x) in the documentation above. + outer: sym.Polynomial + + def add_constraint( + self, prog, inner_poly: sym.Polynomial, outer_poly: sym.Polynomial + ) -> Tuple[sym.Polynomial, np.ndarray]: + return prog.AddSosConstraint( + -1 - self.outer * outer_poly + self.inner * inner_poly + ) + + +@dataclasses.dataclass +class ContainmentLagrangianDegree: + """ + The degree of the polynomials in ContainmentLagrangian + If degree < 0, then the Lagrangian polynomial is 1. + """ + + inner: int = -1 + outer: int = -1 + + def construct_lagrangian( + self, prog: solvers.MathematicalProgram, x: sym.Variables + ) -> ContainmentLagrangian: + if self.inner < 0: + inner_lagrangian = sym.Polynomial(1) + elif self.inner == 0: + inner_lagrangian_var = prog.NewContinuousVariables(1)[0] + prog.AddBoundingBoxConstraint(0, np.inf, inner_lagrangian_var) + inner_lagrangian = sym.Polynomial( + {sym.Monomial(): sym.Expression(inner_lagrangian_var)} + ) + else: + inner_lagrangian, _ = prog.NewSosPolynomial(x, self.inner) + if self.outer < 0: + outer_lagrangian = sym.Polynomial(1) + elif self.outer == 0: + outer_lagrangian_var = prog.NewContinuousVariables(1)[0] + prog.AddBoundingBoxConstraint(0, np.inf, outer_lagrangian_var) + outer_lagrangian = sym.Polynomial( + {sym.Monomial(): sym.Expression(outer_lagrangian_var)} + ) + else: + outer_lagrangian, _ = prog.NewSosPolynomial(x, self.outer) + return ContainmentLagrangian(inner=inner_lagrangian, outer=outer_lagrangian) + + +def to_lower_triangular_columns(mat: np.ndarray) -> np.ndarray: + assert mat.shape[0] == mat.shape[1] + dim = mat.shape[0] + ret = np.empty(int((dim + 1) * dim / 2), dtype=mat.dtype) + count = 0 + for col in range(dim): + ret[count: count + dim - col] = mat[col:, col] + count += dim - col + return ret diff --git a/tests/test_clf_cbf.py b/tests/test_clf_cbf.py index 39ecad7..2a52dee 100644 --- a/tests/test_clf_cbf.py +++ b/tests/test_clf_cbf.py @@ -8,8 +8,8 @@ import pydrake.solvers as solvers import pydrake.symbolic as sym +import compatible_clf_cbf.ellipsoid_utils as ellipsoid_utils import compatible_clf_cbf.utils as utils -import tests.test_utils as test_utils class TestCompatibleLagrangianDegrees(object): @@ -365,6 +365,50 @@ def test_certify_cbf_unsafe_region(self): for i in range(dut.unsafe_regions[0].size): assert utils.is_sos(lagrangians.unsafe_region[i]) + def test_find_max_inner_ellipsoid(self): + dut = mut.CompatibleClfCbf( + f=self.f, + g=self.g, + x=self.x, + unsafe_regions=self.unsafe_regions, + Au=None, + bu=None, + with_clf=True, + use_y_squared=True, + ) + S_ellipsoid = np.diag(np.array([1, 2.0, 3.0])) + b_ellipsoid = np.array([0.5, 2, 1]) + c_ellipsoid = b_ellipsoid.dot(np.linalg.solve(S_ellipsoid, b_ellipsoid)) / 4 + V = sym.Polynomial( + self.x.dot(S_ellipsoid @ self.x) + b_ellipsoid.dot(self.x) + c_ellipsoid + ) + b = np.array([sym.Polynomial(1 - self.x.dot(self.x))]) + rho = 2 + S_sol, b_sol, c_sol = dut._find_max_inner_ellipsoid( + V, + b, + rho, + V_contain_lagrangian_degree=utils.ContainmentLagrangianDegree( + inner=-1, outer=0 + ), + 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, + max_iter=10, + convergence_tol=1e-4, + trust_region=100, + ) + # Make sure the ellipsoid is within V<= rho and b >= 0 + assert ellipsoid_utils.is_ellipsoid_contained( + S_sol, b_sol, c_sol, S_ellipsoid, b_ellipsoid, c_ellipsoid - rho + ) + assert ellipsoid_utils.is_ellipsoid_contained( + S_sol, b_sol, c_sol, np.eye(3), np.zeros(3), -1 + ) + class TestClfCbfToy: """ @@ -481,7 +525,7 @@ def test_search_clf_cbf(self): env = {self.x[i]: 0 for i in range(self.nx)} assert V_result.Evaluate(env) == 0 assert sym.Monomial() not in V.monomial_to_coefficient_map() - assert test_utils.is_sos(V_result, solvers.ClarabelSolver.id()) + assert utils.is_sos(V_result, solvers.ClarabelSolver.id()) assert V_result.TotalDegree() == 2 rho_result = result.GetSolution(rho) assert rho_result >= 0 diff --git a/tests/test_utils.py b/tests/test_utils.py index 4560853..d788f8a 100644 --- a/tests/test_utils.py +++ b/tests/test_utils.py @@ -1,7 +1,5 @@ import compatible_clf_cbf.utils as mut -from typing import Optional - import numpy as np import pytest # noqa @@ -37,18 +35,6 @@ def tester(lower): tester(3.0) -def is_sos( - poly: sym.Polynomial, - solver_id: Optional[solvers.SolverId] = None, - solver_options: Optional[solvers.SolverOptions] = None, -): - prog = solvers.MathematicalProgram() - prog.AddIndeterminates(poly.indeterminates()) - assert poly.decision_variables().empty() - prog.AddSosConstraint(poly) - 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.is_success() +def test_to_lower_triangular_columns(): + vec = mut.to_lower_triangular_columns(np.array([[1, 2, 3], [4, 5, 6], [7, 8, 9]])) + np.testing.assert_equal(vec, np.array([1, 4, 7, 5, 8, 9]))