From e513061290033e00eaae2fc4b72140229a32f52f Mon Sep 17 00:00:00 2001 From: Hongkai Dai Date: Sat, 5 Oct 2024 08:08:20 -0700 Subject: [PATCH] Add demo for quadrotor taylor dynamics. (#77) This has input limit. The V-rep formulation can work but H-rep will use up the memory. Also supports homogeneous_y tag in the Lagrangian. --- compatible_clf_cbf/clf_cbf.py | 34 ++++- examples/quadrotor/demo_clf.py | 49 +++---- examples/quadrotor/demo_taylor.py | 176 ++++++++++++++++++++++++++ examples/quadrotor/demo_taylor_clf.py | 95 ++++++++++++++ examples/quadrotor/plant.py | 32 ++++- tests/test_clf_cbf.py | 18 +++ 6 files changed, 371 insertions(+), 33 deletions(-) create mode 100644 examples/quadrotor/demo_taylor.py create mode 100644 examples/quadrotor/demo_taylor_clf.py diff --git a/compatible_clf_cbf/clf_cbf.py b/compatible_clf_cbf/clf_cbf.py index f0ce863..97d3ae0 100644 --- a/compatible_clf_cbf/clf_cbf.py +++ b/compatible_clf_cbf/clf_cbf.py @@ -119,6 +119,11 @@ class XYDegree: x: int y: int + # If set to True, then the each monomial in the polynomial will have y's + # degree equal to self.y. For example if self.y = 2 and + # homogeneous_y = True, then the polynomial can have term like + # x₁²y₁y₂, y₂², but not x₁y₁ (because the degree in y is 1). + homogeneous_y: bool = False def construct_polynomial( self, @@ -136,9 +141,24 @@ def construct_polynomial( basis = sym.MonomialBasis( {x: int(np.floor(self.x / 2)), y: int(np.floor(self.y / 2))} ) + if self.homogeneous_y: + basis_prune = np.array( + [ + m + for m in basis + if np.sum([m.degree(y_i) for y_i in y]) + == int(np.floor(self.y / 2)) + ] + ) + basis = basis_prune poly, _ = prog.NewSosPolynomial(basis, type=sos_type) else: basis = sym.MonomialBasis({x: self.x, y: self.y}) + if self.homogeneous_y: + basis_prune = np.array( + [m for m in basis if np.sum([m.degree(y_i) for y_i in y]) == self.y] + ) + basis = basis_prune coeffs = prog.NewContinuousVariables(basis.size) poly = sym.Polynomial({basis[i]: coeffs[i] for i in range(basis.size)}) return poly @@ -1253,6 +1273,13 @@ def search_lagrangians_given_clf_cbf( Optional[CompatibleLagrangians], Optional[SafetySetLagrangians], ]: + safety_set_lagrangians_result = self.certify_cbf_safety_set( + h, + safety_set_lagrangian_degrees, + solver_id, + solver_options, + lagrangian_coefficient_tol, + ) ( prog_compatible, compatible_lagrangians, @@ -1276,13 +1303,6 @@ def search_lagrangians_given_clf_cbf( else None ) - safety_set_lagrangians_result = self.certify_cbf_safety_set( - h, - safety_set_lagrangian_degrees, - solver_id, - solver_options, - lagrangian_coefficient_tol, - ) return compatible_lagrangians_result, safety_set_lagrangians_result def search_clf_cbf_given_lagrangian( diff --git a/examples/quadrotor/demo_clf.py b/examples/quadrotor/demo_clf.py index f7f9a81..10a03fc 100644 --- a/examples/quadrotor/demo_clf.py +++ b/examples/quadrotor/demo_clf.py @@ -5,6 +5,7 @@ on the quaternion. """ +import itertools import os from typing import Tuple @@ -84,32 +85,14 @@ def main(with_u_bound: bool): if with_u_bound: thrust_max = quadrotor.m * quadrotor.g - u_vertices = thrust_max * np.array( - [ - [0, 0, 0, 0], - [0, 0, 0, 1], - [0, 0, 1, 0], - [0, 0, 1, 1], - [0, 1, 0, 0], - [0, 1, 0, 1], - [0, 1, 1, 0], - [0, 1, 1, 1], - [1, 0, 0, 0], - [1, 0, 0, 1], - [1, 0, 1, 0], - [1, 0, 1, 1], - [1, 1, 0, 0], - [1, 1, 0, 1], - [1, 1, 1, 0], - [1, 1, 1, 1], - ] - ) + u_vertices = np.array(list(itertools.product([0, thrust_max], repeat=4))) else: u_vertices = None state_eq_constraints = quadrotor.equality_constraint(x) V_degree = 2 + print("Find regional CLF.") V_init = find_trig_regional_clf(V_degree, x) kappa_V = 0.1 solver_options = solvers.SolverOptions() @@ -134,13 +117,33 @@ def main(with_u_bound: bool): rho_minus_V=4, state_eq_constraints=[4], ) - clf_search.search_lagrangian_given_clf( + candidate_stable_states = np.zeros((4, 13)) + candidate_stable_states[0, 4:7] = np.array([1, 0, 0]) + candidate_stable_states[1, 4:7] = np.array([-1, 0, 0]) + candidate_stable_states[2, 4:7] = np.array([0, 1, 0]) + candidate_stable_states[3, 4:7] = np.array([0, -1, 0]) + stable_states_options = clf.StableStatesOptions( + candidate_stable_states=candidate_stable_states, V_margin=0.01 + ) + print("Bilinear alternation.") + 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((13,)), + max_iter=5, + stable_states_options=stable_states_options, solver_options=solver_options, ) + clf.save_clf( + V, + clf_search.x_set, + kappa=kappa_V, + pickle_path=os.path.join( + os.path.dirname(os.path.abspath(__file__)), "../../data/quadrotor_clf.pkl" + ), + ) return diff --git a/examples/quadrotor/demo_taylor.py b/examples/quadrotor/demo_taylor.py new file mode 100644 index 0000000..143dd06 --- /dev/null +++ b/examples/quadrotor/demo_taylor.py @@ -0,0 +1,176 @@ +""" +Certify the compatible CLF/CBF using taylor expansion of the 12-state quadrotor +dynamics. +""" + +import itertools +import os + +import numpy as np +import pydrake.solvers as solvers +import pydrake.symbolic as sym + +import compatible_clf_cbf.clf_cbf as clf_cbf +import compatible_clf_cbf.clf as clf +from compatible_clf_cbf.utils import BackoffScale +from examples.quadrotor.plant import QuadrotorPlant + + +def search(use_y_squared: bool, with_u_bound: bool, use_v_rep: bool): + x = sym.MakeVectorContinuousVariable(12, "x") + quadrotor = QuadrotorPlant() + f, g = quadrotor.affine_dynamics_taylor(x, np.zeros((12,)), f_degree=3, g_degree=2) + + if with_u_bound: + u_bound = quadrotor.m * quadrotor.g + if use_v_rep: + u_vertices = np.array(list(itertools.product([0, u_bound], repeat=4))) + u_extreme_rays = None + Au = None + bu = None + else: + Au = np.concatenate((np.eye(4), -np.eye(4)), axis=0) + bu = np.concatenate((np.full((4,), u_bound), np.zeros((4,)))) + u_vertices = None + u_extreme_rays = None + else: + u_vertices = None + u_extreme_rays = None + Au, bu = None, None + + exclude_sets = [clf_cbf.ExcludeSet(np.array([sym.Polynomial(x[2] + 2.5)]))] + + compatible = clf_cbf.CompatibleClfCbf( + f=f, + g=g, + x=x, + exclude_sets=exclude_sets, + 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, + ) + + x_set = sym.Variables(x) + V_init = clf.load_clf( + os.path.join( + os.path.dirname(os.path.abspath(__file__)), + "../../data/quadrotor_taylor_clf.pkl", + ), + x_set, + )["V"] + + h_init = np.array([1 - V_init]) + + if with_u_bound and use_v_rep: + 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=6, y=0) for _ in range(compatible.y.size)] + ), + y_cross=( + None + if use_y_squared + else [ + clf_cbf.XYDegree(x=4, y=0) + for _ in range(compatible.y_cross_poly.size) + ] + ), + rho_minus_V=clf_cbf.XYDegree(x=4, y=2, homogeneous_y=True), + h_plus_eps=[clf_cbf.XYDegree(x=4, y=2, homogeneous_y=True)], + state_eq_constraints=None, + ) + else: + compatible_lagrangian_degrees = clf_cbf.CompatibleLagrangianDegrees( + lambda_y=[clf_cbf.XYDegree(x=2, y=0) for _ in range(4)], + xi_y=clf_cbf.XYDegree(x=1, y=0), + y=( + None + if use_y_squared + else [clf_cbf.XYDegree(x=4, y=0) for _ in range(compatible.y.size)] + ), + y_cross=( + None + if use_y_squared + else [ + clf_cbf.XYDegree(x=4, y=0) + for _ in range(compatible.y_cross_poly.size) + ] + ), + rho_minus_V=clf_cbf.XYDegree(x=2, y=2, homogeneous_y=True), + h_plus_eps=[clf_cbf.XYDegree(x=2, y=2, homogeneous_y=True)], + state_eq_constraints=None, + ) + safety_sets_lagrangian_degrees = clf_cbf.SafetySetLagrangianDegrees( + exclude=[ + clf_cbf.ExcludeRegionLagrangianDegrees( + cbf=[0], unsafe_region=[0], state_eq_constraints=[0] + ) + ], + within=[], + ) + barrier_eps = np.array([0.000]) + x_equilibrium = np.zeros((12,)) + + candidate_compatible_states = np.zeros((4, 12)) + candidate_compatible_states[0, :3] = np.array([-1.5, 0, 0]) + candidate_compatible_states[1, :3] = np.array([1.5, 0, 0]) + candidate_compatible_states[2, :3] = np.array([0, 1.5, 0]) + candidate_compatible_states[3, :3] = np.array([0, -1.5, 0]) + + compatible_states_options = clf_cbf.CompatibleStatesOptions( + candidate_compatible_states=candidate_compatible_states, + anchor_states=np.zeros((1, 12)), + h_anchor_bounds=[(np.array([0.5]), np.array([1.0]))], + weight_V=1, + weight_h=np.array([1]), + V_margin=None, + h_margins=None, + ) + + solver_options = solvers.SolverOptions() + solver_options.SetOption(solvers.CommonSolverOption.kPrintToConsole, True) + + kappa_V = 0.1 + kappa_h = np.array([0.1]) + V_degree = 2 + h_degrees = [2] + backoff_scale = BackoffScale(rel=None, abs=0.001) + V, h = compatible.bilinear_alternation( + V_init, + h_init, + compatible_lagrangian_degrees, + safety_sets_lagrangian_degrees, + kappa_V, + kappa_h, + barrier_eps, + x_equilibrium, + V_degree, + h_degrees, + max_iter=5, + solver_options=solver_options, + lagrangian_coefficient_tol=None, + compatible_states_options=compatible_states_options, + backoff_scale=backoff_scale, + lagrangian_sos_type=solvers.MathematicalProgram.NonnegativePolynomial.kSos, + ) + + +def main(): + # search(use_y_squared=True, with_u_bound=False, use_v_rep=False) + # Using H-rep will cause out-of-memory issue. + # search(use_y_squared=True, with_u_bound=True, use_v_rep=False) + search(use_y_squared=True, with_u_bound=True, use_v_rep=True) + + +if __name__ == "__main__": + main() diff --git a/examples/quadrotor/demo_taylor_clf.py b/examples/quadrotor/demo_taylor_clf.py new file mode 100644 index 0000000..73aacb1 --- /dev/null +++ b/examples/quadrotor/demo_taylor_clf.py @@ -0,0 +1,95 @@ +import itertools +import os +from typing import Tuple + +import numpy as np +import pydrake.solvers as solvers +import pydrake.symbolic as sym +from pydrake.autodiffutils import ( + AutoDiffXd, + InitializeAutoDiff, + ExtractGradient, +) +from pydrake.systems.controllers import LinearQuadraticRegulator + +import compatible_clf_cbf.clf as clf +from examples.quadrotor.plant import QuadrotorPlant + + +def lqr(quadrotor: QuadrotorPlant) -> Tuple[np.ndarray, np.ndarray]: + x_equilibrium = np.zeros((12,)) + u_equilibrium = np.full((4,), quadrotor.m * quadrotor.g / 4) + xu = np.concatenate((x_equilibrium, u_equilibrium)) + xu_ad = InitializeAutoDiff(xu).reshape((-1,)) + xdot_ad = quadrotor.dynamics(xu_ad[:12], xu_ad[-4:], T=AutoDiffXd) + xdot_grad = ExtractGradient(xdot_ad) + A = xdot_grad[:, :12] + B = xdot_grad[:, -4:] + K, S = LinearQuadraticRegulator(A, B, Q=np.eye(12), R=np.eye(4)) + return K, S + + +def search_clf(): + x = sym.MakeVectorContinuousVariable(12, "x") + quadrotor = QuadrotorPlant() + f, g = quadrotor.affine_dynamics_taylor(x, np.zeros((12,)), f_degree=3, g_degree=2) + + u_bound = quadrotor.m * quadrotor.g + u_vertices = np.array(list(itertools.product([0, u_bound], repeat=4))) + + clf_search = clf.ControlLyapunov( + f=f, + g=g, + x=x, + x_equilibrium=np.zeros((12,)), + u_vertices=u_vertices, + state_eq_constraints=None, + ) + lagrangian_degrees = clf.ClfWInputLimitLagrangianDegrees( + V_minus_rho=2, Vdot=[2] * 16, state_eq_constraints=None + ) + + _, lqr_S = lqr(quadrotor) + V_init = sym.Polynomial(x.dot(lqr_S @ x)) + + kappa = 0.1 + clf_degree = 2 + x_equilibrium = np.zeros((12,)) + + candidate_stable_states = np.zeros((4, 12)) + candidate_stable_states[0, :3] = np.array([-1.5, 0, 0]) + candidate_stable_states[1, :3] = np.array([1.5, 0, 0]) + candidate_stable_states[2, :3] = np.array([0, 1.5, 0]) + candidate_stable_states[3, :3] = np.array([0, -1.5, 0]) + + stable_states_options = clf.StableStatesOptions( + candidate_stable_states, V_margin=0.01 + ) + + solver_options = solvers.SolverOptions() + solver_options.SetOption(solvers.CommonSolverOption.kPrintToConsole, 1) + + V = clf_search.bilinear_alternation( + V_init, + lagrangian_degrees, + kappa, + clf_degree, + x_equilibrium, + max_iter=5, + stable_states_options=stable_states_options, + solver_options=solver_options, + ) + x_set = sym.Variables(x) + clf.save_clf( + V, + x_set, + kappa, + os.path.join( + os.path.dirname(os.path.abspath(__file__)), + "../../data/quadrotor_taylor_clf.pkl", + ), + ) + + +if __name__ == "__main__": + search_clf() diff --git a/examples/quadrotor/plant.py b/examples/quadrotor/plant.py index 72985fa..bcfe373 100644 --- a/examples/quadrotor/plant.py +++ b/examples/quadrotor/plant.py @@ -101,7 +101,7 @@ def DoCalcTimeDerivatives( xdot: np.ndarray = self.dynamics(x, u) derivatives.SetFromVector(xdot) - def dynamics(self, x: np.ndarray, u: np.ndarray) -> np.ndarray: + def dynamics(self, x: np.ndarray, u: np.ndarray, T=float) -> np.ndarray: uF_Bz = self.kF * u Faero_B = uF_Bz.sum() * np.array([0, 0, 1]) @@ -113,10 +113,10 @@ def dynamics(self, x: np.ndarray, u: np.ndarray) -> np.ndarray: tau_B = np.stack((Mx, My, Mz)) Fgravity_N = np.array([0, 0, -self.m * self.g]) - rpy = pydrake.math.RollPitchYaw(x[3:6]) + rpy = pydrake.math.RollPitchYaw_[T](x[3:6]) w_NB_B = x[-3:] rpy_dot = rpy.CalcRpyDtFromAngularVelocityInChild(w_NB_B) - R_NB = pydrake.math.RotationMatrix(rpy) + R_NB = pydrake.math.RotationMatrix_[T](rpy) xyzDDt = (Fgravity_N + R_NB @ Faero_B) / self.m @@ -158,6 +158,32 @@ def affine_dynamics(self, x: np.ndarray) -> Tuple[np.ndarray, np.ndarray]: g[9:, :] = self.I_inv @ u_to_M return f, g + def affine_dynamics_taylor( + self, x: np.ndarray, x_val: np.ndarray, f_degree: int, g_degree: int + ) -> Tuple[np.ndarray, np.ndarray]: + """ + Taylor-expand the control-affine dynamics xdot = f(x) + g(x) * u. + Return f(x) and g(x) in the Taylor expansion. + """ + env = {x[i]: x_val[i] for i in range(12)} + f_expr, g_expr = self.affine_dynamics(x) + f = np.array( + [ + sym.Polynomial(sym.TaylorExpand(f_expr[i], env, order=f_degree)) + for i in range(12) + ] + ) + g = np.array( + [ + [ + sym.Polynomial(sym.TaylorExpand(g_expr[i, j], env, order=g_degree)) + for j in range(4) + ] + for i in range(12) + ] + ) + return (f, g) + class QuadrotorPolyPlant(pydrake.systems.framework.LeafSystem): """ diff --git a/tests/test_clf_cbf.py b/tests/test_clf_cbf.py index ba87fae..8b5d552 100644 --- a/tests/test_clf_cbf.py +++ b/tests/test_clf_cbf.py @@ -36,6 +36,24 @@ def test_construct_polynomial(self): assert np.sum([monomial.degree(x[i]) for i in range(x.size)]) <= 3 assert np.sum([monomial.degree(y[i]) for i in range(y.size)]) <= 2 + def test_homogeneous_y(self): + degree = mut.XYDegree(x=3, y=2, homogeneous_y=True) + prog = solvers.MathematicalProgram() + x = prog.NewIndeterminates(3, "x") + y = prog.NewIndeterminates(2, "y") + + sos_poly = degree.construct_polynomial( + prog, sym.Variables(x), sym.Variables(y), is_sos=True + ) + for monomial, _ in sos_poly.monomial_to_coefficient_map().items(): + assert monomial.degree(y[0]) + monomial.degree(y[1]) == 2 + + free_poly = degree.construct_polynomial( + prog, sym.Variables(x), sym.Variables(y), is_sos=False + ) + for monomial, _ in free_poly.monomial_to_coefficient_map().items(): + assert monomial.degree(y[0]) + monomial.degree(y[1]) == 2 + class TestCompatibleStatesOptions: def test_add_cost(self):