Skip to content

Commit

Permalink
Construct the program to search for the compatible Lagrangians. (#17)
Browse files Browse the repository at this point in the history
  • Loading branch information
hongkai-dai authored Nov 30, 2023
1 parent 59b2770 commit 054c3d5
Show file tree
Hide file tree
Showing 3 changed files with 214 additions and 26 deletions.
133 changes: 133 additions & 0 deletions compatible_clf_cbf/clf_cbf.py
Original file line number Diff line number Diff line change
Expand Up @@ -103,6 +103,92 @@ def get_result(
)


@dataclass
class CompatibleLagrangianDegrees:
"""
The degree of the Lagrangian multipliers in CompatibleLagrangians.
"""

@dataclass
class Degree:
"""
The degree of each Lagrangian polynomial in indeterminates x and y. For
example, if we have a polynomial x₀²x₁y₂ + 3x₀y₁y₂³, its degree in x is
3 (from x₀²x₁), and its degree in y is 4 (from y₁y₂³)
"""

x: int
y: int

def construct_polynomial(
self,
prog: solvers.MathematicalProgram,
x: sym.Variables,
y: sym.Variables,
is_sos: bool,
) -> sym.Polynomial:
"""
Args:
is_sos: whether the constructed polynomial is sos or not.
"""
if is_sos:
basis = sym.MonomialBasis(
{x: int(np.floor(self.x / 2)), y: int(np.floor(self.y / 2))}
)
poly, _ = prog.NewSosPolynomial(basis)
else:
basis = sym.MonomialBasis({x: self.x, y: self.y})
coeffs = prog.NewContinuousVariables(basis.size)
poly = sym.Polynomial({basis[i]: coeffs[i] for i in range(basis.size)})
return poly

lambda_y: List[Degree]
xi_y: Degree
y: Optional[List[Degree]]
rho_minus_V: Optional[Degree]
b_plus_eps: Optional[List[Degree]]

def initialize_lagrangians(
self, prog: solvers.MathematicalProgram, x: sym.Variables, y: sym.Variables
) -> CompatibleLagrangians:
lambda_y = np.array(
[
lambda_y_i.construct_polynomial(prog, x, y, is_sos=False)
for lambda_y_i in self.lambda_y
]
)
xi_y = self.xi_y.construct_polynomial(prog, x, y, is_sos=False)
y_lagrangian = (
None
if self.y is None
else np.array(
[y_i.construct_polynomial(prog, x, y, is_sos=True) for y_i in self.y]
)
)
rho_minus_V = (
None
if self.rho_minus_V is None
else self.rho_minus_V.construct_polynomial(prog, x, y, is_sos=True)
)
b_plus_eps = (
None
if self.b_plus_eps is None
else np.array(
[
b_plus_eps_i.construct_polynomial(prog, x, y, is_sos=True)
for b_plus_eps_i in self.b_plus_eps
]
)
)
return CompatibleLagrangians(
lambda_y=lambda_y,
xi_y=xi_y,
y=y_lagrangian,
rho_minus_V=rho_minus_V,
b_plus_eps=b_plus_eps,
)


@dataclass
class UnsafeRegionLagrangians:
"""
Expand Down Expand Up @@ -287,6 +373,53 @@ def certify_cbf_unsafe_region(
assert result.is_success()
return lagrangians.get_result(result)

def construct_search_compatible_lagrangians(
self,
V: Optional[sym.Polynomial],
b: np.ndarray,
kappa_V: Optional[float],
kappa_b: np.ndarray,
lagrangian_degrees: CompatibleLagrangianDegrees,
rho: Optional[float],
barrier_eps: Optional[np.ndarray],
) -> Tuple[solvers.MathematicalProgram, CompatibleLagrangians]:
"""
Given CLF candidate V and CBF candidate b, construct the optimization
program to certify that they are compatible within the region
{x | V(x) <= rho} ∩ {x | b(x) >= -eps}.
Args:
V: The CLF candidate. If empty, then we will certify that the multiple
barrier functions are compatible.
b: The CBF candidates.
kappa_V: The exponential decay rate for CLF. Namely we want V̇ ≤ −κ_V*V
kappa_b: The exponential rate for CBF, namely we want ḃ ≥ −κ_b*b
lagrangian_degrees: The degrees for the Lagrangian polynomials.
rho: The certified inner approximation of ROA is {x | V(x) <= rho}
barrier_eps: The certified safe region is {x | b(x) >= -eps}
coefficient_tol: In the Lagrangian polynomials, we will remove the
coefficients no larger than this tolerance.
Returns:
result: The result for solving the optimization program.
lagrangian_result: The result of the Lagrangian polynomials.
"""
prog = solvers.MathematicalProgram()
prog.AddIndeterminates(self.xy_set)
lagrangians = lagrangian_degrees.initialize_lagrangians(
prog, self.x_set, self.y_set
)
self._add_compatibility(
prog=prog,
V=V,
b=b,
kappa_V=kappa_V,
kappa_b=kappa_b,
lagrangians=lagrangians,
rho=rho,
barrier_eps=barrier_eps,
)
return (prog, lagrangians)

def _calc_xi_Lambda(
self,
*,
Expand Down
39 changes: 14 additions & 25 deletions examples/linear_toy/linear_toy_demo.py
Original file line number Diff line number Diff line change
Expand Up @@ -22,32 +22,21 @@ def search_compatible_lagrangians(
y_size = dut.y.size

# Search for the
lagrangians = clf_cbf.CompatibleLagrangians.reserve(
nu=2,
use_y_squared=dut.use_y_squared,
y_size=y_size,
with_rho_minus_V=False,
b_plus_eps_size=None,
lagrangian_degrees = clf_cbf.CompatibleLagrangianDegrees(
lambda_y=[
clf_cbf.CompatibleLagrangianDegrees.Degree(x=2, y=0) for _ in range(dut.nx)
],
xi_y=clf_cbf.CompatibleLagrangianDegrees.Degree(x=2, y=0),
y=None
if dut.use_y_squared
else [
clf_cbf.CompatibleLagrangianDegrees.Degree(x=2, y=0) for _ in range(y_size)
],
rho_minus_V=None,
b_plus_eps=None,
)
prog = pydrake.solvers.MathematicalProgram()
prog.AddIndeterminates(dut.x)
prog.AddIndeterminates(dut.y)
lagrangians.lambda_y[0] = prog.NewFreePolynomial(dut.xy_set, deg=4)
lagrangians.lambda_y[1] = prog.NewFreePolynomial(dut.xy_set, deg=4)
lagrangians.xi_y = prog.NewFreePolynomial(dut.xy_set, deg=4)
if not dut.use_y_squared:
for i in range(y_size):
lagrangians.y[i] = prog.NewSosPolynomial(dut.xy_set, degree=2)[0]

dut._add_compatibility(
prog=prog,
V=V,
b=b,
kappa_V=kappa_V,
kappa_b=kappa_b,
lagrangians=lagrangians,
rho=None,
barrier_eps=None,
prog, lagrangians = dut.construct_search_compatible_lagrangians(
V, b, kappa_V, kappa_b, lagrangian_degrees, rho=None, barrier_eps=None
)

result = pydrake.solvers.Solve(prog)
Expand Down
68 changes: 67 additions & 1 deletion tests/test_clf_cbf.py
Original file line number Diff line number Diff line change
Expand Up @@ -9,6 +9,31 @@
import compatible_clf_cbf.utils as utils


class TestCompatibleLagrangianDegrees(object):
def test_construct_polynomial(self):
degree = mut.CompatibleLagrangianDegrees.Degree(x=3, y=2)
prog = solvers.MathematicalProgram()
x = prog.NewIndeterminates(3, "x")
y = prog.NewIndeterminates(2, "y")

sos_poly = degree.construct_polynomial(
prog, sym.Variables(x), sym.Variables(y), is_sos=True
)
assert len(prog.positive_semidefinite_constraints()) == 1
free_poly = degree.construct_polynomial(
prog, sym.Variables(x), sym.Variables(y), is_sos=False
)
assert len(prog.positive_semidefinite_constraints()) == 1
for monomial, _ in sos_poly.monomial_to_coefficient_map().items():
# The total degree for x should be <= 2 (the largest even degree <= 3).
assert np.sum([monomial.degree(x[i]) for i in range(x.size)]) <= 2
assert np.sum([monomial.degree(y[i]) for i in range(y.size)]) <= 2
for monomial, _ in free_poly.monomial_to_coefficient_map().items():
# The total degree for x should be <= 3
assert np.sum([monomial.degree(x[i]) for i in range(x.size)]) <= 3
assert np.sum([monomial.degree(y[i]) for i in range(y.size)]) <= 2


class TestClfCbf(object):
@classmethod
def setup_class(cls):
Expand Down Expand Up @@ -154,6 +179,48 @@ def test_calc_xi_Lambda_wo_clf(self):
lambda_mat_expected = -dbdx @ self.g
utils.check_polynomial_arrays_equal(lambda_mat, lambda_mat_expected, 1e-8)

def test_search_compatible_lagrangians_w_clf_y_squared(self):
"""
Test search_compatible_lagrangians with CLF and use_y_squared=True
"""
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,
)
V = sym.Polynomial(dut.x[0] ** 2 + 4 * dut.x[1] ** 2)
b = np.array(
[
sym.Polynomial(1 - dut.x[0] ** 2 - dut.x[1] ** 2),
sym.Polynomial(2 - dut.x[0] ** 2 - dut.x[2] ** 2),
]
)
kappa_V = 0.01
kappa_b = np.array([0.02, 0.03])
lagrangian_degrees = mut.CompatibleLagrangianDegrees(
lambda_y=[
mut.CompatibleLagrangianDegrees.Degree(x=2, y=2) for _ in range(self.nu)
],
xi_y=mut.CompatibleLagrangianDegrees.Degree(x=2, y=2),
y=None,
rho_minus_V=mut.CompatibleLagrangianDegrees.Degree(x=2, y=2),
b_plus_eps=[
mut.CompatibleLagrangianDegrees.Degree(x=2, y=2)
for _ in range(len(self.unsafe_regions))
],
)
rho = 0.1
barrier_eps = np.array([0.01, 0.02])

prog, lagrangians = dut.construct_search_compatible_lagrangians(
V, b, kappa_V, kappa_b, lagrangian_degrees, rho, barrier_eps
)

def test_add_compatibility_w_clf_y_squared(self):
"""
Test _add_compatibility with CLF and use_y_squared=True
Expand All @@ -171,7 +238,6 @@ def test_add_compatibility_w_clf_y_squared(self):
prog = solvers.MathematicalProgram()
prog.AddIndeterminates(dut.xy_set)

V = prog.NewFreePolynomial(dut.x_set, deg=2)
V = sym.Polynomial(dut.x[0] ** 2 + 4 * dut.x[1] ** 2)
b = np.array(
[
Expand Down

0 comments on commit 054c3d5

Please sign in to comment.