diff --git a/examples/quadrotor2d/demo.py b/examples/quadrotor2d/demo.py index 7e21bfd..a2f114b 100644 --- a/examples/quadrotor2d/demo.py +++ b/examples/quadrotor2d/demo.py @@ -5,6 +5,7 @@ cos/sin. """ +import itertools import os import numpy as np @@ -12,25 +13,32 @@ import pydrake.symbolic as sym from compatible_clf_cbf import clf_cbf +import compatible_clf_cbf.clf as clf from examples.quadrotor2d.plant import Quadrotor2dTrigPlant -import examples.quadrotor2d.demo_clf as demo_clf import compatible_clf_cbf.utils -def main(use_y_squared: bool, with_u_bound: bool): +def main(use_y_squared: bool, with_u_bound: bool, use_v_rep: bool): x = sym.MakeVectorContinuousVariable(7, "x") quadrotor = Quadrotor2dTrigPlant() f, g = quadrotor.affine_dynamics(x) if with_u_bound: - Au = np.concatenate((np.eye(2), -np.eye(2)), axis=0) u_bound = quadrotor.m * quadrotor.g * 1.5 - bu = np.concatenate((np.full((2,), u_bound), np.zeros((2,)))) + if use_v_rep: + u_vertices = np.array(list(itertools.product([0, u_bound], repeat=2))) + u_extreme_rays = None + Au, bu = None, None + else: + Au = np.concatenate((np.eye(2), -np.eye(2)), axis=0) + bu = np.concatenate((np.full((2,), u_bound), np.zeros((2,)))) + u_vertices, u_extreme_rays = None, None else: Au, bu = None, None + u_vertices, u_extreme_rays = None, None # Ground as the unsafe region. - exclude_sets = [clf_cbf.ExcludeSet(np.array([sym.Polynomial(x[1] + 0.5)]))] + exclude_sets = [clf_cbf.ExcludeSet(np.array([sym.Polynomial(x[1] + 2)]))] state_eq_constraints = quadrotor.equality_constraint(x) compatible = clf_cbf.CompatibleClfCbf( f=f, @@ -40,6 +48,8 @@ def main(use_y_squared: bool, with_u_bound: bool): within_set=None, Au=Au, bu=bu, + u_vertices=u_vertices, + u_extreme_rays=u_extreme_rays, num_cbf=1, with_clf=True, use_y_squared=use_y_squared, @@ -47,26 +57,50 @@ def main(use_y_squared: bool, with_u_bound: bool): ) V_degree = 2 - V_init = 10 * demo_clf.find_trig_regional_clf(V_degree, x) + # V_init = 10 * demo_clf.find_trig_regional_clf(V_degree, x) + clf_data = clf.load_clf( + os.path.join( + os.path.dirname(os.path.abspath(__file__)), "../../data/quadrotor2d_clf.pkl" + ), + compatible.x_set, + ) + V_init = clf_data["V"] h_degrees = [2] h_init = np.array([1 - V_init]) - kappa_V = 0 + kappa_V = 0.1 kappa_h = np.array([kappa_V]) solver_options = solvers.SolverOptions() solver_options.SetOption(solvers.CommonSolverOption.kPrintToConsole, True) - compatible_lagrangian_degrees = clf_cbf.CompatibleLagrangianDegrees( - lambda_y=[clf_cbf.XYDegree(x=1, y=0 if use_y_squared else 1) for _ in range(2)], - xi_y=clf_cbf.XYDegree(x=2, y=0 if use_y_squared else 1), - y=( - None - if use_y_squared - else [clf_cbf.XYDegree(x=4, y=0) for _ in range(compatible.y.size)] - ), - rho_minus_V=clf_cbf.XYDegree(x=2, y=2), - h_plus_eps=[clf_cbf.XYDegree(x=2, y=2)], - state_eq_constraints=[clf_cbf.XYDegree(x=2, y=2)], - ) + if use_v_rep and with_u_bound: + compatible_lagrangian_degrees = clf_cbf.CompatibleWVrepLagrangianDegrees( + u_vertices=[clf_cbf.XYDegree(x=2, y=0) for _ in range(u_vertices.shape[0])], + u_extreme_rays=None, + xi_y=None, + y=( + None + if use_y_squared + else [clf_cbf.XYDegree(x=4, y=2) for _ in range(compatible.y.size)] + ), + rho_minus_V=clf_cbf.XYDegree(x=2, y=2), + h_plus_eps=[clf_cbf.XYDegree(x=2, y=2)], + state_eq_constraints=[clf_cbf.XYDegree(x=4, y=2)], + ) + else: + compatible_lagrangian_degrees = clf_cbf.CompatibleLagrangianDegrees( + lambda_y=[ + clf_cbf.XYDegree(x=1, y=0 if use_y_squared else 1) for _ in range(2) + ], + xi_y=clf_cbf.XYDegree(x=2, y=0 if use_y_squared else 1), + y=( + None + if use_y_squared + else [clf_cbf.XYDegree(x=4, y=0) for _ in range(compatible.y.size)] + ), + rho_minus_V=clf_cbf.XYDegree(x=2, y=2), + h_plus_eps=[clf_cbf.XYDegree(x=2, y=2)], + state_eq_constraints=[clf_cbf.XYDegree(x=2, y=2)], + ) safety_sets_lagrangian_degrees = clf_cbf.SafetySetLagrangianDegrees( exclude=[ clf_cbf.ExcludeRegionLagrangianDegrees( @@ -81,10 +115,11 @@ def main(use_y_squared: bool, with_u_bound: bool): compatible_states_options = clf_cbf.CompatibleStatesOptions( candidate_compatible_states=np.array( [ - [0, 0, 0, 0, 0, 0, 0], - [0, -0.3, 0, 0, 0, 0, 0], - [0.5, -0.3, 0, 0, 0, 0, 0], - [-0.5, -0.3, 0, 0, 0, 0, 0], + [-3, 0, 0, 0, 0, 0, 0], + [3, 0, 0, 0, 0, 0, 0], + [-1.5, -0.5, 0, 0, 0, 0, 0], + [1.5, 0.5, 0, 0, 0, 0, 0], + [0, -1.5, 0, 0, 0, 0, 0], ] ), anchor_states=np.zeros((1, 7)), @@ -111,12 +146,19 @@ def main(use_y_squared: bool, with_u_bound: bool): # solver_id = solvers.ClarabelSolver().id(), lagrangian_coefficient_tol=None, compatible_states_options=compatible_states_options, - backoff_scale=compatible_clf_cbf.utils.BackoffScale(rel=0.02, abs=None), + backoff_scale=compatible_clf_cbf.utils.BackoffScale(rel=None, abs=0.01), ) x_set = sym.Variables(x) + + filename = ( + "quadrotor2d_clf_cbf" + + ("_w_y_square" if use_y_squared else "_wo_y_squared") + + ("_vrep" if use_v_rep else "_hrep") + + ("_w_u_bound" if with_u_bound else "_wo_u_bound") + + ".pkl" + ) pickle_path = os.path.join( - os.path.dirname(os.path.abspath(__file__)), - "../../data/quadrotor2d_clf_cbf.pkl", + os.path.dirname(os.path.abspath(__file__)), "../../data/", filename ) clf_cbf.save_clf_cbf( V, @@ -127,23 +169,8 @@ def main(use_y_squared: bool, with_u_bound: bool): pickle_path, ) - # Check the CLF/CBF with a positive kappa. - ( - compatible_lagrangians, - safety_sets_lagrangians, - ) = compatible.search_lagrangians_given_clf_cbf( - V, - h, - kappa_V=1e-4, - kappa_h=np.array([1e-4]), - barrier_eps=barrier_eps, - compatible_lagrangian_degrees=compatible_lagrangian_degrees, - safety_set_lagrangian_degrees=safety_sets_lagrangian_degrees, - solver_options=solver_options, - ) - assert compatible_lagrangians is not None - assert safety_sets_lagrangians is not None - if __name__ == "__main__": - main(use_y_squared=False, with_u_bound=False) + # main(use_y_squared=False, with_u_bound=False, use_v_rep=False) + main(use_y_squared=True, with_u_bound=True, use_v_rep=False) + # main(use_y_squared=True, with_u_bound=True, use_v_rep=True) diff --git a/examples/quadrotor2d/demo_clf.py b/examples/quadrotor2d/demo_clf.py index 65f64a5..ee4f4a3 100644 --- a/examples/quadrotor2d/demo_clf.py +++ b/examples/quadrotor2d/demo_clf.py @@ -5,6 +5,7 @@ cos/sin. """ +import os from typing import Tuple import numpy as np @@ -86,7 +87,7 @@ def main(with_u_bound: bool): V_degree = 2 V_init = find_trig_regional_clf(V_degree, x) - kappa_V = 1e-4 + kappa_V = 0.1 solver_options = solvers.SolverOptions() solver_options.SetOption(solvers.CommonSolverOption.kPrintToConsole, True) @@ -106,13 +107,31 @@ def main(with_u_bound: bool): clf_lagrangian_degrees = clf.ClfWoInputLimitLagrangianDegrees( dVdx_times_f=4, dVdx_times_g=[3, 3], rho_minus_V=4, state_eq_constraints=[4] ) - clf_search.search_lagrangian_given_clf( + + candidate_stable_states = np.zeros((2, 7)) + candidate_stable_states[0, :4] = np.array([2, 0, 0, 0]) + candidate_stable_states[1, :4] = np.array([-2, 0, 0, 0]) + stable_states_options = clf.StableStatesOptions( + candidate_stable_states=candidate_stable_states, V_margin=0.01 + ) + V = clf_search.bilinear_alternation( V_init, - rho=1, + clf_lagrangian_degrees, kappa=kappa_V, - lagrangian_degrees=clf_lagrangian_degrees, + clf_degree=2, + x_equilibrium=np.zeros((7,)), + max_iter=10, + stable_states_options=stable_states_options, solver_options=solver_options, ) + clf.save_clf( + V, + clf_search.x_set, + kappa_V, + pickle_path=os.path.join( + os.path.dirname(os.path.abspath(__file__)), "../../data/quadrotor2d_clf.pkl" + ), + ) return diff --git a/examples/quadrotor2d/demo_taylor.py b/examples/quadrotor2d/demo_taylor.py index f482644..97dbe30 100644 --- a/examples/quadrotor2d/demo_taylor.py +++ b/examples/quadrotor2d/demo_taylor.py @@ -4,6 +4,7 @@ """ from enum import Enum +import itertools from typing import Tuple import numpy as np @@ -35,18 +36,28 @@ def lqr(quadrotor: Quadrotor2dPlant) -> Tuple[np.ndarray, np.ndarray]: def search_clf_cbf( - use_y_squared: bool, with_u_bound: bool, grow_heuristics: GrowHeuristics + use_y_squared: bool, + with_u_bound: bool, + grow_heuristics: GrowHeuristics, + use_v_rep: bool, ): x = sym.MakeVectorContinuousVariable(6, "x") quadrotor = Quadrotor2dPlant() f, g = quadrotor.taylor_affine_dynamics(x) if with_u_bound: - Au = np.concatenate((np.eye(2), -np.eye(2)), axis=0) u_bound = quadrotor.m * quadrotor.g * 1.5 - bu = np.concatenate((np.full((2,), u_bound), np.zeros((2,)))) + if use_v_rep: + u_vertices = np.array(list(itertools.product([0, u_bound], repeat=2))) + u_extreme_rays = None + Au = bu = None + else: + Au = np.concatenate((np.eye(2), -np.eye(2)), axis=0) + bu = np.concatenate((np.full((2,), u_bound), np.zeros((2,)))) + u_vertices = u_extreme_rays = None else: Au, bu = None, None + u_vertices = u_extreme_rays = None K_lqr, S_lqr = lqr(quadrotor) V_init = sym.Polynomial(x.dot(S_lqr @ x) / 0.01) @@ -65,23 +76,40 @@ def search_clf_cbf( within_set=None, Au=Au, bu=bu, + u_vertices=u_vertices, + u_extreme_rays=u_extreme_rays, num_cbf=1, with_clf=True, use_y_squared=use_y_squared, state_eq_constraints=None, ) - lagrangian_degrees = clf_cbf.CompatibleLagrangianDegrees( - lambda_y=[clf_cbf.XYDegree(x=2, y=0) for _ in range(2)], - xi_y=clf_cbf.XYDegree(x=4, y=0), - y=( - None - if use_y_squared - else [clf_cbf.XYDegree(x=4, y=0) for _ in range(compatible.y.size)] - ), - rho_minus_V=clf_cbf.XYDegree(x=4, y=2), - h_plus_eps=[clf_cbf.XYDegree(x=4, y=2)], - state_eq_constraints=None, - ) + if use_v_rep and with_u_bound: + lagrangian_degrees = clf_cbf.CompatibleWVrepLagrangianDegrees( + u_vertices=[clf_cbf.XYDegree(x=2, y=0) for _ in range(u_vertices.shape[0])], + u_extreme_rays=None, + xi_y=None, + y=( + None + if use_y_squared + else [clf_cbf.XYDegree(x=4, y=0) for _ in range(compatible.y.size)] + ), + rho_minus_V=clf_cbf.XYDegree(x=4, y=2), + h_plus_eps=[clf_cbf.XYDegree(x=4, y=2)], + state_eq_constraints=None, + ) + else: + lagrangian_degrees = clf_cbf.CompatibleLagrangianDegrees( + lambda_y=[clf_cbf.XYDegree(x=2, y=0) for _ in range(2)], + xi_y=clf_cbf.XYDegree(x=4, y=0), + y=( + None + if use_y_squared + else [clf_cbf.XYDegree(x=4, y=0) for _ in range(compatible.y.size)] + ), + rho_minus_V=clf_cbf.XYDegree(x=4, y=2), + h_plus_eps=[clf_cbf.XYDegree(x=4, y=2)], + state_eq_constraints=None, + ) barrier_eps = np.array([0.0]) safety_sets_lagrangian_degrees = clf_cbf.SafetySetLagrangianDegrees( @@ -153,12 +181,20 @@ def main(): use_y_squared=True, with_u_bound=False, grow_heuristics=GrowHeuristics.kCompatibleStates, + use_v_rep=False, ) - # SDP with u_bound is a lot slower than without u_bound. # search_clf_cbf( # use_y_squared=True, # with_u_bound=True, - # grow_heuristics=GrowHeuristics.kInnerEllipsoid, + # grow_heuristics=GrowHeuristics.kCompatibleStates, + # use_v_rep=True, + # ) + # SDP with u_bound is a lot slower with H-rep than with V-rep. + # search_clf_cbf( + # use_y_squared=True, + # with_u_bound=True, + # grow_heuristics=GrowHeuristics.kCompatibleStates, + # use_v_rep=False, # )