From 12c4b423e95f48db5167463e2652686a6c119763 Mon Sep 17 00:00:00 2001 From: Hongkai Dai Date: Wed, 17 Jan 2024 22:29:32 -0800 Subject: [PATCH] Support a general basic semialgebraic set in the containment. (#36) Now the inner set can be a general basic semi-algebraic set, including multiple polynomial inequalities and equalities. --- compatible_clf_cbf/clf_cbf.py | 26 +++++++++--- compatible_clf_cbf/utils.py | 78 ++++++++++++++++++++++------------- tests/test_clf_cbf.py | 12 +++--- tests/test_utils.py | 65 ++++++++++++++++++++++------- 4 files changed, 127 insertions(+), 54 deletions(-) diff --git a/compatible_clf_cbf/clf_cbf.py b/compatible_clf_cbf/clf_cbf.py index d29337a..7481f86 100644 --- a/compatible_clf_cbf/clf_cbf.py +++ b/compatible_clf_cbf/clf_cbf.py @@ -947,14 +947,24 @@ def _find_max_inner_ellipsoid( ) assert V is not None assert rho is not None - V_contain_lagrangian.add_constraint(prog, ellipsoid, V - rho) + V_contain_lagrangian.add_constraint( + prog, + inner_ineq_poly=np.array([ellipsoid]), + inner_eq_poly=np.array([]), + outer_poly=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]) + b_contain_lagrangians[i].add_constraint( + prog, + inner_ineq_poly=np.array([ellipsoid]), + inner_eq_poly=np.array([]), + outer_poly=-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)} @@ -1019,7 +1029,7 @@ def _add_ellipsoid_in_compatible_region_constraint( if V is not None: V_degree = V.TotalDegree() ellipsoid_in_V_lagrangian_degree = ContainmentLagrangianDegree( - inner=V_degree - 2, outer=-1 + inner_ineq=[V_degree - 2], inner_eq=[], outer=-1 ) ellipsoid_in_V_lagrangian = ( ellipsoid_in_V_lagrangian_degree.construct_lagrangian(prog, self.x_set) @@ -1027,17 +1037,21 @@ def _add_ellipsoid_in_compatible_region_constraint( assert rho is not None ellipsoid_in_V_lagrangian.add_constraint( prog, - inner_poly=ellipsoid_poly, + inner_ineq_poly=np.array([ellipsoid_poly]), + inner_eq_poly=np.array([]), outer_poly=V - sym.Polynomial({sym.Monomial(): sym.Expression(rho)}), ) for i in range(b.size): b_degree = b[i].TotalDegree() ellipsoid_in_b_lagrangian_degree = ContainmentLagrangianDegree( - inner=b_degree - 2, outer=-1 + inner_ineq=[b_degree - 2], inner_eq=[], outer=-1 ) ellipsoid_in_b_lagrangian = ( ellipsoid_in_b_lagrangian_degree.construct_lagrangian(prog, self.x_set) ) ellipsoid_in_b_lagrangian.add_constraint( - prog, inner_poly=ellipsoid_poly, outer_poly=-b[i] + prog, + inner_ineq_poly=np.array([ellipsoid_poly]), + inner_eq_poly=np.array([]), + outer_poly=-b[i], ) diff --git a/compatible_clf_cbf/utils.py b/compatible_clf_cbf/utils.py index a0bd760..5af587a 100644 --- a/compatible_clf_cbf/utils.py +++ b/compatible_clf_cbf/utils.py @@ -1,5 +1,6 @@ import dataclasses -from typing import Optional, Tuple, Union +from typing import List, Optional, Tuple, Union +from typing_extensions import Self import numpy as np import pydrake.symbolic as sym @@ -132,22 +133,37 @@ def add_log_det_lower( @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 + To certify that an algebraic set { x | f(x) <= 0, g(x)=0} is contained in another + algebraic set {x | h(x) <= 0}, we impose the condition + -(1 + ϕ₁(x))h(x) + ϕ₂(x)f(x) + ϕ₃(x)*g(x) is sos ϕ₁(x) is sos, ϕ₂(x) is sos """ - # ϕ₂(x) in the documentation above. - inner: sym.Polynomial + # ϕ₂(x) in the documentation above, an array of polynomials. + inner_ineq: np.ndarray + # ϕ₃(x) in the documentation above, an array of polynomials. + inner_eq: np.ndarray # ϕ₁(x) in the documentation above. outer: sym.Polynomial def add_constraint( - self, prog, inner_poly: sym.Polynomial, outer_poly: sym.Polynomial + self, + prog, + inner_ineq_poly: np.ndarray, + inner_eq_poly: np.ndarray, + outer_poly: sym.Polynomial, ) -> Tuple[sym.Polynomial, np.ndarray]: return prog.AddSosConstraint( - -(1 + self.outer) * outer_poly + self.inner * inner_poly + -(1 + self.outer) * outer_poly + + self.inner_ineq.dot(inner_ineq_poly) + + self.inner_eq.dot(inner_eq_poly) + ) + + def get_result(self, result: solvers.MathematicalProgramResult) -> Self: + return ContainmentLagrangian( + inner_ineq=get_polynomial_result(result, self.inner_ineq), + inner_eq=get_polynomial_result(result, self.inner_eq), + outer=get_polynomial_result(result, self.outer), ) @@ -158,36 +174,42 @@ class ContainmentLagrangianDegree: If degree < 0, then the Lagrangian polynomial is 1. """ - inner: int = -1 - outer: int = -1 + inner_ineq: List[int] + inner_eq: List[int] + outer: int 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) + inner_ineq_lagrangians = [None] * len(self.inner_ineq) + inner_eq_lagrangians = [None] * len(self.inner_eq) + for i, inner_ineq_i in enumerate(self.inner_ineq): + if inner_ineq_i < 0: + inner_ineq_lagrangians[i] = sym.Polynomial(1) + else: + inner_ineq_lagrangians[i] = new_sos_polynomial(prog, x, inner_ineq_i)[0] + inner_ineq_lagrangians = np.array(inner_ineq_lagrangians) + + for i, inner_eq_i in enumerate(self.inner_eq): + if inner_eq_i < 0: + raise Exception( + f"inner_eq[{i}] = {inner_eq_i}, should be non-negative." + ) + else: + inner_eq_lagrangians[i] = prog.NewFreePolynomial(x, inner_eq_i) + inner_eq_lagrangians = np.array(inner_eq_lagrangians) if self.outer < 0: # The Lagrangian multiply with the outer set is # (1 + outer_lagrangian), hence we can set outer_lagrangian = 0, # such that the multiplied Lagrangian is 1. outer_lagrangian = sym.Polynomial(0) - 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) + outer_lagrangian = new_sos_polynomial(prog, x, self.outer)[0] + return ContainmentLagrangian( + inner_ineq=inner_ineq_lagrangians, + inner_eq=inner_eq_lagrangians, + outer=outer_lagrangian, + ) def to_lower_triangular_columns(mat: np.ndarray) -> np.ndarray: diff --git a/tests/test_clf_cbf.py b/tests/test_clf_cbf.py index ef2e139..804da2b 100644 --- a/tests/test_clf_cbf.py +++ b/tests/test_clf_cbf.py @@ -392,10 +392,10 @@ def test_find_max_inner_ellipsoid(self): b, rho, V_contain_lagrangian_degree=utils.ContainmentLagrangianDegree( - inner=-1, outer=0 + inner_ineq=[-1], inner_eq=[], outer=0 ), b_contain_lagrangian_degree=[ - utils.ContainmentLagrangianDegree(inner=-1, outer=0) + utils.ContainmentLagrangianDegree(inner_ineq=[-1], inner_eq=[], outer=0) ], x_inner_init=np.linalg.solve(S_ellipsoid, b_ellipsoid) / -2, max_iter=10, @@ -638,10 +638,10 @@ def test_search_clf_cbf_given_lagrangian(self): b, rho, V_contain_lagrangian_degree=mut.ContainmentLagrangianDegree( - inner=-1, outer=0 + inner_ineq=[-1], inner_eq=[], outer=0 ), b_contain_lagrangian_degree=[ - mut.ContainmentLagrangianDegree(inner=-1, outer=0) + mut.ContainmentLagrangianDegree(inner_ineq=[-1], inner_eq=[], outer=0) ], x_inner_init=x_equilibrium, max_iter=10, @@ -692,10 +692,10 @@ def test_binary_search_clf_cbf_given_lagrangian(self): b_init, rho_init, V_contain_lagrangian_degree=mut.ContainmentLagrangianDegree( - inner=-1, outer=0 + inner_ineq=[-1], inner_eq=[], outer=0 ), b_contain_lagrangian_degree=[ - mut.ContainmentLagrangianDegree(inner=-1, outer=0) + mut.ContainmentLagrangianDegree(inner_ineq=[-1], inner_eq=[], outer=0) ], x_inner_init=x_equilibrium, max_iter=10, diff --git a/tests/test_utils.py b/tests/test_utils.py index 4d1e708..c3d2ca7 100644 --- a/tests/test_utils.py +++ b/tests/test_utils.py @@ -41,38 +41,75 @@ def test_to_lower_triangular_columns(): class TestContainmentLagrangian: - @pytest.mark.parametrize("inner_degree,outer_degree", [(0, 0), (0, -1), (-1, 0)]) - def test_add_constraint1(self, inner_degree, outer_degree): + @pytest.mark.parametrize( + "inner_ineq_degree,outer_degree", [([0], 0), ([0], -1), ([-1], 0)] + ) + def test_add_constraint1(self, inner_ineq_degree, outer_degree): x = sym.MakeVectorContinuousVariable(2, "x") x_set = sym.Variables(x) - inner_poly = sym.Polynomial(x[0] ** 2 + x[1] ** 2 - 1) + inner_ineq_poly = np.array([sym.Polynomial(x[0] ** 2 + x[1] ** 2 - 1)]) outer_poly = sym.Polynomial(x[0] ** 2 + x[1] ** 2 - 1.01) prog = solvers.MathematicalProgram() prog.AddIndeterminates(x_set) containment_lagrangian_degree = mut.ContainmentLagrangianDegree( - inner=inner_degree, outer=outer_degree + inner_ineq=inner_ineq_degree, inner_eq=[], outer=outer_degree + ) + lagrangians = containment_lagrangian_degree.construct_lagrangian(prog, x_set) + lagrangians.add_constraint( + prog, inner_ineq_poly, inner_eq_poly=np.array([]), outer_poly=outer_poly ) - lagrangian = containment_lagrangian_degree.construct_lagrangian(prog, x_set) - dut = mut.ContainmentLagrangian(inner=lagrangian.inner, outer=lagrangian.outer) - dut.add_constraint(prog, inner_poly, outer_poly) result = solvers.Solve(prog) assert result.is_success() - @pytest.mark.parametrize("inner_degree,outer_degree", [(0, 0), (0, -1), (-1, 0)]) - def test_add_constraint2(self, inner_degree, outer_degree): + @pytest.mark.parametrize( + "inner_ineq_degree,outer_degree", [([0], 0), ([0], -1), ([-1], 0)] + ) + def test_add_constraint2(self, inner_ineq_degree, outer_degree): # The inner_poly sub-level set is larger than the outer_poly sub-level # set. The program should fail. x = sym.MakeVectorContinuousVariable(2, "x") x_set = sym.Variables(x) - inner_poly = sym.Polynomial(x[0] ** 2 + x[1] ** 2 - 1.01) + inner_ineq_poly = np.array([sym.Polynomial(x[0] ** 2 + x[1] ** 2 - 1.01)]) outer_poly = sym.Polynomial(x[0] ** 2 + x[1] ** 2 - 1.0) prog = solvers.MathematicalProgram() prog.AddIndeterminates(x_set) containment_lagrangian_degree = mut.ContainmentLagrangianDegree( - inner=inner_degree, outer=outer_degree + inner_ineq=inner_ineq_degree, inner_eq=[], outer=outer_degree + ) + lagrangians = containment_lagrangian_degree.construct_lagrangian(prog, x_set) + lagrangians.add_constraint( + prog, inner_ineq_poly, inner_eq_poly=np.array([]), outer_poly=outer_poly ) - lagrangian = containment_lagrangian_degree.construct_lagrangian(prog, x_set) - dut = mut.ContainmentLagrangian(inner=lagrangian.inner, outer=lagrangian.outer) - dut.add_constraint(prog, inner_poly, outer_poly) result = solvers.Solve(prog) assert not result.is_success() + + def test_add_constraint3(self): + """ + The inner algebraic set has multiple polynomials, with both equalities + and inequalities. + """ + x = sym.MakeVectorContinuousVariable(2, "x") + x_set = sym.Variables(x) + inner_ineq_poly = np.array( + [ + sym.Polynomial(x[0] ** 2 + x[1] ** 2 - 1), + sym.Polynomial(0.5 - x[0] ** 2 - 2 * x[1] ** 2), + ] + ) + inner_eq_poly = np.array([sym.Polynomial(x[0] + x[1])]) + outer_poly = sym.Polynomial(x[0] ** 2 + 0.5 * x[1] ** 2 - 1) + + lagrangian_degrees = mut.ContainmentLagrangianDegree( + inner_ineq=[0, 0], inner_eq=[1], outer=0 + ) + prog = solvers.MathematicalProgram() + prog.AddIndeterminates(x_set) + lagrangians = lagrangian_degrees.construct_lagrangian(prog, x_set) + lagrangians.add_constraint(prog, inner_ineq_poly, inner_eq_poly, outer_poly) + result = solvers.Solve(prog) + assert result.is_success() + lagrangians_result = lagrangians.get_result(result) + assert lagrangians_result.inner_ineq[0].TotalDegree() == 0 + assert lagrangians_result.inner_ineq[1].TotalDegree() == 0 + assert lagrangians_result.inner_eq[0].TotalDegree() == 1 + assert lagrangians_result.outer.TotalDegree() == 0