├── LICENSE ├── README.md ├── __init__.py ├── acc.py ├── acc_execute.py ├── cbf_clf_qp.py ├── define_system.py ├── images ├── cbf.png ├── clf.png ├── control.png ├── relative_distance.png ├── slack.png └── velocity.png ├── inverted_pendulum.py ├── ode_solver.py └── requirements.txt /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2021 Muhan Zhao 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # CLF-CBF-python 2 | This is a python repo that implements control barrier function quadratic programming method. 3 | 4 | # Dependencies 5 | cvxpy 6 | numpy 7 | sympy 8 | matplotlib 9 | 10 | # How to use 11 | Run ``acc_execute.py`` 12 | 13 | # Credit 14 | https://github.com/HybridRobotics/CBF-CLF-Helper 15 | 16 | # Contribution 17 | Feel free to fork this repo or create any pull request 18 | -------------------------------------------------------------------------------- /__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/kimukook/CLF-CBF-python/28a46a4f9abf095e1f1b92e6cc056956caab5374/__init__.py -------------------------------------------------------------------------------- /acc.py: -------------------------------------------------------------------------------- 1 | ''' 2 | ===================================== 3 | Author : Muhan Zhao 4 | Date : Feb. 11, 2020 5 | Location: UC San Diego, La Jolla, CA 6 | ===================================== 7 | ''' 8 | import numpy as np 9 | import sympy as sp 10 | # from define_system import ControlAffineSystem 11 | 12 | 13 | class AdaptiveCruiseControl: 14 | """ 15 | Define the symbolic dynamic: dx = f(x) + g(x) * u 16 | x contains 3 states: p -> position v -> velocity relative z -> distance 17 | 18 | 19 | """ 20 | def __init__(self, params): 21 | """ 22 | The input 'params' is a dictionary type argument which contains the following parameters: 23 | 24 | :param f0 : To define the rolling resistance; 25 | :param f1 : To define the rolling resistance; 26 | :param f2 : To define the rolling resistance; 27 | :param m : The mass; 28 | :param v0 : The speed of leading cruise; 29 | :param T : The time horizon for defining cbf; 30 | :param cd : The deceleration parameter in cbf; 31 | :param vd : The desired velocity in clf; 32 | :param udim : The dimension of control profile u 33 | """ 34 | self.f0 = params['f0'] 35 | self.f1 = params['f1'] 36 | self.f2 = params['f2'] 37 | self.v0 = params['v0'] 38 | self.m = params['m'] 39 | 40 | self.T = params['T'] 41 | self.cd = params['cd'] 42 | self.G = params['G'] 43 | 44 | self.vd = params['vd'] 45 | 46 | p, v, z = sp.symbols('p v z') 47 | self.x = sp.Matrix([p, v, z]) 48 | 49 | self.Fr = None 50 | 51 | # Define the symbolic expression for system dynamic, CLF and CBF 52 | self.f, self.g = self.simple_car_dynamics() 53 | self.cbf = self.define_cbf() 54 | self.clf = self.define_clf() 55 | 56 | if 'udim' in params.keys(): 57 | self.udim = params['udim'] 58 | else: 59 | print(f'The dimension of input u is not given, set it to be default 1') 60 | self.udim = 1 61 | 62 | def simple_car_dynamics(self): 63 | self.Fr = self.Fr_() 64 | # f, g both column vector 65 | f = sp.Matrix([self.x[1], -self.Fr / self.m, self.v0 - self.x[1]]) 66 | g = sp.Matrix([0, 1/self.m, 0]) 67 | return f, g 68 | 69 | def Fr_(self): 70 | self.Fr = self.f0 + self.f1 * self.x[1] + self.f2 * self.x[1] ** 2 71 | return self.Fr 72 | 73 | def getFr(self, x): 74 | return np.array([self.f0 + self.f1 * x[1] + self.f2 * x[1] ** 2]) 75 | 76 | def define_cbf(self): 77 | cbf = self.x[2] - self.T * self.x[1] - .5 * (self.x[1] - self.v0) ** 2 / (self.cd * self.G) 78 | return cbf 79 | 80 | def define_clf(self): 81 | clf = (self.x[1] - self.vd) ** 2 82 | return clf 83 | -------------------------------------------------------------------------------- /acc_execute.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import acc 3 | import define_system 4 | import cbf_clf_qp as ccq 5 | import ode_solver 6 | import matplotlib.pyplot as plt 7 | 8 | parameters = { 9 | 'f0': .1, 10 | 'f1': 5, 11 | 'f2': .25, 12 | 'v0': 10, 13 | 'm': 1650, 14 | 'T': 1.8, 15 | 'cd': .3, 16 | 'vd': 24, 17 | 'G': 9.8, 18 | 'udim': 1 19 | } 20 | 21 | vehicle = acc.AdaptiveCruiseControl(parameters) 22 | ds = define_system.ControlAffineSystem(vehicle) 23 | 24 | QPoption = ccq.CbfClfQpOptions() 25 | QPoption.set_option('u_max', np.array([.3 * parameters['G'] * parameters['m']])) 26 | QPoption.set_option('u_min', np.array([-.3 * parameters['G'] * parameters['m']])) 27 | QPoption.set_option('clf_lambda', 5.0) 28 | QPoption.set_option('cbf_gamma', 5.0) 29 | QPoption.set_option('weight_input', np.array([2/parameters['m']**2])) 30 | QPoption.set_option('weight_slack', 2e-2) 31 | 32 | qp = ccq.CbfClfQp(ds, QPoption) 33 | 34 | 35 | T = 20 36 | dt = .02 37 | x0 = np.array([[0, 20, 100]]).T 38 | time_steps = int(np.ceil(T / dt)) 39 | 40 | ode_sol = ode_solver.OdeSolver(ds, dt) 41 | 42 | # initialize the input matrices 43 | xt = np.zeros((3, time_steps)) 44 | tt = np.zeros((1, time_steps)) 45 | ut = np.zeros((1, time_steps)) 46 | 47 | slackt = np.zeros((1, time_steps)) 48 | Vt = np.zeros((1, time_steps)) 49 | Bt = np.zeros((1, time_steps)) 50 | xt[:, 0] = np.copy(x0.T[0]) 51 | 52 | for t in range(time_steps-1): 53 | if t % 100 == 0: 54 | print(f't = {t}') 55 | # print(t) 56 | 57 | # reference control input u_ref 58 | Fr = vehicle.getFr(xt[:, t]) 59 | 60 | # solve for control u at current time step 61 | u, delta, B, V, feas = qp.cbf_clf_qp(xt[:, t], Fr) 62 | if feas == -1: 63 | # infeasible 64 | break 65 | else: 66 | pass 67 | 68 | ut[:, t] = np.copy(u) 69 | slackt[:, t] = np.copy(delta) 70 | Vt[:, t] = np.copy(V) 71 | Bt[:, t] = np.copy(B) 72 | 73 | # propagate the system with control u using RK4 74 | xt[:, t + 1] = ode_sol.time_marching(xt[:, t], u) 75 | 76 | 77 | def state_velocity(xt, dt, T, show=1, save=0): 78 | t = np.arange(0, T, dt) 79 | fig = plt.figure(figsize=[16, 9]) 80 | plt.grid() 81 | plt.plot(t, xt[1, :], linewidth=3, color='magenta') 82 | plt.plot(t, 24 * np.ones(t.shape[0]), 'k--') 83 | plt.title('State - Velocity') 84 | plt.ylabel('v') 85 | if show == 1: 86 | plt.show() 87 | if save == 1: 88 | plt.savefig('velocity.png', format='png', dpi=300) 89 | plt.close(fig) 90 | 91 | 92 | def state_relative_distance(xt, dt, T, show=1, save=0): 93 | t = np.arange(0, T, dt) 94 | fig = plt.figure(figsize=[16, 9]) 95 | plt.grid() 96 | plt.plot(t, xt[2, :], linewidth=3, color='black') 97 | plt.ylim(0, 100) 98 | plt.title('State - Relative distance') 99 | plt.ylabel('z') 100 | if show == 1: 101 | plt.show() 102 | if save == 1: 103 | plt.savefig('relative_distance.png', format='png', dpi=300) 104 | plt.close(fig) 105 | 106 | 107 | def slack(slack, dt, T, show=1, save=0): 108 | t = np.arange(0, T, dt) 109 | fig = plt.figure(figsize=[16, 9]) 110 | plt.grid() 111 | plt.plot(t[:-1], slack[0, :-1], linewidth=3, color='orange') 112 | plt.title('Slack') 113 | plt.ylabel('B(x)') 114 | if show == 1: 115 | plt.show() 116 | if save == 1: 117 | plt.savefig('slack.png', format='png', dpi=300) 118 | plt.close(fig) 119 | 120 | 121 | def cbf(Bt, dt, T, show=1, save=0): 122 | t = np.arange(0, T, dt) 123 | fig = plt.figure(figsize=[16, 9]) 124 | plt.grid() 125 | plt.plot(t[:-1], Bt[0, :-1], linewidth=3, color='red') 126 | plt.title('cbf') 127 | plt.ylabel('B(x)') 128 | if show == 1: 129 | plt.show() 130 | if save == 1: 131 | plt.savefig('cbf.png', format='png', dpi=300) 132 | plt.close(fig) 133 | 134 | 135 | def clf(Vt, dt, T, show=1, save=0): 136 | t = np.arange(0, T, dt) 137 | fig = plt.figure(figsize=[16, 9]) 138 | plt.grid() 139 | plt.plot(t[:-1], Vt[0, :-1], linewidth=3, color='cyan') 140 | plt.title('clf') 141 | plt.ylabel('V(x)') 142 | if show == 1: 143 | plt.show() 144 | if save == 1: 145 | plt.savefig('clf.png', format='png', dpi=300) 146 | plt.close(fig) 147 | 148 | 149 | def control(u, dt, T, show=1, save=0): 150 | u_max = .3 * 9.8 * 1650 151 | t = np.arange(0, T, dt) 152 | fig = plt.figure(figsize=[16, 9]) 153 | plt.grid() 154 | plt.plot(t[:-1], u[0, :-1], linewidth=3, color='dodgerblue') 155 | plt.plot(t, u_max * np.ones(t.shape[0]), 'k--') 156 | plt.plot(t, -u_max * np.ones(t.shape[0]), 'k--') 157 | plt.title('control') 158 | plt.ylabel('u(t, x)') 159 | if show == 1: 160 | plt.show() 161 | if save == 1: 162 | plt.savefig('control.png', format='png', dpi=300) 163 | 164 | plt.close(fig) 165 | 166 | 167 | state_velocity(xt, dt, T) 168 | state_relative_distance(xt, dt, T) 169 | cbf(Bt, dt, T) 170 | clf(Vt, dt, T) 171 | slack(slackt, dt, T) 172 | control(ut, dt, T) 173 | 174 | 175 | 176 | -------------------------------------------------------------------------------- /cbf_clf_qp.py: -------------------------------------------------------------------------------- 1 | ''' 2 | ===================================== 3 | Author : Muhan Zhao 4 | Date : Feb. 16, 2020 5 | Location: UC San Diego, La Jolla, CA 6 | ===================================== 7 | ''' 8 | 9 | import numpy as np 10 | import cvxpy as cp 11 | 12 | 13 | class OptionsClass: 14 | """ 15 | Options Class 16 | """ 17 | 18 | def __init__(self): 19 | self.options = None 20 | self.solverName = 'None' 21 | 22 | def set_option(self, key, value): 23 | try: 24 | if type(value) is self.options[key][2]: 25 | self.options[key][0] = value 26 | else: 27 | print(f"The type of value for the keyword '{key}' should be '{self.options[key][2]}'.") 28 | except: 29 | raise ValueError('Incorrect option keyword or type: ' + key) 30 | 31 | def get_option(self, key): 32 | try: 33 | value = self.options[key][0] 34 | return value 35 | except: 36 | raise ValueError('Incorrect option keyword: ' + key) 37 | 38 | def reset_options(self, key): 39 | try: 40 | self.options[key] = self.options[key][1] 41 | except: 42 | raise ValueError('Incorrect option keyword: ' + key) 43 | 44 | 45 | class CbfClfQpOptions(OptionsClass): 46 | def __init__(self): 47 | OptionsClass.__init__(self) 48 | self.setup() 49 | self.solver_name = 'CBF-CLF' 50 | 51 | def setup(self): 52 | self.options = { 53 | # [Current value, default value, type] 54 | 'u_max': [None, None, np.ndarray], 55 | 'u_min': [None, None, np.ndarray], 56 | 'clf_lambda': [None, 5, float], 57 | 'cbf_gamma': [None, 5, float], 58 | 'weight_input': [None, None, np.ndarray], 59 | 'weight_slack': [None, 2e-2, float], 60 | } 61 | 62 | # def define_slack(self): 63 | # TODO 64 | 65 | 66 | class CbfClfQp: 67 | """ 68 | This is the implementation of the vanilla CBF-CLF-QP method. The optimization problem is: 69 | 70 | min (u-u_ref).T * H * (u-u_ref) + p * delta**2 71 | s.t. L_f V(x) + L_g V(x) * u + lambda * V(x) <= delta ---> CLF constraint 72 | L_f B(x) + L_g B(x) * u + gamma * B(x) >= 0 ---> CBF constraint 73 | 74 | Input: 75 | :param system : The dynamic system of interest, containing CBF, CLF, and their Lie derivatives 76 | :param x : The current state x 77 | :param u_ref : The reference control input 78 | :param slack : The slack activated or not, 1 -> activate while 0 -> not activate 79 | :param verbose : Show the optimization log or not 80 | """ 81 | def __init__(self, system, option_class): 82 | if hasattr(system, 'udim'): 83 | self.udim = system.udim 84 | else: 85 | raise KeyError('udim is not given in the system dynamic!') 86 | 87 | self.cbf = system.cbf 88 | 89 | # todo check lf.lg/cbf clfs symbolic expression and their size! 90 | self.lf_cbf = system.lf_cbf 91 | self.lg_cbf = system.lg_cbf 92 | 93 | self.clf = system.clf 94 | self.lf_clf = system.lf_clf 95 | self.lg_clf = system.lg_clf 96 | 97 | # todo take input from the option class 98 | self.weight_input = np.atleast_2d(option_class.get_option('weight_input')) 99 | self.weight_slack = np.atleast_2d(option_class.get_option('weight_slack')) 100 | self.H = None 101 | self.slack_H = None 102 | 103 | # todo 104 | self.A = None 105 | self.b = None 106 | 107 | # Hyperparameters: CLF <- Lambda & CBF <- Gamma 108 | self.clf_lambda = option_class.get_option('clf_lambda') 109 | self.cbf_gamma = option_class.get_option('cbf_gamma') 110 | 111 | self.u_max = option_class.get_option('u_max') 112 | if self.u_max.shape != (self.udim,): 113 | raise ValueError('The size of u_max should be udim-by-, a one dimensional vector in python.') 114 | self.u_min = option_class.get_option('u_min') 115 | if self.u_min.shape != (self.udim,): 116 | raise ValueError('The size of u_min should be udim-by-, a one dimensional vector in python.') 117 | 118 | self.with_slack = None 119 | 120 | def cbf_clf_qp(self, x, u_ref=None, with_slack=1, verbose=0): 121 | """ 122 | 123 | :param x : The current state 124 | :param u_ref : A real number of 1D vector with shape (udim,) 125 | :param with_slack: Indicator if there is slack variable 126 | :param verbose : Indicator if QP info is displayed 127 | :return: 128 | """ 129 | inf = np.inf 130 | self.with_slack = with_slack 131 | 132 | slack = None 133 | if u_ref is None: 134 | u_ref = np.zeros(self.udim) 135 | else: 136 | if u_ref.shape != (self.udim,): 137 | raise ValueError(f'u_ref should have the shape size (u_dim,), now it is {u_ref.shape}') 138 | 139 | # Read the weight input and build up the matrix H in the cost function 140 | if self.weight_input.shape == (1, 1): 141 | # Weight input is a scalar 142 | self.H = self.weight_input * np.eye(self.udim) 143 | 144 | elif self.weight_input.shape == (self.udim, 1): 145 | # Weight_input is a vector, use it to form the diagonal of the H matrix 146 | self.H = np.diag(self.weight_input) 147 | 148 | elif self.weight_input.shape == (self.udim, self.udim): 149 | # Weight_input is a udim * udim matrix 150 | self.H = np.copy(self.weight_input) 151 | else: 152 | self.H = np.eye(self.udim) 153 | 154 | V = self.clf(x) 155 | lf_V = self.lf_clf(x) 156 | lg_V = self.lg_clf(x) 157 | 158 | B = self.cbf(x) 159 | lf_B = self.lf_cbf(x) 160 | lg_B = self.lg_cbf(x) 161 | 162 | if self.with_slack: 163 | # slack variable is activated 164 | # Constraints: A [u; slack] <= b 165 | # LfV + LgV * u + lambda * V <= slack 166 | # LfB + LgB * u + gamma * B >= 0 167 | lg_V = np.hstack((lg_V, -np.ones((1, 1)))) 168 | lg_B = np.hstack((-lg_B, np.zeros((1, 1)))) 169 | 170 | self.A = np.vstack((lg_V, lg_B)) 171 | self.b = np.hstack((-lf_V - self.clf_lambda * V, lf_B + self.cbf_gamma * B)) 172 | 173 | # make sure that b is just a 1D vector with the shape (udim+1,) 174 | self.b = np.atleast_2d(self.b)[0] 175 | 176 | # Slack -> unconstrained 177 | u_min = np.hstack((self.u_min, -inf * np.ones(1))) 178 | u_max = np.hstack((self.u_max, inf * np.ones(1))) 179 | 180 | u = cp.Variable(self.udim + 1) 181 | 182 | # H_new = [H, 0; 0, p] 183 | self.slack_H = np.hstack((self.H, np.zeros((1, 1)))) 184 | self.slack_H = np.vstack((self.slack_H, np.hstack((np.zeros((1, 1)), self.weight_slack * np.ones((1, 1)))))) 185 | 186 | # Cost -> (u-u_ref)' * H_new * (u-u_ref) + p * delta**2 187 | # -> (1/2) * [u slack]' * H_new * [u slack] - [u slack]' * H_new * [u_ref 0] 188 | u_ref = np.hstack((u_ref, np.zeros(1))) 189 | objective = cp.Minimize((1/2) * cp.quad_form(u, self.slack_H) - (self.slack_H @ u_ref).T @ u) 190 | 191 | # Constraints: A * u <= b and u_min, u_max 192 | constraints = [u_min <= u, u <= u_max, self.A @ u <= self.b] 193 | # constraints = [self.u_min <= u, u <= self.u_max, np.eye(2) @ u <= np.zeros(2)] 194 | 195 | problem = cp.Problem(objective, constraints) 196 | 197 | problem.solve() 198 | 199 | # what if infeasible? 200 | if problem.status != 'infeasible': 201 | slack = u.value[-1] 202 | u = u.value[:self.udim] 203 | feas = 1 204 | else: 205 | u = None 206 | slack = None 207 | feas = -1 208 | 209 | else: 210 | # Slack variable is not activated: 211 | # Constraints: A u <= b 212 | # LfV + LgV * u + lambda * V <= 0 213 | # LfB + LgB * u + gamma * B >= 0 214 | self.A = np.vstack((lg_V, -lg_B)) 215 | # b -> one dimensional vector 216 | self.b = np.hstack((-lf_V - self.clf_lambda * V, lf_B + self.cbf_gamma * B)) 217 | self.b = np.atleast_2d(self.b)[0] 218 | 219 | u = cp.Variable(self.udim) 220 | 221 | # Cost -> (u-u_ref)' * H * (u-u_ref) -> (1/2) * u'*H*u - u'*H*u_ref 222 | objective = cp.Minimize((1/2)*cp.quad_form(u, self.H) - (self.H @ u_ref).T @ u) 223 | 224 | # cons: A * u <= b and u_min, u_max 225 | constraints = [self.u_min <= u, u <= self.u_max, self.A @ x <= self.b] 226 | 227 | problem = cp.Problem(objective, constraints) 228 | 229 | problem.solve() 230 | 231 | if problem.status != 'infeasible': 232 | u = u.value 233 | feas = 1 234 | else: 235 | u = None 236 | feas = -1 237 | 238 | return u, slack, B, V, feas 239 | 240 | 241 | 242 | -------------------------------------------------------------------------------- /define_system.py: -------------------------------------------------------------------------------- 1 | ''' 2 | ===================================== 3 | Author : Muhan Zhao 4 | Date : Feb. 11, 2020 5 | Location: UC San Diego, La Jolla, CA 6 | ===================================== 7 | ''' 8 | 9 | import sympy as sp 10 | from sympy.utilities.lambdify import lambdify 11 | import numpy as np 12 | 13 | 14 | class ControlAffineSystem: 15 | ''' 16 | This class defines the dynamic of the control affine system: 17 | 18 | dx = f(x) + g(x) * u 19 | f -> 2D column-wise vector 20 | g -> 2D column-wise vector 21 | 22 | This class also includes the control barrier function (cbf) and control Lyapunov function (clf) in a symbolic way. 23 | 24 | This class has the following methods: 25 | 26 | - Compute the Lie derivative of CLF and CBF w.r.t. f(x) and g(x): 27 | 28 | 29 | 30 | ''' 31 | def __init__(self, system): 32 | self.x = system.x # column-wise vector 33 | self.xdim = system.x.shape[0] 34 | self.xdim = self.x.shape[0] 35 | self.udim = system.udim 36 | 37 | self.f = None 38 | self.f_symbolic = None 39 | 40 | self.g = None 41 | self.g_symbolic = None 42 | 43 | self.cbf = None 44 | self.cbf_symbolic = None 45 | self.clf = None 46 | self.clf_symbolic = None 47 | 48 | # Lie derivative of clf w.r.t f as a function 49 | self.lf_clf = None 50 | self.lf_clf_symbolic = None 51 | 52 | # Lie derivative of clf w.r.t g as a function 53 | self.lg_clf = None 54 | self.lg_clf_symbolic = None 55 | 56 | # Lie derivative of cbf w.r.t f as a function 57 | self.lf_cbf = None 58 | self.lf_cbf_symbolic = None 59 | 60 | # Lie derivative of cbf w.r.t f as a function 61 | self.lg_cbf = None 62 | self.lg_cbf_symbolic = None 63 | 64 | self.define_system(system) 65 | self.define_cbf(system.cbf) 66 | self.define_clf(system.clf) 67 | self.lie_derivatives_calculator() 68 | 69 | def define_system(self, dynamic_system_class): 70 | # todo check f and g both are symbolic expression 71 | 72 | # f & g are both functions; given input as num_states-by-, the output is num_states-by-1 73 | self.f_symbolic = dynamic_system_class.f 74 | self.f = lambdify(np.array(self.x.T), self.f_symbolic, 'numpy') 75 | if self.f(np.ones(self.xdim)).shape != (self.xdim, 1): 76 | raise ValueError(f'The output of f(x) should be (xdim, 1), now it is {self.f(np.ones(self.xdim)).shape}') 77 | 78 | self.g_symbolic = dynamic_system_class.g 79 | self.g = lambdify(np.array(self.x.T), self.g_symbolic, 'numpy') 80 | if self.g(np.ones(self.xdim)).shape != (self.xdim, 1): 81 | raise ValueError(f'The output of g(x) should be (xdim, 1), now it is {self.g(np.ones(self.xdim)).shape}') 82 | 83 | def define_cbf(self, cbf): 84 | """ 85 | Define the symbolic control barrier function 86 | :param cbf: 87 | :return: 88 | """ 89 | # todo check cbf is symbolic input 90 | # clf: given input as num_states-by-, the output is a scalar 91 | self.cbf_symbolic = cbf 92 | # input for cbf has to be a 2D column-wise vector with size (self.xdim, 1) 93 | self.cbf = lambdify(np.array(self.x.T), self.cbf_symbolic, 'numpy') 94 | 95 | def define_clf(self, clf): 96 | """ 97 | Define the symbolic control Lyapunov function 98 | :param clf: 99 | :return: 100 | """ 101 | # todo check clf is symbolic input 102 | # lf_clf: given input as num_states-by-, the output is a 1-by-1 array 103 | self.clf_symbolic = clf 104 | # input for clf has to be a column-wise vector with size (self.xdim, 1) 105 | self.clf = lambdify(np.array(self.x.T), self.clf_symbolic, 'numpy') 106 | 107 | def lie_derivatives_calculator(self): 108 | """ 109 | Compute the Lie derivatives of CBF and CLF w.r.t to x 110 | :return: 111 | """ 112 | dx_cbf_symbolic = sp.Matrix([self.cbf_symbolic]).jacobian(self.x) 113 | 114 | self.lf_cbf_symbolic = dx_cbf_symbolic * self.f_symbolic 115 | self.lg_cbf_symbolic = dx_cbf_symbolic * self.g_symbolic 116 | 117 | # input for lf_cbf and lg_cbf has to be a column-wise vector with size (self.xdim, 1) 118 | # lf_clf: given input as num_states-by-, the output is a 1-by-1 array 119 | self.lf_cbf = lambdify(np.array(self.x.T), self.lf_cbf_symbolic, 'numpy') 120 | # lg_cbf: given input as num_states-by-, the output is a x-by-1 array, x depends on CLF and udim; 121 | self.lg_cbf = lambdify(np.array(self.x.T), self.lg_cbf_symbolic, 'numpy') 122 | 123 | dx_clf_symbolic = sp.Matrix([self.clf_symbolic]).jacobian(self.x) 124 | 125 | self.lf_clf_symbolic = dx_clf_symbolic * self.f_symbolic 126 | self.lg_clf_symbolic = dx_clf_symbolic * self.g_symbolic 127 | 128 | # input for lf_clf and lg_clf has to be a column-wise vector with size (self.xdim, 1) 129 | # lf_clf: given input as num_states-by-, the output is a 1-by-1 array 130 | self.lf_clf = lambdify(np.array(self.x.T), self.lf_clf_symbolic, 'numpy') 131 | # lg_clf: given input as num_states-by-, the output is a x-by-1 array, x depends on CLF and udim 132 | self.lg_clf = lambdify(np.array(self.x.T), self.lg_clf_symbolic, 'numpy') 133 | 134 | def __str__(self): 135 | return f'Class contains the states {self.x}, \n' + \ 136 | f'system dynamic f {self.f} and g {self.g} \n' \ 137 | f'CLF {self.clf}, \n' + \ 138 | f'CBF {self.cbf}, \n' 139 | 140 | def __repr__(self): 141 | # TODO 142 | return f'Class contains the states {self.x}' 143 | 144 | -------------------------------------------------------------------------------- /images/cbf.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/kimukook/CLF-CBF-python/28a46a4f9abf095e1f1b92e6cc056956caab5374/images/cbf.png -------------------------------------------------------------------------------- /images/clf.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/kimukook/CLF-CBF-python/28a46a4f9abf095e1f1b92e6cc056956caab5374/images/clf.png -------------------------------------------------------------------------------- /images/control.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/kimukook/CLF-CBF-python/28a46a4f9abf095e1f1b92e6cc056956caab5374/images/control.png -------------------------------------------------------------------------------- /images/relative_distance.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/kimukook/CLF-CBF-python/28a46a4f9abf095e1f1b92e6cc056956caab5374/images/relative_distance.png -------------------------------------------------------------------------------- /images/slack.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/kimukook/CLF-CBF-python/28a46a4f9abf095e1f1b92e6cc056956caab5374/images/slack.png -------------------------------------------------------------------------------- /images/velocity.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/kimukook/CLF-CBF-python/28a46a4f9abf095e1f1b92e6cc056956caab5374/images/velocity.png -------------------------------------------------------------------------------- /inverted_pendulum.py: -------------------------------------------------------------------------------- 1 | ''' 2 | ===================================== 3 | Author : Muhan Zhao 4 | Date : Feb. 11, 2020 5 | Location: UC San Diego, La Jolla, CA 6 | ===================================== 7 | ''' 8 | import numpy as np 9 | import sympy as sp 10 | from define_system import ControlAffineSystem 11 | 12 | 13 | class InvertedPendulum: 14 | ''' 15 | Define the symbolic dynamic: dx = f(x) + g(x) * u 16 | x contains two states: x1 -> theta x2 -> theta-dot 17 | 18 | 19 | ''' 20 | def __init__(self, l, m, g, b=0): 21 | self.l = l 22 | self.m = m 23 | self.g = g 24 | self.b = b 25 | x1, x2 = sp.symbols('x1 x2') 26 | self.x = [x1, x2] 27 | self.f, self.g = self.inverted_pendulum_dynamics() 28 | 29 | def inverted_pendulum_dynamics(self): 30 | 31 | f = sp.Matrix([[self.x[2]], 32 | [-self.b*self.x[2] + (self.m*self.g*self.l*np.sin(self.x[0])/2)/(self.m*self.l**2/3)]]) 33 | g = sp.Matrix([[0], [-1/(self.m*self.l**2/3)]]) 34 | return f, g 35 | 36 | 37 | 38 | 39 | -------------------------------------------------------------------------------- /ode_solver.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | 3 | 4 | class OdeSolver: 5 | """ 6 | 7 | """ 8 | def __init__(self, system, dt): 9 | self.f = system.f 10 | self.g = system.g 11 | self.dt = dt 12 | 13 | def dynamic(self, x, u): 14 | return self.f(x) + self.g(x) @ np.atleast_2d(u) 15 | 16 | def time_marching(self, x, u): 17 | return self.runge_kutta4(x, u) 18 | 19 | def runge_kutta4(self, x, u): 20 | # issue might be raised on f1->f4, all has to be 1D row-vector in numpy 21 | f1 = self.dynamic(x, u).T[0] 22 | f2 = self.dynamic(x + self.dt/2 * f1, u).T[0] 23 | f3 = self.dynamic(x + self.dt/2 * f2, u).T[0] 24 | f4 = self.dynamic(x + self.dt * f3, u).T[0] 25 | x_new = x + self.dt/6 * (f1 + 2*f2 + 2*f3 + f4) 26 | return x_new 27 | -------------------------------------------------------------------------------- /requirements.txt: -------------------------------------------------------------------------------- 1 | sympy 2 | cvxpy 3 | numpy 4 | --------------------------------------------------------------------------------