From 1eaffa06cd10d12c521886cc24a12e3567a9adf3 Mon Sep 17 00:00:00 2001 From: Hongkai Dai Date: Mon, 22 Jan 2024 23:51:07 -0800 Subject: [PATCH] Find CLF and CBF separately for single integrator. (#41) --- compatible_clf_cbf/cbf.py | 65 ++++++++- compatible_clf_cbf/clf.py | 14 +- compatible_clf_cbf/clf_cbf.py | 19 ++- .../single_integrator/wo_input_limit_demo.py | 124 ++++++++++++++++++ tests/test_cbf.py | 6 +- 5 files changed, 211 insertions(+), 17 deletions(-) create mode 100644 examples/single_integrator/wo_input_limit_demo.py diff --git a/compatible_clf_cbf/cbf.py b/compatible_clf_cbf/cbf.py index 39354e8..1eb9052 100644 --- a/compatible_clf_cbf/cbf.py +++ b/compatible_clf_cbf/cbf.py @@ -2,7 +2,7 @@ Certify and search Control Barrier Function (CBF) through sum-of-squares optimization. """ from dataclasses import dataclass -from typing import List, Optional, Union +from typing import List, Optional, Tuple, Union from typing_extensions import Self import numpy as np @@ -13,9 +13,13 @@ check_array_of_polynomials, get_polynomial_result, new_sos_polynomial, + solve_with_id, ) -from compatible_clf_cbf.clf_cbf import UnsafeRegionLagrangians +from compatible_clf_cbf.clf_cbf import ( + UnsafeRegionLagrangians, + UnsafeRegionLagrangianDegrees, +) @dataclass @@ -256,6 +260,61 @@ def __init__( check_array_of_polynomials(state_eq_constraints, self.x_set) self.state_eq_constraints = state_eq_constraints + def search_lagrangians_given_cbf( + self, + b: sym.Polynomial, + eps: float, + kappa: float, + cbf_derivative_lagrangian_degrees: Union[ + CbfWInputLimitLagrangianDegrees, CbfWoInputLimitLagrangianDegrees + ], + unsafe_region_lagrangian_degrees: UnsafeRegionLagrangianDegrees, + solver_id: Optional[solvers.SolverId] = None, + solver_options: Optional[solvers.SolverOptions] = None, + lagrangian_coefficient_tol: Optional[float] = None, + ) -> Tuple[ + Optional[Union[CbfWInputLimitLagrangian, CbfWoInputLimitLagrangian]], + Optional[UnsafeRegionLagrangians], + ]: + """ + For a given CBF candidate, certify the CBF conditions by finding the + Lagrangian multipliers. + """ + prog_unsafe = solvers.MathematicalProgram() + prog_unsafe.AddIndeterminates(self.x_set) + unsafe_lagrangians = unsafe_region_lagrangian_degrees.to_lagrangians( + prog_unsafe, self.x_set + ) + self._add_barrier_safe_constraint(prog_unsafe, b, unsafe_lagrangians) + result_unsafe = solve_with_id(prog_unsafe, solver_id, solver_options) + if result_unsafe.is_success(): + unsafe_lagrangians_result = unsafe_lagrangians.get_result( + result_unsafe, lagrangian_coefficient_tol + ) + else: + unsafe_lagrangians_result = None + + prog_cbf_derivative = solvers.MathematicalProgram() + prog_cbf_derivative.AddIndeterminates(self.x_set) + cbf_derivative_lagrangians = cbf_derivative_lagrangian_degrees.to_lagrangians( + prog_cbf_derivative, self.x_set + ) + self._add_cbf_derivative_condition( + prog_cbf_derivative, b, cbf_derivative_lagrangians, eps, kappa + ) + result_cbf_derivative = solve_with_id( + prog_cbf_derivative, solver_id, solver_options + ) + + cbf_derivative_lagrangians_result = ( + cbf_derivative_lagrangians.get_result( + result_cbf_derivative, lagrangian_coefficient_tol + ) + if result_cbf_derivative.is_success() + else None + ) + return cbf_derivative_lagrangians_result, unsafe_lagrangians_result + def _add_barrier_safe_constraint( self, prog: solvers.MathematicalProgram, @@ -292,7 +351,7 @@ def _add_barrier_safe_constraint( prog.AddSosConstraint(poly) return poly - def _add_cbf_condition( + def _add_cbf_derivative_condition( self, prog: solvers.MathematicalProgram, b: sym.Polynomial, diff --git a/compatible_clf_cbf/clf.py b/compatible_clf_cbf/clf.py index 9d23b3f..8373b90 100644 --- a/compatible_clf_cbf/clf.py +++ b/compatible_clf_cbf/clf.py @@ -190,7 +190,7 @@ class ControlLyapunov: By positivestellasatz, this means that there exists λ₀(x), λ₁(x), λ₂(x) satisfying -(1+λ₀(x))*(∂V/∂x*f(x)+κ*V(x))−λ₁(x)*∂V/∂x*g(x)−λ₂(x)*(ρ−V(x)) is sos. - λ₀(x), λ₁(x), λ₂(x) are sos. + λ₀(x), λ₂(x) are sos. If the set 𝒰 is a polytope with vertices u₁,..., uₙ, then V is an CLF iff (V(x)-ρ)xᵀx is always non-negative on the semi-algebraic set @@ -268,7 +268,7 @@ def search_lagrangian_given_clf( solver_id: Optional[solvers.SolverId] = None, solver_options: Optional[solvers.SolverOptions] = None, coefficient_tol: Optional[float] = None, - ) -> Union[ClfWInputLimitLagrangian, ClfWoInputLimitLagrangian]: + ) -> Optional[Union[ClfWInputLimitLagrangian, ClfWoInputLimitLagrangian]]: prog = solvers.MathematicalProgram() prog.AddIndeterminates(self.x_set) lagrangians = lagrangian_degrees.to_lagrangians( @@ -276,8 +276,12 @@ def search_lagrangian_given_clf( ) self._add_clf_condition(prog, V, lagrangians, rho, kappa) result = solve_with_id(prog, solver_id, solver_options) - assert result.is_success() - return lagrangians.get_result(result, coefficient_tol) + lagrangians_result = ( + lagrangians.get_result(result, coefficient_tol) + if result.is_success() + else None + ) + return lagrangians_result def construct_search_clf_given_lagrangian( self, @@ -323,7 +327,7 @@ def _add_clf_condition( assert isinstance(lagrangians, ClfWoInputLimitLagrangian) sos_poly = ( -(1 + lagrangians.dVdx_times_f) * (dVdx_times_f + kappa * V) - - dVdx_times_g.squeeze().dot(lagrangians.dVdx_times_g) + - dVdx_times_g.reshape((-1,)).dot(lagrangians.dVdx_times_g) - lagrangians.rho_minus_V * (rho - V) ) else: diff --git a/compatible_clf_cbf/clf_cbf.py b/compatible_clf_cbf/clf_cbf.py index 29f4333..d0dad22 100644 --- a/compatible_clf_cbf/clf_cbf.py +++ b/compatible_clf_cbf/clf_cbf.py @@ -210,16 +210,20 @@ class UnsafeRegionLagrangians: # (such as sin²θ+cos²θ=1) state_eq_constraints: Optional[np.ndarray] - def get_result(self, result: solvers.MathematicalProgramResult) -> Self: + def get_result( + self, + result: solvers.MathematicalProgramResult, + coefficient_tol: Optional[float], + ) -> Self: return UnsafeRegionLagrangians( - cbf=result.GetSolution(self.cbf), - unsafe_region=np.array( - [result.GetSolution(phi) for phi in self.unsafe_region] + cbf=get_polynomial_result(result, self.cbf, coefficient_tol), + unsafe_region=get_polynomial_result( + result, self.unsafe_region, coefficient_tol ), state_eq_constraints=None if self.state_eq_constraints is None - else np.array( - [result.GetSolution(poly) for poly in self.state_eq_constraints] + else get_polynomial_result( + result, self.state_eq_constraints, coefficient_tol ), ) @@ -385,6 +389,7 @@ def certify_cbf_unsafe_region( cbf: sym.Polynomial, lagrangian_degrees: UnsafeRegionLagrangianDegrees, solver_options: Optional[solvers.SolverOptions] = None, + lagrangian_coefficient_tol: Optional[float] = None, ) -> UnsafeRegionLagrangians: """ Certifies that the 0-superlevel set {x | bᵢ(x) >= 0} does not intersect @@ -408,7 +413,7 @@ def certify_cbf_unsafe_region( self._add_barrier_safe_constraint(prog, unsafe_region_index, cbf, lagrangians) result = solvers.Solve(prog, None, solver_options) assert result.is_success() - return lagrangians.get_result(result) + return lagrangians.get_result(result, lagrangian_coefficient_tol) def construct_search_compatible_lagrangians( self, diff --git a/examples/single_integrator/wo_input_limit_demo.py b/examples/single_integrator/wo_input_limit_demo.py new file mode 100644 index 0000000..a953faa --- /dev/null +++ b/examples/single_integrator/wo_input_limit_demo.py @@ -0,0 +1,124 @@ +""" +Find the CLF and CBF jointly, versus finding CLF and CBF separately for a +2-dimensional single-integrator system xdot = u. I assume the obstacle is a +sphere. +""" +from typing import Tuple + +import numpy as np + +import pydrake.symbolic as sym + +from compatible_clf_cbf.clf import ControlLyapunov, ClfWoInputLimitLagrangianDegrees +from compatible_clf_cbf.cbf import ControlBarrier, CbfWoInputLimitLagrangianDegrees +from compatible_clf_cbf.clf_cbf import UnsafeRegionLagrangianDegrees + + +def affine_dynamics() -> Tuple[np.ndarray, np.ndarray]: + """ + Return the dynamics as f(x) + g(x)*u + """ + f = np.array([sym.Polynomial(0), sym.Polynomial(0)]) + g = np.array( + [[sym.Polynomial(1), sym.Polynomial(0)], [sym.Polynomial(0), sym.Polynomial(1)]] + ) + return f, g + + +def find_clf_cbf_separately( + x: np.ndarray, obstacle_center: np.ndarray, obstacle_radius: float +) -> Tuple[sym.Polynomial, sym.Polynomial]: + assert x.shape == (2,) + clf = sym.Polynomial(x[0] ** 2 + x[1] ** 2) + assert obstacle_center.shape == (2,) + cbf = sym.Polynomial( + (x[0] - obstacle_center[0]) ** 2 + + (x[1] - obstacle_center[1]) ** 2 + - obstacle_radius**2 + ) + return clf, cbf + + +def certify_clf_cbf_separately( + x: np.ndarray, + clf: sym.Polynomial, + cbf: sym.Polynomial, + rho: float, + kappa_V: float, + kappa_b: float, + barrier_eps: float, + obstacle_center: np.ndarray, + obstacle_radius: float, +): + f, g = affine_dynamics() + control_lyapunov = ControlLyapunov( + f=f, + g=g, + x=x, + x_equilibrium=np.zeros(2), + u_vertices=None, + state_eq_constraints=None, + ) + clf_lagrangian = control_lyapunov.search_lagrangian_given_clf( + clf, + rho, + kappa_V, + ClfWoInputLimitLagrangianDegrees( + dVdx_times_f=0, + dVdx_times_g=[1, 1], + rho_minus_V=0, + state_eq_constraints=None, + ), + ) + assert clf_lagrangian is not None + + control_barrier = ControlBarrier( + f=f, + g=g, + x=x, + unsafe_region=np.array( + [ + sym.Polynomial( + (x[0] - obstacle_center[0]) ** 2 + + (x[1] - obstacle_center[1]) ** 2 + - obstacle_radius**2 + ) + ] + ), + u_vertices=None, + state_eq_constraints=None, + ) + ( + cbf_derivative_lagrangians, + unsafe_lagrangians, + ) = control_barrier.search_lagrangians_given_cbf( + cbf, + barrier_eps, + kappa_b, + CbfWoInputLimitLagrangianDegrees( + dbdx_times_f=0, dbdx_times_g=[1, 1], b_plus_eps=0, state_eq_constraints=None + ), + UnsafeRegionLagrangianDegrees( + cbf=0, unsafe_region=[0], state_eq_constraints=None + ), + ) + assert cbf_derivative_lagrangians is not None + assert unsafe_lagrangians is not None + + +def main(): + x = sym.MakeVectorContinuousVariable(2, "x") + obstacle_center = np.array([1.0, 0.0]) + obstacle_radius = 0.5 + V, b = find_clf_cbf_separately(x, obstacle_center, obstacle_radius) + rho = 10 + kappa_V = 0.01 + kappa_b = 0.01 + barrier_eps = 0.0 + certify_clf_cbf_separately( + x, V, b, rho, kappa_V, kappa_b, barrier_eps, obstacle_center, obstacle_radius + ) + + +if __name__ == "__main__": + main() diff --git a/tests/test_cbf.py b/tests/test_cbf.py index 236961b..f6c8a7f 100644 --- a/tests/test_cbf.py +++ b/tests/test_cbf.py @@ -89,7 +89,7 @@ def test_add_barrier_safe_constraint(self): ) assert poly.CoefficientsAlmostEqual(poly_expected, 1e-8) - def test_add_cbf_condition(self): + def test_add_cbf_derivative_condition(self): dut = mut.ControlBarrier( f=self.f, g=self.g, @@ -112,7 +112,9 @@ def test_add_cbf_condition(self): lagrangians = lagrangian_degrees.to_lagrangians(prog, dut.x_set) eps = 0.01 kappa = 0.001 - sos_poly = dut._add_cbf_condition(prog, b_init, lagrangians, eps, kappa) + sos_poly = dut._add_cbf_derivative_condition( + prog, b_init, lagrangians, eps, kappa + ) result = solvers.Solve(prog) assert result.is_success()