Skip to content

Commit

Permalink
Support a general basic semialgebraic set in the containment. (#36)
Browse files Browse the repository at this point in the history
Now the inner set can be a general basic semi-algebraic set, including multiple polynomial inequalities and equalities.
  • Loading branch information
hongkai-dai authored Jan 18, 2024
1 parent 9ef0627 commit 12c4b42
Show file tree
Hide file tree
Showing 4 changed files with 127 additions and 54 deletions.
26 changes: 20 additions & 6 deletions compatible_clf_cbf/clf_cbf.py
Original file line number Diff line number Diff line change
Expand Up @@ -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)}
Expand Down Expand Up @@ -1019,25 +1029,29 @@ 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)
)
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],
)
78 changes: 50 additions & 28 deletions compatible_clf_cbf/utils.py
Original file line number Diff line number Diff line change
@@ -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
Expand Down Expand Up @@ -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),
)


Expand All @@ -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:
Expand Down
12 changes: 6 additions & 6 deletions tests/test_clf_cbf.py
Original file line number Diff line number Diff line change
Expand Up @@ -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,
Expand Down Expand Up @@ -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,
Expand Down Expand Up @@ -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,
Expand Down
65 changes: 51 additions & 14 deletions tests/test_utils.py
Original file line number Diff line number Diff line change
Expand Up @@ -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

0 comments on commit 12c4b42

Please sign in to comment.