diff --git a/compatible_clf_cbf/clf_cbf.py b/compatible_clf_cbf/clf_cbf.py index 8e2e5a7..5521d84 100644 --- a/compatible_clf_cbf/clf_cbf.py +++ b/compatible_clf_cbf/clf_cbf.py @@ -591,3 +591,99 @@ def _add_barrier_safe_constraint( ) prog.AddSosConstraint(poly) return poly + + def _construct_search_clf_cbf_program( + self, + compatible_lagrangians: CompatibleLagrangians, + unsafe_regions_lagrangians: List[UnsafeRegionLagrangians], + clf_degree: Optional[int], + cbf_degrees: List[int], + x_equilibrium: Optional[np.ndarray], + kappa_V: Optional[float], + kappa_b: np.ndarray, + barrier_eps: np.ndarray, + ) -> Tuple[ + solvers.MathematicalProgram, + Optional[sym.Polynomial], + np.ndarray, + Optional[sym.Variable], + ]: + """ + Construct a program to search for compatible CLF/CBFs given the Lagrangians. + Notice that we have not imposed the cost to the program yet. + + Args: + compatible_lagrangians: The Lagrangian polynomials. Result from + solving construct_search_compatible_lagrangians(). + unsafe_regions_lagrangians: unsafe_regions_lagrangians[i] is the + Lagrangian polynomial for the i'th CBF to certify that the CBF + 0-super level set doesn't intersect with the i'th unsafe region. + clf_degree: if not None, the total degree of CLF. + cbf_degrees: cbf_degrees[i] is the total degree of the i'th CBF. + x_equilibrium: if not None, the equilibrium state. + """ + assert len(unsafe_regions_lagrangians) == len(self.unsafe_regions) + assert len(cbf_degrees) == len(self.unsafe_regions) + prog = solvers.MathematicalProgram() + prog.AddIndeterminates(self.xy_set) + + if clf_degree is not None: + assert x_equilibrium is not None + clf_monomials = sym.MonomialBasis(self.x, int(np.floor(clf_degree / 2))) + if np.all(x_equilibrium == 0): + # If the equilibrium state x* = 0, then we know that V(x*)=0 + # and V(x) > 0 forall x != x*. This means that the linear and + # constant coefficient of V is zero. Hence we remove "1" from + # clf_monomials. + clf_monomials = np.array( + [ + monomial + for monomial in clf_monomials + if monomial.total_degree() != 0 + ] + ) + V, clf_gram = prog.NewSosPolynomial(clf_monomials) + if np.any(x_equilibrium != 0): + # Add the constraint V(x*) = 0 + ( + V_x_equilibrium_coeff, + V_x_equilibrium_var, + V_x_equilibrium_constant, + ) = V.EvaluateWithAffineCoefficients( + self.x, x_equilibrium.reshape((-1, 1)) + ) + prog.AddLinearEqualityConstraint( + V_x_equilibrium_coeff.reshape((1, -1)), + -V_x_equilibrium_coeff[0], + V_x_equilibrium_var, + ) + else: + V = None + + # Add CBF. + b = np.array( + [ + prog.NewFreePolynomial(self.x_set, cbf_degree) + for cbf_degree in cbf_degrees + ] + ) + for i in range(len(self.unsafe_regions)): + self._add_barrier_safe_constraint( + prog, i, b[i], unsafe_regions_lagrangians[i] + ) + rho = None if V is None else prog.NewContinuousVariables(1, "rho")[0] + if rho is not None: + prog.AddBoundingBoxConstraint(0, np.inf, rho) + + self._add_compatibility( + prog=prog, + V=V, + b=b, + kappa_V=kappa_V, + kappa_b=kappa_b, + lagrangians=compatible_lagrangians, + rho=rho, + barrier_eps=barrier_eps, + ) + + return (prog, V, b, rho) diff --git a/tests/test_clf_cbf.py b/tests/test_clf_cbf.py index 231af5c..5f647c8 100644 --- a/tests/test_clf_cbf.py +++ b/tests/test_clf_cbf.py @@ -1,5 +1,7 @@ import compatible_clf_cbf.clf_cbf as mut +from typing import List, Tuple + import numpy as np import pytest # noqa @@ -44,16 +46,16 @@ def setup_class(cls): # test the functionality of the code. cls.f = np.array( [ - sym.Polynomial(cls.x[0] * cls.x[0]), - sym.Polynomial(cls.x[1] * cls.x[0]), + sym.Polynomial(cls.x[0] ** 3), + sym.Polynomial(-cls.x[1] * cls.x[0]), sym.Polynomial(cls.x[0] + cls.x[2]), ] ) cls.g = np.array( [ - [sym.Polynomial(cls.x[0] + cls.x[1]), sym.Polynomial()], + [sym.Polynomial(cls.x[0] ** 2 + cls.x[1] ** 2), sym.Polynomial()], [sym.Polynomial(), sym.Polynomial(cls.x[1] * cls.x[2])], - [sym.Polynomial(cls.x[0] + cls.x[2]), sym.Polynomial(cls.x[1])], + [sym.Polynomial(cls.x[0] * cls.x[2]), sym.Polynomial(cls.x[1])], ] ) cls.unsafe_regions = [ @@ -61,6 +63,22 @@ def setup_class(cls): np.array([sym.Polynomial(1 - cls.x[1]), sym.Polynomial(1 - cls.x[0])]), ] + def linearize(self) -> Tuple[np.ndarray, np.ndarray]: + """ + Linearize the dynamics at x=0 at u = 0 + """ + env = {self.x[i]: 0 for i in range(self.nx)} + for i in range(self.nx): + assert self.f[i].Evaluate(env) == 0 + A = np.empty((self.nx, self.nx)) + B = np.empty((self.nx, self.nu)) + for i in range(self.nx): + for j in range(self.nx): + A[i, j] = self.f[i].Differentiate(self.x[j]).Evaluate(env) + for j in range(self.nu): + B[i, j] = self.g[i, j].Evaluate(env) + return (A, B) + def test_constructor(self): dut = mut.CompatibleClfCbf( f=self.f, @@ -193,7 +211,7 @@ def test_search_compatible_lagrangians_w_clf_y_squared(self): with_clf=True, use_y_squared=True, ) - V = sym.Polynomial(dut.x[0] ** 2 + 4 * dut.x[1] ** 2) + V = sym.Polynomial(dut.x[0] ** 2 + 4 * dut.x[1] ** 2 + dut.x[2] ** 2) b = np.array( [ sym.Polynomial(1 - dut.x[0] ** 2 - dut.x[1] ** 2), @@ -204,17 +222,17 @@ def test_search_compatible_lagrangians_w_clf_y_squared(self): 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) + mut.CompatibleLagrangianDegrees.Degree(x=2, y=0) for _ in range(self.nu) ], - xi_y=mut.CompatibleLagrangianDegrees.Degree(x=2, y=2), + xi_y=mut.CompatibleLagrangianDegrees.Degree(x=2, y=0), y=None, - rho_minus_V=mut.CompatibleLagrangianDegrees.Degree(x=2, y=2), + rho_minus_V=mut.CompatibleLagrangianDegrees.Degree(x=4, y=2), b_plus_eps=[ - mut.CompatibleLagrangianDegrees.Degree(x=2, y=2) + mut.CompatibleLagrangianDegrees.Degree(x=4, y=2) for _ in range(len(self.unsafe_regions)) ], ) - rho = 0.1 + rho = 0.001 barrier_eps = np.array([0.01, 0.02]) prog, lagrangians = dut.construct_search_compatible_lagrangians( @@ -345,3 +363,120 @@ def test_certify_cbf_unsafe_region(self): assert utils.is_sos(lagrangians.cbf) for i in range(dut.unsafe_regions[0].size): assert utils.is_sos(lagrangians.unsafe_region[i]) + + +class TestClfCbfToy: + """ + In this test, we construct a dynamical system that we know how to compute + compatible CLF/CBF as initial guess. This class is used for testing + searching the CLF/CBF starting from that initial guess. + """ + + @classmethod + def setup_class(cls): + cls.nx = 2 + cls.nu = 1 + cls.x = sym.MakeVectorContinuousVariable(2, "x") + cls.f = np.array( + [ + sym.Polynomial(), + sym.Polynomial(-cls.x[0] - 1.0 / 6 * cls.x[0] ** 3), + ] + ) + cls.g = np.array([[sym.Polynomial(1)], [sym.Polynomial(-1)]]) + + cls.unsafe_regions = [np.array([sym.Polynomial(cls.x[0] + 10)])] + + cls.kappa_V = 0.001 + cls.kappa_b = np.array([cls.kappa_V]) + cls.barrier_eps = np.array([0.01]) + + def search_lagrangians( + self, + ) -> Tuple[ + mut.CompatibleClfCbf, + mut.CompatibleLagrangians, + List[mut.UnsafeRegionLagrangians], + ]: + 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=use_y_squared, + ) + V_init = sym.Polynomial(self.x[0] ** 2 + self.x[1] ** 2) + b_init = np.array([sym.Polynomial(0.001 - self.x[0] ** 2 - self.x[1] ** 2)]) + + lagrangian_degrees = mut.CompatibleLagrangianDegrees( + lambda_y=[mut.CompatibleLagrangianDegrees.Degree(x=3, y=0)], + xi_y=mut.CompatibleLagrangianDegrees.Degree(x=0, y=0), + y=None, + rho_minus_V=mut.CompatibleLagrangianDegrees.Degree(x=2, y=0), + b_plus_eps=[mut.CompatibleLagrangianDegrees.Degree(x=2, y=0)], + ) + rho = 0.01 + + ( + compatible_prog, + compatible_lagrangians, + ) = dut.construct_search_compatible_lagrangians( + V_init, + b_init, + self.kappa_V, + self.kappa_b, + lagrangian_degrees, + rho, + self.barrier_eps, + ) + solver_options = solvers.SolverOptions() + solver_options.SetOption(solvers.CommonSolverOption.kPrintToConsole, 1) + solver = solvers.ClarabelSolver() + compatible_result = solver.Solve(compatible_prog, None, solver_options) + assert compatible_result.is_success() + + compatible_lagrangians_result = compatible_lagrangians.get_result( + compatible_result, coefficient_tol=1e-5 + ) + + unsafe_lagrangians = [ + dut.certify_cbf_unsafe_region( + unsafe_region_index=0, + cbf=b_init[0], + cbf_lagrangian_degree=0, + unsafe_region_lagrangian_degrees=[0], + solver_options=None, + ) + ] + + return dut, compatible_lagrangians_result, unsafe_lagrangians + + def test_search_clf_cbf(self): + ( + dut, + compatible_lagrangians, + unsafe_lagrangians, + ) = self.search_lagrangians() + prog, V, b, rho = dut._construct_search_clf_cbf_program( + compatible_lagrangians, + unsafe_lagrangians, + clf_degree=2, + cbf_degrees=[2], + x_equilibrium=np.array([0, 0.0]), + kappa_V=self.kappa_V, + kappa_b=self.kappa_b, + barrier_eps=self.barrier_eps, + ) + solver_options = solvers.SolverOptions() + solver_options.SetOption(solvers.CommonSolverOption.kPrintToConsole, 1) + solver = solvers.ClarabelSolver() + result = solver.Solve(prog, None, solver_options) + assert result.is_success() + V_result = result.GetSolution(V) + 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()