diff --git a/compatible_clf_cbf/clf.py b/compatible_clf_cbf/clf.py index 5bd4872..5f9ffc4 100644 --- a/compatible_clf_cbf/clf.py +++ b/compatible_clf_cbf/clf.py @@ -9,6 +9,7 @@ import numpy as np import pydrake.solvers as solvers import pydrake.symbolic as sym +import pydrake.autodiffutils from compatible_clf_cbf.utils import ( check_array_of_polynomials, @@ -342,3 +343,129 @@ def _add_clf_condition( sos_poly -= lagrangians.state_eq_constraints.dot(self.state_eq_constraints) prog.AddSosConstraint(sos_poly) return sos_poly + + +class FindClfWoInputLimitsCounterExample: + """ + For a candidate CLF function (without input limits), find the counter-example + (that violates the CLF condition) through nonlinear optimization. + Namely we solve this problem: + max ∂V/∂x*f(x)+κ*V + s.t ∂V/∂x*g(x) = 0 + V(x) <= ρ + + This class constructs the optimization program above (but doesn't solve it). + You can access the constructed program through self.prog. + """ + + def __init__( + self, + x: np.ndarray, + f: np.ndarray, + g: np.ndarray, + V: sym.Polynomial, + rho: float, + kappa: float, + state_eq_constraints: Optional[np.ndarray], + ): + self.x = x + self.nx = x.size + self.f = f + assert f.shape == (self.nx,) + self.g = g + assert g.shape[0] == self.nx + self.nu = g.shape[1] + self.V = V + self.dVdx = V.Jacobian(self.x) + self.dVdx_times_f: sym.Polynomial = self.dVdx.dot(f) + self.J_dVdx_times_f = self.dVdx_times_f.Jacobian(self.x) + self.dVdx_times_g = (self.dVdx.reshape((1, -1)) @ self.g).reshape((-1,)) + self.J_dVdx_times_g = np.array( + [self.dVdx_times_g[i].Jacobian(self.x) for i in range(self.nu)] + ) + self.rho = rho + self.kappa = kappa + self.h = state_eq_constraints + self.h_size = 0 if self.h is None else self.h.size + self.J_h = ( + np.array([self.h[i].Jacobian(x) for i in range(self.h.size)]) + if self.h is not None + else None + ) + self.cost_poly = -(self.dVdx_times_f + self.kappa * V) + self.J_cost_poly = -(self.J_dVdx_times_f + self.kappa * self.dVdx) + + def constraint(x_eval): + if x_eval.dtype == object: + x_val = pydrake.autodiffutils.ExtractValue(x_eval) + x_grad = pydrake.autodiffutils.ExtractGradient(x_eval) + else: + x_val = x_eval + env = {x[i]: x_val[i] for i in range(self.nx)} + constraint_val = np.empty((1 + self.nu + self.h_size,)) + constraint_val[: self.nu] = np.array( + [self.dVdx_times_g[i].Evaluate(env) for i in range(self.nu)] + ) + constraint_val[self.nu] = self.V.Evaluate(env) + if self.h is not None: + constraint_val[-self.h_size :] = np.array( + [self.h[i].Evaluate(env) for i in range(self.h_size)] + ) + # Now evaluate the gradient. + if x_eval.dtype == object: + dconstraint_dx = np.zeros((constraint_val.size, self.nx)) + dconstraint_dx[: self.nu, :] = np.array( + [ + [ + self.J_dVdx_times_g[i, j].Evaluate(env) + for j in range(self.nx) + ] + for i in range(self.nu) + ] + ) + dconstraint_dx[self.nu, :] = np.array( + [self.dVdx[i].Evaluate(env) for i in range(self.nx)] + ) + if self.h is not None: + dconstraint_dx[-self.h_size :, :] = np.array( + [ + [self.J_h[i, j].Evaluate(env) for j in range(self.nx)] + for i in range(self.h_size) + ] + ) + d_constraint = dconstraint_dx @ x_grad + return pydrake.autodiffutils.InitializeAutoDiff( + constraint_val, d_constraint + ) + else: + return constraint_val + + def cost(x_eval): + if x_eval.dtype == object: + x_val = pydrake.autodiffutils.ExtractValue(x_eval) + x_grad = pydrake.autodiffutils.ExtractGradient(x_eval) + else: + x_val = x_eval + env = {x[i]: x_val[i] for i in range(self.nx)} + cost_val = self.cost_poly.Evaluate(env) + + if x_eval.dtype == object: + cost_grad = np.array( + [self.J_cost_poly[i].Evaluate(env) for i in range(self.nx)] + ) + d_cost = (cost_grad.reshape((1, -1)) @ x_grad).reshape((-1,)) + return pydrake.autodiffutils.AutoDiffXd(cost_val, d_cost) + else: + return cost_val + + self.prog = solvers.MathematicalProgram() + self.prog.AddDecisionVariables(x) + self.prog.AddCost(cost, x) + constraint_lb = np.concatenate( + (np.zeros((self.nu,)), np.array([-np.inf]), np.zeros((self.h_size,))) + ) + constraint_ub = np.concatenate( + (np.zeros((self.nu,)), np.array([self.rho]), np.zeros((self.h_size,))) + ) + + self.prog.AddConstraint(constraint, constraint_lb, constraint_ub, x) diff --git a/compatible_clf_cbf/clf_cbf.py b/compatible_clf_cbf/clf_cbf.py index 7d264ef..f6c0e02 100644 --- a/compatible_clf_cbf/clf_cbf.py +++ b/compatible_clf_cbf/clf_cbf.py @@ -266,6 +266,33 @@ def to_lagrangians( ) +@dataclass +class InnerEllipsoidOptions: + """ + This option is used to encourage the compatible region to cover an inscribed + ellipsoid. + """ + + # A state that should be contained in the inscribed ellipsoid + x_inner: np.ndarray + # when we search for the ellipsoid, we put a trust region constraint. This + # is the squared radius of that trust region. + ellipsoid_trust_region: float + # We enlarge the inner ellipsoid through a sequence of SDPs. This is the max + # number of iterations in that sequence. + find_inner_ellipsoid_max_iter: int + + def __init__( + self, + x_inner: np.ndarray, + ellipsoid_trust_region: float = 100.0, + find_inner_ellipsoid_max_iter: int = 3, + ): + self.x_inner = x_inner + self.ellipsoid_trust_region = ellipsoid_trust_region + self.find_inner_ellipsoid_max_iter = find_inner_ellipsoid_max_iter + + @dataclass class CompatibleStatesOptions: """ @@ -837,13 +864,11 @@ def bilinear_alternation( cbf_degrees: List[int], max_iter: int, *, - x_inner: np.ndarray, solver_id: Optional[solvers.SolverId] = None, solver_options: Optional[solvers.SolverOptions] = None, lagrangian_coefficient_tol: Optional[float] = None, - ellipsoid_trust_region: Optional[float] = 100, + inner_ellipsoid_options: Optional[InnerEllipsoidOptions] = None, binary_search_scale_options: Optional[BinarySearchOptions] = None, - find_inner_ellipsoid_max_iter: int = 3, compatible_states_options: Optional[CompatibleStatesOptions] = None, backoff_scale: Optional[float] = None, ) -> Tuple[Optional[sym.Polynomial], np.ndarray]: @@ -860,16 +885,18 @@ def bilinear_alternation( - Expand the compatible region to cover some candidate states. Args: - x_inner: Each row of x_inner is a state that should be contained in - the compatible region. max_iter: The maximal number of bilinear alternation iterations. lagrangian_coefficient_tol: We remove the coefficients whose absolute value is smaller than this tolerance in the Lagrangian polynomials. Use None to preserve all coefficients. - ellipsoid_trust_region: when we search for the ellipsoid, we put a - trust region constraint. This is the squared radius of that trust - region. """ + + # One and only one of inner_ellipsoid_options and compatible_states_options is None. + assert ( + inner_ellipsoid_options is not None and compatible_states_options is None + ) or (inner_ellipsoid_options is None and compatible_states_options is not None) + if inner_ellipsoid_options is not None: + assert binary_search_scale_options is not None assert isinstance(binary_search_scale_options, Optional[BinarySearchOptions]) assert isinstance(compatible_states_options, Optional[CompatibleStatesOptions]) @@ -904,7 +931,7 @@ def bilinear_alternation( assert compatible_lagrangians is not None assert all(unsafe_lagrangians) - if find_inner_ellipsoid_max_iter > 0: + if inner_ellipsoid_options is not None: # We use the heuristics to grow the inner ellipsoid. assert compatible_states_options is None # Search for the inner ellipsoid. @@ -923,10 +950,11 @@ def bilinear_alternation( cbf, V_contain_ellipsoid_lagrangian_degree, b_contain_ellipsoid_lagrangian_degree, - x_inner, + inner_ellipsoid_options.x_inner, solver_id=solver_id, - max_iter=find_inner_ellipsoid_max_iter, - trust_region=ellipsoid_trust_region, + solver_options=solver_options, + max_iter=inner_ellipsoid_options.find_inner_ellipsoid_max_iter, + trust_region=inner_ellipsoid_options.ellipsoid_trust_region, ) assert binary_search_scale_options is not None @@ -1144,6 +1172,7 @@ def _add_compatibility( xi, lambda_mat = self._calc_xi_Lambda( V=V, b=b, kappa_V=kappa_V, kappa_b=kappa_b ) + # This is just polynomial 1. poly_one = sym.Polynomial(sym.Monomial()) poly = -poly_one @@ -1155,7 +1184,6 @@ def _add_compatibility( poly -= lagrangians.lambda_y.dot(lambda_y) # Compute s₁(x, y)(ξ(x)ᵀy+1) - # This is just polynomial 1. if self.use_y_squared: xi_y = xi.dot(self.y_squared_poly) + poly_one else: @@ -1321,6 +1349,7 @@ def _find_max_inner_ellipsoid( max_iter: int = 10, convergence_tol: float = 1e-3, solver_id: Optional[solvers.SolverId] = None, + solver_options: Optional[solvers.SolverOptions] = None, trust_region: Optional[float] = None, ) -> Tuple[np.ndarray, np.ndarray, float]: """ @@ -1401,6 +1430,7 @@ def _find_max_inner_ellipsoid( max_iter, convergence_tol, solver_id, + solver_options, trust_region, ) return (S_sol, b_sol, c_sol) diff --git a/compatible_clf_cbf/ellipsoid_utils.py b/compatible_clf_cbf/ellipsoid_utils.py index bf62e7a..7c25926 100644 --- a/compatible_clf_cbf/ellipsoid_utils.py +++ b/compatible_clf_cbf/ellipsoid_utils.py @@ -120,6 +120,7 @@ def maximize_inner_ellipsoid_sequentially( max_iter: int = 10, convergence_tol: float = 1e-3, solver_id: Optional[solvers.SolverId] = None, + solver_options: Optional[solvers.SolverOptions] = None, trust_region: Optional[float] = None, ) -> Tuple[np.ndarray, np.ndarray, float]: """ @@ -173,8 +174,10 @@ def volume(S: np.ndarray, b: np.ndarray, c: float) -> float: trust_region_constraint = _add_ellipsoid_trust_region( prog, S, b, c, S_bar, b_bar, c_bar, trust_region ) - result = utils.solve_with_id(prog, solver_id, solver_options=None) - assert result.is_success() + result = utils.solve_with_id(prog, solver_id, solver_options) + assert ( + result.is_success() + ), f"Fails in iter={i} with {result.get_solution_result()}" S_result = result.GetSolution(S) b_result = result.GetSolution(b) c_result = result.GetSolution(c) diff --git a/examples/nonlinear_toy/synthesize_demo.py b/examples/nonlinear_toy/synthesize_demo.py index a46cfb6..b0843d3 100644 --- a/examples/nonlinear_toy/synthesize_demo.py +++ b/examples/nonlinear_toy/synthesize_demo.py @@ -67,6 +67,7 @@ def main(with_u_bound: bool): solver_options = solvers.SolverOptions() solver_options.SetOption(solvers.CommonSolverOption.kPrintToConsole, 0) solver_options.SetOption(solvers.ClarabelSolver.id(), "max_iter", 10000) + inner_ellipsoid_options = None V, b = compatible.bilinear_alternation( V_init, @@ -80,9 +81,8 @@ def main(with_u_bound: bool): clf_degree, cbf_degrees, max_iter, - x_inner=x_equilibrium, + inner_ellipsoid_options=None, binary_search_scale_options=None, - find_inner_ellipsoid_max_iter=0, compatible_states_options=compatible_states_options, solver_options=solver_options, backoff_scale=0.02, diff --git a/examples/nonlinear_toy/wo_input_limits_trigpoly_demo.py b/examples/nonlinear_toy/wo_input_limits_trigpoly_demo.py index 5e426ea..c516658 100644 --- a/examples/nonlinear_toy/wo_input_limits_trigpoly_demo.py +++ b/examples/nonlinear_toy/wo_input_limits_trigpoly_demo.py @@ -64,12 +64,14 @@ def main(grow_heuristics: GrowHeuristics): if grow_heuristics == GrowHeuristics.kInnerEllipsoid: binary_search_scale_options = utils.BinarySearchOptions(min=1, max=2, tol=0.1) - find_inner_ellipsoid_max_iter = 1 + inner_ellipsoid_options = clf_cbf.InnerEllipsoidOptions( + x_inner=x_equilibrium, find_inner_ellipsoid_max_iter=1 + ) compatible_states_options = None solver_options = None elif grow_heuristics == GrowHeuristics.kCompatibleStates: binary_search_scale_options = None - find_inner_ellipsoid_max_iter = 0 + inner_ellipsoid_options = None compatible_states_options = clf_cbf.CompatibleStatesOptions( candidate_compatible_states=np.array( [ @@ -97,9 +99,8 @@ def main(grow_heuristics: GrowHeuristics): clf_degree, cbf_degrees, max_iter, - x_inner=x_equilibrium, + inner_ellipsoid_options=inner_ellipsoid_options, binary_search_scale_options=binary_search_scale_options, - find_inner_ellipsoid_max_iter=find_inner_ellipsoid_max_iter, compatible_states_options=compatible_states_options, solver_options=solver_options, )