From 37a6a1e7c2e1ff89146c81a0dab4fb7cee58fc76 Mon Sep 17 00:00:00 2001 From: Hongkai Dai Date: Sat, 27 Jan 2024 17:04:58 -0800 Subject: [PATCH] Add CompatibleStateOptions to encourage growing the compatible region to cover certain states. --- .flake8 | 2 + .gitignore | 1 + compatible_clf_cbf/cbf.py | 1 + compatible_clf_cbf/clf.py | 1 + compatible_clf_cbf/clf_cbf.py | 188 +++++++++++++++--- compatible_clf_cbf/ellipsoid_utils.py | 17 ++ examples/linear_toy/linear_toy_demo.py | 14 +- .../nonlinear_toy/wo_input_limits_demo.py | 15 +- .../single_integrator/wo_input_limit_demo.py | 1 + tests/test_clf_cbf.py | 121 +++++++++-- 10 files changed, 305 insertions(+), 56 deletions(-) diff --git a/.flake8 b/.flake8 index 2bcd70e..122739a 100644 --- a/.flake8 +++ b/.flake8 @@ -1,2 +1,4 @@ [flake8] max-line-length = 88 +exclude = + venv diff --git a/.gitignore b/.gitignore index 7b1d1d3..808faa5 100644 --- a/.gitignore +++ b/.gitignore @@ -1,2 +1,3 @@ *.pyc compatible_clf_cbf.egg-info/* +venv/* diff --git a/compatible_clf_cbf/cbf.py b/compatible_clf_cbf/cbf.py index 1eb9052..a776fc4 100644 --- a/compatible_clf_cbf/cbf.py +++ b/compatible_clf_cbf/cbf.py @@ -1,6 +1,7 @@ """ Certify and search Control Barrier Function (CBF) through sum-of-squares optimization. """ + from dataclasses import dataclass from typing import List, Optional, Tuple, Union from typing_extensions import Self diff --git a/compatible_clf_cbf/clf.py b/compatible_clf_cbf/clf.py index 408daac..5bd4872 100644 --- a/compatible_clf_cbf/clf.py +++ b/compatible_clf_cbf/clf.py @@ -1,6 +1,7 @@ """ Certify and search Control Lyapunov function (CLF) through sum-of-squares optimization. """ + from dataclasses import dataclass from typing import List, Optional, Tuple, Union from typing_extensions import Self diff --git a/compatible_clf_cbf/clf_cbf.py b/compatible_clf_cbf/clf_cbf.py index dfcd9eb..6a25967 100644 --- a/compatible_clf_cbf/clf_cbf.py +++ b/compatible_clf_cbf/clf_cbf.py @@ -220,10 +220,12 @@ def get_result( unsafe_region=get_polynomial_result( result, self.unsafe_region, coefficient_tol ), - state_eq_constraints=None - if self.state_eq_constraints is None - else get_polynomial_result( - result, self.state_eq_constraints, coefficient_tol + state_eq_constraints=( + None + if self.state_eq_constraints is None + else get_polynomial_result( + result, self.state_eq_constraints, coefficient_tol + ) ), ) @@ -263,6 +265,123 @@ def to_lagrangians( ) +@dataclass +class CompatibleStatesOptions: + """ + This option is used to encourage the compatible region to include certain + candidate states. Namely b(x_candidate) >= 0 and V(x_candidate) <= 1. + """ + + candidate_compatible_states: np.ndarray + # To avoid arbitrarily scaling the CBF, we need to impose the + # constraint that + # b_anchor_bounds[i][0] <= b[i](anchor_states) <= b_anchor_bounds[i][1] + anchor_states: Optional[np.ndarray] + b_anchor_bounds: Optional[List[Tuple[np.ndarray, np.ndarray]]] + + # To encourage the compatible region to cover the candidate states, we add + # this cost + # weight_V * ReLU(V(x_candidates) - 1) + # + weight_b[i] * ReLU(-b[i](x_candidates)) + weight_V: Optional[float] + weight_b: np.ndarray + + def add_cost( + self, + prog: solvers.MathematicalProgram, + x: np.ndarray, + V: Optional[sym.Polynomial], + b: np.ndarray, + ) -> Tuple[solvers.Binding[solvers.LinearCost], Optional[np.ndarray], np.ndarray]: + """ + Adds the cost + weight_V * ReLU(V(x_candidates) - 1) + + weight_b[i] * ReLU(-b[i](x_candidates)) + """ + assert b.shape == self.weight_b.shape + num_candidates = self.candidate_compatible_states.shape[0] + if V is not None: + # Add the slack variable representing ReLU(V(x_candidates)-1) + V_relu = prog.NewContinuousVariables(num_candidates, "V_relu") + prog.AddBoundingBoxConstraint(0, np.inf, V_relu) + # Now evaluate V(x_candidates) as A_v * V_decision_vars + b_v + A_v, V_decision_vars, b_v = V.EvaluateWithAffineCoefficients( + x, self.candidate_compatible_states.T + ) + # Now impose the constraint V_relu >= V(x_candidates) - 1 as + # V_relu - A_v * V_decision_vars >= b_v -1 + prog.AddLinearConstraint( + np.concatenate((-A_v, np.eye(num_candidates)), axis=1), + b_v - 1, + np.full_like(b_v, np.inf), + np.concatenate((V_decision_vars, V_relu)), + ) + else: + V_relu = None + # Add the slack variable b_relu[i] representing ReLU(-b[i](x_candidates)) + b_relu = prog.NewContinuousVariables(b.shape[0], num_candidates, "b_relu") + prog.AddBoundingBoxConstraint(0, np.inf, b_relu.reshape((-1,))) + for i in range(b.shape[0]): + A_b, b_decision_vars, b_b = b[i].EvaluateWithAffineCoefficients( + x, self.candidate_compatible_states.T + ) + # Now impose the constraint b_relu[i] >= -b[i](x_candidates) as + # A_b * b_decision_vars + b_relu[i] >= - b_b + prog.AddLinearConstraint( + np.concatenate((A_b, np.eye(num_candidates)), axis=1), + -b_b, + np.full_like(b_b, np.inf), + np.concatenate((b_decision_vars, b_relu[i])), + ) + + cost_coeff = (self.weight_b.reshape((-1, 1)) * np.ones_like(b_relu)).reshape( + (-1,) + ) + cost_vars = b_relu.reshape((-1,)) + if V is not None: + assert self.weight_V is not None + cost_coeff = np.concatenate( + (cost_coeff, self.weight_V * np.ones(num_candidates)) + ) + assert V_relu is not None + cost_vars = np.concatenate((cost_vars, V_relu)) + cost = prog.AddLinearCost(cost_coeff, 0.0, cost_vars) + return cost, V_relu, b_relu + + def add_constraint( + self, prog: solvers.MathematicalProgram, x: np.ndarray, b: np.ndarray + ) -> Optional[List[solvers.Binding[solvers.LinearConstraint]]]: + """ + Add the constraint + b_anchor_bounds[i][0] <= b[i](anchor_states) <= b_anchor_bounds[i][1] + """ + if self.b_anchor_bounds is not None: + assert b.shape == (len(self.b_anchor_bounds),) + assert self.anchor_states is not None + constraints: List[solvers.Binding[solvers.LinearConstraint]] = [None] * len( + self.b_anchor_bounds + ) + for i in range(len(self.b_anchor_bounds)): + assert ( + self.b_anchor_bounds[i][0].size + == self.b_anchor_bounds[i][1].size + == self.anchor_states.shape[0] + ) + # Evaluate b[i](anchor_states) as A_b * decision_vars_b + b_b + A_b, decision_vars_b, b_b = b[i].EvaluateWithAffineCoefficients( + x, self.anchor_states.T + ) + # Adds the constraint + constraints[i] = prog.AddLinearConstraint( + A_b, + self.b_anchor_bounds[i][0] - b_b, + self.b_anchor_bounds[i][1] - b_b, + decision_vars_b, + ) + return constraints + return None + + class CompatibleClfCbf: """ Certify and synthesize compatible Control Lyapunov Function (CLF) and @@ -518,9 +637,7 @@ def search_clf_cbf_given_lagrangian( kappa_V: Optional[float], kappa_b: np.ndarray, barrier_eps: np.ndarray, - S_ellipsoid_inner: np.ndarray, - b_ellipsoid_inner: np.ndarray, - c_ellipsoid_inner: float, + ellipsoid_inner: Optional[ellipsoid_utils.Ellipsoid], solver_id: Optional[solvers.SolverId] = None, solver_options: Optional[solvers.SolverOptions] = None, ) -> Tuple[ @@ -548,9 +665,10 @@ def search_clf_cbf_given_lagrangian( barrier_eps, ) - self._add_ellipsoid_in_compatible_region_constraint( - prog, V, b, S_ellipsoid_inner, b_ellipsoid_inner, c_ellipsoid_inner - ) + if ellipsoid_inner is not None: + self._add_ellipsoid_in_compatible_region_constraint( + prog, V, b, ellipsoid_inner.S, ellipsoid_inner.b, ellipsoid_inner.c + ) result = solve_with_id(prog, solver_id, solver_options) if result.is_success(): @@ -571,9 +689,7 @@ def binary_search_clf_cbf( kappa_V: Optional[float], kappa_b: np.ndarray, barrier_eps: np.ndarray, - S_ellipsoid_inner: np.ndarray, - b_ellipsoid_inner: np.ndarray, - c_ellipsoid_inner: float, + ellipsoid_inner: ellipsoid_utils.Ellipsoid, scale_min: float, scale_max: float, scale_tol: float, @@ -605,7 +721,7 @@ def search( solvers.MathematicalProgramResult, ]: c_new = ellipsoid_utils.scale_ellipsoid( - S_ellipsoid_inner, b_ellipsoid_inner, c_ellipsoid_inner, scale + ellipsoid_inner.S, ellipsoid_inner.b, ellipsoid_inner.c, scale ) V, b, result = self.search_clf_cbf_given_lagrangian( compatible_lagrangians, @@ -616,9 +732,7 @@ def search( kappa_V, kappa_b, barrier_eps, - S_ellipsoid_inner, - b_ellipsoid_inner, - c_new, + ellipsoid_utils.Ellipsoid(ellipsoid_inner.S, ellipsoid_inner.b, c_new), solver_id, solver_options, ) @@ -789,9 +903,9 @@ def bilinear_alternation( kappa_V, kappa_b, barrier_eps, - S_ellipsoid_inner, - b_ellipsoid_inner, - c_ellipsoid_inner, + ellipsoid_utils.Ellipsoid( + S_ellipsoid_inner, b_ellipsoid_inner, c_ellipsoid_inner + ), binary_search_scale_min, binary_search_scale_max, binary_search_scale_tol, @@ -1044,7 +1158,11 @@ def _construct_search_clf_cbf_program( kappa_b: np.ndarray, barrier_eps: np.ndarray, local_clf: bool = True, - ) -> Tuple[solvers.MathematicalProgram, Optional[sym.Polynomial], np.ndarray,]: + ) -> Tuple[ + solvers.MathematicalProgram, + Optional[sym.Polynomial], + np.ndarray, + ]: """ Construct a program to search for compatible CLF/CBFs given the Lagrangians. Notice that we have not imposed the cost to the program yet. @@ -1275,12 +1393,14 @@ def _get_V_contain_ellipsoid_lagrangian_degree( else: return ContainmentLagrangianDegree( inner_ineq=[-1], - inner_eq=[] - if self.state_eq_constraints is None - else [ - np.maximum(0, V.TotalDegree() - poly.TotalDegree()) - for poly in self.state_eq_constraints - ], + inner_eq=( + [] + if self.state_eq_constraints is None + else [ + np.maximum(0, V.TotalDegree() - poly.TotalDegree()) + for poly in self.state_eq_constraints + ] + ), outer=0, ) @@ -1290,12 +1410,14 @@ def _get_b_contain_ellipsoid_lagrangian_degrees( return [ ContainmentLagrangianDegree( inner_ineq=[-1], - inner_eq=[] - if self.state_eq_constraints is None - else [ - np.maximum(0, b_i.TotalDegree() - poly.TotalDegree()) - for poly in self.state_eq_constraints - ], + inner_eq=( + [] + if self.state_eq_constraints is None + else [ + np.maximum(0, b_i.TotalDegree() - poly.TotalDegree()) + for poly in self.state_eq_constraints + ] + ), outer=0, ) for b_i in b diff --git a/compatible_clf_cbf/ellipsoid_utils.py b/compatible_clf_cbf/ellipsoid_utils.py index 07576a5..bf62e7a 100644 --- a/compatible_clf_cbf/ellipsoid_utils.py +++ b/compatible_clf_cbf/ellipsoid_utils.py @@ -367,3 +367,20 @@ def draw_ellipsoid2d( + d.reshape((-1, 1)) ) return ax.plot(xy_pts[0], xy_pts[1], **kwargs) + + +class Ellipsoid: + """ + An ellipsoid as + {x | xᵀSx+bᵀx+c≤0} + """ + + def __init__(self, S: np.ndarray, b: np.ndarray, c: float): + assert S.shape[0] == S.shape[1] + assert b.shape == (S.shape[0],) + self.S = (S + S.T) / 2 + self.b = b + self.c = c + + def to_affine_ball(self) -> Tuple[np.ndarray, np.ndarray]: + return to_affine_ball(self.S, self.b, self.c) diff --git a/examples/linear_toy/linear_toy_demo.py b/examples/linear_toy/linear_toy_demo.py index 83546c6..dd7a2c5 100644 --- a/examples/linear_toy/linear_toy_demo.py +++ b/examples/linear_toy/linear_toy_demo.py @@ -1,6 +1,7 @@ """ We search for compatible CLF and CBF for a 2D linear system. """ + from typing import List, Tuple import numpy as np @@ -27,11 +28,14 @@ def search_compatible_lagrangians( 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) - ], + 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, state_eq_constraints=None, diff --git a/examples/nonlinear_toy/wo_input_limits_demo.py b/examples/nonlinear_toy/wo_input_limits_demo.py index a84f33b..3e4054e 100644 --- a/examples/nonlinear_toy/wo_input_limits_demo.py +++ b/examples/nonlinear_toy/wo_input_limits_demo.py @@ -1,6 +1,7 @@ """ Find the compatible CLF/CBF with no input limits. """ + import numpy as np import pydrake.solvers as solvers import pydrake.symbolic as sym @@ -31,12 +32,14 @@ def main(use_y_squared: bool): lagrangian_degrees = clf_cbf.CompatibleLagrangianDegrees( lambda_y=[clf_cbf.CompatibleLagrangianDegrees.Degree(x=3, y=0)], xi_y=clf_cbf.CompatibleLagrangianDegrees.Degree(x=2, y=0), - y=None - if use_y_squared - else [ - clf_cbf.CompatibleLagrangianDegrees.Degree(x=4, y=0) - for _ in range(compatible.y.size) - ], + y=( + None + if use_y_squared + else [ + clf_cbf.CompatibleLagrangianDegrees.Degree(x=4, y=0) + for _ in range(compatible.y.size) + ] + ), rho_minus_V=clf_cbf.CompatibleLagrangianDegrees.Degree(x=2, y=0), b_plus_eps=[clf_cbf.CompatibleLagrangianDegrees.Degree(x=2, y=0)], state_eq_constraints=None, diff --git a/examples/single_integrator/wo_input_limit_demo.py b/examples/single_integrator/wo_input_limit_demo.py index d30a4fe..68acdd7 100644 --- a/examples/single_integrator/wo_input_limit_demo.py +++ b/examples/single_integrator/wo_input_limit_demo.py @@ -3,6 +3,7 @@ 2-dimensional single-integrator system xdot = u. I assume the obstacle is a sphere. """ + from typing import Tuple import numpy as np diff --git a/tests/test_clf_cbf.py b/tests/test_clf_cbf.py index 8bf0e62..d75d210 100644 --- a/tests/test_clf_cbf.py +++ b/tests/test_clf_cbf.py @@ -37,6 +37,105 @@ def test_construct_polynomial(self): assert np.sum([monomial.degree(y[i]) for i in range(y.size)]) <= 2 +class TestCompatibleStatesOptions: + def test_add_cost(self): + dut = mut.CompatibleStatesOptions( + candidate_compatible_states=np.array([[0.2, 0.5], [-0.1, 1.2], [0.3, 2]]), + anchor_states=None, + b_anchor_bounds=None, + weight_V=1.5, + weight_b=np.array([1.2, 1.5]), + ) + prog = solvers.MathematicalProgram() + x = prog.NewIndeterminates(2, "x") + x_set = sym.Variables(x) + V = prog.NewFreePolynomial(x_set, 2) + b = np.array([prog.NewFreePolynomial(x_set, 3) for i in range(2)]) + cost, V_relu, b_relu = dut.add_cost(prog, x, V, b) + assert V_relu is not None + + def check_feasible( + V_val: sym.Polynomial, + b_val: np.ndarray, + ): + constraint1 = prog.AddEqualityConstraintBetweenPolynomials(V, V_val) + constraint2 = prog.AddEqualityConstraintBetweenPolynomials(b[0], b_val[0]) + constraint3 = prog.AddEqualityConstraintBetweenPolynomials(b[1], b_val[1]) + result = solvers.Solve(prog) + + V_relu_expected = np.maximum( + V_val.EvaluateIndeterminates(x, dut.candidate_compatible_states.T) - 1, + np.zeros(dut.candidate_compatible_states.shape[0]), + ) + b_relu_expected = np.array( + [ + np.maximum( + -b_val[i].EvaluateIndeterminates( + x, dut.candidate_compatible_states.T + ), + np.zeros(dut.candidate_compatible_states.shape[0]), + ).reshape((-1,)) + for i in range(2) + ] + ) + np.testing.assert_allclose(result.GetSolution(V_relu), V_relu_expected) + np.testing.assert_allclose(result.GetSolution(b_relu), b_relu_expected) + + cost_expected = dut.weight_V * V_relu_expected.sum() + dut.weight_b.dot( + b_relu_expected.sum(axis=1) + ) + np.testing.assert_allclose(result.get_optimal_cost(), cost_expected) + for constraints in [constraint1, constraint2, constraint3]: + for c in constraints: + prog.RemoveConstraint(c) + + check_feasible( + sym.Polynomial(2 * x[0] * x[0] + 3 * x[1] * x[0] + 2 + 2 * x[1]), + np.array( + [ + sym.Polynomial(x[0] * x[1] * x[0] + 3), + sym.Polynomial(x[0] * x[1] * x[1] - x[0] ** 3 + 2 * x[0] + 1), + ] + ), + ) + check_feasible( + sym.Polynomial(-2 * x[0] * x[0] + 3 * x[1] * x[0] + 2 + 2 * x[1]), + np.array( + [ + sym.Polynomial(x[0] * x[1] + 3), + sym.Polynomial(x[0] * x[1] * x[1] - x[0] ** 3 + 2 * x[0] + 1), + ] + ), + ) + + def test_add_constraint(self): + dut = mut.CompatibleStatesOptions( + candidate_compatible_states=np.array([[0.2, 0.5], [-0.1, 1.2], [0.3, 2]]), + anchor_states=np.array([[0.5, 0.3], [0.2, 0.1], [0.9, 2]]), + b_anchor_bounds=[(np.array([-0.5, 0.3, -3]), np.array([1, 4, 0.5]))], + weight_V=1.5, + weight_b=np.array([1.2, 1.5]), + ) + + prog = solvers.MathematicalProgram() + x = prog.NewIndeterminates(2, "x") + x_set = sym.Variables(x) + b = np.array([prog.NewFreePolynomial(x_set, 2)]) + + constraints = dut.add_constraint(prog, x, b) + assert constraints is not None + assert len(constraints) == 1 + + result = solvers.Solve(prog) + assert result.is_success() + b_result = np.array([result.GetSolution(b_i) for b_i in b]) + assert dut.anchor_states is not None + b_result_at_anchor = b_result[0].EvaluateIndeterminates(x, dut.anchor_states.T) + assert dut.b_anchor_bounds is not None + assert np.all(b_result_at_anchor >= dut.b_anchor_bounds[0][0] - 1e-6) + assert np.all(b_result_at_anchor <= dut.b_anchor_bounds[0][1] + 1e-6) + + class TestClfCbf(object): @classmethod def setup_class(cls): @@ -655,9 +754,9 @@ def test_search_clf_cbf_given_lagrangian(self): kappa_V=self.kappa_V, kappa_b=self.kappa_b, barrier_eps=self.barrier_eps, - S_ellipsoid_inner=S_ellipsoid_inner, - b_ellipsoid_inner=b_ellipsoid_inner, - c_ellipsoid_inner=c_ellipsoid_inner, + ellipsoid_inner=ellipsoid_utils.Ellipsoid( + S_ellipsoid_inner, b_ellipsoid_inner, c_ellipsoid_inner + ), ) assert result.is_success() # Check that the compatible region contains the inner_ellipsoid. @@ -710,9 +809,9 @@ def test_binary_search_clf_cbf_given_lagrangian(self): kappa_V=self.kappa_V, kappa_b=self.kappa_b, barrier_eps=self.barrier_eps, - S_ellipsoid_inner=S_ellipsoid_inner, - b_ellipsoid_inner=b_ellipsoid_inner, - c_ellipsoid_inner=c_ellipsoid_inner, + ellipsoid_inner=ellipsoid_utils.Ellipsoid( + S_ellipsoid_inner, b_ellipsoid_inner, c_ellipsoid_inner + ), scale_min=1, scale_max=50, scale_tol=0.1, @@ -782,9 +881,7 @@ def setup_class(cls): cls.kappa_b = np.array([cls.kappa_V]) cls.barrier_eps = np.array([0.01]) - def search_lagrangians( - self, check_result=False - ) -> Tuple[ + def search_lagrangians(self, check_result=False) -> Tuple[ mut.CompatibleClfCbf, mut.CompatibleLagrangians, List[mut.UnsafeRegionLagrangians], @@ -907,9 +1004,9 @@ def test_search_clf_cbf(self): kappa_V=self.kappa_V, kappa_b=self.kappa_b, barrier_eps=self.barrier_eps, - S_ellipsoid_inner=S_ellipsoid_inner, - b_ellipsoid_inner=b_ellipsoid_inner, - c_ellipsoid_inner=c_ellipsoid_inner, + ellipsoid_inner=ellipsoid_utils.Ellipsoid( + S_ellipsoid_inner, b_ellipsoid_inner, c_ellipsoid_inner + ), ) assert result.is_success() assert V is not None