├── .DS_Store ├── .gitattributes ├── .gitignore ├── Cases ├── CExample.py ├── Case.py ├── Darboux.py ├── LinearSat.py ├── ObsAvoid.py ├── Quads.py └── __pycache__ │ ├── Case.cpython-310.pyc │ ├── Case.cpython-311.pyc │ ├── LinearSat.cpython-310.pyc │ ├── LinearSat.cpython-311.pyc │ ├── ObsAvoid.cpython-310.pyc │ ├── ObsAvoid.cpython-311.pyc │ └── Quads.cpython-310.pyc ├── Controller.py ├── FTEst ├── EKF.py ├── FTEst.py ├── RdObsEKF.py ├── SensorFaults.py ├── ekftest.py └── linearKF.py ├── FTFrameworkLinearSat.py ├── FTNCBF_comp.gif ├── ICRA2024_FTNCBF_CameraReady.pdf ├── LICENSE ├── Modules ├── NCBF.py └── NNet.py ├── NCBFSynth ├── NBF_Synth.py ├── NCBF_Synth.py ├── NCBF_SynthBF.py ├── NCBF_SynthCBF.py └── NCBF_Synth_AutoTune.py ├── README.md ├── SNBF_Synth.py ├── SNCBF_Synth.py ├── Visualization ├── ObsVis.py ├── Plot_6Dbound.py ├── Plot_6Dtraj.py ├── Plot_TrainingData.py ├── visualization.py └── visualizer.py ├── car.py ├── main.py ├── main_FTLinearSat.py ├── main_Obs.py ├── requirements.txt └── util.py /.DS_Store: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/HongchaoZhang-HZ/FTNCBF/82239a3b0973f8d5f629e7230f9887895b315095/.DS_Store -------------------------------------------------------------------------------- /.gitattributes: -------------------------------------------------------------------------------- 1 | # Auto detect text files and perform LF normalization 2 | * text=auto 3 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | runs/ 2 | Traned_model/ 3 | Critic_Synth/ 4 | CRNN_v1.py 5 | CRNN_v2.py 6 | CRNN.py 7 | GAN.py 8 | *.csv 9 | *.pt 10 | __pycache__/ 11 | __pycache__ 12 | NCBFSynth/runs/ -------------------------------------------------------------------------------- /Cases/CExample.py: -------------------------------------------------------------------------------- 1 | import torch 2 | 3 | from Cases.Case import * 4 | 5 | class CExample(case): 6 | ''' 7 | Define classical control case Obstacle Avoidance 8 | x0_dot = x1 + u 9 | x1_dot = -x1 + 5 x2 10 | ''' 11 | def __init__(self): 12 | DOMAIN = [[-4, 4], [-4, 4]] 13 | CTRLDOM = [[-2, 2]] 14 | discrete = False 15 | super().__init__(DOMAIN, CTRLDOM, discrete=discrete) 16 | 17 | def f_x(self, x): 18 | ''' 19 | Control affine model f(x) 20 | f0 = x1 21 | f1 = -x1 + 5 x2 22 | :param x: [np.array/torch.Tensor] input state x in R^n 23 | :return: [np.array/torch.Tensor] output in R^n 24 | ''' 25 | 26 | x0_dot = x[:, 0] 27 | x1_dot = -x[:, 0] + 5*x[:, 1] 28 | x_dot = torch.vstack([x0_dot, x1_dot]) 29 | return x_dot 30 | 31 | def g_x(self, x): 32 | ''' 33 | Control affine model g(x)=[1 0]' 34 | :param x: [np.array/torch.Tensor] input state x in R^n 35 | :return: [np.array/torch.Tensor] output in R^n 36 | ''' 37 | 38 | g_x0 = torch.ones(len(x)) 39 | g_x1 = torch.zeros(len(x)) 40 | gx = torch.vstack([g_x0, g_x1]) 41 | return gx 42 | 43 | def h_x(self, x): 44 | ''' 45 | Define safe region C:={x|h_x(x) >= 0} 46 | The safe region is a pole centered at (0,0,any) with radius 0.2 47 | :param x: [np.array/torch.Tensor] input state x in R^n 48 | :return: [np.array/torch.Tensor] scalar output in R 49 | ''' 50 | hx = -(x[:, 0]**2 + x[:, 1] ** 2) + 9 51 | return hx 52 | -------------------------------------------------------------------------------- /Cases/Case.py: -------------------------------------------------------------------------------- 1 | import scipy 2 | import torch 3 | import sympy as sp 4 | 5 | class case: 6 | ''' 7 | Define base class for different case studies 8 | Input Basic information including 9 | DOMAIN: State space 10 | CTRLDOM: Control space 11 | discrete: If the system is discrete-time (DT) then True 12 | otherwise False for continuous-time (CT) 13 | Background: 14 | The system is considered as CT/DT nonlinear control-affine system 15 | with state 'x' whose dimension is determined by len(DOMAIN). 16 | The dynamical model is defined as follows: 17 | DT: x_nxt = f(x) + g(x) * u 18 | CT: x_dot = f(x) + g(x) * u 19 | ''' 20 | def __init__(self, DOMAIN: list, CTRLDOM: list, discrete=False): 21 | ''' 22 | :param DOMAIN: [list] State space 23 | :param CTRLDOM: [list] Control space 24 | :param discrete: [bool] If the system is discrete-time then True 25 | ''' 26 | self.CTRLDOM = CTRLDOM 27 | self.CTRLDIM = len(self.CTRLDOM) 28 | self.DOMAIN = DOMAIN 29 | self.DIM = len(self.DOMAIN) 30 | self.discrete = discrete 31 | 32 | def f_x(self, x): 33 | ''' 34 | Control affine model f(x) 35 | :param x: [np.array/torch.Tensor] input state x in R^n 36 | :return: [np.array/torch.Tensor] output in R^n 37 | ''' 38 | f_x = self.fx * x 39 | return f_x 40 | 41 | def g_x(self, x): 42 | ''' 43 | Control affine model g(x) 44 | :param x: [np.array/torch.Tensor] input state x in R^n 45 | :return: [np.array/torch.Tensor] output in R^n 46 | ''' 47 | g_x = self.gx * x 48 | return g_x 49 | 50 | def xdot(self, x, u): 51 | ''' 52 | The dynamical model is defined as follows: 53 | DT: x_nxt = f(x) + g(x) * u 54 | CT: x_dot = f(x) + g(x) * u 55 | :param x: [np.array/torch.Tensor] input state x in R^n 56 | :param u: [np.array/torch.Tensor] control input u in R^m 57 | :return: [np.array/torch.Tensor] output in R^n 58 | ''' 59 | xdot = self.f_x(x) + self.g_x(x) @ u 60 | return xdot -------------------------------------------------------------------------------- /Cases/Darboux.py: -------------------------------------------------------------------------------- 1 | import torch 2 | 3 | from Cases.Case import * 4 | 5 | class Darboux(case): 6 | ''' 7 | Define classical control case Darboux 8 | ''' 9 | def __init__(self): 10 | ''' 11 | Define classical control case Darboux. 12 | The system is 2D open-loop nonlinear CT system 13 | ''' 14 | DOMAIN = [[-2, 2], [-2, 2]] 15 | CTRLDOM = [[0, 0]] 16 | discrete = False 17 | super().__init__(DOMAIN, CTRLDOM, discrete=discrete) 18 | 19 | def f_x(self, x): 20 | ''' 21 | Control affine model f(x) 22 | f0 = x0 + 2*x0*x1 23 | f1 = -x0 + 2*x0^2 - x1^2 24 | :param x: [np.array/torch.Tensor] input state x in R^n 25 | :return: [np.array/torch.Tensor] output in R^n 26 | ''' 27 | x0_dot = x[:, 0] + 2 * x[:, 0] * x[:, 1] 28 | x1_dot = -x[:, 0] + 2 * x[:, 0] ** 2 - x[:, 1] ** 2 29 | x_dot = torch.vstack([x0_dot, x1_dot]) 30 | return x_dot 31 | 32 | def g_x(self, x): 33 | ''' 34 | Control affine model g(x)=[0 0 0]' 35 | :param x: [np.array/torch.Tensor] input state x in R^n 36 | :return: [np.array/torch.Tensor] output in R^n 37 | ''' 38 | gx = torch.zeros([self.DIM, 1]) 39 | return gx 40 | 41 | def h_x(self, x): 42 | ''' 43 | Define safe region C:={x|h_x(x) >= 0} 44 | :param x: [np.array/torch.Tensor] input state x in R^n 45 | :return: [np.array/torch.Tensor] scalar output in R 46 | ''' 47 | hx = (x[:, 0] + x[:, 1] ** 2) 48 | return hx 49 | -------------------------------------------------------------------------------- /Cases/LinearSat.py: -------------------------------------------------------------------------------- 1 | import torch 2 | from math import sqrt 3 | from Cases.Case import * 4 | 5 | class LinearSat(case): 6 | ''' 7 | Define Quadcopters model 8 | f(x, u) = [x(3); x(4); x(5); 9 | (-x(2)/mass) *(u(0)+u(1)); 10 | (1/mass) *(u(0)+u(1))-gravity; 11 | (length/inertia) *(u(0)-u(1))] 12 | ''' 13 | def __init__(self): 14 | DOMAIN = [[-2, 2], [-2, 2], [-2, 2], [-2, 2], [-2, 2], [-2, 2]] 15 | CTRLDOM = [[-2, 2], [-2, 2], [-2, 2]] 16 | discrete = False 17 | super().__init__(DOMAIN, CTRLDOM, discrete=discrete) 18 | 19 | def f_x(self, x): 20 | ''' 21 | Control affine model f(x) 22 | f(x) = [x(3); x(4); x(5); 23 | 0; -gravity; 0] 24 | :param x: [np.array/torch.Tensor] input state x in R^n 25 | :return: [np.array/torch.Tensor] output in R^n 26 | ''' 27 | MU = 3.986e14 28 | a = 500e3 29 | n = sqrt(MU / a ** 3) 30 | A = torch.Tensor([[1, 0, 0, 0, 0, 0], 31 | [0, 1, 0, 0, 0, 0], 32 | [0, 0, 1, 0, 0, 0], 33 | [3*n**2, 0, 0, 0, 2*n, 0], 34 | [0, 0, 0, -2*n, 0, 0], 35 | [0, 0, -n**2, 0, 0, 0]]) 36 | fx_dot = (A @ x.unsqueeze(-1)).squeeze() 37 | return fx_dot 38 | 39 | def g_x(self, x): 40 | ''' 41 | Control affine model 42 | g(x) = [0 0 0; 43 | 0 0 0; 44 | 0 0 0; 45 | 1 0 0; 46 | 0 1 0; 47 | 0 0 1;] 48 | :param x: [np.array/torch.Tensor] input state x in R^n 49 | :return: [np.array/torch.Tensor] output in R^n 50 | ''' 51 | B = torch.Tensor([[0, 0, 0], 52 | [0, 0, 0], 53 | [0, 0, 0], 54 | [1, 0, 0], 55 | [0, 1, 0], 56 | [0, 0, 1]]) 57 | gx_dot = B 58 | return gx_dot 59 | 60 | def h_x(self, x): 61 | ''' 62 | Define safe region C:={x|h_x(x) >= 0} 63 | The safe region is a pole centered at (0,0,any) with radius 0.2 64 | :param x: [np.array/torch.Tensor] input state x in R^n 65 | :return: [np.array/torch.Tensor] scalar output in R 66 | ''' 67 | r = torch.sqrt(torch.sum(x[:, :3]**2, dim=1)) 68 | in_range = (r >= 0.25) & (r <= 1.5) 69 | 70 | # Define the function for the in-range and out-of-range cases 71 | h_in_range = torch.exp(-1 / (r ** 2)) # You can choose any differentiable function here 72 | h_out_of_range = -torch.exp(-1 / (r ** 2)) # You can choose any differentiable function here 73 | 74 | # Combine the two cases using a conditional statement 75 | return torch.where(in_range, h_in_range, h_out_of_range) 76 | # return -hx 77 | -------------------------------------------------------------------------------- /Cases/ObsAvoid.py: -------------------------------------------------------------------------------- 1 | import torch 2 | 3 | from Cases.Case import * 4 | 5 | class ObsAvoid(case): 6 | ''' 7 | Define classical control case Obstacle Avoidance 8 | x0_dot = v sin(phi) + 0 9 | x1_dot = v cos(phi) + 0 10 | phi_dot = 0 + u 11 | ''' 12 | def __init__(self): 13 | DOMAIN = [[-2, 2], [-2, 2], [-2, 2]] 14 | CTRLDOM = [[-2, 2]] 15 | discrete = False 16 | self.v = 1 17 | super().__init__(DOMAIN, CTRLDOM, discrete=discrete) 18 | 19 | def f_x(self, x): 20 | ''' 21 | Control affine model f(x) 22 | f0 = v sin(phi) 23 | f1 = v cos(phi) 24 | f2 = 0 25 | :param x: [np.array/torch.Tensor] input state x in R^n 26 | :return: [np.array/torch.Tensor] output in R^n 27 | ''' 28 | 29 | v = self.v 30 | x0_dot = v * torch.sin(x[:, 2]) 31 | x1_dot = v * torch.cos(x[:, 2]) 32 | phi_dot = torch.zeros([len(x)]) 33 | x_dot = torch.vstack([x0_dot, x1_dot, phi_dot]) 34 | return x_dot 35 | 36 | def g_x(self, x): 37 | ''' 38 | Control affine model g(x)=[0 0 1]' 39 | :param x: [np.array/torch.Tensor] input state x in R^n 40 | :return: [np.array/torch.Tensor] output in R^n 41 | ''' 42 | g_x0 = torch.zeros(len(x)) 43 | g_x1 = torch.zeros(len(x)) 44 | g_phi = torch.ones(len(x)) 45 | gx = torch.vstack([g_x0, g_x1, g_phi]) 46 | return gx 47 | 48 | def h_x(self, x): 49 | ''' 50 | Define safe region C:={x|h_x(x) >= 0} 51 | The safe region is a pole centered at (0,0,any) with radius 0.2 52 | :param x: [np.array/torch.Tensor] input state x in R^n 53 | :return: [np.array/torch.Tensor] scalar output in R 54 | ''' 55 | hx = (x[:, 0]**2 + x[:, 1] ** 2) - 0.04 56 | return hx 57 | -------------------------------------------------------------------------------- /Cases/Quads.py: -------------------------------------------------------------------------------- 1 | import torch 2 | 3 | from Cases.Case import * 4 | 5 | class Quads(case): 6 | ''' 7 | Define Quadcopters model 8 | f(x, u) = [x(3); x(4); x(5); 9 | (-x(2)/mass) *(u(0)+u(1)); 10 | (1/mass) *(u(0)+u(1))-gravity; 11 | (length/inertia) *(u(0)-u(1))] 12 | ''' 13 | def __init__(self): 14 | DOMAIN = [[-2, 2], [-2, 2], [-2, 2], [-2, 2], [-2, 2], [-2, 2]] 15 | CTRLDOM = [[-2, 2], [-2, 2]] 16 | self.length = 0.25 17 | self.mass = 0.486 18 | self.inertia = 0.00383 19 | self.gravity = 9.81 20 | discrete = False 21 | super().__init__(DOMAIN, CTRLDOM, discrete=discrete) 22 | 23 | def f_x(self, x): 24 | ''' 25 | Control affine model f(x) 26 | f(x) = [x(3); x(4); x(5); 27 | 0; -gravity; 0] 28 | :param x: [np.array/torch.Tensor] input state x in R^n 29 | :return: [np.array/torch.Tensor] output in R^n 30 | ''' 31 | 32 | x0 = x[:, 3] 33 | x1 = x[:, 4] 34 | x2 = x[:, 5] 35 | x3 = torch.zeros(len(x)) 36 | x4 = -self.gravity * torch.ones(len(x)) 37 | x5 = torch.zeros(len(x)) 38 | x_dot = torch.vstack([x0, x1, x2, x3, x4, x5]) 39 | return x_dot 40 | 41 | def g_x(self, x): 42 | ''' 43 | Control affine model 44 | g(x) = [0 0; 45 | 0 0; 46 | 0 0; 47 | (-x(3)/mass) (-x(3)/mass); 48 | (1/mass) (1/mass); 49 | (length/inertia) -(length/inertia)] 50 | :param x: [np.array/torch.Tensor] input state x in R^n 51 | :return: [np.array/torch.Tensor] output in R^n 52 | ''' 53 | g_x0 = torch.zeros([2, len(x)]) 54 | g_x1 = torch.zeros([2, len(x)]) 55 | g_x2 = torch.zeros([2, len(x)]) 56 | g_x3 = torch.vstack([-x[:, 3]/self.mass, -x[:, 3]/self.mass]) 57 | g_x4 = torch.vstack([1/self.mass * torch.ones([len(x)]), 1/self.mass * torch.ones([len(x)])]) 58 | g_x5 = torch.vstack([self.length/self.inertia * torch.ones([len(x)]), 59 | -self.length/self.inertia * torch.ones([len(x)])]) 60 | gx = torch.dstack([g_x0, g_x1, g_x2, g_x3, g_x4, g_x5]) 61 | return gx.reshape([self.DIM, len(x), self.CTRLDIM]) 62 | 63 | def h_x(self, x): 64 | ''' 65 | Define safe region C:={x|h_x(x) >= 0} 66 | The safe region is a pole centered at (0,0,any) with radius 0.2 67 | :param x: [np.array/torch.Tensor] input state x in R^n 68 | :return: [np.array/torch.Tensor] scalar output in R 69 | ''' 70 | hx = (x[:, 2]**2 ) - 1 71 | return -hx 72 | -------------------------------------------------------------------------------- /Cases/__pycache__/Case.cpython-310.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/HongchaoZhang-HZ/FTNCBF/82239a3b0973f8d5f629e7230f9887895b315095/Cases/__pycache__/Case.cpython-310.pyc -------------------------------------------------------------------------------- /Cases/__pycache__/Case.cpython-311.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/HongchaoZhang-HZ/FTNCBF/82239a3b0973f8d5f629e7230f9887895b315095/Cases/__pycache__/Case.cpython-311.pyc -------------------------------------------------------------------------------- /Cases/__pycache__/LinearSat.cpython-310.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/HongchaoZhang-HZ/FTNCBF/82239a3b0973f8d5f629e7230f9887895b315095/Cases/__pycache__/LinearSat.cpython-310.pyc -------------------------------------------------------------------------------- /Cases/__pycache__/LinearSat.cpython-311.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/HongchaoZhang-HZ/FTNCBF/82239a3b0973f8d5f629e7230f9887895b315095/Cases/__pycache__/LinearSat.cpython-311.pyc -------------------------------------------------------------------------------- /Cases/__pycache__/ObsAvoid.cpython-310.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/HongchaoZhang-HZ/FTNCBF/82239a3b0973f8d5f629e7230f9887895b315095/Cases/__pycache__/ObsAvoid.cpython-310.pyc -------------------------------------------------------------------------------- /Cases/__pycache__/ObsAvoid.cpython-311.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/HongchaoZhang-HZ/FTNCBF/82239a3b0973f8d5f629e7230f9887895b315095/Cases/__pycache__/ObsAvoid.cpython-311.pyc -------------------------------------------------------------------------------- /Cases/__pycache__/Quads.cpython-310.pyc: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/HongchaoZhang-HZ/FTNCBF/82239a3b0973f8d5f629e7230f9887895b315095/Cases/__pycache__/Quads.cpython-310.pyc -------------------------------------------------------------------------------- /Controller.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import torch 3 | from Cases.Case import case 4 | from scipy.optimize import NonlinearConstraint 5 | from scipy.optimize import minimize 6 | from torch.autograd.functional import hessian 7 | 8 | from FTEst.FTEst import FTEst 9 | from SNCBF_Synth import * 10 | from Cases.ObsAvoid import ObsAvoid 11 | from FTEst.SensorFaults import * 12 | 13 | class NCBFCtrl: 14 | def __init__(self, DIM, SNCBF_list, FTEst, 15 | case: object, 16 | sigma, nu, 17 | gamma_list): 18 | self.DIM = DIM 19 | self.SNCBF_list = SNCBF_list 20 | self.num_SNCBF = len(SNCBF_list) 21 | self.FTEst = FTEst 22 | self.FTEKF_gain_list = self.FTEst.EKFgain_list 23 | self.case = case 24 | self.fx = self.case.f_x 25 | self.gx = self.case.g_x 26 | self.gamma_list = gamma_list 27 | self.sigma = sigma 28 | self.nu = nu 29 | # self.grad_max_list = self.grad_max_Init() 30 | 31 | def compute_u(self, x): 32 | def fcn(u): 33 | return (u**2).sum() 34 | # minimize ||u|| 35 | u0 = np.array([0]) 36 | res = minimize(fcn, u0) 37 | return res 38 | 39 | def d1bdx1(self, SNCBF, x:float): 40 | grad_input = torch.tensor(x, requires_grad=True) 41 | dbdx = torch.autograd.grad(SNCBF.model.forward(grad_input), grad_input) 42 | return dbdx 43 | 44 | def d2bdx2(self, SNCBF, x): 45 | # grad_input = torch.tensor([[0.0,0.0,0.0]], requires_grad=True) 46 | # out = FTNCBF.SNCBF_list[0].model.forward(grad_input) 47 | # out.backward(create_graph=True) # first order grad 48 | # out.backward(retain_graph=True) # second order grad 49 | 50 | 51 | grad_input = torch.tensor(x, dtype=torch.float, requires_grad=True) 52 | hessian_matrix = hessian(SNCBF.model.forward, grad_input).squeeze() 53 | 54 | # grad_input = torch.tensor(x, requires_grad=True) 55 | # d2bdx2 = torch.autograd.grad(self.d1bdx1(SNCBF, grad_input), 56 | # SNCBF.model.forward(grad_input)) 57 | return hessian_matrix 58 | 59 | # def grad_max_Init(self): 60 | # return 61 | 62 | def solo_SCBF_condition(self, SNCBF, x, EKFGain, obsMatrix, gamma): 63 | dbdx = SNCBF.get_grad(x) 64 | # stochastic version 65 | fx = self.fx(torch.Tensor(x).reshape([1, self.DIM])).numpy() 66 | dbdxf = dbdx @ fx 67 | EKF_term = dbdx @ EKFGain @ obsMatrix 68 | # stochastic_term = gamma * np.linalg.norm(EKF_term) - grad_max * gamma 69 | stochastic_term = gamma * np.linalg.norm(EKF_term) 70 | 71 | # second order derivative term 72 | hessian = self.d2bdx2(SNCBF, x) 73 | second_order_term = self.nu.transpose(0, 1).numpy() @ EKFGain.transpose() \ 74 | @ hessian.numpy() @ EKFGain @ self.nu.numpy() 75 | 76 | # if second_order_term.shape == torch.Size([1]): 77 | # trace_term = second_order_term.item() 78 | # else: 79 | # trace_term = second_order_term.trace() 80 | trace_term = second_order_term.trace() 81 | return dbdxf - stochastic_term + trace_term 82 | 83 | def multi_SCBF_conditions(self, x): 84 | cons = [] 85 | gain_list = [] 86 | for SNCBF_idx in range(self.num_SNCBF): 87 | # Update observation matrix 88 | obsMatrix = self.FTEst.fault_list.fault_mask_list[SNCBF_idx] 89 | # Update EKF gain 90 | EKFGain = self.FTEKF_gain_list[SNCBF_idx] 91 | # Compute SCBF constraint 92 | SCBF_cons = self.solo_SCBF_condition(self.SNCBF_list[SNCBF_idx], 93 | x, EKFGain, obsMatrix, 94 | self.gamma_list[SNCBF_idx]) 95 | cons.append(SCBF_cons) 96 | 97 | # Compute Affine Gain 98 | affine_gain = torch.stack(self.SNCBF_list[SNCBF_idx].get_grad(x)) @ self.gx(x) 99 | gain_list.append(affine_gain) 100 | return cons, gain_list 101 | 102 | def CBF_based_u(self, x): 103 | # compute based on self.CBF 104 | SCBF_cons, affine_gain = self.multi_SCBF_conditions(x) 105 | cons = tuple() 106 | for idx in range(self.num_SNCBF): 107 | SoloCBFCon = lambda u: (affine_gain[idx] @ u).squeeze() + (SCBF_cons[idx]).squeeze() 108 | SoloOptCBFCon = NonlinearConstraint(SoloCBFCon, 0, np.inf) 109 | cons = cons + (SoloOptCBFCon,) 110 | def fcn(u): 111 | return (u**2).sum() 112 | # minimize ||u|| 113 | u0 = np.zeros(self.case.CTRLDIM) 114 | # minimize ||u|| 115 | # constraint: affine_gain @ u + self.SCBF_conditions(x) 116 | res = minimize(fcn, u0, constraints=SoloOptCBFCon) 117 | return res 118 | 119 | # sensor_list = SensorSet([0, 1, 1, 2, 2], [0.001, 0.002, 0.0015, 0.001, 0.01]) 120 | # fault_list = FaultPattern(sensor_list, 121 | # fault_target=[[1], [2, 3]], 122 | # fault_value=[[0.1], [0.15, 2]]) 123 | # ObsAvoid = ObsAvoid() 124 | # gamma_list = [0.001, 0.002, 0.0015, 0.001, 0.01] 125 | # SNCBF0 = SNCBF_Synth([32, 32], [True, True], ObsAvoid, verbose=True) 126 | # SNCBF0.model.load_state_dict(torch.load('Trained_model/SNCBF/SNCBFGood/SNCBF_Obs0.pt'), strict=True) 127 | # SNCBF1 = SNCBF_Synth([32, 32], [True, True], ObsAvoid, verbose=True) 128 | # SNCBF1.model.load_state_dict(torch.load('Trained_model/SNCBF/SNCBFGood/SNCBF_Obs1.pt'), strict=True) 129 | # FTEst = FTEst(None, sensor_list, fault_list) 130 | # ctrl = NCBFCtrl(ObsAvoid.DIM, [SNCBF0, SNCBF1], FTEst, ObsAvoid, gamma_list) 131 | # res = ctrl.CBF_based_u(np.array([[0,0,0]],dtype=np.float32)) 132 | -------------------------------------------------------------------------------- /FTEst/EKF.py: -------------------------------------------------------------------------------- 1 | from filterpy.kalman import ExtendedKalmanFilter as EKF 2 | import numpy as np 3 | import sympy 4 | from sympy.abc import alpha, x, y, v, w, R, theta 5 | from sympy import symbols, Matrix 6 | from math import sqrt, tan, cos, sin, atan2 7 | 8 | from filterpy.stats import plot_covariance_ellipse 9 | import matplotlib.pyplot as plt 10 | 11 | class RobotEKF(EKF): 12 | def __init__(self, dt, wheelbase, std_vel, std_steer, verbose=False): 13 | EKF.__init__(self, 3, 3, 1) 14 | self.dt = dt 15 | self.wheelbase = wheelbase 16 | self.std_vel = std_vel 17 | self.std_steer = std_steer 18 | self.verbose = verbose 19 | 20 | a, x, y, v, w, theta, time = symbols( 21 | 'a, x, y, v, w, theta, t') 22 | d = v * time 23 | beta = (d / w) * sympy.tan(a) 24 | r = w / sympy.tan(a) 25 | 26 | self.fxu = Matrix( 27 | [[x - r * sympy.sin(theta) + r * sympy.sin(theta + beta)], 28 | [y + r * sympy.cos(theta) - r * sympy.cos(theta + beta)], 29 | [theta + beta]]) 30 | 31 | self.F_j = self.fxu.jacobian(Matrix([x, y, theta])) 32 | self.V_j = self.fxu.jacobian(Matrix([a])) 33 | 34 | # save dictionary and it's variables for later use 35 | self.subs = {x: 0, y: 0, v: 0, a: 0, 36 | time: dt, w: wheelbase, theta: 0} 37 | self.x_x, self.x_y, = x, y 38 | self.v, self.a, self.theta = v, a, theta 39 | 40 | def predict(self, u): 41 | self.x = self.move(self.x, u, self.dt) 42 | self.subs[self.x_x] = self.x[0, 0] 43 | self.subs[self.x_y] = self.x[1, 0] 44 | 45 | self.subs[self.theta] = self.x[2, 0] 46 | self.subs[self.v] = 1 47 | self.subs[self.a] = u[0] 48 | 49 | F = np.array(self.F_j.evalf(subs=self.subs)).astype(float) 50 | V = np.array(self.V_j.evalf(subs=self.subs)).astype(float) 51 | 52 | # covariance of motion noise in control space 53 | M = np.array([[self.std_steer ** 2]]) 54 | 55 | self.P = F @ self.P @ F.T + V @ M @ V.T 56 | 57 | def move(self, x, u, dt): 58 | hdg = x[2, 0] 59 | vel = 1 60 | steering_angle = u[0] 61 | dist = vel * dt 62 | 63 | if abs(steering_angle) > 0.001: # is robot turning? 64 | beta = (dist / self.wheelbase) * tan(steering_angle) 65 | r = self.wheelbase / tan(steering_angle) # radius 66 | 67 | dx = np.array([[-r * sin(hdg) + r * sin(hdg + beta)], 68 | [r * cos(hdg) - r * cos(hdg + beta)], 69 | [beta]]) 70 | else: # moving in straight line 71 | dx = np.array([[dist * cos(hdg)], 72 | [dist * sin(hdg)], 73 | [0]]) 74 | return x + dx 75 | 76 | def H_of(x, landmark_pos): 77 | """ compute Jacobian of H matrix where h(x) computes 78 | the range and bearing to a landmark for state x """ 79 | 80 | px = landmark_pos[0] 81 | py = landmark_pos[1] 82 | hyp = (px - x[0, 0])**2 + (py - x[1, 0])**2 83 | dist = sqrt(hyp) 84 | 85 | # H = np.array( 86 | # [[-(px - x[0, 0]) / dist, -(py - x[1, 0]) / dist, 0], 87 | # [ (py - x[1, 0]) / hyp, -(px - x[0, 0]) / hyp, -1]]) 88 | H = np.array( 89 | [[-(px - x[0, 0]) / dist, 0, 0], 90 | [0, -(py - x[1, 0]) / dist, 0], 91 | [ (py - x[1, 0]) / hyp, -(px - x[0, 0]) / hyp, -1]]) 92 | return H 93 | 94 | def Hx(x, landmark_pos): 95 | """ takes a state variable and returns the measurement 96 | that would correspond to that state. 97 | """ 98 | px = landmark_pos[0] 99 | py = landmark_pos[1] 100 | # dist = sqrt((px - x[0, 0])**2 + (py - x[1, 0])**2) 101 | 102 | # Hx = np.array([[dist], 103 | # [atan2(py - x[1, 0], px - x[0, 0]) - x[2, 0]]]) 104 | Hx = np.array([[sqrt((px - x[0, 0])**2)], 105 | [sqrt((py - x[1, 0])**2)], 106 | [atan2(py - x[1, 0], px - x[0, 0]) - x[2, 0]]]) 107 | return Hx 108 | 109 | def residual(a, b): 110 | """ compute residual (a-b) between measurements containing 111 | [range, bearing]. Bearing is normalized to [-pi, pi)""" 112 | y = a - b 113 | y[2] = y[2] % (2 * np.pi) # force in range [0, 2 pi) 114 | if y[2] > np.pi: # move to [-pi, pi) 115 | y[2] -= 2 * np.pi 116 | return y 117 | 118 | 119 | dt = 1.0 120 | def z_landmark(lmark, sim_pos, std_rng, std_brg): 121 | x, y = sim_pos[0, 0], sim_pos[1, 0] 122 | d = np.sqrt((lmark[0] - x) ** 2 + (lmark[1] - y) ** 2) 123 | a = atan2(lmark[1] - y, lmark[0] - x) - sim_pos[2, 0] 124 | z = np.array([[d + np.random.randn() * std_rng], 125 | [d + np.random.randn() * std_rng], 126 | [a + np.random.randn() * std_brg]]) 127 | return z 128 | 129 | 130 | def ekf_update(ekf, z, landmark): 131 | ekf.update(z, HJacobian=H_of, Hx=Hx, 132 | residual=residual, 133 | args=(landmark), hx_args=(landmark)) 134 | 135 | 136 | # def run_localization(landmarks, std_vel, std_steer, 137 | # std_range, std_bearing, 138 | # step=10, ellipse_step=20, ylim=None): 139 | # ekf = RobotEKF(dt, wheelbase=0.5, std_vel=std_vel, 140 | # std_steer=std_steer) 141 | # ekf.x = np.array([[2, 6, .3]]).T # x, y, steer angle 142 | # ekf.P = np.diag([.1, .1, .1]) 143 | # ekf.R = np.diag([std_range ** 2, std_range ** 2, std_bearing ** 2]) 144 | # 145 | # sim_pos = ekf.x.copy() # simulated position 146 | # # steering command (vel, steering angle radians) 147 | # u = np.array([.01]) 148 | # 149 | # plt.figure() 150 | # plt.scatter(landmarks[:, 0], landmarks[:, 1], 151 | # marker='s', s=60) 152 | # 153 | # track = [] 154 | # for i in range(200): 155 | # sim_pos = ekf.move(sim_pos, u, dt / 10.) # simulate robot 156 | # track.append(sim_pos) 157 | # # print(ekf.K) 158 | # 159 | # if i % step == 0: 160 | # ekf.predict(u=u) 161 | # 162 | # if i % ellipse_step == 0: 163 | # plot_covariance_ellipse( 164 | # (ekf.x[0, 0], ekf.x[1, 0]), ekf.P[0:2, 0:2], 165 | # std=6, facecolor='k', alpha=0.3) 166 | # 167 | # x, y = sim_pos[0, 0], sim_pos[1, 0] 168 | # for lmark in landmarks: 169 | # z = z_landmark(lmark, sim_pos, 170 | # std_range, std_bearing) 171 | # ekf_update(ekf, z, lmark) 172 | # 173 | # if i % ellipse_step == 0: 174 | # plot_covariance_ellipse( 175 | # (ekf.x[0, 0], ekf.x[1, 0]), ekf.P[0:2, 0:2], 176 | # std=6, facecolor='g', alpha=0.8) 177 | # track = np.array(track) 178 | # plt.plot(track[:, 0], track[:, 1], color='k', lw=2) 179 | # plt.axis('equal') 180 | # plt.title("EKF Robot localization") 181 | # if ylim is not None: plt.ylim(*ylim) 182 | # plt.show() 183 | # return ekf 184 | 185 | # landmarks = np.array([[5, 10, 0.5], [10, 5, 0.5], [15, 15, 0.5]]) 186 | # 187 | # ekf = run_localization( 188 | # landmarks, std_vel=0.1, std_steer=np.radians(1), 189 | # std_range=0.3, std_bearing=0.1) 190 | # print('Final P:', ekf.P.diagonal()) -------------------------------------------------------------------------------- /FTEst/FTEst.py: -------------------------------------------------------------------------------- 1 | from SensorFaults import * 2 | from RdObsEKF import * 3 | 4 | class FTEst: 5 | ''' 6 | Define fault-tolerant estimators. 7 | The base FT-Estimator is: FT-EKF. 8 | FT-EKF maps sensor readings to state estimates. 9 | Input: Faults, Sensors 10 | ''' 11 | def __init__(self, gamma_list, 12 | sensor_list: SensorSet, 13 | fault_list: FaultPattern): 14 | # Initialize sensors and faults 15 | self.sensor_list = sensor_list 16 | self.num_sensors = self.sensor_list.num_sensors 17 | self.fault_list = fault_list 18 | self.num_faults = self.fault_list.num_faults 19 | if gamma_list is None: 20 | self.gamma_list = 0.001 * np.ones(self.num_faults) 21 | else: 22 | self.gamma_list = gamma_list 23 | self.EKF = RdObsEKF(self.sensor_list, dt, wheelbase=0.5, std_vel=0.1, 24 | std_steer=np.radians(1), std_range=0.3, std_bearing=0.1, verbose=False) 25 | self.FTEst_list = [self.EKF] 26 | self.EKFgain_list = [] 27 | self.RdEKF_Init() 28 | self.RdEKF_Trail() 29 | 30 | @property 31 | def get_sensors(self): 32 | return self.sensor_list 33 | 34 | @property 35 | def get_fault_pattern(self): 36 | return self.fault_list 37 | 38 | def RdEKF_Init(self): 39 | for idx in range(self.num_faults): 40 | # TODO: make it customizable 41 | ekf = RdObsEKF(self.sensor_list, dt, wheelbase=0.5, std_vel=0.1, 42 | std_steer=np.radians(1), std_range=0.3, std_bearing=0.1, verbose=False) 43 | ekf.obs_change(self.fault_list.fault_mask_list[idx]) 44 | self.FTEst_list.append(ekf) 45 | 46 | def RdEKF_Trail(self): 47 | # TODO: make it customizable 48 | landmarks = np.array([[5, 10, 0.5], [10, 5, 0.5], [15, 15, 0.5]]) 49 | for est_item in self.FTEst_list: 50 | est_item.run_localization(landmarks) 51 | self.EKFgain_list.append(est_item.K) 52 | 53 | # 54 | # sensor_list = SensorSet([0, 1, 1, 2, 2], [0.001, 0.002, 0.0015, 0.001, 0.01]) 55 | # fault_list = FaultPattern(sensor_list, 56 | # fault_target=[[1], [2, 3]], 57 | # fault_value=[[0.1], [0.15, 2]]) 58 | # FTE = FTEst(None, sensor_list, fault_list) -------------------------------------------------------------------------------- /FTEst/RdObsEKF.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | 3 | from EKF import * 4 | from SensorFaults import * 5 | 6 | 7 | class RdObsEKF(RobotEKF): 8 | def __init__(self, sensors: SensorSet, 9 | dt, wheelbase, std_vel, std_steer, 10 | std_range, std_bearing, verbose=False): 11 | self.verbose = verbose 12 | self.sensor_list = sensors 13 | self.num_sensors = self.sensor_list.num_sensors 14 | self.obsMatrix = self.sensor_list.obs_matrix 15 | # self.obsVector = np.linalg.norm(self.obsMatrix, axis=1).reshape([self.num_sensors, 1]) 16 | # TODO: redundancy check 17 | # TODO: observability check 18 | EKF.__init__(self, 3, self.num_sensors, 1) 19 | self.dt = dt 20 | self.wheelbase = wheelbase 21 | self.std_vel = std_vel 22 | self.std_steer = std_steer 23 | self.std_range = std_range 24 | self.std_bearing = std_bearing 25 | 26 | a, x, y, v, w, theta, time = symbols( 27 | 'a, x, y, v, w, theta, t') 28 | d = v * time 29 | beta = (d / w) * sympy.tan(a) 30 | r = w / sympy.tan(a) 31 | 32 | self.fxu = Matrix( 33 | [[x - r * sympy.sin(theta) + r * sympy.sin(theta + beta)], 34 | [y + r * sympy.cos(theta) - r * sympy.cos(theta + beta)], 35 | [theta + beta]]) 36 | 37 | self.F_j = self.fxu.jacobian(Matrix([x, y, theta])) 38 | self.V_j = self.fxu.jacobian(Matrix([a])) 39 | 40 | # save dictionary and it's variables for later use 41 | self.subs = {x: 0, y: 0, v: 0, a: 0, 42 | time: dt, w: wheelbase, theta: 0} 43 | self.x_x, self.x_y, = x, y 44 | self.v, self.a, self.theta = v, a, theta 45 | 46 | def predict(self, u): 47 | self.x = self.move(self.x, u, self.dt) 48 | self.subs[self.x_x] = self.x[0, 0] 49 | self.subs[self.x_y] = self.x[1, 0] 50 | 51 | self.subs[self.theta] = self.x[2, 0] 52 | self.subs[self.v] = 1 53 | self.subs[self.a] = u[0] 54 | 55 | F = np.array(self.F_j.evalf(subs=self.subs)).astype(float) 56 | V = np.array(self.V_j.evalf(subs=self.subs)).astype(float) 57 | 58 | # covariance of motion noise in control space 59 | M = np.array([[self.std_steer ** 2]]) 60 | 61 | self.P = F @ self.P @ F.T + V @ M @ V.T 62 | 63 | def obs_change(self, new_obsMatrix): 64 | self.obsMatrix = new_obsMatrix 65 | 66 | def move(self, x, u, dt): 67 | hdg = x[2, 0] 68 | vel = 1 69 | steering_angle = u[0] 70 | # dist = vel * dt 71 | dx = np.array([[sin(hdg + steering_angle)], 72 | [cos(hdg + steering_angle)], 73 | [hdg + steering_angle]]) 74 | # if abs(steering_angle) > 0.001: # is robot turning? 75 | # beta = (dist / self.wheelbase) * tan(steering_angle) 76 | # r = self.wheelbase / tan(steering_angle) # radius 77 | # 78 | # dx = np.array([[-r * sin(hdg) + r * sin(hdg + beta)], 79 | # [r * cos(hdg) - r * cos(hdg + beta)], 80 | # [beta]]) 81 | # else: # moving in straight line 82 | # dx = np.array([[dist * cos(hdg)], 83 | # [dist * sin(hdg)], 84 | # [0]]) 85 | return x + dx 86 | # if abs(steering_angle) > 0.001: # is robot turning? 87 | # beta = (dist / self.wheelbase) * tan(steering_angle) 88 | # r = self.wheelbase / tan(steering_angle) # radius 89 | # 90 | # # dx = np.array([[-r * sin(hdg) + r * sin(hdg + beta)], 91 | # # [r * cos(hdg) - r * cos(hdg + beta)], 92 | # # [beta]]) 93 | # dx = np.array([[dist * sin(hdg)], 94 | # [dist * cos(hdg)], 95 | # [u[0] * dt]]) 96 | # else: # moving in straight line 97 | # dx = np.array([[dist * cos(hdg)], 98 | # [dist * sin(hdg)], 99 | # [0]]) 100 | # return x + dx 101 | 102 | def H_of(self, x): 103 | """ compute Jacobian of H matrix where h(x) computes 104 | the range and bearing to a landmark for state x """ 105 | 106 | # px = landmark_pos[0] 107 | # py = landmark_pos[1] 108 | # hyp = (px - x[0, 0]) ** 2 + (py - x[1, 0]) ** 2 109 | # dist = sqrt(hyp) 110 | 111 | # H = np.array( 112 | # [[-(px - x[0, 0]) / dist, -(py - x[1, 0]) / dist, 0], 113 | # [ (py - x[1, 0]) / hyp, -(px - x[0, 0]) / hyp, -1]]) 114 | H = np.array([[x[0, 0], 0, 0], 115 | [0, x[1, 0], 0], 116 | [0, x[1, 0], 0], 117 | [0, 0, x[2, 0]], 118 | [0, 0, x[2, 0]]]) 119 | return H 120 | 121 | def Hx(self, x): 122 | """ takes a state variable and returns the measurement 123 | that would correspond to that state. 124 | """ 125 | # dist = sqrt((px - x[0, 0])**2 + (py - x[1, 0])**2) 126 | 127 | # Hx = np.array([[dist], 128 | # [atan2(py - x[1, 0], px - x[0, 0]) - x[2, 0]]]) 129 | Hx = np.array([[x[0, 0]], 130 | [x[1, 0]], 131 | [x[1, 0]], 132 | [x[2, 0]], 133 | [x[2, 0]]]) 134 | return Hx 135 | 136 | def residual(self, a, b): 137 | """ compute residual (a-b) between measurements containing 138 | [range, bearing]. Bearing is normalized to [-pi, pi)""" 139 | y = a - b 140 | y[3] = y[3] % (2 * np.pi) # force in range [0, 2 pi) 141 | if y[3] > np.pi: # move to [-pi, pi) 142 | y[3] -= 2 * np.pi 143 | y[4] = y[4] % (2 * np.pi) # force in range [0, 2 pi) 144 | if y[4] > np.pi: # move to [-pi, pi) 145 | y[4] -= 2 * np.pi 146 | return y 147 | 148 | def z_landmark(self, sim_pos, std_rng, std_brg): 149 | x, y = sim_pos[0, 0], sim_pos[1, 0] 150 | a = sim_pos[2, 0] 151 | # z = np.array([[d + np.random.randn() * std_rng], 152 | # [d + np.random.randn() * std_rng], 153 | # [a + np.random.randn() * std_brg]]) 154 | # z = np.array([[(lmark[0] - x) + np.random.randn() * std_rng], 155 | # [(lmark[1] - y) + np.random.randn() * std_rng], 156 | # [(lmark[1] - y) + np.random.randn() * std_rng], 157 | # [a + np.random.randn() * std_brg], 158 | # [a + np.random.randn() * std_brg]]) 159 | z = np.array([[x + np.random.randn() * std_rng], 160 | [y + np.random.randn() * std_rng], 161 | [y + np.random.randn() * std_rng], 162 | [a + np.random.randn() * std_brg], 163 | [a + np.random.randn() * std_brg]]) 164 | return z 165 | 166 | def ekf_update(self, z): 167 | self.update(z, HJacobian=self.H_of, Hx=self.Hx, 168 | residual=self.residual) 169 | 170 | def run_localization(self, landmarks, 171 | step=10, ellipse_step=20, ylim=None): 172 | # TODO: get P being a function of x and K as a function of x 173 | self.x = np.array([[0.2, 0.6, 0.3]]).T # x, y, steer angle 174 | self.P = np.diag([.01, .01, .01]) 175 | self.R = np.diag([self.std_range ** 2, 176 | self.std_range ** 2, self.std_range ** 2, 177 | self.std_bearing ** 2, self.std_bearing ** 2]) 178 | 179 | sim_pos = self.x.copy() # simulated position 180 | # steering command (vel, steering angle radians) 181 | u = np.array([0.1]) 182 | 183 | if self.verbose: 184 | plt.figure() 185 | plt.scatter(landmarks[:, 0], landmarks[:, 1], 186 | marker='s', s=60) 187 | 188 | track = [] 189 | for i in range(100): 190 | sim_pos = self.move(sim_pos, u, dt ) # simulate robot 191 | track.append(sim_pos) 192 | # print(ekf.K) 193 | 194 | if i % step == 0: 195 | self.predict(u=u) 196 | 197 | if i % ellipse_step == 0 and self.verbose: 198 | plot_covariance_ellipse( 199 | (self.x[0, 0], self.x[1, 0]), self.P[0:4, 0:4], 200 | std=6, facecolor='k', alpha=0.3) 201 | 202 | x, y = sim_pos[0, 0], sim_pos[2, 0] 203 | # for lmark in landmarks: 204 | z = self.z_landmark(sim_pos, self.std_range, self.std_bearing) 205 | self.ekf_update(z) 206 | 207 | if i % ellipse_step == 0 and self.verbose: 208 | plot_covariance_ellipse( 209 | (self.x[0, 0], self.x[1, 0]), self.P[0:4, 0:4], 210 | std=6, facecolor='g', alpha=0.8) 211 | track = np.array(track) 212 | if self.verbose: 213 | plt.plot(track[:, 0], track[:, 1], color='k', lw=2) 214 | plt.axis('equal') 215 | plt.title("EKF Robot localization") 216 | if ylim is not None: plt.ylim(*ylim) 217 | plt.show() 218 | 219 | # fault_list = FaultPattern(sensor_list, 220 | # fault_target=[[1], [2, 3]], 221 | # fault_value=[[0.1], [0.15, 2]]) 222 | 223 | # dt = 0.001 224 | # sensor_list = SensorSet([0, 1, 1, 2, 2], [0.001, 0.002, 0.0015, 0.001, 0.01]) 225 | # 226 | # landmarks = np.array([[0.5, 0.5, 0.10, 0.10, 0.05], [1, 1, 0.5, 0.5, 0.05], [1.5, 1.5, 1.5, 1.5, 0.05]]) 227 | # ekf = RdObsEKF(sensor_list, dt, wheelbase=0.5, std_vel=0.01, 228 | # std_steer=np.radians(0.01), std_range=0.05, std_bearing=0.05, verbose=True) 229 | # ekf.run_localization(landmarks) 230 | # print('Final K:', ekf.x) 231 | -------------------------------------------------------------------------------- /FTEst/SensorFaults.py: -------------------------------------------------------------------------------- 1 | import torch 2 | import numpy as np 3 | from dataclasses import dataclass 4 | 5 | 6 | @dataclass 7 | class Sensor: 8 | obs: int 9 | noise: float 10 | 11 | 12 | class SensorSet: 13 | num_sensors: int 14 | 15 | def __init__(self, sensor_obs=None, sensor_noise=None): 16 | self.obs_matrix = None 17 | if sensor_noise is None: 18 | sensor_noise = [0.01, 0.01, 0.01] 19 | if sensor_obs is None: 20 | sensor_obs = [0, 1, 2] 21 | self.sensor_list = [] 22 | self.state_dim = np.max(sensor_obs) + 1 23 | self.sensor_set_init(sensor_obs, sensor_noise) 24 | self.num_sensors = len(self.sensor_list) 25 | self.obs_matrix_init() 26 | 27 | def sensor_set_init(self, sensor_obs, sensor_noise): 28 | sensor_list = [] 29 | for item in range(len(sensor_obs)): 30 | obs = sensor_obs[item] 31 | sigma = sensor_noise[item] 32 | sensor_list.append(Sensor(obs, sigma)) 33 | self.sensor_list = sensor_list 34 | 35 | def obs_matrix_init(self): 36 | obs_matrix = np.zeros([self.num_sensors, self.state_dim]) 37 | for idx in range(self.num_sensors): 38 | obs_matrix[idx][self.sensor_list[idx].obs] = 1 39 | self.obs_matrix = obs_matrix 40 | 41 | @property 42 | def view_sensors(self): 43 | return self.sensor_list 44 | 45 | 46 | @dataclass 47 | class Fault: 48 | idx: int 49 | target: list 50 | value: list 51 | 52 | 53 | class FaultPattern: 54 | def __init__(self, sensors, fault_target=None, fault_value=None): 55 | self.fault_mask_list = [] 56 | self.fault_list = [] 57 | if fault_target is None: 58 | fault_target = [[0], [1, 2]] 59 | if fault_value is None: 60 | fault_value = [[0.01], [0.01, 0.01]] 61 | self.Sensors = sensors 62 | self.fault_list_init(fault_target, fault_value) 63 | self.num_faults = len(self.fault_list) 64 | self.fault_mask_init() 65 | 66 | def fault_list_init(self, fault_target, fault_value): 67 | fault_list = [] 68 | for fidx in range(len(fault_target)): 69 | target = fault_target[fidx] 70 | attack = fault_value[fidx] 71 | fault_list.append(Fault(fidx, target, attack)) 72 | self.fault_list = fault_list 73 | 74 | def fault_mask_init(self): 75 | for fault in self.fault_list: 76 | fault_mask_matrix = self.Sensors.obs_matrix.copy() 77 | # If there is no fault, fault_mask = self.Sensors.obs_matrix 78 | for target in fault.target: 79 | fault_mask_matrix[target][self.Sensors.sensor_list[target].obs] = 0 80 | self.fault_mask_list.append(fault_mask_matrix) 81 | 82 | 83 | # sensor_list = SensorSet([0, 1, 1, 2, 2], [0.001, 0.002, 0.0015, 0.001, 0.01]) 84 | # fault_list = FaultPattern(sensor_list, 85 | # fault_target=[[1], [2, 3]], 86 | # fault_value=[[0.1], [0.15, 2]]) 87 | -------------------------------------------------------------------------------- /FTEst/ekftest.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | 3 | # Author: Addison Sears-Collins 4 | # https://automaticaddison.com 5 | # Description: Extended Kalman Filter example (two-wheeled mobile robot) 6 | 7 | # Supress scientific notation when printing NumPy arrays 8 | np.set_printoptions(precision=3, suppress=True) 9 | 10 | # A matrix 11 | # 3x3 matrix -> number of states x number of states matrix 12 | # Expresses how the state of the system [x,y,yaw] changes 13 | # from k-1 to k when no control command is executed. 14 | # Typically a robot on wheels only drives when the wheels are told to turn. 15 | # For this case, A is the identity matrix. 16 | # A is sometimes F in the literature. 17 | A_k_minus_1 = np.array([[1.0, 0, 0], 18 | [0, 1.0, 0], 19 | [0, 0, 1.0]]) 20 | 21 | # Noise applied to the forward kinematics (calculation 22 | # of the estimated state at time k from the state 23 | # transition model of the mobile robot). This is a vector 24 | # with the number of elements equal to the number of states 25 | process_noise_v_k_minus_1 = np.array([0.01, 0.01, 0.003]) 26 | 27 | # State model noise covariance matrix Q_k 28 | # When Q is large, the Kalman Filter tracks large changes in 29 | # the sensor measurements more closely than for smaller Q. 30 | # Q is a square matrix that has the same number of rows as states. 31 | Q_k = np.array([[1.0, 0, 0], 32 | [0, 1.0, 0], 33 | [0, 0, 1.0]]) 34 | 35 | # Measurement matrix H_k 36 | # Used to convert the predicted state estimate at time k 37 | # into predicted sensor measurements at time k. 38 | # In this case, H will be the identity matrix since the 39 | # estimated state maps directly to state measurements from the 40 | # odometry data [x, y, yaw] 41 | # H has the same number of rows as sensor measurements 42 | # and same number of columns as states. 43 | H_k = np.array([[1.0, 0, 0], 44 | [1.0, 0, 0], 45 | [0, 1.0, 0], 46 | [0, 1.0, 0], 47 | [0, 0, 1.0]]) 48 | 49 | H1_k = np.array([[1.0, 0, 0], 50 | [0, 0, 0], 51 | [0, 1.0, 0], 52 | [0, 1.0, 0], 53 | [0, 0, 1.0]]) 54 | 55 | H3_k = np.array([[1.0, 0, 0], 56 | [1.0, 0, 0], 57 | [0, 1.0, 0], 58 | [0, 0, 0], 59 | [0, 0, 1.0]]) 60 | 61 | H13_k = np.array([[1.0, 0, 0], 62 | [0, 0, 0], 63 | [0, 1.0, 0], 64 | [0, 0, 0], 65 | [0, 0, 1.0]]) 66 | 67 | # Sensor measurement noise covariance matrix R_k 68 | # Has the same number of rows and columns as sensor measurements. 69 | # If we are sure about the measurements, R will be near zero. 70 | # R_k = np.array([[1.0, 0, 0], 71 | # [0, 1.0, 0], 72 | # [0, 0, 1.0]]) 73 | R_k = np.eye(5) 74 | 75 | # Sensor noise. This is a vector with the 76 | # number of elements equal to the number of sensor measurements. 77 | sensor_noise_w_k = np.array([0.07, 0.07, 0.07, 0.07, 0.04]) 78 | 79 | 80 | def getB(yaw, deltak): 81 | """ 82 | Calculates and returns the B matrix 83 | 3x2 matix -> number of states x number of control inputs 84 | The control inputs are the forward speed and the 85 | rotation rate around the z axis from the x-axis in the 86 | counterclockwise direction. 87 | [v,yaw_rate] 88 | Expresses how the state of the system [x,y,yaw] changes 89 | from k-1 to k due to the control commands (i.e. control input). 90 | :param yaw: The yaw angle (rotation angle around the z axis) in rad 91 | :param deltak: The change in time from time step k-1 to k in sec 92 | """ 93 | B = np.array([[np.cos(yaw) * deltak, 0], 94 | [np.sin(yaw) * deltak, 0], 95 | [0, deltak]]) 96 | return B 97 | 98 | 99 | def ekftest(z_k_observation_vector, state_estimate_k_minus_1, 100 | control_vector_k_minus_1, P_k_minus_1, dk): 101 | """ 102 | Extended Kalman Filter. Fuses noisy sensor measurement to 103 | create an optimal estimate of the state of the robotic system. 104 | 105 | INPUT 106 | :param z_k_observation_vector The observation from the Odometry 107 | 3x1 NumPy Array [x,y,yaw] in the global reference frame 108 | in [meters,meters,radians]. 109 | :param state_estimate_k_minus_1 The state estimate at time k-1 110 | 3x1 NumPy Array [x,y,yaw] in the global reference frame 111 | in [meters,meters,radians]. 112 | :param control_vector_k_minus_1 The control vector applied at time k-1 113 | 3x1 NumPy Array [v,v,yaw rate] in the global reference frame 114 | in [meters per second,meters per second,radians per second]. 115 | :param P_k_minus_1 The state covariance matrix estimate at time k-1 116 | 3x3 NumPy Array 117 | :param dk Time interval in seconds 118 | 119 | OUTPUT 120 | :return state_estimate_k near-optimal state estimate at time k 121 | 3x1 NumPy Array ---> [meters,meters,radians] 122 | :return P_k state covariance_estimate for time k 123 | 3x3 NumPy Array 124 | """ 125 | ######################### Predict ############################# 126 | # Predict the state estimate at time k based on the state 127 | # estimate at time k-1 and the control input applied at time k-1. 128 | state_estimate_k = A_k_minus_1 @ ( 129 | state_estimate_k_minus_1) + ( 130 | getB(state_estimate_k_minus_1[2], dk)) @ ( 131 | control_vector_k_minus_1) + ( 132 | process_noise_v_k_minus_1) 133 | 134 | # print(f'State Estimate Before EKF={state_estimate_k}') 135 | 136 | # Predict the state covariance estimate based on the previous 137 | # covariance and some noise 138 | P_k = A_k_minus_1 @ P_k_minus_1 @ A_k_minus_1.T + ( 139 | Q_k) 140 | 141 | ################### Update (Correct) ########################## 142 | # Calculate the difference between the actual sensor measurements 143 | # at time k minus what the measurement model predicted 144 | # the sensor measurements would be for the current timestep k. 145 | measurement_residual_y_k = z_k_observation_vector - ( 146 | (H_k @ state_estimate_k) + (sensor_noise_w_k)) 147 | 148 | # print(f'Observation={z_k_observation_vector}') 149 | 150 | # Calculate the measurement residual covariance 151 | S_k = H_k @ P_k @ H_k.T + R_k 152 | 153 | # Calculate the near-optimal Kalman gain 154 | # We use pseudoinverse since some of the matrices might be 155 | # non-square or singular. 156 | K_k = P_k @ H_k.T @ np.linalg.pinv(S_k) 157 | 158 | # Calculate an updated state estimate for time k 159 | state_estimate_k = state_estimate_k + (K_k @ measurement_residual_y_k) 160 | 161 | # Update the state covariance estimate for time k 162 | P_k = P_k - (K_k @ H_k @ P_k) 163 | 164 | # Print the best (near-optimal) estimate of the current state of the robot 165 | # print(f'State Estimate After EKF={state_estimate_k}') 166 | 167 | # Return the updated state and covariance estimates 168 | return state_estimate_k, P_k, K_k 169 | 170 | def ekf0test(z_k_observation_vector, state_estimate_k0_minus_1, 171 | control_vector_k0_minus_1, P_k0_minus_1, H_k, dk): 172 | """ 173 | Extended Kalman Filter. Fuses noisy sensor measurement to 174 | create an optimal estimate of the state of the robotic system. 175 | 176 | INPUT 177 | :param z_k_observation_vector The observation from the Odometry 178 | 3x1 NumPy Array [x,y,yaw] in the global reference frame 179 | in [meters,meters,radians]. 180 | :param state_estimate_k_minus_1 The state estimate at time k-1 181 | 3x1 NumPy Array [x,y,yaw] in the global reference frame 182 | in [meters,meters,radians]. 183 | :param control_vector_k_minus_1 The control vector applied at time k-1 184 | 3x1 NumPy Array [v,v,yaw rate] in the global reference frame 185 | in [meters per second,meters per second,radians per second]. 186 | :param P_k_minus_1 The state covariance matrix estimate at time k-1 187 | 3x3 NumPy Array 188 | :param dk Time interval in seconds 189 | 190 | OUTPUT 191 | :return state_estimate_k near-optimal state estimate at time k 192 | 3x1 NumPy Array ---> [meters,meters,radians] 193 | :return P_k state covariance_estimate for time k 194 | 3x3 NumPy Array 195 | """ 196 | ######################### Predict ############################# 197 | # Predict the state estimate at time k based on the state 198 | # estimate at time k-1 and the control input applied at time k-1. 199 | state_estimate_k = A_k_minus_1 @ ( 200 | state_estimate_k0_minus_1) + ( 201 | getB(state_estimate_k0_minus_1[2], dk)) @ ( 202 | control_vector_k0_minus_1) + ( 203 | process_noise_v_k_minus_1) 204 | 205 | # print(f'State Estimate Before EKF={state_estimate_k}') 206 | 207 | # Predict the state covariance estimate based on the previous 208 | # covariance and some noise 209 | P_k = A_k_minus_1 @ P_k0_minus_1 @ A_k_minus_1.T + ( 210 | Q_k) 211 | 212 | ################### Update (Correct) ########################## 213 | # Calculate the difference between the actual sensor measurements 214 | # at time k minus what the measurement model predicted 215 | # the sensor measurements would be for the current timestep k. 216 | measurement_residual_y_k = z_k_observation_vector - ( 217 | (H_k @ state_estimate_k) + (sensor_noise_w_k)) 218 | 219 | # print(f'Observation={z_k_observation_vector}') 220 | 221 | # Calculate the measurement residual covariance 222 | S_k = H_k @ P_k @ H_k.T + R_k 223 | 224 | # Calculate the near-optimal Kalman gain 225 | # We use pseudoinverse since some of the matrices might be 226 | # non-square or singular. 227 | K_k = P_k @ H_k.T @ np.linalg.pinv(S_k) 228 | 229 | # Calculate an updated state estimate for time k 230 | state_estimate_k = state_estimate_k + (K_k @ measurement_residual_y_k) 231 | 232 | # Update the state covariance estimate for time k 233 | P_k = P_k - (K_k @ H_k @ P_k) 234 | 235 | # Print the best (near-optimal) estimate of the current state of the robot 236 | # print(f'State Estimate After EKF={state_estimate_k}') 237 | 238 | # Return the updated state and covariance estimates 239 | return state_estimate_k, P_k, K_k 240 | 241 | 242 | def main(): 243 | # We start at time k=1 244 | k = 1 245 | 246 | # Time interval in seconds 247 | dk = 1 248 | 249 | # Create a list of sensor observations at successive timesteps 250 | # Each list within z_k is an observation vector. 251 | z_k = np.array([[4.721, 0.143, 0.006], # k=1 252 | [9.353, 0.284, 0.007], # k=2 253 | [14.773, 0.422, 0.009], # k=3 254 | [18.246, 0.555, 0.011], # k=4 255 | [22.609, 0.715, 0.012]]) # k=5 256 | 257 | # The estimated state vector at time k-1 in the global reference frame. 258 | # [x_k_minus_1, y_k_minus_1, yaw_k_minus_1] 259 | # [meters, meters, radians] 260 | state_estimate_k_minus_1 = np.array([0.0, 0.0, 0.0]) 261 | 262 | # The control input vector at time k-1 in the global reference frame. 263 | # [v, yaw_rate] 264 | # [meters/second, radians/second] 265 | # In the literature, this is commonly u. 266 | # Because there is no angular velocity and the robot begins at the 267 | # origin with a 0 radians yaw angle, this robot is traveling along 268 | # the positive x-axis in the global reference frame. 269 | control_vector_k_minus_1 = np.array([4.5, 0.0]) 270 | 271 | # State covariance matrix P_k_minus_1 272 | # This matrix has the same number of rows (and columns) as the 273 | # number of states (i.e. 3x3 matrix). P is sometimes referred 274 | # to as Sigma in the literature. It represents an estimate of 275 | # the accuracy of the state estimate at time k made using the 276 | # state transition matrix. We start off with guessed values. 277 | P_k_minus_1 = np.array([[0.1, 0, 0], 278 | [0, 0.1, 0], 279 | [0, 0, 0.1]]) 280 | 281 | # Start at k=1 and go through each of the 5 sensor observations, 282 | # one at a time. 283 | # We stop right after timestep k=5 (i.e. the last sensor observation) 284 | for k, obs_vector_z_k in enumerate(z_k, start=1): 285 | # Print the current timestep 286 | print(f'Timestep k={k}') 287 | 288 | # Run the Extended Kalman Filter and store the 289 | # near-optimal state and covariance estimates 290 | optimal_state_estimate_k, covariance_estimate_k = ekf( 291 | obs_vector_z_k, # Most recent sensor measurement 292 | state_estimate_k_minus_1, # Our most recent estimate of the state 293 | control_vector_k_minus_1, # Our most recent control input 294 | P_k_minus_1, # Our most recent state covariance matrix 295 | dk) # Time interval 296 | 297 | # Get ready for the next timestep by updating the variable values 298 | state_estimate_k_minus_1 = optimal_state_estimate_k 299 | P_k_minus_1 = covariance_estimate_k 300 | 301 | # Print a blank line 302 | print() 303 | 304 | 305 | # Program starts running here with the main method 306 | # main() -------------------------------------------------------------------------------- /FTEst/linearKF.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | 3 | # Define system matrices A, B, and C 4 | def linearKF(): 5 | n = 1 6 | A = np.array([[1, 0, 0, 0, 0, 0], 7 | [0, 1, 0, 0, 0, 0], 8 | [0, 0, 1, 0, 0, 0], 9 | [3 * n ** 2, 0, 0, 0, 2 * n, 0], 10 | [0, 0, 0, -2 * n, 0, 0], 11 | [0, 0, -n ** 2, 0, 0, 0]]) 12 | 13 | B = np.array([[0, 0, 0], 14 | [0, 0, 0], 15 | [0, 0, 0], 16 | [1, 0, 0], 17 | [0, 1, 0], 18 | [0, 0, 1]]) 19 | 20 | C1 = np.array([[1, 0, 0, 0, 0, 0], 21 | [0, 0, 0, 0, 0, 0], 22 | [0, 1, 0, 0, 0, 0], 23 | [0, 1, 0, 0, 0, 0], 24 | [0, 0, 1, 0, 0, 0], 25 | [0, 0, 0, 1, 0, 0], 26 | [0, 0, 0, 0, 1, 0], 27 | [0, 0, 0, 0, 0, 1]]) 28 | 29 | C3 = np.array([[1, 0, 0, 0, 0, 0], 30 | [1, 0, 0, 0, 0, 0], 31 | [0, 1, 0, 0, 0, 0], 32 | [0, 0, 0, 0, 0, 0], 33 | [0, 0, 1, 0, 0, 0], 34 | [0, 0, 0, 1, 0, 0], 35 | [0, 0, 0, 0, 1, 0], 36 | [0, 0, 0, 0, 0, 1]]) 37 | 38 | # Define initial state estimate and covariance 39 | x_hat = np.zeros((6, 1)) # Initial state estimate 40 | P = np.eye(6) # Initial state covariance 41 | 42 | # Define process and measurement noise covariances 43 | Q = np.eye(6) # Process noise covariance (adjust as needed) 44 | R = np.eye(8) # Measurement noise covariance (adjust as needed) 45 | 46 | # Simulated measurements (replace with your measurements) 47 | num_time_steps = 100 48 | measurements = np.random.randn(8, num_time_steps) 49 | u = np.zeros((3, 1)) # Control input (if any) 50 | 51 | 52 | x_hat = np.zeros((6, 1)) # Initial state estimate 53 | P = np.eye(6) 54 | # Kalman filter loop 55 | for k in range(num_time_steps): 56 | # Prediction step 57 | x_hat_minus = np.dot(A, x_hat) + np.dot(B, u) # u is the control input (if any) 58 | P_minus = np.dot(np.dot(A, P), A.T) + Q 59 | 60 | # Update step 61 | K1 = np.dot(np.dot(P_minus, C1.T), np.linalg.inv(np.dot(np.dot(C1, P_minus), C1.T) + R)) 62 | x_hat = x_hat_minus + np.dot(K1, (measurements[:, k] - np.dot(C1, x_hat_minus))) 63 | P = np.dot((np.eye(6) - np.dot(K1, C1)), P_minus) 64 | 65 | x_hat = np.zeros((6, 1)) # Initial state estimate 66 | P = np.eye(6) 67 | # Kalman filter loop 68 | for k in range(num_time_steps): 69 | # Prediction step 70 | x_hat_minus = np.dot(A, x_hat) + np.dot(B, u) # u is the control input (if any) 71 | P_minus = np.dot(np.dot(A, P), A.T) + Q 72 | 73 | # Update step 74 | K3 = np.dot(np.dot(P_minus, C3.T), np.linalg.inv(np.dot(np.dot(C3, P_minus), C3.T) + R)) 75 | x_hat = x_hat_minus + np.dot(K3, (measurements[:, k] - np.dot(C3, x_hat_minus))) 76 | P = np.dot((np.eye(6) - np.dot(K3, C3)), P_minus) 77 | 78 | return K1, K3 79 | -------------------------------------------------------------------------------- /FTNCBF_comp.gif: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/HongchaoZhang-HZ/FTNCBF/82239a3b0973f8d5f629e7230f9887895b315095/FTNCBF_comp.gif -------------------------------------------------------------------------------- /ICRA2024_FTNCBF_CameraReady.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/HongchaoZhang-HZ/FTNCBF/82239a3b0973f8d5f629e7230f9887895b315095/ICRA2024_FTNCBF_CameraReady.pdf -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2023 Hongchao Zhanng 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 | -------------------------------------------------------------------------------- /Modules/NCBF.py: -------------------------------------------------------------------------------- 1 | from Modules.NNet import * 2 | from torch import optim 3 | from sklearn.linear_model import LinearRegression 4 | from sklearn.preprocessing import PolynomialFeatures 5 | from sympy.polys.orderings import monomial_key 6 | from sympy.polys.monomials import itermonomials 7 | 8 | class NCBF(NNet): 9 | def __init__(self, arch, act_layer, DOMAIN): 10 | ''' 11 | Initialize NCBF with a given architecture and ReLU layers 12 | :param arch: a vector of # of neurons in each layer 13 | :param act_layer: a vector of # of ReLU layers 14 | :param DOMAIN: state space domain 15 | ''' 16 | super().__init__(arch, act_layer, DOMAIN) 17 | 18 | def generate_input(self, shape=[100,100]): 19 | state_space = self.DOMAIN 20 | noise = 1e-2 * torch.rand(shape) 21 | cell_length = (state_space[0][1] - state_space[0][0]) / shape[0] 22 | nx = torch.linspace(state_space[0][0] + cell_length / 2, state_space[0][1] - cell_length / 2, shape[0]) 23 | ny = torch.linspace(state_space[1][0] + cell_length / 2, state_space[1][1] - cell_length / 2, shape[1]) 24 | vxo, vyo = torch.meshgrid(nx, ny) 25 | vx = vxo + noise 26 | vy = vyo + noise 27 | data = np.dstack([vx.reshape([shape[0], shape[1], 1]), vy.reshape([shape[0], shape[1], 1])]) 28 | data = torch.Tensor(data.reshape(shape[0] * shape[1], 2)) 29 | return data 30 | 31 | def generate_data(self, size: int = 100) -> torch.Tensor: 32 | ''' 33 | Generate data for training or plotting 34 | :param size: the number of samples on each dimension 35 | :return: a mesh grid torch data 36 | ''' 37 | state_space = self.DOMAIN 38 | shape = [] 39 | for _ in range(self.DIM): 40 | shape.append(size) 41 | noise = 1e-2 * torch.rand(shape) 42 | cell_length = (state_space[0][1] - state_space[0][0]) / size 43 | raw_data = [] 44 | for i in range(self.DIM): 45 | data_element = torch.linspace(state_space[i][0] + cell_length/2, state_space[i][1] - cell_length/2, shape[0]) 46 | raw_data.append(data_element) 47 | raw_data_grid = torch.meshgrid(raw_data) 48 | noisy_data = [] 49 | for i in range(self.DIM): 50 | noisy_data_item = raw_data_grid[i] + noise 51 | # noisy_data_item = np.expand_dims(noisy_data_item, axis=self.DIM) 52 | noisy_data_item = noisy_data_item.reshape([torch.prod(torch.Tensor(shape),dtype=int), 1]) 53 | noisy_data.append(noisy_data_item) 54 | data = torch.hstack([torch.Tensor(item) for item in noisy_data]) 55 | 56 | return data 57 | 58 | def correctness(self, ref_output, model_output, l_co=1): 59 | ''' 60 | Correctness loss function 61 | :param ref_output: h(x) output 62 | :param model_output: nn output 63 | :param l_co: lagrangian coefficient 64 | :return: number of correctness violation 65 | ''' 66 | norm_model_output = torch.tanh(model_output) 67 | length = len(-ref_output + norm_model_output) 68 | # norm_ref_output = torch.tanh(ref_output) 69 | violations = torch.sigmoid((-ref_output + norm_model_output).reshape([1, length])) 70 | loss = l_co * torch.sum(violations) 71 | return loss 72 | 73 | def warm_start(self, ref_output, model_output): 74 | ''' 75 | MSE loss between ref and model 76 | :param ref_output: h(x) output 77 | :param model_output: nn output 78 | :return: MSE 79 | ''' 80 | # loss = -torch.sum(torch.tanh(output)) 81 | loss = nn.MSELoss() 82 | loss_fcn = loss(model_output, ref_output) 83 | return loss_fcn 84 | 85 | def def_loss(self, *loss): 86 | ''' 87 | Define loss function by adding all loss 88 | :param loss: *loss allows multiple inputs 89 | :return: total loss 90 | ''' 91 | total_loss = 0 92 | for l in loss: 93 | total_loss += l 94 | return total_loss 95 | 96 | def topolyCBF(self, deg: int = 5): 97 | ''' 98 | Polynomial approximation of a NN CBF 99 | :param deg: degree of polynomials to fit 100 | :return: names: polynomial terms, coeff: coefficients 101 | ''' 102 | shape = [100, 100] 103 | vx, vy, rdm_input = self.generate_input(shape) 104 | NN_output = self.forward(rdm_input) 105 | x_train = rdm_input.numpy() 106 | y_train = NN_output.detach().numpy() 107 | 108 | poly_reg = PolynomialFeatures(degree=deg) 109 | X_poly = poly_reg.fit_transform(x_train) 110 | poly_reg.fit(X_poly, y_train) 111 | lin_reg = LinearRegression() 112 | lin_reg.fit(X_poly, y_train) 113 | # return lin_reg, poly_reg 114 | names = poly_reg.get_feature_names_out() 115 | coeff = lin_reg.coef_ 116 | return names, coeff 117 | 118 | def SymPoly(self, coeff: list, x, degree: int = 4): 119 | ''' 120 | Symbolic polynomial expression of approximated function 121 | :param coeff: coefficients from polynomial approximation 122 | :param x: symbolic variable 123 | :param degree: int 124 | :return: symbolic polynomial 125 | ''' 126 | x0 = x[0] 127 | x1 = x[1] 128 | exp = 0 129 | list_name = sorted(itermonomials([x0, x1], degree), key=monomial_key('grlex', [x1, x0])) 130 | # list_coeff = coeff.reshape(len(list_name)) 131 | list_coeff = coeff[0] 132 | for idx in range(len(list_name)): 133 | exp += list_coeff[idx]*list_name[idx] 134 | return exp 135 | 136 | 137 | def train(self, num_epoch): 138 | # Default training 139 | optimizer = optim.SGD(self.model.parameters(), lr=0.001) 140 | 141 | for epoch in range(num_epoch): 142 | # Generate data 143 | shape = [100,100] 144 | vx, vy, rdm_input = self.generate_input(shape) 145 | 146 | running_loss = 0.0 147 | for i, data in enumerate(rdm_input, 0): 148 | 149 | optimizer.zero_grad() 150 | 151 | model_output = self.forward(rdm_input) 152 | ref_output = torch.tanh(self.h_x(rdm_input.transpose(0, 1)).reshape([shape[0]*shape[1], 1])) 153 | 154 | warm_start_loss = self.warm_start(ref_output, model_output) 155 | correctness_loss = self.correctness(ref_output, model_output, 1) 156 | loss = self.def_loss(warm_start_loss + correctness_loss) 157 | 158 | loss.backward() 159 | optimizer.step() 160 | 161 | running_loss += loss.item() 162 | if i % 2000 == 1999: 163 | print('[%d, %5d] loss: %.3f' % (epoch + 1, i + 1, running_loss / 2000)) 164 | running_loss = 0.0 165 | 166 | 167 | 168 | -------------------------------------------------------------------------------- /Modules/NNet.py: -------------------------------------------------------------------------------- 1 | import torch 2 | import torch.nn as nn 3 | import numpy as np 4 | 5 | 6 | class NNet(nn.Module): 7 | def __init__(self, arch, act_layer, DOMAIN, n_output=1, verbose=False): 8 | ''' 9 | Initialize NCBF with a given architecture and ReLU layers 10 | :param arch: a vector of # of neurons in each layer 11 | :param act_layer: a vector of # of ReLU layers 12 | :param DOMAIN: state space domain 13 | ''' 14 | assert len(arch) == len(act_layer), 'Arch should match act_layer' 15 | super(NNet, self).__init__() 16 | 17 | self.verbose = verbose 18 | self.arch = arch 19 | self.DOMAIN = DOMAIN 20 | self.act_fun = nn.Tanh() 21 | self.act_layer = act_layer 22 | self.device = self.get_device() 23 | self.DIM = len(self.DOMAIN) 24 | 25 | 26 | self.layer_input = [nn.Linear(len(DOMAIN), self.arch[0], bias=True)] 27 | self.layer_output = [self.act_fun, nn.Linear(self.arch[-1], n_output, bias=True)] 28 | 29 | # hidden layer 30 | self.module_hidden = [] 31 | for i in range(len(arch) - 1): 32 | if self.act_layer[i]: 33 | self.module_hidden.append([self.act_fun, nn.Linear(self.arch[i], self.arch[i + 1], bias=True)]) 34 | else: 35 | self.module_hidden.append([nn.Identity(), nn.Linear(self.arch[i], self.arch[i + 1], bias=True)]) 36 | # self.module_hidden = [[self.act_fun, nn.Linear(self.arch[i], self.arch[i], bias=True)] for i in range(len(arch)-1)] 37 | self.layer_hidden = list(np.array(self.module_hidden).flatten()) 38 | 39 | # nn model 40 | self.layers = self.layer_input + self.layer_hidden + self.layer_output 41 | self.model = nn.Sequential(*self.layers) 42 | self.model.to(self.device) 43 | 44 | def forward(self, x): 45 | return self.model.forward(x) 46 | 47 | def get_device(self): 48 | if torch.cuda.is_available(): 49 | device = 'cuda:0' 50 | if self.verbose: 51 | print('Using cuda:0') 52 | else: 53 | device = 'cpu' 54 | if self.verbose: 55 | print('Using cpu') 56 | return device -------------------------------------------------------------------------------- /NCBFSynth/NBF_Synth.py: -------------------------------------------------------------------------------- 1 | import sys, os 2 | sys.path.append(os.path.realpath(os.path.dirname(__file__)+"/..")) 3 | import torch 4 | 5 | from Modules.NCBF import * 6 | from torch import optim 7 | from torch.optim.lr_scheduler import ExponentialLR 8 | from torch.utils.tensorboard import SummaryWriter 9 | import torchvision.transforms as transforms 10 | from torch.utils.data import DataLoader 11 | from Cases.Darboux import Darboux 12 | # import cma 13 | # from cmaes import CMA 14 | # from Verifier import Verifier 15 | from collections import OrderedDict 16 | from Critic_Synth.NCritic import * 17 | 18 | class NCBF_Synth(NCBF): 19 | def __init__(self,arch, act_layer, case, verbose=False): 20 | self.case = case 21 | DOMAIN = self.case.DOMAIN 22 | super().__init__(arch, act_layer, DOMAIN) 23 | self.critic = NeuralCritic(case) 24 | # self.veri = Verifier(NCBF=self, case=case, grid_shape=[100, 100], verbose=verbose) 25 | 26 | def numerical_gradient(self, X_batch, model_output, batch_length, epsilon=0.001): 27 | grad = [] 28 | for i in range(self.DIM): 29 | gradStep = torch.zeros(self.DIM) 30 | gradStep[i] += epsilon 31 | gradData = X_batch + gradStep 32 | dbdxi = ((self.forward(gradData) - model_output) / epsilon).reshape([batch_length]) 33 | grad.append(dbdxi) 34 | 35 | return grad 36 | 37 | def feasibility_loss(self, model_output, grad_condition, l_co=1): 38 | # when model_output is close to the boundary grad condition comes in 39 | violations = grad_condition.reshape(torch.sigmoid(model_output).shape) * torch.sigmoid(model_output) 40 | # loss = torch.sum(torch.sigmoid(-violations).reshape([1, 10000])) 41 | violations = torch.max((-violations).reshape([1, 10000]), torch.zeros([1, 10000])) 42 | loss = l_co * torch.sum(violations) 43 | return loss 44 | 45 | def safe_correctness(self, ref_output, model_output, l_co=1, alpha1=1, alpha2=0.001): 46 | norm_model_output = torch.tanh(model_output) 47 | length = len(-ref_output + norm_model_output) 48 | # norm_ref_output = torch.tanh(ref_output) 49 | FalsePositive_loss = torch.max(-ref_output.reshape([1, length]), torch.zeros([1, length])) * \ 50 | torch.max((model_output + 0.01).reshape([1, length]), torch.zeros([1, length])) 51 | FalseNegative_loss = torch.max(ref_output.reshape([1, length]), torch.zeros([1, length])) * \ 52 | torch.max((-model_output + 0.01).reshape([1, length]), torch.zeros([1, length])) 53 | loss = l_co * torch.sum(alpha1*FalsePositive_loss + alpha2*FalseNegative_loss) 54 | return loss 55 | 56 | def trivial_panelty(self, ref_output, model_output, coeff=1, epsilon=0.1): 57 | min_ref = torch.max(ref_output) 58 | max_ref = torch.min(ref_output) 59 | # if max_ref >= 1e-4 and min_ref <= -1e-4: 60 | # non_pos_loss = coeff * torch.max(0.5 - torch.max(model_output), torch.zeros(1)) 61 | # non_neg_loss = coeff * torch.max(0.5 - torch.max(-model_output), torch.zeros(1)) 62 | if max_ref >= 1e-4 and min_ref >= 1e-4: 63 | non_pos_loss = torch.zeros(1) 64 | non_neg_loss = torch.zeros(1) 65 | elif max_ref <= -1e-4 and min_ref <= -1e-4: 66 | non_pos_loss = torch.zeros(1) 67 | non_neg_loss = torch.zeros(1) 68 | else: 69 | non_pos_loss = coeff * torch.max(epsilon - torch.max(model_output), torch.zeros(1)) 70 | non_neg_loss = coeff * torch.max(epsilon - torch.max(-model_output), torch.zeros(1)) 71 | loss = non_pos_loss + non_neg_loss 72 | return loss 73 | 74 | def train(self, num_epoch): 75 | optimizer = optim.SGD(self.model.parameters(), lr=1e-4) 76 | scheduler = ExponentialLR(optimizer, gamma=0.9) 77 | # Generate data 78 | size = 100 79 | shape = [] 80 | for _ in range(self.DIM): 81 | shape.append(size) 82 | rlambda = 1 83 | rdm_input = self.generate_data(size) 84 | # rdm_input = self.generate_input(shape) 85 | # ref_output = torch.unsqueeze(self.h_x(rdm_input.transpose(0, self.DIM)), self.DIM) 86 | ref_output = self.case.h_x(rdm_input.transpose(0, 1)).unsqueeze(1) 87 | batch_length = 16 88 | training_loader = DataLoader(list(zip(rdm_input, ref_output)), batch_size=batch_length, shuffle=True) 89 | for epoch in range(num_epoch): 90 | running_loss = 0.0 91 | feasibility_running_loss = 0.0 92 | correctness_running_loss = 0.0 93 | trivial_running_loss = 0.0 94 | for X_batch, y_batch in training_loader: 95 | 96 | optimizer.zero_grad() 97 | model_output = self.forward(X_batch) 98 | 99 | warm_start_loss = self.warm_start(y_batch, model_output) 100 | correctness_loss = self.safe_correctness(y_batch, model_output, l_co=1, alpha1=1, alpha2=0) 101 | trivial_loss = self.trivial_panelty(ref_output, self.model.forward(rdm_input), 1) 102 | # x[1] + 2 * x[0] * x[1], -x[0] + 2 * x[0] ** 2 - x[1] ** 2 103 | # dx0data = X_batch + torch.Tensor([0.001, 0]) 104 | # dx1data = X_batch + torch.Tensor([0, 0.001]) 105 | # dbdx0 = ((self.forward(dx0data) - model_output)/0.001).reshape([batch_length]) 106 | # dbdx1 = ((self.forward(dx1data) - model_output)/0.001).reshape([batch_length]) 107 | grad = self.numerical_gradient(X_batch, model_output, batch_length, epsilon=0.001) 108 | grad_vector = torch.vstack(grad) 109 | feasibility_output = (grad_vector.transpose(0, 1).unsqueeze(1) \ 110 | @ self.case.f_x(X_batch).transpose(0, 1).unsqueeze(2)).squeeze() 111 | # feasibility_output = grad[0] * (X_batch[:,0] + 2*X_batch[:,0]*X_batch[:,1]) \ 112 | # + grad[1] * (-X_batch[:,0] + 2*X_batch[:,0]**2 - X_batch[:,1]**2) 113 | check_item = torch.max((-torch.abs(model_output)+0.1).reshape([1, batch_length]), torch.zeros([1, batch_length])) 114 | # feasibility_loss = torch.sum(torch.tanh(check_item*feasibility_output)) 115 | 116 | # Our loss function 117 | # violations = -check_item * feasibility_output 118 | # Chuchu Fan loss function 119 | violations = -1 * feasibility_output - torch.max(rlambda * torch.abs(model_output.transpose(0, 1)), 120 | torch.zeros([1, batch_length])) 121 | feasibility_loss = 100 * torch.sum(torch.max(violations - 1e-4, torch.zeros([1, batch_length]))) 122 | loss = self.def_loss(1*correctness_loss + 1*feasibility_loss + 1*trivial_loss) 123 | 124 | loss.backward() 125 | optimizer.step() 126 | 127 | running_loss += loss.item() 128 | feasibility_running_loss += feasibility_loss.item() 129 | correctness_running_loss += correctness_loss.item() 130 | trivial_running_loss += trivial_loss.item() 131 | # if epoch % 50 == 49: 132 | # print('[%d] loss: %.3f' % (epoch + 1, running_loss / 2000)) 133 | if epoch % 25 == 24: 134 | print('[%d] loss: %.3f' % (epoch + 1, running_loss)) 135 | print('[%d] Floss: %.3f' % (epoch + 1, feasibility_running_loss)) 136 | print('[%d] Closs: %.3f' % (epoch + 1, correctness_running_loss)) 137 | print('[%d] Tloss: %.3f' % (epoch + 1, trivial_running_loss)) 138 | running_loss = 0.0 139 | if epoch % 100 == 99: 140 | visualize(self.model) 141 | scheduler.step() 142 | if epoch % 200 == 199: 143 | veri_result, num = self.veri.proceed_verification() 144 | print(veri_result) 145 | 146 | 147 | 148 | # Define Case 149 | # x0, x1 = sp.symbols('x0, x1') 150 | 151 | Darboux = Darboux() 152 | newCBF = NCBF_Synth([32, 32], [True, True], Darboux, verbose=False) 153 | newCBF.veri.proceed_verification() 154 | for restart in range(3): 155 | newCBF.train(1000) 156 | # newCBF.model.load_state_dict(torch.load('darboux_2_10.pt')) 157 | visualize(newCBF) -------------------------------------------------------------------------------- /NCBFSynth/NCBF_Synth.py: -------------------------------------------------------------------------------- 1 | import sys, os 2 | sys.path.append(os.path.realpath(os.path.dirname(__file__)+"/..")) 3 | import torch 4 | from tqdm import tqdm 5 | from scipy.optimize import minimize 6 | # from progress.bar import Bar 7 | from Modules.NCBF import * 8 | from torch import optim 9 | from torch.optim.lr_scheduler import ExponentialLR 10 | from torch.utils.tensorboard import SummaryWriter 11 | import torchvision 12 | import torchvision.transforms as transforms 13 | from torch.utils.data import DataLoader 14 | from Cases.ObsAvoid import ObsAvoid 15 | # from Verifier.Verifier import Verifier 16 | # from Critic_Synth.NCritic import * 17 | import time 18 | from Visualization.visualization import visualize 19 | # from collections import OrderedDict 20 | 21 | class NCBF_Synth(NCBF): 22 | ''' 23 | Synthesize an NCBF for a given safe region h(x) 24 | for given a system with polynomial f(x) and g(x) 25 | ''' 26 | def __init__(self,arch, act_layer, case, verbose=False): 27 | ''' 28 | Input architecture and ReLU layers, input case, verbose for display 29 | :param arch: [list of int] architecture of the NN 30 | :param act_layer: [list of bool] if the corresponding layer with ReLU, then True 31 | :param case: Pre-defined case class, with f(x), g(x) and h(x) 32 | :param verbose: Flag for display or not 33 | ''' 34 | self.case = case 35 | DOMAIN = self.case.DOMAIN 36 | super().__init__(arch, act_layer, DOMAIN) 37 | # Under construction: Critic is designed to tuning loss fcn automatically 38 | # self.critic = NeuralCritic(case) 39 | # Verifier proposed to verify feasibility 40 | # self.veri = Verifier(NCBF=self, case=case, grid_shape=[100, 100, 100], verbose=verbose) 41 | # lctime = time.ctime(time.time()) 42 | lctime = time.strftime("%Y%m%d%H%M%S") 43 | # Tensorboard 44 | self.writer = SummaryWriter(f'./runs/NCBF/{lctime}'.format(lctime)) 45 | self.run = 0 46 | 47 | # def numerical_gradient(self, X_batch, model_output, batch_length, epsilon=0.001): 48 | # # compute numerical gradient for each dimension by (x+dx)/dx 49 | # grad = [] 50 | # for i in range(self.DIM): 51 | # gradStep = torch.zeros(self.DIM) 52 | # gradStep[i] += epsilon 53 | # gradData = X_batch + gradStep 54 | # dbdxi = ((self.forward(gradData) - model_output) / epsilon).reshape([batch_length]) 55 | # grad.append(dbdxi) 56 | # 57 | # return grad 58 | 59 | def get_grad(self, x): 60 | grad_input = torch.tensor(x, requires_grad=True, dtype=torch.float) 61 | return torch.autograd.grad(self.model.forward(grad_input), grad_input) 62 | 63 | def feasible_con(self, u, dbdxfx, dbdxgx): 64 | # function to maximize: (db/dx)*fx + (db/dx)*gx*u 65 | return dbdxfx + dbdxgx * u 66 | 67 | def feasible_u(self, dbdxfx, dbdxgx, min_flag=False): 68 | # find u that minimize (db/dx)*fx + (db/dx)*gx*u 69 | if min_flag: 70 | df = dbdxfx.detach().numpy() 71 | dg = dbdxgx.detach().numpy() 72 | res_list = [] 73 | for i in range(len(df)): 74 | res = minimize(self.feasible_con, x0=np.zeros(1), args=(df[i], dg[i])) 75 | # pos_res = np.max([res.fun, np.zeros(len([res.fun]))]) 76 | res_list.append(res.x) 77 | return torch.Tensor(res_list).squeeze() 78 | else: 79 | # Quick check for scalar u only 80 | [[u_lb, u_ub]] = torch.Tensor(self.case.CTRLDOM) 81 | res_list = [] 82 | for i in range(len(dbdxfx)): 83 | res_ulb = dbdxfx[i] + dbdxgx[i] * u_lb 84 | res_uub = dbdxfx[i] + dbdxgx[i] * u_ub 85 | # res = torch.max(res_ulb, res_uub) 86 | res = [u_lb, u_ub][np.argmax([res_ulb, res_uub])] 87 | res_list.append(res) 88 | return torch.Tensor(res_list).squeeze() 89 | 90 | def feasibility_loss(self, grad_vector, X_batch): 91 | # compute loss based on (db/dx)*fx + (db/dx)*gx*u 92 | dbdxfx = (grad_vector.unsqueeze(-1).transpose(-1, -2) 93 | @ self.case.f_x(X_batch).transpose(0, 1).unsqueeze(2)).squeeze() 94 | dbdxgx = (grad_vector.unsqueeze(-1).transpose(-1, -2) 95 | @ self.case.g_x(X_batch).transpose(0, 1).unsqueeze(2)).squeeze() 96 | u = self.feasible_u(dbdxfx, dbdxgx) 97 | # u = -X_batch[:,-1] 98 | feasibility_output = dbdxfx + dbdxgx * u 99 | # feasibility_output = dbdxfx 100 | return feasibility_output 101 | 102 | def feasible_violations(self, model_output, feasibility_output, batch_length, rlambda): 103 | # violations = -1 * feasibility_output - rlambda * torch.abs(model_output.transpose(0, 1)) 104 | violations = -1 * feasibility_output - rlambda * model_output.squeeze() 105 | # return torch.max(violations, torch.zeros([1,batch_length])) 106 | return violations 107 | 108 | def safe_correctness(self, ref_output, model_output, 109 | l_co: float = 1, alpha1: float = 1, 110 | alpha2: float = 0.001) -> torch.Tensor: 111 | ''' 112 | Penalize the incorrectness based on the h(x). 113 | If h(x) < 0 meaning the state x is unsafe, b(x) has to be negative. 114 | Therefore, alpha1, the gain of the penalty, can be large. 115 | If h(x) > 0 meaning the state is temporarily safe, b(x) can be +/-. 116 | To maximize coverage of b(x), a small penalty alpha2 is applied. 117 | :param ref_output: output of h(x) 118 | :param model_output: output of NN(x) 119 | :param l_co: gain of the loss 120 | :param alpha1: penalty for unsafe incorrectness 121 | :param alpha2: penalty for coverage 122 | :return: safety oriented correctness loss 123 | ''' 124 | norm_model_output = torch.tanh(model_output) 125 | length = len(-ref_output + norm_model_output) 126 | # norm_ref_output = torch.tanh(ref_output) 127 | # FalsePositive_loss = torch.max(-ref_output.reshape([1, length]), torch.zeros([1, length])) * \ 128 | # torch.max((model_output + 0.01).reshape([1, length]), torch.zeros([1, length])) 129 | # FalseNegative_loss = torch.max(ref_output.reshape([1, length]), torch.zeros([1, length])) * \ 130 | # torch.max((-model_output + 0.01).reshape([1, length]), torch.zeros([1, length])) 131 | FalsePositive_loss = torch.relu(-ref_output) * torch.relu((model_output-1e-4)) 132 | FalseNegative_loss = torch.relu(ref_output) * torch.relu((-model_output+1e-4)) 133 | # loss = l_co * torch.sum(alpha1*FalsePositive_loss + alpha2*FalseNegative_loss) 134 | # return loss 135 | return torch.sum(FalsePositive_loss), torch.sum(FalseNegative_loss) 136 | 137 | def trivial_panelty(self, ref_output, model_output, coeff=1, epsilon=0.001): 138 | min_ref = torch.max(ref_output) 139 | max_ref = torch.min(ref_output) 140 | # if max_ref >= 1e-4 and min_ref <= -1e-4: 141 | # non_pos_loss = coeff * torch.max(0.5 - torch.max(model_output), torch.zeros(1)) 142 | # non_neg_loss = coeff * torch.max(0.5 - torch.max(-model_output), torch.zeros(1)) 143 | if max_ref >= 1e-4 and min_ref >= 1e-4: 144 | non_pos_loss = torch.zeros(1) 145 | non_neg_loss = torch.zeros(1) 146 | elif max_ref <= -1e-4 and min_ref <= -1e-4: 147 | non_pos_loss = torch.zeros(1) 148 | non_neg_loss = torch.zeros(1) 149 | else: 150 | non_pos_loss = coeff * torch.max(epsilon - torch.max(model_output), torch.zeros(1)) 151 | non_neg_loss = coeff * torch.max(-epsilon - torch.max(-model_output), torch.zeros(1)) 152 | loss = non_pos_loss + non_neg_loss 153 | return loss 154 | 155 | def compute_volume(self, rdm_input, model_output=None): 156 | ''' 157 | Compute volume covered by b(x) 158 | :param rdm_input: random uniform samples 159 | :return: numbers of samples (volume) 160 | ''' 161 | # compute the positive volume contained by the NCBF 162 | if model_output is None: 163 | model_output = self.forward(rdm_input).squeeze() 164 | pos_output = torch.max(model_output, torch.zeros(len(rdm_input))) 165 | return torch.sum(pos_output > 0)/len(rdm_input) 166 | 167 | 168 | def train(self, num_epoch, num_restart=10, warm_start=False): 169 | if warm_start: 170 | learning_rate = 1e-2 171 | else: 172 | learning_rate = 1e-2 173 | optimizer = optim.Adam(self.model.parameters(), lr=learning_rate, weight_decay=0.01) 174 | scheduler = ExponentialLR(optimizer, gamma=0.99) 175 | # define hyper-parameters 176 | alphaf, alpha1, alpha2 = 1, 1, 0.0001 177 | # 1, 1e-8 178 | # Set alpha2=0 for feasibility test with Floss quickly converge to 0 179 | # If set alpha2 converges but does not pass the verification, then increase the sampling number. 180 | # This problem is caused by lack of counter examples and can be solved by introducing CE from Verifier 181 | rlambda = 1 182 | 183 | # Generate data 184 | size = 128 185 | volume = torch.Tensor([0]) 186 | for self.run in range(num_restart): 187 | rdm_input = self.generate_data(size) 188 | # rdm_input = self.generate_input(shape) 189 | # ref_output = torch.unsqueeze(self.h_x(rdm_input.transpose(0, self.DIM)), self.DIM) 190 | ref_output = self.case.h_x(rdm_input).unsqueeze(1) 191 | normalized_ref_output = torch.tanh(10 * ref_output) 192 | # batch_length = 8**self.DIM 193 | batch_length = size ** (self.DIM-1) 194 | training_loader = DataLoader(list(zip(rdm_input, normalized_ref_output)), batch_size=batch_length, 195 | shuffle=True) 196 | 197 | pbar = tqdm(total=num_epoch) 198 | veri_result = False 199 | if not warm_start: 200 | pass 201 | for epoch in range(num_epoch): 202 | # Initialize loss 203 | running_loss = 0.0 204 | feasibility_running_loss = torch.Tensor([0.0]) 205 | correctness_running_loss = torch.Tensor([0.0]) 206 | trivial_running_loss = torch.Tensor([0.0]) 207 | 208 | # Batch Training 209 | for X_batch, y_batch in training_loader: 210 | model_output = self.forward(X_batch) 211 | 212 | # warm_start_loss = self.warm_start(y_batch, model_output) 213 | correctness_loss, coverage_loss = self.safe_correctness(y_batch, model_output, l_co=1, alpha1=alpha1, alpha2=alpha2) 214 | # trivial_loss = self.trivial_panelty(ref_output, self.model.forward(rdm_input), 1) 215 | trivial_loss = self.trivial_panelty(y_batch, model_output, 1) 216 | 217 | # grad = self.numerical_gradient(X_batch, model_output, batch_length, epsilon=0.001) 218 | # grad_vector = torch.vstack(grad) 219 | grad_vector = torch.vstack([self.get_grad(x)[0] for x in X_batch]) 220 | # dbdx(fx + gx*u) should be >=0. If <0, a penalty will be added. 221 | # feasibility_output = (torch.relu(model_output)*self.feasibility_loss(grad_vector, X_batch)) 222 | feasibility_output = batch_length * (torch.relu(model_output.squeeze()) * torch.relu(-model_output.squeeze()+1e-2) * 223 | (self.feasibility_loss(grad_vector, X_batch) + model_output.squeeze())) 224 | ce_indicator = (torch.relu(model_output.squeeze())/model_output.squeeze() * 225 | torch.relu(-model_output.squeeze()+1e-2)/(-model_output.squeeze()+1e-2) * 226 | torch.relu(-self.feasibility_loss(grad_vector, X_batch) - model_output.squeeze()) 227 | /((-self.feasibility_loss(grad_vector, X_batch) - model_output.squeeze()))) 228 | # check_item = torch.max((-torch.abs(model_output)+0.2).reshape([1, len(model_output)]), torch.zeros([1, len(model_output)])) 229 | # feasibility_loss = torch.sum(torch.tanh(check_item*feasibility_output)) 230 | 231 | # Our loss function 232 | # violations = -check_item * self.feasible_violations(model_output, feasibility_output, batch_length, rlambda) 233 | feasibility_loss = torch.relu(-feasibility_output).sum() 234 | # Chuchu Fan loss function 235 | # violations = check_item * self.feasible_violations(model_output, feasibility_output, batch_length, rlambda) 236 | # violations = -1 * feasibility_output - torch.max(rlambda * torch.abs(model_output.transpose(0, 1)), 237 | # torch.zeros([1, batch_length])) 238 | # feasibility_loss = 2 * torch.sum(torch.max(violations - 1e-4, torch.zeros([1, len(model_output)]))) 239 | if feasibility_loss <= 0.001: 240 | pass 241 | mseloss = torch.nn.MSELoss() 242 | # loss = self.def_loss(1 * correctness_loss + 1 * feasibility_loss + 1 * trivial_loss) 243 | # floss = mseloss(torch.max(violations - 1e-4, torch.zeros([1, batch_length])), torch.zeros(batch_length)) 244 | # tloss = mseloss(trivial_loss, torch.Tensor([0.0])) 245 | if warm_start: 246 | correctness_loss, coverage_loss = self.safe_correctness(y_batch, model_output, l_co=1, alpha1=1, alpha2=0.0001) 247 | loss = correctness_loss + coverage_loss + trivial_loss 248 | else: 249 | # loss = feasibility_loss 250 | loss = (alpha1*correctness_loss + alpha2*coverage_loss 251 | + alphaf*feasibility_loss + trivial_loss) 252 | 253 | 254 | loss.backward() 255 | # with torch.no_grad(): 256 | # loss = torch.max(loss) 257 | optimizer.step() 258 | optimizer.zero_grad() 259 | alpha1 += 0.1 * correctness_loss.item() 260 | # alphaf += 0.1 * feasibility_loss.item() 261 | 262 | # Print Detailed Loss 263 | running_loss += loss.item() 264 | feasibility_running_loss += feasibility_loss.item() 265 | correctness_running_loss += correctness_loss.item() 266 | trivial_running_loss += trivial_loss.item() 267 | 268 | # if feasibility_running_loss <= 0.001 and correctness_loss <= 0.01: 269 | # alpha2 = 0.01 270 | # else: 271 | # alpha2 = 0 272 | # Log details of losses 273 | # if not warm_start: 274 | self.writer.add_scalar('Loss/Loss', running_loss, self.run*num_epoch+epoch) 275 | self.writer.add_scalar('Loss/FLoss', feasibility_running_loss.item(), self.run*num_epoch+epoch) 276 | self.writer.add_scalar('Loss/CLoss', correctness_running_loss.item(), self.run*num_epoch+epoch) 277 | self.writer.add_scalar('Loss/TLoss', trivial_running_loss.item(), self.run*num_epoch+epoch) 278 | pbar.set_postfix({'Loss': running_loss, 279 | 'Floss': feasibility_running_loss.item(), 280 | 'Closs': correctness_running_loss.item(), 281 | 'Tloss': trivial_running_loss.item(), 282 | 'WarmUp': str(warm_start), 283 | 'FCEnum': ce_indicator.sum().item(), 284 | 'Vol': volume.item()}) 285 | pbar.update(1) 286 | scheduler.step() 287 | # Log volume of safe region 288 | volume = self.compute_volume(rdm_input) 289 | self.writer.add_scalar('Volume', volume, self.run*num_epoch+epoch) 290 | # self.writer.add_scalar('Verifiable', veri_result, self.run * num_epoch + epoch) 291 | # Process Bar Print Losses 292 | 293 | 294 | 295 | pbar.close() 296 | # visualize(self.model) 297 | # torch.save(self.model.state_dict(), f'Trained_model/NCBF/NCBF_Obs{self.run}.pt'.format(self.run)) 298 | 299 | # ObsAvoid = ObsAvoid() 300 | # 301 | # newCBF = NCBF_Synth([32, 32], [True, True], ObsAvoid, verbose=True) 302 | # newCBF.model.load_state_dict(torch.load('WarmModel2.pt')) 303 | # # newCBF.train(num_epoch=20, num_restart=2, warm_start=True) 304 | # # layers_to_freeze = ['0.weight', '0.bias'] 305 | # # for name, param in newCBF.model.named_parameters(): 306 | # # # Check if the current parameter belongs to a layer you want to freeze 307 | # # if any(layer_name in name for layer_name in layers_to_freeze): 308 | # # param.requires_grad = False 309 | # # else: 310 | # # param.requires_grad = True 311 | # newCBF.train(num_epoch=10, num_restart=2, warm_start=False) 312 | # newCBF.train(num_epoch=10, num_restart=2, warm_start=False) 313 | # newCBF.train(num_epoch=10, num_restart=2, warm_start=False) 314 | # newCBF.train(num_epoch=10, num_restart=2, warm_start=False) 315 | # newCBF.train(num_epoch=10, num_restart=2, warm_start=False) 316 | # newCBF.train(num_epoch=10, num_restart=2, warm_start=False) 317 | # # newCBF.run += 1 318 | # newCBF.train(num_epoch=10, num_restart=8, warm_start=False) 319 | # # newCBF.model.load_state_dict(torch.load('Trained_model/NCBF/NCBF_Obs4.pt')) 320 | # 321 | # # There is a bug in verifier that causes memory error due to too many intersections to verify 322 | # veri_result, num = newCBF.veri.proceed_verification() 323 | -------------------------------------------------------------------------------- /NCBFSynth/NCBF_SynthBF.py: -------------------------------------------------------------------------------- 1 | import sys, os 2 | sys.path.append(os.path.realpath(os.path.dirname(__file__)+"/..")) 3 | import torch 4 | from tqdm import tqdm 5 | from scipy.optimize import minimize 6 | # from progress.bar import Bar 7 | from Modules.NCBF import * 8 | from torch import optim 9 | from torch.optim.lr_scheduler import ExponentialLR 10 | from torch.utils.tensorboard import SummaryWriter 11 | import torchvision 12 | import torchvision.transforms as transforms 13 | from torch.utils.data import DataLoader 14 | from Cases.Darboux import Darboux 15 | # from Verifier.Verifier import Verifier 16 | # from Critic_Synth.NCritic import * 17 | import time 18 | from Visualization.visualization import visualize 19 | # from collections import OrderedDict 20 | 21 | class NCBF_Synth(NCBF): 22 | ''' 23 | Synthesize an NCBF for a given safe region h(x) 24 | for given a system with polynomial f(x) and g(x) 25 | ''' 26 | def __init__(self,arch, act_layer, case, verbose=False): 27 | ''' 28 | Input architecture and ReLU layers, input case, verbose for display 29 | :param arch: [list of int] architecture of the NN 30 | :param act_layer: [list of bool] if the corresponding layer with ReLU, then True 31 | :param case: Pre-defined case class, with f(x), g(x) and h(x) 32 | :param verbose: Flag for display or not 33 | ''' 34 | self.case = case 35 | DOMAIN = self.case.DOMAIN 36 | super().__init__(arch, act_layer, DOMAIN) 37 | # Under construction: Critic is designed to tuning loss fcn automatically 38 | # self.critic = NeuralCritic(case) 39 | # Verifier proposed to verify feasibility 40 | self.veri = Verifier(NCBF=self, case=case, grid_shape=[100, 100, 100], verbose=verbose) 41 | # lctime = time.ctime(time.time()) 42 | lctime = time.strftime("%Y%m%d%H%M%S") 43 | # Tensorboard 44 | self.writer = SummaryWriter(f'./runs/NCBF/{lctime}'.format(lctime)) 45 | self.run = 0 46 | 47 | # def numerical_gradient(self, X_batch, model_output, batch_length, epsilon=0.001): 48 | # # compute numerical gradient for each dimension by (x+dx)/dx 49 | # grad = [] 50 | # for i in range(self.DIM): 51 | # gradStep = torch.zeros(self.DIM) 52 | # gradStep[i] += epsilon 53 | # gradData = X_batch + gradStep 54 | # dbdxi = ((self.forward(gradData) - model_output) / epsilon).reshape([batch_length]) 55 | # grad.append(dbdxi) 56 | # 57 | # return grad 58 | 59 | def get_grad(self, x): 60 | grad_input = torch.tensor(x, requires_grad=True, dtype=torch.float) 61 | return torch.autograd.grad(self.model.forward(grad_input), grad_input) 62 | 63 | def feasible_con(self, u, dbdxfx, dbdxgx): 64 | # function to maximize: (db/dx)*fx + (db/dx)*gx*u 65 | return np.max(-dbdxfx - dbdxgx * u) 66 | 67 | def feasible_u(self, dbdxfx, dbdxgx, min_flag=False): 68 | # find u that minimize (db/dx)*fx + (db/dx)*gx*u 69 | if min_flag: 70 | df = dbdxfx.detach().numpy() 71 | dg = dbdxgx.detach().numpy() 72 | res_list = [] 73 | for i in range(len(df)): 74 | res = minimize(self.feasible_con, x0=np.zeros(1), args=(df[i], dg[i])) 75 | # pos_res = np.max([res.fun, np.zeros(len([res.fun]))]) 76 | res_list.append(res.x) 77 | return torch.Tensor(res_list).squeeze() 78 | else: 79 | # Quick check for scalar u only 80 | [[u_lb, u_ub]] = torch.Tensor(self.case.CTRLDOM) 81 | res_list = [] 82 | for i in range(len(dbdxfx)): 83 | res_ulb = dbdxfx[i] + dbdxgx[i] * u_lb 84 | res_uub = dbdxfx[i] + dbdxgx[i] * u_ub 85 | res = torch.max(res_ulb, res_uub) 86 | res_list.append(res) 87 | return torch.Tensor(res_list).squeeze() 88 | 89 | def feasibility_loss(self, grad_vector, X_batch): 90 | # compute loss based on (db/dx)*fx + (db/dx)*gx*u 91 | dbdxfx = (grad_vector.unsqueeze(-1).transpose(-1, -2) 92 | @ self.case.f_x(X_batch).transpose(0, 1).unsqueeze(2)).squeeze() 93 | dbdxgx = (grad_vector.unsqueeze(-1).transpose(-1, -2) 94 | @ self.case.g_x(X_batch).transpose(0, 1).unsqueeze(2)).squeeze() 95 | u = self.feasible_u(dbdxfx, dbdxgx) 96 | # u = -X_batch[:,-1] 97 | feasibility_output = dbdxfx + dbdxgx * u 98 | # feasibility_output = dbdxfx 99 | return feasibility_output 100 | 101 | def feasible_violations(self, model_output, feasibility_output, batch_length, rlambda): 102 | # violations = -1 * feasibility_output - rlambda * torch.abs(model_output.transpose(0, 1)) 103 | violations = -1 * feasibility_output - rlambda * model_output.squeeze() 104 | # return torch.max(violations, torch.zeros([1,batch_length])) 105 | return violations 106 | 107 | def safe_correctness(self, ref_output, model_output, 108 | l_co: float = 1, alpha1: float = 1, 109 | alpha2: float = 0.001) -> torch.Tensor: 110 | ''' 111 | Penalize the incorrectness based on the h(x). 112 | If h(x) < 0 meaning the state x is unsafe, b(x) has to be negative. 113 | Therefore, alpha1, the gain of the penalty, can be large. 114 | If h(x) > 0 meaning the state is temporarily safe, b(x) can be +/-. 115 | To maximize coverage of b(x), a small penalty alpha2 is applied. 116 | :param ref_output: output of h(x) 117 | :param model_output: output of NN(x) 118 | :param l_co: gain of the loss 119 | :param alpha1: penalty for unsafe incorrectness 120 | :param alpha2: penalty for coverage 121 | :return: safety oriented correctness loss 122 | ''' 123 | norm_model_output = torch.tanh(model_output) 124 | length = len(-ref_output + norm_model_output) 125 | # norm_ref_output = torch.tanh(ref_output) 126 | # FalsePositive_loss = torch.max(-ref_output.reshape([1, length]), torch.zeros([1, length])) * \ 127 | # torch.max((model_output + 0.01).reshape([1, length]), torch.zeros([1, length])) 128 | # FalseNegative_loss = torch.max(ref_output.reshape([1, length]), torch.zeros([1, length])) * \ 129 | # torch.max((-model_output + 0.01).reshape([1, length]), torch.zeros([1, length])) 130 | FalsePositive_loss = torch.relu(-ref_output) * torch.relu((model_output)) 131 | FalseNegative_loss = torch.relu(ref_output) * torch.relu((-model_output)) 132 | # loss = l_co * torch.sum(alpha1*FalsePositive_loss + alpha2*FalseNegative_loss) 133 | # return loss 134 | return torch.sum(FalsePositive_loss), torch.sum(FalseNegative_loss) 135 | 136 | def trivial_panelty(self, ref_output, model_output, coeff=1, epsilon=0.001): 137 | min_ref = torch.max(ref_output) 138 | max_ref = torch.min(ref_output) 139 | # if max_ref >= 1e-4 and min_ref <= -1e-4: 140 | # non_pos_loss = coeff * torch.max(0.5 - torch.max(model_output), torch.zeros(1)) 141 | # non_neg_loss = coeff * torch.max(0.5 - torch.max(-model_output), torch.zeros(1)) 142 | if max_ref >= 1e-4 and min_ref >= 1e-4: 143 | non_pos_loss = torch.zeros(1) 144 | non_neg_loss = torch.zeros(1) 145 | elif max_ref <= -1e-4 and min_ref <= -1e-4: 146 | non_pos_loss = torch.zeros(1) 147 | non_neg_loss = torch.zeros(1) 148 | else: 149 | non_pos_loss = coeff * torch.max(epsilon - torch.max(model_output), torch.zeros(1)) 150 | non_neg_loss = coeff * torch.max(-epsilon - torch.max(-model_output), torch.zeros(1)) 151 | loss = non_pos_loss + non_neg_loss 152 | return loss 153 | 154 | def compute_volume(self, rdm_input, model_output=None): 155 | ''' 156 | Compute volume covered by b(x) 157 | :param rdm_input: random uniform samples 158 | :return: numbers of samples (volume) 159 | ''' 160 | # compute the positive volume contained by the NCBF 161 | if model_output is None: 162 | model_output = self.forward(rdm_input).squeeze() 163 | pos_output = torch.max(model_output, torch.zeros(len(rdm_input))) 164 | return torch.sum(pos_output > 0)/len(rdm_input) 165 | 166 | 167 | def train(self, num_epoch, num_restart=10, warm_start=False): 168 | if warm_start: 169 | learning_rate = 1e-2 170 | else: 171 | learning_rate = 1e-2 172 | optimizer = optim.Adam(self.model.parameters(), lr=learning_rate, weight_decay=0.01) 173 | scheduler = ExponentialLR(optimizer, gamma=0.999) 174 | # define hyper-parameters 175 | alphaf, alpha1, alpha2 = 1, 1, 0.0001 176 | # 1, 1e-8 177 | # Set alpha2=0 for feasibility test with Floss quickly converge to 0 178 | # If set alpha2 converges but does not pass the verification, then increase the sampling number. 179 | # This problem is caused by lack of counter examples and can be solved by introducing CE from Verifier 180 | rlambda = 1 181 | 182 | # Generate data 183 | size = 128 184 | volume = torch.Tensor([0]) 185 | for self.run in range(num_restart): 186 | rdm_input = self.generate_data(size) 187 | # rdm_input = self.generate_input(shape) 188 | # ref_output = torch.unsqueeze(self.h_x(rdm_input.transpose(0, self.DIM)), self.DIM) 189 | ref_output = self.case.h_x(rdm_input).unsqueeze(1) 190 | normalized_ref_output = torch.tanh(10 * ref_output) 191 | # batch_length = 8**self.DIM 192 | batch_length = size ** self.DIM 193 | training_loader = DataLoader(list(zip(rdm_input, normalized_ref_output)), batch_size=batch_length, 194 | shuffle=True) 195 | 196 | pbar = tqdm(total=num_epoch) 197 | veri_result = False 198 | if not warm_start: 199 | pass 200 | for epoch in range(num_epoch): 201 | # Initialize loss 202 | running_loss = 0.0 203 | feasibility_running_loss = torch.Tensor([0.0]) 204 | correctness_running_loss = torch.Tensor([0.0]) 205 | trivial_running_loss = torch.Tensor([0.0]) 206 | 207 | # Batch Training 208 | for X_batch, y_batch in training_loader: 209 | model_output = self.forward(X_batch) 210 | 211 | # warm_start_loss = self.warm_start(y_batch, model_output) 212 | correctness_loss, coverage_loss = self.safe_correctness(y_batch, model_output, l_co=1, alpha1=alpha1, alpha2=alpha2) 213 | # trivial_loss = self.trivial_panelty(ref_output, self.model.forward(rdm_input), 1) 214 | trivial_loss = self.trivial_panelty(y_batch, model_output, 1) 215 | 216 | # grad = self.numerical_gradient(X_batch, model_output, batch_length, epsilon=0.001) 217 | # grad_vector = torch.vstack(grad) 218 | grad_vector = torch.vstack([self.get_grad(x)[0] for x in X_batch]) 219 | # dbdx(fx + gx*u) should be >=0. If <0, a penalty will be added. 220 | # feasibility_output = (torch.relu(model_output)*self.feasibility_loss(grad_vector, X_batch)) 221 | feasibility_output = batch_length * (torch.relu(model_output.squeeze()) * torch.relu(-model_output.squeeze()+1e-2) * 222 | (self.feasibility_loss(grad_vector, X_batch) + model_output.squeeze())) 223 | ce_indicator = (torch.relu(model_output.squeeze())/model_output.squeeze() * 224 | torch.relu(-model_output.squeeze()+1e-2)/(-model_output.squeeze()+1e-2) * 225 | torch.relu(-self.feasibility_loss(grad_vector, X_batch) - model_output.squeeze()) 226 | /((-self.feasibility_loss(grad_vector, X_batch) - model_output.squeeze()))) 227 | # check_item = torch.max((-torch.abs(model_output)+0.2).reshape([1, len(model_output)]), torch.zeros([1, len(model_output)])) 228 | # feasibility_loss = torch.sum(torch.tanh(check_item*feasibility_output)) 229 | 230 | # Our loss function 231 | # violations = -check_item * self.feasible_violations(model_output, feasibility_output, batch_length, rlambda) 232 | feasibility_loss = torch.relu(-feasibility_output).sum() 233 | # Chuchu Fan loss function 234 | # violations = check_item * self.feasible_violations(model_output, feasibility_output, batch_length, rlambda) 235 | # violations = -1 * feasibility_output - torch.max(rlambda * torch.abs(model_output.transpose(0, 1)), 236 | # torch.zeros([1, batch_length])) 237 | # feasibility_loss = 2 * torch.sum(torch.max(violations - 1e-4, torch.zeros([1, len(model_output)]))) 238 | if feasibility_loss <= 0.001: 239 | pass 240 | mseloss = torch.nn.MSELoss() 241 | # loss = self.def_loss(1 * correctness_loss + 1 * feasibility_loss + 1 * trivial_loss) 242 | # floss = mseloss(torch.max(violations - 1e-4, torch.zeros([1, batch_length])), torch.zeros(batch_length)) 243 | # tloss = mseloss(trivial_loss, torch.Tensor([0.0])) 244 | if warm_start: 245 | correctness_loss, coverage_loss = self.safe_correctness(y_batch, model_output, l_co=1, alpha1=1, alpha2=0.0001) 246 | loss = correctness_loss + coverage_loss + trivial_loss 247 | else: 248 | # loss = feasibility_loss 249 | loss = (alpha1*correctness_loss + alpha2*coverage_loss 250 | + alphaf*feasibility_loss + trivial_loss) 251 | 252 | 253 | loss.backward() 254 | # with torch.no_grad(): 255 | # loss = torch.max(loss) 256 | optimizer.step() 257 | optimizer.zero_grad() 258 | alpha1 += 0.1 * correctness_loss.item() 259 | alphaf += 0.1 * feasibility_loss.item() 260 | 261 | # Print Detailed Loss 262 | running_loss += loss.item() 263 | feasibility_running_loss += feasibility_loss.item() 264 | correctness_running_loss += correctness_loss.item() 265 | trivial_running_loss += trivial_loss.item() 266 | 267 | # if feasibility_running_loss <= 0.001 and correctness_loss <= 0.01: 268 | # alpha2 = 0.01 269 | # else: 270 | # alpha2 = 0 271 | # Log details of losses 272 | # if not warm_start: 273 | self.writer.add_scalar('Loss/Loss', running_loss, self.run*num_epoch+epoch) 274 | self.writer.add_scalar('Loss/FLoss', feasibility_running_loss.item(), self.run*num_epoch+epoch) 275 | self.writer.add_scalar('Loss/CLoss', correctness_running_loss.item(), self.run*num_epoch+epoch) 276 | self.writer.add_scalar('Loss/TLoss', trivial_running_loss.item(), self.run*num_epoch+epoch) 277 | pbar.set_postfix({'Loss': running_loss, 278 | 'Floss': feasibility_running_loss.item(), 279 | 'Closs': correctness_running_loss.item(), 280 | 'Tloss': trivial_running_loss.item(), 281 | 'WarmUp': str(warm_start), 282 | 'FCEnum': ce_indicator.sum().item(), 283 | 'Vol': volume.item()}) 284 | pbar.update(1) 285 | scheduler.step() 286 | # Log volume of safe region 287 | volume = self.compute_volume(rdm_input) 288 | self.writer.add_scalar('Volume', volume, self.run*num_epoch+epoch) 289 | # self.writer.add_scalar('Verifiable', veri_result, self.run * num_epoch + epoch) 290 | # Process Bar Print Losses 291 | 292 | 293 | 294 | pbar.close() 295 | visualize(self.model) 296 | # torch.save(self.model.state_dict(), f'Trained_model/NCBF/NCBF_Obs{self.run}.pt'.format(self.run)) 297 | 298 | Darboux = Darboux() 299 | 300 | newCBF = NCBF_Synth([32, 32], [True, True], Darboux, verbose=True) 301 | # newCBF.model.load_state_dict(torch.load('WarmModel2.pt')) 302 | newCBF.train(num_epoch=20, num_restart=2, warm_start=True) 303 | # layers_to_freeze = ['0.weight', '0.bias'] 304 | # for name, param in newCBF.model.named_parameters(): 305 | # # Check if the current parameter belongs to a layer you want to freeze 306 | # if any(layer_name in name for layer_name in layers_to_freeze): 307 | # param.requires_grad = False 308 | # else: 309 | # param.requires_grad = True 310 | newCBF.train(num_epoch=10, num_restart=2, warm_start=False) 311 | newCBF.train(num_epoch=10, num_restart=2, warm_start=False) 312 | newCBF.train(num_epoch=10, num_restart=2, warm_start=False) 313 | newCBF.train(num_epoch=10, num_restart=2, warm_start=False) 314 | newCBF.train(num_epoch=10, num_restart=2, warm_start=False) 315 | newCBF.train(num_epoch=10, num_restart=2, warm_start=False) 316 | # # newCBF.run += 1 317 | # newCBF.train(num_epoch=10, num_restart=8, warm_start=False) 318 | # # newCBF.model.load_state_dict(torch.load('Trained_model/NCBF/NCBF_Obs4.pt')) 319 | # 320 | # # There is a bug in verifier that causes memory error due to too many intersections to verify 321 | # veri_result, num = newCBF.veri.proceed_verification() 322 | -------------------------------------------------------------------------------- /NCBFSynth/NCBF_SynthCBF.py: -------------------------------------------------------------------------------- 1 | import sys, os 2 | sys.path.append(os.path.realpath(os.path.dirname(__file__)+"/..")) 3 | import torch 4 | from tqdm import tqdm 5 | from scipy.optimize import minimize 6 | # from progress.bar import Bar 7 | from Modules.NCBF import * 8 | from torch import optim 9 | from torch.optim.lr_scheduler import ExponentialLR 10 | from torch.utils.tensorboard import SummaryWriter 11 | import torchvision 12 | import torchvision.transforms as transforms 13 | from torch.utils.data import DataLoader 14 | from Cases.ObsAvoid import ObsAvoid 15 | # from Verifier.Verifier import Verifier 16 | # from Critic_Synth.NCritic import * 17 | import time 18 | from Visualization.visualization import visualize 19 | # from collections import OrderedDict 20 | 21 | class NCBF_Synth(NCBF): 22 | ''' 23 | Synthesize an NCBF for a given safe region h(x) 24 | for given a system with polynomial f(x) and g(x) 25 | ''' 26 | def __init__(self,arch, act_layer, case, verbose=False): 27 | ''' 28 | Input architecture and ReLU layers, input case, verbose for display 29 | :param arch: [list of int] architecture of the NN 30 | :param act_layer: [list of bool] if the corresponding layer with ReLU, then True 31 | :param case: Pre-defined case class, with f(x), g(x) and h(x) 32 | :param verbose: Flag for display or not 33 | ''' 34 | self.case = case 35 | DOMAIN = self.case.DOMAIN 36 | super().__init__(arch, act_layer, DOMAIN) 37 | # Under construction: Critic is designed to tuning loss fcn automatically 38 | # self.critic = NeuralCritic(case) 39 | # Verifier proposed to verify feasibility 40 | self.veri = Verifier(NCBF=self, case=case, grid_shape=[100, 100, 100], verbose=verbose) 41 | # lctime = time.ctime(time.time()) 42 | lctime = time.strftime("%Y%m%d%H%M%S") 43 | # Tensorboard 44 | self.writer = SummaryWriter(f'./runs/NCBF/{lctime}'.format(lctime)) 45 | self.run = 0 46 | 47 | # def numerical_gradient(self, X_batch, model_output, batch_length, epsilon=0.001): 48 | # # compute numerical gradient for each dimension by (x+dx)/dx 49 | # grad = [] 50 | # for i in range(self.DIM): 51 | # gradStep = torch.zeros(self.DIM) 52 | # gradStep[i] += epsilon 53 | # gradData = X_batch + gradStep 54 | # dbdxi = ((self.forward(gradData) - model_output) / epsilon).reshape([batch_length]) 55 | # grad.append(dbdxi) 56 | # 57 | # return grad 58 | 59 | def get_grad(self, x): 60 | grad_input = torch.tensor(x, requires_grad=True, dtype=torch.float) 61 | return torch.autograd.grad(self.model.forward(grad_input), grad_input) 62 | 63 | def feasible_con(self, u, dbdxfx, dbdxgx): 64 | # function to maximize: (db/dx)*fx + (db/dx)*gx*u 65 | return dbdxfx + dbdxgx * u 66 | 67 | def feasible_u(self, dbdxfx, dbdxgx, min_flag=False): 68 | # find u that minimize (db/dx)*fx + (db/dx)*gx*u 69 | if min_flag: 70 | df = dbdxfx.detach().numpy() 71 | dg = dbdxgx.detach().numpy() 72 | res_list = [] 73 | for i in range(len(df)): 74 | res = minimize(self.feasible_con, x0=np.zeros(1), args=(df[i], dg[i])) 75 | # pos_res = np.max([res.fun, np.zeros(len([res.fun]))]) 76 | res_list.append(res.x) 77 | return torch.Tensor(res_list).squeeze() 78 | else: 79 | # Quick check for scalar u only 80 | [[u_lb, u_ub]] = torch.Tensor(self.case.CTRLDOM) 81 | res_list = [] 82 | for i in range(len(dbdxfx)): 83 | res_ulb = dbdxfx[i] + dbdxgx[i] * u_lb 84 | res_uub = dbdxfx[i] + dbdxgx[i] * u_ub 85 | # res = torch.max(res_ulb, res_uub) 86 | res = [u_lb, u_ub][np.argmax([res_ulb, res_uub])] 87 | res_list.append(res) 88 | return torch.Tensor(res_list).squeeze() 89 | 90 | def feasibility_loss(self, grad_vector, X_batch): 91 | # compute loss based on (db/dx)*fx + (db/dx)*gx*u 92 | dbdxfx = (grad_vector.unsqueeze(-1).transpose(-1, -2) 93 | @ self.case.f_x(X_batch).transpose(0, 1).unsqueeze(2)).squeeze() 94 | dbdxgx = (grad_vector.unsqueeze(-1).transpose(-1, -2) 95 | @ self.case.g_x(X_batch).transpose(0, 1).unsqueeze(2)).squeeze() 96 | u = self.feasible_u(dbdxfx, dbdxgx) 97 | # u = -X_batch[:,-1] 98 | feasibility_output = dbdxfx + dbdxgx * u 99 | # feasibility_output = dbdxfx 100 | return feasibility_output 101 | 102 | def feasible_violations(self, model_output, feasibility_output, batch_length, rlambda): 103 | # violations = -1 * feasibility_output - rlambda * torch.abs(model_output.transpose(0, 1)) 104 | violations = -1 * feasibility_output - rlambda * model_output.squeeze() 105 | # return torch.max(violations, torch.zeros([1,batch_length])) 106 | return violations 107 | 108 | def safe_correctness(self, ref_output, model_output, 109 | l_co: float = 1, alpha1: float = 1, 110 | alpha2: float = 0.001) -> torch.Tensor: 111 | ''' 112 | Penalize the incorrectness based on the h(x). 113 | If h(x) < 0 meaning the state x is unsafe, b(x) has to be negative. 114 | Therefore, alpha1, the gain of the penalty, can be large. 115 | If h(x) > 0 meaning the state is temporarily safe, b(x) can be +/-. 116 | To maximize coverage of b(x), a small penalty alpha2 is applied. 117 | :param ref_output: output of h(x) 118 | :param model_output: output of NN(x) 119 | :param l_co: gain of the loss 120 | :param alpha1: penalty for unsafe incorrectness 121 | :param alpha2: penalty for coverage 122 | :return: safety oriented correctness loss 123 | ''' 124 | norm_model_output = torch.tanh(model_output) 125 | length = len(-ref_output + norm_model_output) 126 | # norm_ref_output = torch.tanh(ref_output) 127 | # FalsePositive_loss = torch.max(-ref_output.reshape([1, length]), torch.zeros([1, length])) * \ 128 | # torch.max((model_output + 0.01).reshape([1, length]), torch.zeros([1, length])) 129 | # FalseNegative_loss = torch.max(ref_output.reshape([1, length]), torch.zeros([1, length])) * \ 130 | # torch.max((-model_output + 0.01).reshape([1, length]), torch.zeros([1, length])) 131 | FalsePositive_loss = torch.relu(-ref_output) * torch.relu((model_output)) 132 | FalseNegative_loss = torch.relu(ref_output) * torch.relu((-model_output)) 133 | # loss = l_co * torch.sum(alpha1*FalsePositive_loss + alpha2*FalseNegative_loss) 134 | # return loss 135 | return torch.sum(FalsePositive_loss), torch.sum(FalseNegative_loss) 136 | 137 | def trivial_panelty(self, ref_output, model_output, coeff=1, epsilon=0.001): 138 | min_ref = torch.max(ref_output) 139 | max_ref = torch.min(ref_output) 140 | # if max_ref >= 1e-4 and min_ref <= -1e-4: 141 | # non_pos_loss = coeff * torch.max(0.5 - torch.max(model_output), torch.zeros(1)) 142 | # non_neg_loss = coeff * torch.max(0.5 - torch.max(-model_output), torch.zeros(1)) 143 | if max_ref >= 1e-4 and min_ref >= 1e-4: 144 | non_pos_loss = torch.zeros(1) 145 | non_neg_loss = torch.zeros(1) 146 | elif max_ref <= -1e-4 and min_ref <= -1e-4: 147 | non_pos_loss = torch.zeros(1) 148 | non_neg_loss = torch.zeros(1) 149 | else: 150 | non_pos_loss = coeff * torch.max(epsilon - torch.max(model_output), torch.zeros(1)) 151 | non_neg_loss = coeff * torch.max(-epsilon - torch.max(-model_output), torch.zeros(1)) 152 | loss = non_pos_loss + non_neg_loss 153 | return loss 154 | 155 | def compute_volume(self, rdm_input, model_output=None): 156 | ''' 157 | Compute volume covered by b(x) 158 | :param rdm_input: random uniform samples 159 | :return: numbers of samples (volume) 160 | ''' 161 | # compute the positive volume contained by the NCBF 162 | if model_output is None: 163 | model_output = self.forward(rdm_input).squeeze() 164 | pos_output = torch.max(model_output, torch.zeros(len(rdm_input))) 165 | return torch.sum(pos_output > 0)/len(rdm_input) 166 | 167 | 168 | def train(self, num_epoch, num_restart=10, warm_start=False): 169 | if warm_start: 170 | learning_rate = 1e-2 171 | else: 172 | learning_rate = 1e-2 173 | optimizer = optim.Adam(self.model.parameters(), lr=learning_rate, weight_decay=0.01) 174 | scheduler = ExponentialLR(optimizer, gamma=0.99) 175 | # define hyper-parameters 176 | alphaf, alpha1, alpha2 = 1, 1, 0.0001 177 | # 1, 1e-8 178 | # Set alpha2=0 for feasibility test with Floss quickly converge to 0 179 | # If set alpha2 converges but does not pass the verification, then increase the sampling number. 180 | # This problem is caused by lack of counter examples and can be solved by introducing CE from Verifier 181 | rlambda = 1 182 | 183 | # Generate data 184 | size = 128 185 | volume = torch.Tensor([0]) 186 | for self.run in range(num_restart): 187 | rdm_input = self.generate_data(size) 188 | # rdm_input = self.generate_input(shape) 189 | # ref_output = torch.unsqueeze(self.h_x(rdm_input.transpose(0, self.DIM)), self.DIM) 190 | ref_output = self.case.h_x(rdm_input).unsqueeze(1) 191 | normalized_ref_output = torch.tanh(10 * ref_output) 192 | # batch_length = 8**self.DIM 193 | batch_length = size ** (self.DIM-1) 194 | training_loader = DataLoader(list(zip(rdm_input, normalized_ref_output)), batch_size=batch_length, 195 | shuffle=True) 196 | 197 | pbar = tqdm(total=num_epoch) 198 | veri_result = False 199 | if not warm_start: 200 | pass 201 | for epoch in range(num_epoch): 202 | # Initialize loss 203 | running_loss = 0.0 204 | feasibility_running_loss = torch.Tensor([0.0]) 205 | correctness_running_loss = torch.Tensor([0.0]) 206 | trivial_running_loss = torch.Tensor([0.0]) 207 | 208 | # Batch Training 209 | for X_batch, y_batch in training_loader: 210 | model_output = self.forward(X_batch) 211 | 212 | # warm_start_loss = self.warm_start(y_batch, model_output) 213 | correctness_loss, coverage_loss = self.safe_correctness(y_batch, model_output, l_co=1, alpha1=alpha1, alpha2=alpha2) 214 | # trivial_loss = self.trivial_panelty(ref_output, self.model.forward(rdm_input), 1) 215 | trivial_loss = self.trivial_panelty(y_batch, model_output, 1) 216 | 217 | # grad = self.numerical_gradient(X_batch, model_output, batch_length, epsilon=0.001) 218 | # grad_vector = torch.vstack(grad) 219 | grad_vector = torch.vstack([self.get_grad(x)[0] for x in X_batch]) 220 | # dbdx(fx + gx*u) should be >=0. If <0, a penalty will be added. 221 | # feasibility_output = (torch.relu(model_output)*self.feasibility_loss(grad_vector, X_batch)) 222 | feasibility_output = batch_length * (torch.relu(model_output.squeeze()) * torch.relu(-model_output.squeeze()+1e-2) * 223 | (self.feasibility_loss(grad_vector, X_batch) + model_output.squeeze())) 224 | ce_indicator = (torch.relu(model_output.squeeze())/model_output.squeeze() * 225 | torch.relu(-model_output.squeeze()+1e-2)/(-model_output.squeeze()+1e-2) * 226 | torch.relu(-self.feasibility_loss(grad_vector, X_batch) - model_output.squeeze()) 227 | /((-self.feasibility_loss(grad_vector, X_batch) - model_output.squeeze()))) 228 | # check_item = torch.max((-torch.abs(model_output)+0.2).reshape([1, len(model_output)]), torch.zeros([1, len(model_output)])) 229 | # feasibility_loss = torch.sum(torch.tanh(check_item*feasibility_output)) 230 | 231 | # Our loss function 232 | # violations = -check_item * self.feasible_violations(model_output, feasibility_output, batch_length, rlambda) 233 | feasibility_loss = torch.relu(-feasibility_output).sum() 234 | # Chuchu Fan loss function 235 | # violations = check_item * self.feasible_violations(model_output, feasibility_output, batch_length, rlambda) 236 | # violations = -1 * feasibility_output - torch.max(rlambda * torch.abs(model_output.transpose(0, 1)), 237 | # torch.zeros([1, batch_length])) 238 | # feasibility_loss = 2 * torch.sum(torch.max(violations - 1e-4, torch.zeros([1, len(model_output)]))) 239 | if feasibility_loss <= 0.001: 240 | pass 241 | mseloss = torch.nn.MSELoss() 242 | # loss = self.def_loss(1 * correctness_loss + 1 * feasibility_loss + 1 * trivial_loss) 243 | # floss = mseloss(torch.max(violations - 1e-4, torch.zeros([1, batch_length])), torch.zeros(batch_length)) 244 | # tloss = mseloss(trivial_loss, torch.Tensor([0.0])) 245 | if warm_start: 246 | correctness_loss, coverage_loss = self.safe_correctness(y_batch, model_output, l_co=1, alpha1=1, alpha2=0.0001) 247 | loss = correctness_loss + coverage_loss + trivial_loss 248 | else: 249 | # loss = feasibility_loss 250 | loss = (alpha1*correctness_loss + alpha2*coverage_loss 251 | + alphaf*feasibility_loss + trivial_loss) 252 | 253 | 254 | loss.backward() 255 | # with torch.no_grad(): 256 | # loss = torch.max(loss) 257 | optimizer.step() 258 | optimizer.zero_grad() 259 | alpha1 += 0.1 * correctness_loss.item() 260 | # alphaf += 0.1 * feasibility_loss.item() 261 | 262 | # Print Detailed Loss 263 | running_loss += loss.item() 264 | feasibility_running_loss += feasibility_loss.item() 265 | correctness_running_loss += correctness_loss.item() 266 | trivial_running_loss += trivial_loss.item() 267 | 268 | # if feasibility_running_loss <= 0.001 and correctness_loss <= 0.01: 269 | # alpha2 = 0.01 270 | # else: 271 | # alpha2 = 0 272 | # Log details of losses 273 | # if not warm_start: 274 | self.writer.add_scalar('Loss/Loss', running_loss, self.run*num_epoch+epoch) 275 | self.writer.add_scalar('Loss/FLoss', feasibility_running_loss.item(), self.run*num_epoch+epoch) 276 | self.writer.add_scalar('Loss/CLoss', correctness_running_loss.item(), self.run*num_epoch+epoch) 277 | self.writer.add_scalar('Loss/TLoss', trivial_running_loss.item(), self.run*num_epoch+epoch) 278 | pbar.set_postfix({'Loss': running_loss, 279 | 'Floss': feasibility_running_loss.item(), 280 | 'Closs': correctness_running_loss.item(), 281 | 'Tloss': trivial_running_loss.item(), 282 | 'WarmUp': str(warm_start), 283 | 'FCEnum': ce_indicator.sum().item(), 284 | 'Vol': volume.item()}) 285 | pbar.update(1) 286 | scheduler.step() 287 | # Log volume of safe region 288 | volume = self.compute_volume(rdm_input) 289 | self.writer.add_scalar('Volume', volume, self.run*num_epoch+epoch) 290 | # self.writer.add_scalar('Verifiable', veri_result, self.run * num_epoch + epoch) 291 | # Process Bar Print Losses 292 | 293 | 294 | 295 | pbar.close() 296 | # visualize(self.model) 297 | # torch.save(self.model.state_dict(), f'Trained_model/NCBF/NCBF_Obs{self.run}.pt'.format(self.run)) 298 | 299 | ObsAvoid = ObsAvoid() 300 | 301 | newCBF = NCBF_Synth([32, 32], [True, True], ObsAvoid, verbose=True) 302 | # newCBF.model.load_state_dict(torch.load('WarmModel2.pt')) 303 | # newCBF.train(num_epoch=20, num_restart=2, warm_start=True) 304 | # layers_to_freeze = ['0.weight', '0.bias'] 305 | # for name, param in newCBF.model.named_parameters(): 306 | # # Check if the current parameter belongs to a layer you want to freeze 307 | # if any(layer_name in name for layer_name in layers_to_freeze): 308 | # param.requires_grad = False 309 | # else: 310 | # param.requires_grad = True 311 | newCBF.train(num_epoch=10, num_restart=2, warm_start=False) 312 | newCBF.train(num_epoch=10, num_restart=2, warm_start=False) 313 | newCBF.train(num_epoch=10, num_restart=2, warm_start=False) 314 | newCBF.train(num_epoch=10, num_restart=2, warm_start=False) 315 | newCBF.train(num_epoch=10, num_restart=2, warm_start=False) 316 | newCBF.train(num_epoch=10, num_restart=2, warm_start=False) 317 | # # newCBF.run += 1 318 | # newCBF.train(num_epoch=10, num_restart=8, warm_start=False) 319 | # # newCBF.model.load_state_dict(torch.load('Trained_model/NCBF/NCBF_Obs4.pt')) 320 | # 321 | # # There is a bug in verifier that causes memory error due to too many intersections to verify 322 | # veri_result, num = newCBF.veri.proceed_verification() 323 | -------------------------------------------------------------------------------- /NCBFSynth/NCBF_Synth_AutoTune.py: -------------------------------------------------------------------------------- 1 | import sys, os 2 | sys.path.append(os.path.realpath(os.path.dirname(__file__)+"/..")) 3 | import numpy as np 4 | import torch 5 | from tqdm import tqdm 6 | from scipy.optimize import minimize 7 | # from progress.bar import Bar 8 | from Modules.NCBF import * 9 | import pandas as pd 10 | from torch import optim 11 | from torch.optim.lr_scheduler import ExponentialLR 12 | from torch.utils.tensorboard import SummaryWriter 13 | import torchvision 14 | import torchvision.transforms as transforms 15 | from torch.utils.data import DataLoader 16 | from Cases.ObsAvoid import ObsAvoid 17 | # from Verifier.Verifier import Verifier 18 | # from Critic_Synth.NCritic import * 19 | import time 20 | from Visualization.visualization import visualize 21 | # from collections import OrderedDict 22 | 23 | class NCBF_Synth(NCBF): 24 | ''' 25 | Synthesize an NCBF for a given safe region h(x) 26 | for given a system with polynomial f(x) and g(x) 27 | ''' 28 | def __init__(self,arch, act_layer, case, 29 | learning_rate=None, batch_size=None, verbose=False): 30 | ''' 31 | Input architecture and ReLU layers, input case, verbose for display 32 | :param arch: [list of int] architecture of the NN 33 | :param act_layer: [list of bool] if the corresponding layer with ReLU, then True 34 | :param case: Pre-defined case class, with f(x), g(x) and h(x) 35 | :param verbose: Flag for display or not 36 | ''' 37 | self.case = case 38 | if learning_rate is None: 39 | self.lr = 1e-3 40 | else: 41 | self.lr = learning_rate 42 | if batch_size is None: 43 | self.bs = 128 44 | else: 45 | self.bs = batch_size 46 | DOMAIN = self.case.DOMAIN 47 | super().__init__(arch, act_layer, DOMAIN) 48 | lctime = time.strftime("%Y%m%d%H%M%S") 49 | # Tensorboard 50 | self.writer = SummaryWriter(f'./runs/NCBF/{lctime}'.format(lctime)) 51 | self.run = 0 52 | self.volume_out = 0 53 | self.closs_out = 0 54 | self.floss_out = 0 55 | 56 | def get_grad(self, x): 57 | grad_input = torch.tensor(x, requires_grad=True, dtype=torch.float) 58 | return torch.autograd.grad(self.model.forward(grad_input), grad_input) 59 | 60 | def feasible_con(self, u, dbdxfx, dbdxgx): 61 | # function to maximize: (db/dx)*fx + (db/dx)*gx*u 62 | return dbdxfx + dbdxgx * u 63 | 64 | def feasible_u(self, dbdxfx, dbdxgx, min_flag=False): 65 | # find u that minimize (db/dx)*fx + (db/dx)*gx*u 66 | if min_flag: 67 | df = dbdxfx.detach().numpy() 68 | dg = dbdxgx.detach().numpy() 69 | res_list = [] 70 | for i in range(len(df)): 71 | res = minimize(self.feasible_con, x0=np.zeros(1), args=(df[i], dg[i])) 72 | # pos_res = np.max([res.fun, np.zeros(len([res.fun]))]) 73 | res_list.append(res.x) 74 | return torch.Tensor(res_list).squeeze() 75 | else: 76 | # Quick check for scalar u only 77 | [[u_lb, u_ub]] = torch.Tensor(self.case.CTRLDOM) 78 | res_list = [] 79 | for i in range(len(dbdxfx)): 80 | res_ulb = dbdxfx[i] + dbdxgx[i] * u_lb 81 | res_uub = dbdxfx[i] + dbdxgx[i] * u_ub 82 | # res = torch.max(res_ulb, res_uub) 83 | res = [u_lb, u_ub][np.argmax([res_ulb, res_uub])] 84 | res_list.append(res) 85 | return torch.Tensor(res_list).squeeze() 86 | 87 | def feasibility_loss(self, grad_vector, X_batch): 88 | # compute loss based on (db/dx)*fx + (db/dx)*gx*u 89 | dbdxfx = (grad_vector.unsqueeze(-1).transpose(-1, -2) 90 | @ self.case.f_x(X_batch).transpose(0, 1).unsqueeze(2)).squeeze() 91 | dbdxgx = (grad_vector.unsqueeze(-1).transpose(-1, -2) 92 | @ self.case.g_x(X_batch).transpose(0, 1).unsqueeze(2)).squeeze() 93 | u = self.feasible_u(dbdxfx, dbdxgx) 94 | # u = -X_batch[:,-1] 95 | feasibility_output = dbdxfx + dbdxgx * u 96 | # feasibility_output = dbdxfx 97 | return feasibility_output 98 | 99 | def feasible_violations(self, model_output, feasibility_output, batch_length, rlambda): 100 | # violations = -1 * feasibility_output - rlambda * torch.abs(model_output.transpose(0, 1)) 101 | violations = -1 * feasibility_output - rlambda * model_output.squeeze() 102 | # return torch.max(violations, torch.zeros([1,batch_length])) 103 | return violations 104 | 105 | def safe_correctness(self, ref_output, model_output, 106 | l_co: float = 1, alpha1: float = 1, 107 | alpha2: float = 0.001) -> torch.Tensor: 108 | ''' 109 | Penalize the incorrectness based on the h(x). 110 | If h(x) < 0 meaning the state x is unsafe, b(x) has to be negative. 111 | Therefore, alpha1, the gain of the penalty, can be large. 112 | If h(x) > 0 meaning the state is temporarily safe, b(x) can be +/-. 113 | To maximize coverage of b(x), a small penalty alpha2 is applied. 114 | :param ref_output: output of h(x) 115 | :param model_output: output of NN(x) 116 | :param l_co: gain of the loss 117 | :param alpha1: penalty for unsafe incorrectness 118 | :param alpha2: penalty for coverage 119 | :return: safety oriented correctness loss 120 | ''' 121 | norm_model_output = torch.sigmoid(model_output) 122 | # length = len(-ref_output + norm_model_output) 123 | # FalsePositive_loss = torch.relu(-ref_output)/(-ref_output) * torch.relu((model_output)) 124 | # FalseNegative_loss = torch.relu(ref_output)/ref_output * torch.relu((-model_output)) 125 | 126 | # loss = nn.BCELoss() 127 | # FalsePositive_loss = loss(torch.sigmoid(model_output), torch.sign(-ref_output)) 128 | # FalseNegative_loss = loss(torch.sigmoid(-model_output), torch.sign(ref_output)) 129 | FalsePositive_loss = torch.relu(-ref_output) * torch.relu((model_output)) 130 | FalseNegative_loss = torch.relu(ref_output) * torch.relu((-model_output)) 131 | # loss = l_co * torch.sum(alpha1*FalsePositive_loss + alpha2*FalseNegative_loss) 132 | # return loss 133 | return torch.sum(FalsePositive_loss), torch.sum(FalseNegative_loss) 134 | 135 | def trivial_panelty(self, ref_output, model_output, coeff=1, epsilon=0.0): 136 | min_ref = torch.max(ref_output) 137 | max_ref = torch.min(ref_output) 138 | # if max_ref >= 1e-4 and min_ref <= -1e-4: 139 | # non_pos_loss = coeff * torch.max(0.5 - torch.max(model_output), torch.zeros(1)) 140 | # non_neg_loss = coeff * torch.max(0.5 - torch.max(-model_output), torch.zeros(1)) 141 | if max_ref >= 0 and min_ref >= 0: 142 | non_pos_loss = torch.zeros(1) 143 | non_neg_loss = torch.zeros(1) 144 | elif max_ref <= 0 and min_ref <= 0: 145 | non_pos_loss = torch.zeros(1) 146 | non_neg_loss = torch.zeros(1) 147 | else: 148 | non_pos_loss = coeff * torch.relu(epsilon - torch.max(model_output)) 149 | non_neg_loss = coeff * torch.relu(-epsilon - torch.max(-model_output)) 150 | loss = non_pos_loss + non_neg_loss 151 | return loss 152 | 153 | def compute_volume(self, rdm_input, model_output=None): 154 | ''' 155 | Compute volume covered by b(x) 156 | :param rdm_input: random uniform samples 157 | :return: numbers of samples (volume) 158 | ''' 159 | # compute the positive volume contained by the NCBF 160 | if model_output is None: 161 | model_output = self.forward(rdm_input).squeeze() 162 | pos_output = torch.max(model_output, torch.zeros(len(rdm_input))) 163 | return torch.sum(pos_output > 0)/len(rdm_input) 164 | 165 | def eval_score(self): 166 | return self.volume_out, self.closs_out, self.floss_out 167 | def train(self, num_epoch, num_restart=10, warm_start=False): 168 | if warm_start: 169 | learning_rate = 1e-2 170 | else: 171 | learning_rate = self.lr 172 | optimizer = optim.Adam(self.model.parameters(), lr=learning_rate, weight_decay=0.01) 173 | scheduler = ExponentialLR(optimizer, gamma=0.99) 174 | # define hyper-parameters 175 | alphaf, alpha1, alpha2 = 1, 1, 0.001 176 | # 1, 1e-8 177 | # Set alpha2=0 for feasibility test with Floss quickly converge to 0 178 | # If set alpha2 converges but does not pass the verification, then increase the sampling number. 179 | # This problem is caused by lack of counter examples and can be solved by introducing CE from Verifier 180 | rlambda = 1 181 | 182 | # Generate data 183 | size = self.bs 184 | volume = torch.Tensor([0]) 185 | for self.run in range(num_restart): 186 | rdm_input = self.generate_data(size) 187 | # rdm_input = self.generate_input(shape) 188 | # ref_output = torch.unsqueeze(self.h_x(rdm_input.transpose(0, self.DIM)), self.DIM) 189 | ref_output = self.case.h_x(rdm_input).unsqueeze(1) 190 | normalized_ref_output = torch.tanh(10 * ref_output) 191 | # batch_length = 8**self.DIM 192 | # batch_length = size ** (self.DIM-1) 193 | batch_length = self.bs 194 | training_loader = DataLoader(list(zip(rdm_input, normalized_ref_output)), batch_size=batch_length, 195 | shuffle=True) 196 | 197 | pbar = tqdm(total=num_epoch) 198 | veri_result = False 199 | if not warm_start: 200 | pass 201 | for epoch in range(num_epoch): 202 | # Initialize loss 203 | running_loss = 0.0 204 | feasibility_running_loss = torch.Tensor([0.0]) 205 | correctness_running_loss = torch.Tensor([0.0]) 206 | trivial_running_loss = torch.Tensor([0.0]) 207 | 208 | # Batch Training 209 | for X_batch, y_batch in training_loader: 210 | model_output = self.forward(X_batch) 211 | 212 | # warm_start_loss = self.warm_start(y_batch, model_output) 213 | correctness_loss, coverage_loss = self.safe_correctness(y_batch, model_output, l_co=1, alpha1=alpha1, alpha2=alpha2) 214 | # trivial_loss = self.trivial_panelty(ref_output, self.model.forward(rdm_input), 1) 215 | trivial_loss = self.trivial_panelty(y_batch, model_output, 1) 216 | 217 | # grad = self.numerical_gradient(X_batch, model_output, batch_length, epsilon=0.001) 218 | # grad_vector = torch.vstack(grad) 219 | grad_vector = torch.vstack([self.get_grad(x)[0] for x in X_batch]) 220 | # dbdx(fx + gx*u) should be >=0. If <0, a penalty will be added. 221 | # feasibility_output = (torch.relu(model_output)*self.feasibility_loss(grad_vector, X_batch)) 222 | feasibility_output = batch_length * (torch.relu(model_output.squeeze()) * torch.relu(-model_output.squeeze()+1e-2) * 223 | (self.feasibility_loss(grad_vector, X_batch) + model_output.squeeze())) 224 | ce_indicator = (torch.relu(model_output.squeeze())/model_output.squeeze() * 225 | torch.relu(-model_output.squeeze()+1e-2)/(-model_output.squeeze()+1e-2) * 226 | torch.relu(-self.feasibility_loss(grad_vector, X_batch) - model_output.squeeze()) 227 | /((-self.feasibility_loss(grad_vector, X_batch) - model_output.squeeze()))) 228 | # check_item = torch.max((-torch.abs(model_output)+0.2).reshape([1, len(model_output)]), torch.zeros([1, len(model_output)])) 229 | # feasibility_loss = torch.sum(torch.tanh(check_item*feasibility_output)) 230 | 231 | # Our loss function 232 | # violations = -check_item * self.feasible_violations(model_output, feasibility_output, batch_length, rlambda) 233 | feasibility_loss = torch.relu(-feasibility_output).sum() 234 | # Chuchu Fan loss function 235 | # violations = check_item * self.feasible_violations(model_output, feasibility_output, batch_length, rlambda) 236 | # violations = -1 * feasibility_output - torch.max(rlambda * torch.abs(model_output.transpose(0, 1)), 237 | # torch.zeros([1, batch_length])) 238 | # feasibility_loss = 2 * torch.sum(torch.max(violations - 1e-4, torch.zeros([1, len(model_output)]))) 239 | if feasibility_loss <= 0.001: 240 | pass 241 | mseloss = torch.nn.MSELoss() 242 | # loss = self.def_loss(1 * correctness_loss + 1 * feasibility_loss + 1 * trivial_loss) 243 | # floss = mseloss(torch.max(violations - 1e-4, torch.zeros([1, batch_length])), torch.zeros(batch_length)) 244 | # tloss = mseloss(trivial_loss, torch.Tensor([0.0])) 245 | if warm_start: 246 | correctness_loss, coverage_loss = self.safe_correctness(y_batch, model_output, l_co=1, alpha1=1, alpha2=0.0001) 247 | loss = correctness_loss + coverage_loss + trivial_loss 248 | else: 249 | # loss = feasibility_loss 250 | loss = (alpha1*correctness_loss + alpha2*coverage_loss 251 | + alphaf*feasibility_loss + trivial_loss) 252 | 253 | 254 | loss.backward() 255 | # with torch.no_grad(): 256 | # loss = torch.max(loss) 257 | optimizer.step() 258 | optimizer.zero_grad() 259 | alpha1 += 0.001 * correctness_loss.item() 260 | alphaf += 100 * feasibility_loss.item() 261 | 262 | # Print Detailed Loss 263 | running_loss += loss.item() 264 | feasibility_running_loss += feasibility_loss.item() 265 | correctness_running_loss += correctness_loss.item() 266 | trivial_running_loss += trivial_loss.item() 267 | 268 | # if feasibility_running_loss <= 0.001 and correctness_loss <= 0.01: 269 | # alpha2 = 0.01 270 | # else: 271 | # alpha2 = 0 272 | # Log details of losses 273 | # if not warm_start: 274 | volume = self.compute_volume(rdm_input) 275 | self.writer.add_scalar('Loss/Loss', running_loss, self.run*num_epoch+epoch) 276 | self.writer.add_scalar('Loss/FLoss', feasibility_running_loss.item(), self.run*num_epoch+epoch) 277 | self.writer.add_scalar('Loss/CLoss', correctness_running_loss.item(), self.run*num_epoch+epoch) 278 | self.writer.add_scalar('Loss/TLoss', trivial_running_loss.item(), self.run*num_epoch+epoch) 279 | pbar.set_postfix({'Loss': running_loss, 280 | 'Floss': feasibility_running_loss.item(), 281 | 'Closs': correctness_running_loss.item(), 282 | 'Tloss': trivial_running_loss.item(), 283 | 'WarmUp': str(warm_start), 284 | 'FCEnum': ce_indicator.sum().item(), 285 | 'Vol': volume.item()}) 286 | pbar.update(1) 287 | scheduler.step() 288 | # Log volume of safe region 289 | 290 | self.volume_out = volume 291 | self.closs_out = correctness_running_loss.item() 292 | self.floss_out = feasibility_running_loss.item() 293 | self.writer.add_scalar('Volume', volume, self.run*num_epoch+epoch) 294 | # self.writer.add_scalar('Verifiable', veri_result, self.run * num_epoch + epoch) 295 | # Process Bar Print Losses 296 | 297 | 298 | 299 | 300 | pbar.close() 301 | # visualize(self.model) 302 | # torch.save(self.model.state_dict(), 303 | # f'Trained_model/NCBF/Obs_epch{epch}_epsd{epsd}_lrate{lrate}_batsize{batsize}'.format(epch,epsd,lrate,batsize)) 304 | 305 | ObsAvoid = ObsAvoid() 306 | 307 | param_grid = { 308 | 'epochs': [5], 309 | 'episodes': [20], 310 | 'lr': [1e-7], 311 | 'bs': [32] 312 | } 313 | 314 | tune_res = pd.DataFrame(columns=['epochs', 'episodes', 'lr', 'bs', 'v', 'cl', 'fl']) 315 | for epch in param_grid['epochs']: 316 | for epsd in param_grid['episodes']: 317 | for lrate in param_grid['lr']: 318 | for batsize in param_grid['bs']: 319 | # print(epch, epsd, lrate, batsize) 320 | newCBF = NCBF_Synth([32, 32], [True, True], 321 | ObsAvoid, 322 | learning_rate=lrate, 323 | batch_size=batsize, 324 | verbose=True) 325 | newCBF.model.load_state_dict(torch.load('WarmModel2.pt')) 326 | newCBF.train(num_epoch=epsd, num_restart=epch, warm_start=False) 327 | torch.save(newCBF.model.state_dict(), 328 | f'Trained_model/NCBF/Obs_epch{epch}_epsd{epsd}_lrate{lrate}_batsize{batsize}.pt'.format(epch,epsd,lrate,batsize)) 329 | v, cl, fl = newCBF.eval_score() 330 | tune_res = tune_res.append( 331 | {'epochs': epch, 'episodes': epsd, 'lr': lrate, 'bs': batsize, 'v': v, 'cl': cl, 'fl': fl}, 332 | ignore_index=True) 333 | tune_res.to_csv('ObsTuneRes.csv') 334 | # newCBF = NCBF_Synth([32, 32], [True, True], ObsAvoid, verbose=True) 335 | # newCBF.model.load_state_dict(torch.load('WarmModel2.pt')) 336 | 337 | # newCBF.train(num_epoch=10, num_restart=2, warm_start=False) 338 | # newCBF.train(num_epoch=10, num_restart=2, warm_start=False) 339 | # newCBF.train(num_epoch=10, num_restart=2, warm_start=False) 340 | # newCBF.train(num_epoch=10, num_restart=2, warm_start=False) 341 | # newCBF.train(num_epoch=10, num_restart=2, warm_start=False) 342 | # newCBF.train(num_epoch=10, num_restart=2, warm_start=False) 343 | # # newCBF.run += 1 344 | # newCBF.train(num_epoch=10, num_restart=8, warm_start=False) 345 | # # newCBF.model.load_state_dict(torch.load('Trained_model/NCBF/NCBF_Obs4.pt')) 346 | # 347 | # # There is a bug in verifier that causes memory error due to too many intersections to verify 348 | # veri_result, num = newCBF.veri.proceed_verification() 349 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | 2 |
3 | 4 | 5 |

Fault Tolerant Neural Control Barrier Functions

6 | 7 |

8 | Fault Tolerant Neural Control Barrier Functions for Robotic Systems under Sensor Faults and Attacks (ICRA 2024) 9 |
10 | Full Paper » 11 |
12 |

13 |
14 |

15 | Python 16 | PyTorch 17 |

18 | 19 | 20 | 21 | Table of Contents 22 |
    23 | 26 |
  1. Experiments
    2. 27 |
  2. 28 | Getting Started 29 | 33 |
    3. 34 |
  3. Citation
    4. 35 | 38 |
  4. License
    5. 39 |
  5. Contact
    6. 40 |
  6. Acknowledgments
    7. 41 |
42 | 43 | 44 |

45 | 46 | gif 47 | 48 |
49 |

50 | 51 | 52 | ## Experiments 53 | 54 | **Obstacle Avoidance:** We evaluate our proposed method on a controlled system [[1]](barry2). We consider an Unmanned Aerial Vehicle (UAV) to avoid collision with a tree trunk. We model the system as a Dubins-style [[2]](dubins1957curves) aircraft model. The system state consists of a 2D position and aircraft yaw rate $x:=[x_1, x_2, \psi]^T$. We let $u$ denote the control input to manipulate the yaw rate and the dynamics defined in the supplement. 55 | We train the NCBF via the method proposed in [[3]](zhao2020synthesizing) with $v$ assumed to be $1$ and the control law $u$ designed as 56 | $u=\mu_{nom}(x)=-\sin \psi+3 \cdot \frac{x_1 \cdot \sin \psi+x_2 \cdot \cos \psi}{0.5+x_1^2+x_2^2}$. 57 | 58 | **Spacecraft Rendezvous:** We evaluate our approach to a spacecraft rendezvous problem from [[5]](jewison2016spacecraft). A station-keeping controller is required to keep the "chaser" satellite within a certain relative distance from the "target" satellite. The state of the chaser is expressed relative to the target using linearized Clohessy–Wiltshire–Hill equations, with state $x=[p_x, p_y, p_z, v_x, v_y, v_z]^T$, control input $u=[u_x, u_y, u_z]^T$ and dynamics defined in the supplement. We train the NCBF as in [[6]](dawson2023safe). 59 | 60 |

(back to top)

61 | 62 | 63 | ## Getting Started 64 | 65 | This is an example of how you may give instructions on setting up your project locally. 66 | To get a local copy up and running follow these simple example steps. 67 | 68 | ### Installation 69 | 70 | Clone the repo and navigate to the folder 71 | ```sh 72 | git clone https://github.com/HongchaoZhang-HZ/FTNCBF.git 73 | 74 | cd FTNCBF 75 | ``` 76 | 77 | Install packages via pip 78 | ```sh 79 | pip install -r requirements.txt 80 | ``` 81 | 82 | ### Run the code 83 | 84 | Choose the system and corresponding NCBFs, e.g., train NCBF for vehicle obstacle avoidance, to train by running the code 85 | ```sh 86 | python main_Obs.py 87 | ``` 88 | 89 | ### Run Obstacle Avoidance in Carla 90 | Copy code and the trained NCBF to Carla folder `PythonAPI/examples`, and run 91 | ```sh 92 | python main.py 93 | ``` 94 | 95 |

(back to top)

96 | 97 | 98 | ## Citation 99 | If our work is useful for your research, please consider citing: 100 | 101 | 102 | ``` 103 | @INPROCEEDINGS{zhang2024fault, 104 | author={Zhang, Hongchao and Niu, Luyao and Clark, Andrew and Poovendran, Radha}, 105 | booktitle={2024 IEEE International Conference on Robotics and Automation (ICRA)}, 106 | title={Fault Tolerant Neural Control Barrier Functions for Robotic Systems under Sensor Faults and Attacks}, 107 | year={2024}, 108 | volume={}, 109 | number={}} 110 | ``` 111 | 112 |

(back to top)

113 | 114 | 115 | ## License 116 | 117 | Distributed under the MIT License. See `LICENSE.txt` for more information. 118 | 119 |

(back to top)

120 | 121 | 122 | 123 | 124 | ## Contact 125 | 126 | If you have any questions, please feel free to reach out to us. 127 | 128 | Hongchao Zhang - [Homepage](https://hongchaozhang-hz.github.io/) - hongchao@wustl.edu 129 | 130 | 131 | 132 |

(back to top)

133 | 134 | 135 | 136 | 137 | ## Acknowledgments 138 | This research was supported by the AFOSR (grants FA9550-22-1-0054 and FA9550-23-1-0208), and NSF (grants CNS-1941670). 139 | 140 |

(back to top)

141 | 142 | [zeng2016darboux]: https://dl.acm.org/doi/abs/10.1145/2968478.2968491 143 | [barry2012safety]: https://ieeexplore.ieee.org/abstract/document/6224645 144 | [dubiins1957curves]: https://www.jstor.org/stable/2372537?seq=1 145 | [jewison2016spacecraft]: https://ieeexplore.ieee.org/abstract/document/7798763 146 | [dawson2023safe]: https://ieeexplore.ieee.org/abstract/document/9531393 147 | [abate2021fossil]: https://link.springer.com/chapter/10.1007/978-3-030-81608-0_4 148 | [zhao2020synthesizing]: https://dl.acm.org/doi/abs/10.1145/3365365.3382222 149 | -------------------------------------------------------------------------------- /SNBF_Synth.py: -------------------------------------------------------------------------------- 1 | import torch 2 | from tqdm import tqdm 3 | from Modules.NCBF import * 4 | from torch import optim 5 | from filterpy.kalman import ExtendedKalmanFilter 6 | from torch.optim.lr_scheduler import ExponentialLR 7 | from torch.utils.tensorboard import SummaryWriter 8 | import torchvision.transforms as transforms 9 | from torch.utils.data import DataLoader 10 | from Cases.Darboux import Darboux 11 | # import cma 12 | # from cmaes import CMA 13 | # from Verifier import Verifier 14 | from collections import OrderedDict 15 | from Critic_Synth.NCritic import * 16 | 17 | class NCBF_Synth(NCBF): 18 | def __init__(self,arch, act_layer, case, verbose=False): 19 | self.case = case 20 | DOMAIN = self.case.DOMAIN 21 | super().__init__(arch, act_layer, DOMAIN) 22 | self.critic = NeuralCritic(case) 23 | # self.veri = Verifier(NCBF=self, case=case, grid_shape=[100, 100], verbose=verbose) 24 | 25 | def numerical_gradient(self, X_batch, model_output, batch_length, epsilon=0.001): 26 | grad = [] 27 | for i in range(self.DIM): 28 | gradStep = torch.zeros(self.DIM) 29 | gradStep[i] += epsilon 30 | gradData = X_batch + gradStep 31 | dbdxi = ((self.forward(gradData) - model_output) / epsilon).reshape([batch_length]) 32 | grad.append(dbdxi) 33 | return grad 34 | 35 | def numerical_b_gamma(self, grad, gamma): 36 | # todo: debug 37 | return np.max(np.abs(grad)) * gamma 38 | 39 | def EKF(self): 40 | # todo: extended kalman filter gain for different sensor failure 41 | K = torch.ones([self.DIM, self.DIM]) 42 | return K 43 | 44 | def feasibility_loss(self, grad_vector, X_batch, model_output, batch_length, gamma=0.1): 45 | # todo: debug 46 | # todo: tuning 47 | rlambda = 1 48 | c = 1 49 | feasibility_output = (grad_vector.transpose(0, 1).unsqueeze(1) 50 | @ self.case.f_x(X_batch).transpose(0, 1).unsqueeze(2)).squeeze() 51 | check_item = torch.max((-torch.abs(model_output) + 0.1).reshape([1, batch_length]), 52 | torch.zeros([1, batch_length])) 53 | stochastic_term = -gamma * torch.linalg.norm(grad_vector @ self.EKF() * c) 54 | 55 | # Our loss function 56 | # violations = -check_item * feasibility_output 57 | # Chuchu Fan loss function 58 | delta_gamma = self.numerical_b_gamma(grad_vector, gamma) 59 | violations = -1 * feasibility_output - stochastic_term\ 60 | - torch.max(rlambda * torch.abs((model_output-delta_gamma).transpose(0, 1)), 61 | torch.zeros([1, batch_length])) 62 | feasibility_loss = 100 * torch.sum(torch.max(violations - 1e-4, torch.zeros([1, batch_length]))) 63 | return feasibility_loss 64 | 65 | def safe_correctness(self, ref_output, model_output, l_co=1, alpha1=1, alpha2=0.001): 66 | norm_model_output = torch.tanh(model_output) 67 | length = len(-ref_output + norm_model_output) 68 | # norm_ref_output = torch.tanh(ref_output) 69 | FalsePositive_loss = torch.max(-ref_output.reshape([1, length]), torch.zeros([1, length])) * \ 70 | torch.max((model_output + 0.01).reshape([1, length]), torch.zeros([1, length])) 71 | FalseNegative_loss = torch.max(ref_output.reshape([1, length]), torch.zeros([1, length])) * \ 72 | torch.max((-model_output + 0.01).reshape([1, length]), torch.zeros([1, length])) 73 | loss = l_co * torch.sum(alpha1*FalsePositive_loss + alpha2*FalseNegative_loss) 74 | return loss 75 | 76 | def trivial_panelty(self, ref_output, model_output, coeff=1, epsilon=0.1): 77 | min_ref = torch.max(ref_output) 78 | max_ref = torch.min(ref_output) 79 | # if max_ref >= 1e-4 and min_ref <= -1e-4: 80 | # non_pos_loss = coeff * torch.max(0.5 - torch.max(model_output), torch.zeros(1)) 81 | # non_neg_loss = coeff * torch.max(0.5 - torch.max(-model_output), torch.zeros(1)) 82 | if max_ref >= 1e-4 and min_ref >= 1e-4: 83 | non_pos_loss = torch.zeros(1) 84 | non_neg_loss = torch.zeros(1) 85 | elif max_ref <= -1e-4 and min_ref <= -1e-4: 86 | non_pos_loss = torch.zeros(1) 87 | non_neg_loss = torch.zeros(1) 88 | else: 89 | non_pos_loss = coeff * torch.max(epsilon - torch.max(model_output), torch.zeros(1)) 90 | non_neg_loss = coeff * torch.max(epsilon - torch.max(-model_output), torch.zeros(1)) 91 | loss = non_pos_loss + non_neg_loss 92 | return loss 93 | 94 | def train(self, num_epoch): 95 | optimizer = optim.SGD(self.model.parameters(), lr=1e-4) 96 | scheduler = ExponentialLR(optimizer, gamma=0.9) 97 | # Generate data 98 | size = 100 99 | shape = [] 100 | for _ in range(self.DIM): 101 | shape.append(size) 102 | 103 | rdm_input = self.generate_data(size) 104 | # rdm_input = self.generate_input(shape) 105 | # ref_output = torch.unsqueeze(self.h_x(rdm_input.transpose(0, self.DIM)), self.DIM) 106 | ref_output = self.h_x(rdm_input.transpose(0, 1)).unsqueeze(1) 107 | batch_length = 16 108 | training_loader = DataLoader(list(zip(rdm_input, ref_output)), batch_size=batch_length, shuffle=True) 109 | pbar = tqdm(total=num_epoch) 110 | veri_result = False 111 | for epoch in range(num_epoch): 112 | running_loss = 0.0 113 | feasibility_running_loss = 0.0 114 | correctness_running_loss = 0.0 115 | trivial_running_loss = 0.0 116 | 117 | for X_batch, y_batch in training_loader: 118 | 119 | optimizer.zero_grad() 120 | model_output = self.forward(X_batch) 121 | 122 | warm_start_loss = self.warm_start(y_batch, model_output) 123 | correctness_loss = self.safe_correctness(y_batch, model_output, l_co=1, alpha1=1, alpha2=0) 124 | trivial_loss = self.trivial_panelty(ref_output, self.model.forward(rdm_input), 1) 125 | grad = self.numerical_gradient(X_batch, model_output, batch_length, epsilon=0.001) 126 | grad_vector = torch.vstack(grad) 127 | feasibility_loss = self.feasibility_loss(grad_vector, X_batch, model_output, batch_length) 128 | loss = self.def_loss(1*correctness_loss + 1*feasibility_loss + 1*trivial_loss) 129 | 130 | loss.backward() 131 | optimizer.step() 132 | 133 | running_loss += loss.item() 134 | feasibility_running_loss += feasibility_loss.item() 135 | correctness_running_loss += correctness_loss.item() 136 | trivial_running_loss += trivial_loss.item() 137 | # if epoch % 50 == 49: 138 | # print('[%d] loss: %.3f' % (epoch + 1, running_loss / 2000)) 139 | # Print Detailed Loss 140 | running_loss += loss.item() 141 | feasibility_running_loss += feasibility_loss.item() 142 | correctness_running_loss += correctness_loss.item() 143 | trivial_running_loss += trivial_loss.item() 144 | # Process Bar Print Losses 145 | pbar.set_postfix({'Loss': running_loss, 146 | 'Floss': feasibility_running_loss, 147 | 'Closs': correctness_running_loss, 148 | 'Tloss': trivial_running_loss, 149 | 'PVeri': str(veri_result)}) 150 | pbar.update(1) 151 | 152 | if epoch % 50 == 49: 153 | scheduler.step() 154 | if epoch % 100 == 99: 155 | veri_result, num = self.veri.proceed_verification() 156 | visualize(self.model) 157 | # print(veri_result) 158 | 159 | 160 | 161 | # Define Case 162 | # x0, x1 = sp.symbols('x0, x1') 163 | 164 | Darboux = Darboux() 165 | newCBF = NCBF_Synth([10, 10], [True, True], Darboux, verbose=False) 166 | newCBF.veri.proceed_verification() 167 | for restart in range(3): 168 | newCBF.train(1000) 169 | 170 | visualize(newCBF) -------------------------------------------------------------------------------- /SNCBF_Synth.py: -------------------------------------------------------------------------------- 1 | import numpy as np 2 | import torch 3 | from NCBFSynth.NCBF_Synth import NCBF_Synth 4 | from tqdm import tqdm 5 | # from progress.bar import Bar 6 | from Modules.NCBF import * 7 | from torch import optim 8 | from torch.optim.lr_scheduler import ExponentialLR 9 | from torch.utils.tensorboard import SummaryWriter 10 | from torch.autograd.functional import hessian 11 | import torchvision 12 | import torchvision.transforms as transforms 13 | from torch.utils.data import DataLoader 14 | from Cases.ObsAvoid import ObsAvoid 15 | # from Verifier.SVerifier import Stochastic_Verifier 16 | # from Critic_Synth.NCritic import * 17 | import time 18 | from FTEst.EKF import * 19 | # from collections import OrderedDict 20 | 21 | class SNCBF_Synth(NCBF_Synth): 22 | ''' 23 | Synthesize an NCBF for a given safe region h(x) 24 | for given a system with polynomial f(x) and g(x) 25 | ''' 26 | def __init__(self, arch, act_layer, case, sigma, nu, verbose=False): 27 | ''' 28 | Input architecture and ReLU layers, input case, verbose for display 29 | :param arch: [list of int] architecture of the NN 30 | :param act_layer: [list of bool] if the corresponding layer with ReLU, then True 31 | :param case: Pre-defined case class, with f(x), g(x) and h(x) 32 | :param verbose: Flag for display or not 33 | ''' 34 | super().__init__(arch, act_layer, case, verbose=False) 35 | # Under construction: Critic is designed to tuning loss fcn automatically 36 | # self.critic = NeuralCritic(case) 37 | lctime = time.ctime(time.time()) 38 | # Tensorboard 39 | self.writer = SummaryWriter(f'./runs/SNCBF/{lctime}'.format(lctime)) 40 | 41 | # Initialize stochastic term related data 42 | self.gamma = 0.1 43 | self.sigma = torch.tensor(sigma, dtype=torch.float).unsqueeze(1) 44 | self.nu = torch.tensor(nu, dtype=torch.float).unsqueeze(1) 45 | self.delta_gamma = torch.zeros(1) 46 | self.c = torch.diag(torch.ones(self.DIM)) 47 | self.ekf_gain = torch.Tensor([[0.06415174, -0.01436932, -0.04649317], 48 | [-0.06717124, 0.02750288, 0.14107035], 49 | [-0.0201735, 0.00625575, -0.0836058]]) 50 | # [[0.06415174 -0.01436932 -0.04649317] 51 | # [-0.06717124 0.02750288 0.14107035] 52 | # [-0.0201735 0.00625575 -0.0836058]] 53 | self.run = 0 54 | # Verifier proposed to verify feasibility 55 | # self.veri = Stochastic_Verifier(NCBF=self, case=case, 56 | # EKFGain=self.ekf_gain, 57 | # grid_shape=[100, 100, 100], 58 | # verbose=verbose) 59 | 60 | @property 61 | def get_model(self): 62 | return self.model 63 | 64 | def get_grad(self, x): 65 | grad_input = torch.tensor(x, requires_grad=True, dtype=torch.float) 66 | return torch.autograd.grad(self.model.forward(grad_input), grad_input) 67 | 68 | def get_hessian(self, x): 69 | grad_input = torch.tensor(x, requires_grad=True, dtype=torch.float) 70 | hessian_matrix = hessian(self.model.forward, grad_input).squeeze() 71 | return hessian_matrix 72 | 73 | def update_EKF_gain(self, new_gain): 74 | self.ekf_gain = new_gain 75 | 76 | def update_obs_matrix_c(self, obs_matrix): 77 | self.c = obs_matrix 78 | 79 | def numerical_delta_gamma(self, grad, gamma): 80 | ''' 81 | We can numerically get b_gamma by decrease along b, i.e. delta gamma 82 | To compute the delta gamma, we get the largest gradient 83 | :param grad: [torch.Tensor] vector of numerical gradient 84 | :param gamma: [scalar] gamma of state x 85 | :return: delta gamma 86 | ''' 87 | vec_gamma = torch.amax(torch.abs(grad), 1) 88 | delta_gamma = torch.norm(vec_gamma) * gamma 89 | # return torch.max(delta_gamma, self.delta_gamma) 90 | return delta_gamma 91 | 92 | # def EKF(self): 93 | # landmarks = np.array([[5, 10, 0.5], [10, 5, 0.5], [15, 15, 0.5]]) 94 | # 95 | # ekf = run_localization( 96 | # landmarks, std_vel=0.1, std_steer=np.radians(1), 97 | # std_range=0.3, std_bearing=0.1) 98 | # print('Final P:', ekf.P.diagonal()) 99 | # return ekf.K 100 | 101 | def feasibility_loss(self, grad_vector, X_batch): 102 | # compute loss based on (db/dx)*fx + (db/dx)*gx*u 103 | # dbdxfx = (grad_vector.transpose(0, 1).unsqueeze(1) 104 | # @ self.case.f_x(X_batch).transpose(0, 1).unsqueeze(2)).squeeze() 105 | # dbdxgx = (grad_vector.transpose(0, 1).unsqueeze(1) 106 | # @ self.case.g_x(X_batch).transpose(0, 1).unsqueeze(2)).squeeze() 107 | dbdxfx = (grad_vector.unsqueeze(-1).transpose(-1,-2) 108 | @ self.case.f_x(X_batch).transpose(0, 1).unsqueeze(2)).squeeze() 109 | dbdxgx = (grad_vector.unsqueeze(-1).transpose(-1,-2) 110 | @ self.case.g_x(X_batch).transpose(0, 1).unsqueeze(2)).squeeze() 111 | u = self.feasible_u(dbdxfx, dbdxgx) 112 | # update delta_gamma 113 | self.delta_gamma = self.numerical_delta_gamma(grad_vector, self.gamma) 114 | # EKF_term = grad_vector.transpose(0,1) @ self.ekf_gain @ self.c 115 | EKF_term = grad_vector @ self.ekf_gain @ self.c 116 | stochastic_term = -self.gamma * EKF_term.norm(dim=1) 117 | 118 | # second order term 119 | hes_array = [] 120 | trace_term_list = [] 121 | for idx in range(len(X_batch)): 122 | hess = self.get_hessian(X_batch[idx]) 123 | second_order_term = self.nu.transpose(0, 1).numpy() @ self.ekf_gain.numpy().transpose() \ 124 | @ hess.numpy() @ self.ekf_gain.numpy() @ self.nu.numpy() 125 | trace_term = second_order_term.trace() 126 | trace_term_list.append(trace_term) 127 | trace_term_list = torch.tensor(np.array(trace_term_list)) 128 | 129 | feasibility_output = dbdxfx + dbdxgx * u + stochastic_term + trace_term_list 130 | return feasibility_output 131 | 132 | def feasible_violations(self, model_output, feasibility_output, batch_length, rlambda): 133 | # b_gamma = (model_output - self.delta_gamma) 134 | b_gamma = model_output 135 | violations = -1 * feasibility_output - rlambda * torch.abs(b_gamma.transpose(0, 1)) 136 | # return violations 137 | return torch.max(violations, torch.zeros([1, batch_length])) 138 | 139 | def train(self, num_epoch, num_restart=10, alpha1=None, alpha2=None, warm_start=False): 140 | optimizer = optim.Adam(self.model.parameters(), lr=1e-5) 141 | scheduler = ExponentialLR(optimizer, gamma=0.9) 142 | # define hyper-parameters 143 | if alpha1 is None and alpha2 is None: 144 | alpha1, alpha2 = 1, 0 145 | # 1, 1e-8 146 | # Set alpha2=0 for feasibility test with Floss quickly converge to 0 147 | # If set alpha2 converges but does not pass the verification, then increase the sampling number. 148 | # This problem is caused by lack of counter examples and can be solved by introducing CE from Verifier 149 | rlambda = 1 150 | 151 | # Generate data 152 | size = 128 153 | rdm_input = self.generate_data(size) 154 | # rdm_input = self.generate_input(shape) 155 | # ref_output = torch.unsqueeze(self.h_x(rdm_input.transpose(0, self.DIM)), self.DIM) 156 | ref_output = self.case.h_x(rdm_input).unsqueeze(1) 157 | normalized_ref_output = torch.tanh(10*ref_output) 158 | batch_length = 16**self.DIM 159 | training_loader = DataLoader(list(zip(rdm_input, normalized_ref_output)), batch_size=batch_length, shuffle=True) 160 | 161 | for self.run in range(num_restart): 162 | pbar = tqdm(total=num_epoch) 163 | veri_result = False 164 | for epoch in range(num_epoch): 165 | # Initialize loss 166 | running_loss = 0.0 167 | feasibility_running_loss = torch.Tensor([0.0]) 168 | correctness_running_loss = torch.Tensor([0.0]) 169 | trivial_running_loss = torch.Tensor([0.0]) 170 | 171 | # Batch Training 172 | for X_batch, y_batch in training_loader: 173 | model_output = self.forward(X_batch) 174 | 175 | warm_start_loss = self.warm_start(y_batch, model_output) 176 | correctness_loss = self.safe_correctness(y_batch, model_output, l_co=1, alpha1=alpha1, alpha2=alpha2) 177 | # trivial_loss = self.trivial_panelty(ref_output, self.model.forward(rdm_input), 1) 178 | trivial_loss = self.trivial_panelty(y_batch, model_output, 1) 179 | 180 | # grad = self.numerical_gradient(X_batch, model_output, batch_length, epsilon=0.001) 181 | # grad_vector = torch.vstack(grad) 182 | grad_vector = torch.vstack([self.get_grad(x)[0] for x in X_batch]) 183 | feasibility_output = self.feasibility_loss(grad_vector, X_batch) 184 | check_item = torch.max((-torch.abs(model_output)+0.2).reshape([1, batch_length]), torch.zeros([1, batch_length])) 185 | # feasibility_loss = torch.sum(torch.tanh(check_item*feasibility_output)) 186 | 187 | # Our loss function 188 | # violations = -check_item * self.feasible_violations(model_output, feasibility_output, batch_length, rlambda) 189 | # Chuchu Fan loss function 190 | violations = check_item * self.feasible_violations(model_output, feasibility_output, batch_length, rlambda) 191 | # violations = -1 * feasibility_output - torch.max(rlambda * torch.abs(model_output.transpose(0, 1)), 192 | # torch.zeros([1, batch_length])) 193 | feasibility_loss = 2 * torch.sum(torch.max(violations - 1e-4, torch.zeros([1, batch_length]))) 194 | mseloss = torch.nn.MSELoss() 195 | # loss = self.def_loss(1 * correctness_loss + 1 * feasibility_loss + 1 * trivial_loss) 196 | floss = mseloss(torch.max(violations - 1e-4, torch.zeros([1, batch_length])), torch.zeros(batch_length)) 197 | tloss = mseloss(trivial_loss, torch.Tensor([0.0])) 198 | if warm_start: 199 | loss = self.warm_start(y_batch, model_output) 200 | else: 201 | loss = correctness_loss + feasibility_loss + tloss 202 | 203 | 204 | loss.backward() 205 | # with torch.no_grad(): 206 | # loss = torch.max(loss) 207 | optimizer.step() 208 | optimizer.zero_grad() 209 | 210 | # Print Detailed Loss 211 | running_loss += loss.item() 212 | feasibility_running_loss += feasibility_loss.item() 213 | correctness_running_loss += correctness_loss.item() 214 | trivial_running_loss += trivial_loss.item() 215 | 216 | # if feasibility_running_loss <= 0.001 and correctness_loss <= 0.01: 217 | # alpha2 = 0.01 218 | # else: 219 | # alpha2 = 0 220 | 221 | # Log details of losses 222 | if not warm_start: 223 | self.writer.add_scalar('Loss/Loss', running_loss, self.run*num_epoch+epoch) 224 | self.writer.add_scalar('Loss/FLoss', feasibility_running_loss.item(), self.run*num_epoch+epoch) 225 | self.writer.add_scalar('Loss/CLoss', correctness_running_loss.item(), self.run*num_epoch+epoch) 226 | self.writer.add_scalar('Loss/TLoss', trivial_running_loss.item(), self.run*num_epoch+epoch) 227 | # Log volume of safe region 228 | volume = self.compute_volume(rdm_input) 229 | self.writer.add_scalar('Volume', volume, self.run*num_epoch+epoch) 230 | # self.writer.add_scalar('Verifiable', veri_result, self.run * num_epoch + epoch) 231 | # Process Bar Print Losses 232 | pbar.set_postfix({'Loss': running_loss, 233 | 'Floss': feasibility_running_loss.item(), 234 | 'Closs': correctness_running_loss.item(), 235 | 'Tloss': trivial_running_loss.item(), 236 | 'PVeri': str(veri_result), 237 | 'Vol': volume.item()}) 238 | pbar.update(1) 239 | scheduler.step() 240 | # if feasibility_running_loss <= 0.0001 and not warm_start: 241 | # try: 242 | # veri_result, num = self.veri.proceed_verification() 243 | # except: 244 | # pass 245 | 246 | 247 | pbar.close() 248 | # if feasibility_running_loss <= 0.0001 and not warm_start: 249 | # try: 250 | # veri_result, num = self.veri.proceed_verification() 251 | # except: 252 | # pass 253 | # if veri_result: 254 | # torch.save(self.model.state_dict(), f'Trained_model/NCBF/NCBF_Obs{epoch}.pt'.format(epoch)) 255 | torch.save(self.model.state_dict(), f'Trained_model/SNCBF/SNCBF_Obs{self.run}.pt'.format(self.run)) 256 | 257 | # ObsAvoid = ObsAvoid() 258 | # newCBF = SNCBF_Synth([32, 32], [True, True], ObsAvoid, 259 | # sigma=[0.1000, 0.1000, 0.1000], 260 | # nu=[0.1000, 0.1000, 0.1000], verbose=True) 261 | # # newCBF.train(50, warm_start=True) 262 | # # newCBF.run += 1 263 | # newCBF.train(num_epoch=10, num_restart=8, warm_start=False) 264 | # newCBF.model.load_state_dict(torch.load('Trained_model/SNCBF/SNCBFGood/SNCBF_Obs0.pt')) 265 | # 266 | # # There is a bug in verifier that causes memory error due to too many intersections to verify 267 | # veri_result, num = newCBF.veri.proceed_verification() 268 | -------------------------------------------------------------------------------- /Visualization/ObsVis.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/HongchaoZhang-HZ/FTNCBF/82239a3b0973f8d5f629e7230f9887895b315095/Visualization/ObsVis.py -------------------------------------------------------------------------------- /Visualization/Plot_6Dbound.py: -------------------------------------------------------------------------------- 1 | import matplotlib.pyplot as plt 2 | import pandas as pd 3 | import numpy as np 4 | from matplotlib.patches import Rectangle 5 | 6 | import torch 7 | import torch.nn as nn 8 | import numpy as np 9 | from mayavi import mlab 10 | from mayavi.mlab import * 11 | 12 | ############################################ 13 | # set default data type to double 14 | ############################################ 15 | torch.set_default_dtype(torch.float64) 16 | torch.set_default_tensor_type(torch.DoubleTensor) 17 | 18 | # code for plotting cube: from 19 | # https://stackoverflow.com/questions/26888098/draw-cubes-with-colour-intensity-with-python 20 | def mlab_plt_cube(xmin, xmax, ymin, ymax, zmin, zmax, c_color): 21 | def cube_faces(xmin, xmax, ymin, ymax, zmin, zmax): 22 | faces = [] 23 | 24 | x, y = np.mgrid[xmin:xmax:3j, ymin:ymax:3j] 25 | z = np.ones(y.shape) * zmin 26 | faces.append((x, y, z)) 27 | 28 | x, y = np.mgrid[xmin:xmax:3j, ymin:ymax:3j] 29 | z = np.ones(y.shape) * zmax 30 | faces.append((x, y, z)) 31 | 32 | x, z = np.mgrid[xmin:xmax:3j, zmin:zmax:3j] 33 | y = np.ones(z.shape) * ymin 34 | faces.append((x, y, z)) 35 | 36 | x, z = np.mgrid[xmin:xmax:3j, zmin:zmax:3j] 37 | y = np.ones(z.shape) * ymax 38 | faces.append((x, y, z)) 39 | 40 | y, z = np.mgrid[ymin:ymax:3j, zmin:zmax:3j] 41 | x = np.ones(z.shape) * xmin 42 | faces.append((x, y, z)) 43 | 44 | y, z = np.mgrid[ymin:ymax:3j, zmin:zmax:3j] 45 | x = np.ones(z.shape) * xmax 46 | faces.append((x, y, z)) 47 | 48 | return faces 49 | 50 | def trans_x(origin_x): 51 | k = (64 - 1) / (2 - (-2)) 52 | c = 1 - k * (-2) 53 | return k * origin_x + c 54 | 55 | def trans_y(origin_y): 56 | k = (64 - 1) / (2 - (-2)) 57 | c = 1 - k * (-2) 58 | return k * origin_y + c 59 | 60 | def trans_z(origin_z): 61 | k = (64 - 1) / (2 + 2) 62 | c = 1 - k * (-2) 63 | return k * origin_z + c 64 | 65 | faces = cube_faces(trans_x(xmin), trans_x(xmax), trans_y(ymin), trans_y(ymax), trans_z(zmin), trans_z(zmax)) 66 | for grid in faces: 67 | x, y, z = grid 68 | mlab.mesh(x, y, z, color=c_color, opacity=0.3) 69 | 70 | 71 | # plot sphere 72 | def mlab_plt_sphere(center_x, center_y, center_z, s_rad, s_color): 73 | x, y, z = np.ogrid[(-2): (2): complex(0, 64), \ 74 | (-2): (2): complex(0, 64), \ 75 | (-2): (2): complex(0, 64)] 76 | 77 | sphere_scalar = (x - center_x) * (x - center_x) + (y - center_y) * (y - center_y) + (z - center_z) * (z - center_z) 78 | sphere = mlab.contour3d(sphere_scalar, contours=[s_rad * s_rad], color=s_color, opacity=0.3) 79 | 80 | 81 | # plot sphere 82 | def mlab_plt_cylinder(center_x, center_y, s_rad, s_color): 83 | extent = [-0.2, 0.2, -0.2, 0.2, -2, 2] 84 | x, y, z = np.ogrid[(-2): (2): complex(0, 64), \ 85 | (-2): (2): complex(0, 64), \ 86 | (-2): (2): complex(0, 64)] 87 | 88 | cylinder_scalar = (x - center_x) * (x - center_x) + (y - center_y) * (y - center_y) - 0 * z 89 | cylinder = mlab.contour3d(cylinder_scalar, contours=[s_rad * s_rad], color=s_color, opacity=0.3, extent=extent) 90 | 91 | 92 | # plot initial 93 | 94 | # plot unsafe 95 | # def mlab_plt_unsafe(): 96 | # if len(prob.SUB_UNSAFE) == 0: 97 | # if prob.UNSAFE_SHAPE == 1: # cube 98 | # mlab_plt_cube(prob.UNSAFE[0][0], prob.UNSAFE[0][1], prob.UNSAFE[1][0], prob.UNSAFE[1][1], prob.UNSAFE[2][0], 99 | # prob.UNSAFE[2][1], (1, 0, 0)) 100 | # elif prob.UNSAFE_SHAPE == 2: # sphere 101 | # mlab_plt_sphere((prob.UNSAFE[0][0] + prob.UNSAFE[0][1]) / 2, (prob.UNSAFE[1][0] + prob.UNSAFE[1][1]) / 2, \ 102 | # (prob.UNSAFE[2][0] + prob.UNSAFE[2][1]) / 2, (prob.UNSAFE[0][1] - prob.UNSAFE[0][0]) / 2, 103 | # (1, 0, 0)) 104 | # elif prob.UNSAFE_SHAPE == 3: # cylinder 105 | # mlab_plt_cylinder((prob.UNSAFE[0][0] + prob.UNSAFE[0][1]) / 2, (prob.UNSAFE[1][0] + prob.UNSAFE[1][1]) / 2, \ 106 | # (prob.UNSAFE[0][1] - prob.UNSAFE[0][0]) / 2, (1, 0, 0)) 107 | # else: 108 | # x, y, z = np.ogrid[(prob.DOMAIN[0][0]): (prob.DOMAIN[0][1]): complex(0, superp.PLOT_LEN_B[0]), \ 109 | # (prob.DOMAIN[1][0]): (prob.DOMAIN[1][1]): complex(0, superp.PLOT_LEN_B[1]), \ 110 | # (prob.DOMAIN[2][0]): (prob.DOMAIN[2][1]): complex(0, superp.PLOT_LEN_B[2])] 111 | # hyperplane_scalar = x + y + z 112 | 113 | # hyperplane = mlab.contour3d(hyperplane_scalar, contours=[2], color=(1, 0, 0), opacity=0.3, extent=extent) 114 | # 115 | # else: 116 | # for i in range(len(prob.SUB_UNSAFE)): 117 | # curr_shape = prob.SUB_UNSAFE_SHAPE[i] 118 | # curr_range = prob.SUB_UNSAFE[i] 119 | # if curr_shape == 1: # cube 120 | # mlab_plt_cube(curr_range[0][0], curr_range[0][1], curr_range[1][0], curr_range[1][1], curr_range[2][0], 121 | # curr_range[2][1], (1, 0, 0)) 122 | # elif curr_shape == 2: # sphere 123 | # mlab_plt_sphere((curr_range[0][0] + curr_range[0][1]) / 2, (curr_range[1][0] + curr_range[1][1]) / 2, \ 124 | # (curr_range[2][0] + curr_range[2][1]) / 2, (curr_range[0][1] - curr_range[0][0]) / 2, 125 | # (1, 0, 0)) 126 | # else: # cylinder 127 | # mlab_plt_cylinder((curr_range[0][0] + curr_range[0][1]) / 2, (curr_range[1][0] + curr_range[1][1]) / 2, \ 128 | # (curr_range[0][1] - curr_range[0][0]) / 2, (1, 0, 0)) 129 | 130 | 131 | # generating plot data for nn 132 | def gen_plot_data(): 133 | sample_x = torch.linspace(-2, 2, int(64)) 134 | sample_y = torch.linspace(-2, 2, int(64)) 135 | sample_z = torch.linspace(-2, 2, int(64)) 136 | grid_xyz = torch.meshgrid([sample_x, sample_y, sample_z]) 137 | flatten_xyz = [torch.flatten(grid_xyz[i]) for i in range(len(grid_xyz))] 138 | plot_input = torch.stack(flatten_xyz, 1) 139 | 140 | return plot_input 141 | 142 | 143 | # plot barrier 144 | def mlab_plt_barrier(model): 145 | # generating nn_output for plotting 146 | plot_input = gen_plot_data() 147 | nn_output = model(torch.hstack([plot_input, torch.Tensor([-2, -2, -2])*torch.ones([262144, 3])])) 148 | plot_output = (nn_output[:, 0]).reshape(64, 64, 64) 149 | # barrier_plot = mlab.contour3d(plot_output.detach().numpy(), contours = [-superp.TOL_BOUNDARY, 0, -superp.TOL_BOUNDARY], \ 150 | # color = (1, 1, 0), opacity = 0.3) # yellow 151 | extent = [-0.6, 0.6, -0.6, 0.6, -0.6, 0.6] 152 | src = mlab.pipeline.scalar_field(plot_output.detach().numpy(), extent=extent) 153 | barrier_plot = mlab.pipeline.iso_surface(src, contours=[0.02, 0.02, 0.02], \ 154 | color=(1, 1, 0), opacity=1, extent=extent) 155 | 156 | axes = mlab.axes(xlabel='$p_x$', ylabel='$p_y$', zlabel='$p_z$') 157 | axes.label_text_property.font_family = 'times' 158 | axes.label_text_property.font_size = 30 159 | axes.axes.font_factor = 2 160 | 161 | 162 | # plot frame 163 | def mlab_plt_frame(): 164 | x, y, z = np.mgrid[(-2): (2): complex(0, 64), \ 165 | (-2): (2): complex(0, 64), \ 166 | (-2): (2): complex(0, 64)] 167 | u = 0 * x 168 | v = 0 * y 169 | w = 0 * z 170 | src = mlab.pipeline.vector_field(u, v, w) 171 | frame = mlab.pipeline.vectors(src, scale_factor=0, opacity=0) 172 | # frame = mlab.quiver3d(u, v, w, color = (1, 1, 1), scale_factor = 0, opacity = 0.0) 173 | # print(src.get_output_dataset()) 174 | # input 175 | # mlab.outline() 176 | # mlab.axes(xlabel='$p_x$', ylabel='$p_y$', zlabel='$p_z$') 177 | # mlab.axes.font_factor=2 178 | axes = mlab.axes(xlabel='$p_x$', ylabel='$p_y$', zlabel='$p_z$') 179 | axes.label_text_property.font_family = 'times' 180 | axes.label_text_property.font_size = 30 181 | axes.axes.font_factor = 2 182 | 183 | 184 | # plot 3d-system 185 | def plot_barrier_3d(model): 186 | mlab.figure(fgcolor=(0, 0, 0), bgcolor=(1, 1, 1)) 187 | mlab_plt_barrier(model) 188 | # mlab_plt_init() 189 | # mlab_plt_unsafe() 190 | # Create a meshgrid of spherical coordinates 191 | phi, theta = np.mgrid[0.0:2.0 * np.pi:100j, 0.0:np.pi:50j] 192 | 193 | # Convert spherical coordinates to Cartesian coordinates 194 | x = 0.25 * np.sin(theta) * np.cos(phi) 195 | y = 0.25 * np.sin(theta) * np.sin(phi) 196 | z = 0.25 * np.cos(theta) 197 | 198 | # Plot the sphere 199 | mlab.mesh(x, y, z, color=(1, 0, 0)) 200 | # mlab_plt_sphere((-0.25 + 0.25) / 2, (-0.25 + 0.25) / 2, \ 201 | # (-0.25 + 0.25) / 2, (0.5) / 2, 202 | # (1, 0, 0)) 203 | # mlab_plt_vector() 204 | # mlab_plt_flow() 205 | # mlab_plt_frame() 206 | mlab.show() 207 | 208 | from Cases.LinearSat import LinearSat 209 | LinearSat = LinearSat() 210 | from SoloSNCBFLinearSat import SNCBF_Synth 211 | gamma_list = [0.001, 0.002, 0.0015, 0.001, 0.01, 0.001, 0.01, 0.001] 212 | newCBF = SNCBF_Synth([128, 128], [True, True], LinearSat, 213 | sigma=[0.001, 0.001, 0.001, 0.00001, 0.0001, 0.00001, 0.00001, 0.00001], 214 | nu=[0.001, 0.001, 0.001, 0.00001, 0.00001, 0.00001, 0.00001, 0.00001], 215 | gamma_list=gamma_list, 216 | verbose=True) 217 | newCBF.model.load_state_dict(torch.load('Trained_model/NCBF/linearSat30.pt')) 218 | plot_barrier_3d(newCBF.model) -------------------------------------------------------------------------------- /Visualization/Plot_6Dtraj.py: -------------------------------------------------------------------------------- 1 | import matplotlib.pyplot as plt 2 | import pandas as pd 3 | import numpy as np 4 | from matplotlib.patches import Rectangle 5 | 6 | dfs = pd.read_csv("trajr_6d_succ.csv") 7 | dff = pd.read_csv("trajr_6d_fail.csv") 8 | # df1 = pd.read_csv("loss_epoch1_.csv", usecols=columns) 9 | # fig = plt.figure(figsize=[8,5]) 10 | fig, ax = plt.subplots(figsize=[10,5.5], tight_layout=True) 11 | ax.plot(dff['value'], 'crimson', label='Baseline', linewidth=4) 12 | ax.plot(dfs['value'], 'g', label='Proposed Approach', linewidth=4) 13 | ax.add_patch(Rectangle((0,1.5),50,0.25, color='salmon', label='Unsafe Region')) 14 | ax.add_patch(Rectangle((0,0),50,0.25, color='salmon')) 15 | # pl.plot(df.volume, label='volume') 16 | plt.xlabel('Time-steps', fontsize=20) 17 | plt.xticks(fontsize=20) 18 | plt.yticks(fontsize=20) 19 | plt.ylabel('Distance to target satellite', fontsize=20) 20 | plt.text(17, 1.55, 'Unsafe Region', color='brown', fontsize=30) 21 | plt.text(17, 0.05, 'Unsafe Region', color='brown', fontsize=30) 22 | plt.xlim([0, 50]) 23 | plt.ylim([0, 1.75]) 24 | # pl.xticks(np.arange(0, 19, step=2)) 25 | plt.legend(fontsize=20, loc=(0.01, 0.2)) 26 | plt.title('Trajectory comparison between baseline and proposed method', fontsize=20) 27 | plt.show() 28 | -------------------------------------------------------------------------------- /Visualization/Plot_TrainingData.py: -------------------------------------------------------------------------------- 1 | import matplotlib.pyplot as plt 2 | import pandas as pd 3 | import matplotlib.pyplot as pl 4 | import numpy as np 5 | 6 | columns=['loss', 'floss', 'closs', 'tloss', 'FCEnum', 'volume'] 7 | # df = pd.DataFrame(columns=['loss', 'floss', 'closs', 'tloss', 'vloss', 'FCEnum', 'volume']) 8 | # df = pd.read_csv("linearSat_initV1.csv", usecols=columns) 9 | 10 | # obs training 11 | # columns=['loss', 'floss', 'closs', 'tloss', 'FCEnum', 'volume'] 12 | # df = pd.read_csv("loss_epoch0.csv", usecols=columns) 13 | 14 | # linear sat 15 | columns=['loss', 'floss', 'closs', 'tloss', 'vloss', 'FCEnum', 'volume'] 16 | df = pd.read_csv("linearSat_fixn.csv", usecols=columns) 17 | # df1 = pd.read_csv("loss_epoch1_.csv", usecols=columns) 18 | fig = pl.figure(figsize=[7,5], tight_layout=True) 19 | pl.plot(df.loss, label='loss', linewidth=4) 20 | # pl.plot(df.vloss+df.floss+df.closs+df.tloss, label='loss', linewidth=4) 21 | pl.plot(df.floss, label='feasibility', linewidth=4) 22 | pl.plot(df.closs, '--', label='correctness', linewidth=4) 23 | # pl.plot(df.volume, label='volume') 24 | pl.xlabel('Epoch', fontsize=20) 25 | pl.ylabel('Training Loss', fontsize=20) 26 | pl.xlim([0, 19]) 27 | pl.xticks(np.arange(0, 19, step=2), fontsize=20) 28 | pl.yticks(fontsize=20) 29 | pl.legend(fontsize=20) 30 | plt.title('FT-NCBF training loss along epoch', fontsize=20) 31 | 32 | pl.show() 33 | -------------------------------------------------------------------------------- /Visualization/visualization.py: -------------------------------------------------------------------------------- 1 | import matplotlib.pyplot as plt 2 | import numpy as np 3 | # from veri_util import * 4 | import torch 5 | import sympy as sp 6 | 7 | def h_x(x): 8 | hx = (x[0]+x[1]**2) 9 | return torch.tanh(hx) 10 | 11 | def visualize(nnmodel, polyflag=False, polydeg=5): 12 | state_space = [[-2,2],[-2,2]] 13 | shape = [100,100] 14 | cell_length = (state_space[0][1] - state_space[0][0]) / shape[0] 15 | nx = torch.linspace(state_space[0][0] + cell_length / 2, state_space[0][1] - cell_length / 2, shape[0]) 16 | ny = torch.linspace(state_space[1][0] + cell_length / 2, state_space[1][1] - cell_length / 2, shape[1]) 17 | vx, vy = torch.meshgrid(nx, ny) 18 | data = np.dstack([vx.reshape([shape[0], shape[1], 1]), vy.reshape([shape[0], shape[1], 1])]) 19 | data = torch.Tensor(data.reshape(shape[0] * shape[1], 2)) 20 | 21 | if not polyflag: 22 | output = (nnmodel.forward(data)).detach().numpy() 23 | else: 24 | poly_name, poly_coeff = nnmodel.topolyCBF(polydeg) 25 | x0, x1 = sp.symbols('x0, x1') 26 | x = [x0, x1] 27 | fcn = nnmodel.SymPoly(poly_coeff, x, polydeg) 28 | data = data.detach().numpy() 29 | output = np.array([fcn.subs({x0: data[_][0], x1: data[_][1]}) for _ in range(len(data))], dtype=np.float) 30 | # output = h_x(data.transpose(0,1)) 31 | z = output.reshape(shape) 32 | z_min, z_max = -np.abs(z).max(), np.abs(z).max() 33 | 34 | fig, ax = plt.subplots() 35 | 36 | c = ax.pcolormesh(vx, vy, z, cmap='RdBu', vmin=z_min, vmax=z_max) 37 | ax.set_title('pcolormesh') 38 | # set the limits of the plot to the limits of the data 39 | ax.axis([vx.min(), vx.max(), vy.min(), vy.max()]) 40 | fig.colorbar(c, ax=ax) 41 | for i in range(100): 42 | for j in range(100): 43 | if np.linalg.norm(z[i][j])<0.003: 44 | plt.scatter(nx[i], ny[j]) 45 | plt.show() 46 | 47 | def visualization(nnmodel): 48 | state_space = [[-2,2],[-2,2]] 49 | shape = [100,100] 50 | cell_length = (state_space[0][1] - state_space[0][0]) / shape[0] 51 | nx = torch.linspace(state_space[0][0] + cell_length / 2, state_space[0][1] - cell_length / 2, shape[0]) 52 | ny = torch.linspace(state_space[1][0] + cell_length / 2, state_space[1][1] - cell_length / 2, shape[1]) 53 | vx, vy = torch.meshgrid(nx, ny) 54 | data = np.dstack([vx.reshape([shape[0], shape[1], 1]), vy.reshape([shape[0], shape[1], 1])]) 55 | data = torch.Tensor(data.reshape(shape[0] * shape[1], 2)) 56 | 57 | output = (nnmodel.forward(data)).detach().numpy() 58 | # output = h_x(data.transpose(0,1)) 59 | z = output.reshape(shape) 60 | z_min, z_max = -np.abs(z).max(), np.abs(z).max() 61 | 62 | fig, ax = plt.subplots() 63 | 64 | c = ax.pcolormesh(vx, vy, z, cmap='RdBu', vmin=z_min, vmax=z_max) 65 | ax.set_title('pcolormesh') 66 | # set the limits of the plot to the limits of the data 67 | ax.axis([vx.min(), vx.max(), vy.min(), vy.max()]) 68 | fig.colorbar(c, ax=ax) 69 | for i in range(100): 70 | for j in range(100): 71 | if np.linalg.norm(z[i][j])<0.003: 72 | plt.scatter(nx[i], ny[j]) 73 | plt.show() 74 | 75 | 76 | def visualization_fcn(NCBF, Critic): 77 | state_space = [[-2,2],[-2,2]] 78 | shape = [100,100] 79 | cell_length = (state_space[0][1] - state_space[0][0]) / shape[0] 80 | nx = torch.linspace(state_space[0][0] + cell_length / 2, state_space[0][1] - cell_length / 2, shape[0]) 81 | ny = torch.linspace(state_space[1][0] + cell_length / 2, state_space[1][1] - cell_length / 2, shape[1]) 82 | vx, vy = torch.meshgrid(nx, ny) 83 | data = np.dstack([vx.reshape([shape[0], shape[1], 1]), vy.reshape([shape[0], shape[1], 1])]) 84 | data = torch.Tensor(data.reshape(shape[0] * shape[1], 2)) 85 | 86 | poly_name, poly_coeff = NCBF.topolyCBF() 87 | x0, x1 = sp.symbols('x0, x1') 88 | x = [x0, x1] 89 | fcn = Critic.SymPoly(poly_coeff, x) 90 | # output = h_x(data.transpose(0,1)) 91 | data = data.detach().numpy() 92 | output = np.array([fcn.subs({x0:data[_][0], x1:data[_][1]}) for _ in range(len(data))], dtype=np.float) 93 | z = output.reshape(shape) 94 | z_min, z_max = -np.abs(z).max(), np.abs(z).max() 95 | 96 | fig, ax = plt.subplots() 97 | 98 | c = ax.pcolormesh(vx, vy, z, cmap='RdBu', vmin=z_min, vmax=z_max) 99 | ax.set_title('pcolormesh') 100 | # set the limits of the plot to the limits of the data 101 | ax.axis([vx.min(), vx.max(), vy.min(), vy.max()]) 102 | fig.colorbar(c, ax=ax) 103 | for i in range(100): 104 | for j in range(100): 105 | if np.linalg.norm(z[i][j])<0.003: 106 | plt.scatter(nx[i], ny[j]) 107 | plt.show() -------------------------------------------------------------------------------- /Visualization/visualizer.py: -------------------------------------------------------------------------------- 1 | # Author: Shing-Yan Loo (yan99033@gmail.com) 2 | # Visualize the sensor readings and vehicle trajectory 3 | 4 | import matplotlib 5 | matplotlib.use('Qt5Agg') 6 | 7 | import matplotlib.pyplot as plt 8 | import matplotlib.gridspec as gridspec 9 | from mpl_toolkits.mplot3d import Axes3D 10 | 11 | import time 12 | import math 13 | 14 | from numpy.core.shape_base import block 15 | 16 | def add_sensor_reading(sensor_list, new_reading, keep_last_n): 17 | """Add new sensor reading to the list and only keep the last n 18 | readings 19 | 20 | :param sensor_list: Container that keeps the sensor reading 21 | :type sensor_list: list 22 | :param new_reading: latest sensor reading 23 | :type new_reading: float 24 | :param keep_last_n: max number of length for sensor_list 25 | :type keep_last_n: int 26 | """ 27 | sensor_list.append(new_reading) 28 | 29 | if len(sensor_list) > keep_last_n: 30 | # in-place modification 31 | sensor_list[:] = sensor_list[-keep_last_n:] 32 | 33 | def add_to_traj(traj_x, traj_y, traj_z, x, y, z, max_len, min_dist=0.1): 34 | """Add new xyz position to the trajectory 35 | Two important considerations: 36 | 1. remove the old points (sliding window) once the 'buffer' is full 37 | 2. don't add the point if it is too close to the previous one 38 | 39 | :param traj: list of x locations 40 | :type traj: list 41 | :param traj: list of y locations 42 | :type traj: list 43 | :param traj: list of z locations 44 | :type traj: list 45 | :param x: x position 46 | :type x: double 47 | :param y: x position 48 | :type y: double 49 | :param z: z position 50 | :type z: double 51 | :param max_len: keep the last n positions 52 | :type max_len: int 53 | :param min_dist: min distance to consider adding the new point 54 | type min_dist: double 55 | """ 56 | assert len(traj_x) == len(traj_y) and len(traj_y) == len(traj_z) 57 | # Calculate the distance between the current point and the last point 58 | # and skip if the distance is too small (e.g., vehicle is stationery) 59 | if len(traj_x) > 0 and \ 60 | math.sqrt((traj_x[-1] - x)**2 + (traj_y[-1] - y) ** 2 + (traj_z[-1] - z) ** 2) < min_dist: 61 | return 62 | 63 | # Add new position to the trajectory 64 | traj_x.append(x) 65 | traj_y.append(y) 66 | traj_z.append(z) 67 | 68 | # Keep the lists within the maximum length 69 | # in-place modification 70 | if len(traj_x) > max_len: 71 | traj_x[:] = traj_x[-max_len:] 72 | if len(traj_y) > max_len: 73 | traj_y[:] = traj_y[-max_len:] 74 | if len(traj_z) > max_len: 75 | traj_z[:] = traj_z[-max_len:] 76 | 77 | def visualizer(visual_msg_queue, quit): 78 | # Setup layout 79 | fig = plt.figure(figsize=(16, 9), dpi=120, facecolor=(0.6, 0.6, 0.6)) 80 | gs = gridspec.GridSpec(4, 5) 81 | pose_plot = fig.add_subplot(gs[:, :2], projection='3d', facecolor=(1.0, 1.0, 1.0)) 82 | pose_plot.title.set_text('Position') 83 | imu_plot = fig.add_subplot(gs[:2, 2:4], facecolor=(1.0, 1.0, 1.0)) 84 | imu_plot.title.set_text('IMU') 85 | gnss_plot = fig.add_subplot(gs[2:, 2:4], projection='3d', facecolor=(1.0, 1.0, 1.0)) 86 | gnss_plot.title.set_text('GNSS') 87 | 88 | # Expand the plot when needed 89 | pose_plot.autoscale() 90 | 91 | # Trajectory length limit 92 | # Would remove the oldest point as soon as the list reaches the maximum length (sliding window) 93 | max_traj_len = 50000 94 | 95 | # Keep vehicle trajectories 96 | gt_traj_x = [] 97 | gt_traj_y = [] 98 | gt_traj_z = [] 99 | est_traj_x = [] 100 | est_traj_y = [] 101 | est_traj_z = [] 102 | 103 | # GPS trajectory 104 | gnss_traj_x = [] 105 | gnss_traj_y = [] 106 | gnss_traj_z = [] 107 | 108 | # Keep past n IMU sensor reading 109 | keep_last_n = 50 110 | t = 0 111 | ts_imu = [] 112 | acc_x_all = [] 113 | acc_y_all = [] 114 | acc_z_all = [] 115 | gyro_x_all = [] 116 | gyro_y_all = [] 117 | gyro_z_all = [] 118 | 119 | # Font size 120 | fontsize = 14 121 | 122 | # make sure the window is raised, but the script keeps going 123 | plt.show(block=False) 124 | 125 | while True: # not quit.value: 126 | if visual_msg_queue.empty(): 127 | time.sleep(0.01) 128 | continue 129 | 130 | msg = visual_msg_queue.get() 131 | 132 | # Update trajectory 133 | updated_traj = False 134 | if msg.get('gt_traj') is not None: 135 | gt_pos = msg['gt_traj'] 136 | add_to_traj(gt_traj_x, gt_traj_y, gt_traj_z, gt_pos[0], gt_pos[1], gt_pos[2], max_traj_len) 137 | updated_traj = True 138 | if msg.get('est_traj') is not None: 139 | est_pos = msg['est_traj'] 140 | add_to_traj(est_traj_x, est_traj_y, est_traj_z, est_pos[0], est_pos[1], est_pos[2], max_traj_len) 141 | updated_traj = True 142 | 143 | # Visualize vehicle trajectory 144 | if updated_traj: 145 | # Clear previous plot 146 | pose_plot.cla() 147 | 148 | # Update plot 149 | pose_plot.plot(gt_traj_x, gt_traj_y, gt_traj_z, color='green', linestyle='solid', label='GT') 150 | pose_plot.plot(est_traj_x, est_traj_y, est_traj_z, color='blue', linestyle='solid', label='est') 151 | pose_plot.legend(fontsize=fontsize) 152 | 153 | 154 | 155 | # Visualize IMU reading 156 | if msg.get('imu') is not None: 157 | acc_x, acc_y, acc_z, gyro_x, gyro_y, gyro_z = msg['imu'] 158 | add_sensor_reading(acc_x_all, acc_x, keep_last_n) 159 | add_sensor_reading(acc_y_all, acc_y, keep_last_n) 160 | add_sensor_reading(acc_z_all, acc_z, keep_last_n) 161 | add_sensor_reading(gyro_x_all, gyro_x, keep_last_n) 162 | add_sensor_reading(gyro_y_all, gyro_y, keep_last_n) 163 | add_sensor_reading(gyro_z_all, gyro_z, keep_last_n) 164 | 165 | ts_imu.append(t) 166 | if len(ts_imu) > keep_last_n: 167 | ts_imu = ts_imu[-keep_last_n:] 168 | 169 | # Clear previous plot 170 | imu_plot.cla() 171 | 172 | # Update IMU reading 173 | imu_plot.plot(ts_imu, acc_x_all, label='acc_x') 174 | imu_plot.plot(ts_imu, acc_y_all, label='acc_y') 175 | imu_plot.plot(ts_imu, acc_z_all, label='acc_z') 176 | imu_plot.plot(ts_imu, gyro_x_all, label='gyro_x') 177 | imu_plot.plot(ts_imu, gyro_y_all, label='gyro_y') 178 | imu_plot.plot(ts_imu, gyro_z_all, label='gyro_z') 179 | imu_plot.legend(fontsize=fontsize) 180 | 181 | # Visualize GNSS reading 182 | if msg.get('gnss') is not None: 183 | gnss = msg['gnss'] 184 | add_to_traj(gnss_traj_x, gnss_traj_y, gnss_traj_z, gnss[0], gnss[1], gnss[2], max_traj_len) 185 | 186 | # Clear previous plot 187 | gnss_plot.cla() 188 | 189 | # Update GNSS reading 190 | gnss_plot.plot(gnss_traj_x, gnss_traj_y, gnss_traj_z, color='black', linestyle='solid', label='GNSS') 191 | gnss_plot.legend(fontsize=fontsize) 192 | 193 | # flush any pending GUI events, re-painting the screen if needed 194 | fig.canvas.flush_events() 195 | t += 1 196 | 197 | if not quit.value: 198 | fig.canvas.draw_idle() 199 | else: 200 | plt.close('all') 201 | print('quiting visualizer loop') 202 | break 203 | -------------------------------------------------------------------------------- /car.py: -------------------------------------------------------------------------------- 1 | # Author: Shing-Yan Loo (lsyan@ualberta.ca) 2 | # The car class contains the current vehicle state as well as the sensors 3 | # information 4 | # As soon as a sensor reading (be it an image, or IMU reading) is received, 5 | # a callback is received to push the sensor readings to their respective queues 6 | # 7 | # To get the snapshot of the current sensor reading, call the get_sensor_readings 8 | # method. To make sure that the sensor readings are synchronized, only keep the last 9 | # n timestamps and get the readings at the closest timestamp 10 | # 11 | # Credit: 12 | # (Get GNSS reference latitude and longitude) 13 | # https://github.com/carla-simulator/scenario_runner/blob/master/srunner/tools/route_manipulation.py 14 | # 15 | # (Convert GNSS signal to absolute XYZ) 16 | # https://github.com/erdos-project/pylot/blob/master/pylot/utils.py 17 | 18 | import glob 19 | import os 20 | import sys 21 | try: 22 | sys.path.append(glob.glob('../carla/dist/carla-*%d.%d-%s.egg' % ( 23 | sys.version_info.major, 24 | sys.version_info.minor, 25 | 'win-amd64' if os.name == 'nt' else 'linux-x86_64'))[0]) 26 | except IndexError: 27 | pass 28 | import carla 29 | 30 | import numpy as np 31 | import math 32 | 33 | import weakref 34 | 35 | import xml.etree.ElementTree as ET 36 | 37 | from queue import Queue 38 | from collections import OrderedDict 39 | 40 | from util import destroy_queue 41 | 42 | class Gnss: 43 | def __init__(self, x, y, z): 44 | self.x = x 45 | self.y = y 46 | self.z = z 47 | 48 | class Car: 49 | def __init__(self, world, client, spawn_point): 50 | self.world = world 51 | self.client = client 52 | 53 | # Sensor noise profile 54 | NOISE_STDDEV = 5e-8 55 | NOISE_BIAS = 1e-8 56 | NOISE_IMU_ACC_X_STDDEV = NOISE_STDDEV 57 | NOISE_IMU_ACC_Y_STDDEV = NOISE_STDDEV 58 | NOISE_IMU_ACC_Z_STDDEV = NOISE_STDDEV 59 | NOISE_IMU_GYRO_X_BIAS = NOISE_BIAS 60 | NOISE_IMU_GYRO_X_STDDEV = NOISE_STDDEV 61 | NOISE_IMU_GYRO_Y_BIAS = NOISE_BIAS 62 | NOISE_IMU_GYRO_Y_STDDEV = NOISE_STDDEV 63 | NOISE_IMU_GYRO_Z_BIAS = NOISE_BIAS 64 | NOISE_IMU_GYRO_Z_STDDEV = NOISE_STDDEV 65 | NOISE_GNSS_ALT_BIAS = NOISE_BIAS 66 | NOISE_GNSS_ALT_STDDEV = NOISE_STDDEV 67 | NOISE_GNSS_LAT_BIAS = NOISE_BIAS 68 | NOISE_GNSS_LAT_STDDEV = NOISE_STDDEV 69 | NOISE_GNSS_LON_BIAS = NOISE_BIAS 70 | NOISE_GNSS_LON_STDDEV = NOISE_STDDEV 71 | 72 | # Initialize the vehicle and the sensors 73 | bp_lib = world.get_blueprint_library() 74 | vehicle_bp = bp_lib.filter('model3')[0] 75 | camera_bp = bp_lib.filter("sensor.camera.rgb")[0] 76 | imu_bp = bp_lib.filter("sensor.other.imu")[0] 77 | gnss_bp = bp_lib.filter("sensor.other.gnss")[0] 78 | 79 | # Set sensors' noise 80 | imu_bp.set_attribute('noise_accel_stddev_x', str(NOISE_IMU_ACC_X_STDDEV)) 81 | imu_bp.set_attribute('noise_accel_stddev_y', str(NOISE_IMU_ACC_Y_STDDEV)) 82 | imu_bp.set_attribute('noise_accel_stddev_z', str(NOISE_IMU_ACC_Z_STDDEV)) 83 | imu_bp.set_attribute('noise_gyro_stddev_x', str(NOISE_IMU_GYRO_X_STDDEV)) 84 | imu_bp.set_attribute('noise_gyro_stddev_y', str(NOISE_IMU_GYRO_Y_STDDEV)) 85 | imu_bp.set_attribute('noise_gyro_stddev_z', str(NOISE_IMU_GYRO_Z_STDDEV)) 86 | imu_bp.set_attribute('noise_gyro_bias_x', str(NOISE_IMU_GYRO_X_BIAS)) 87 | imu_bp.set_attribute('noise_gyro_bias_y', str(NOISE_IMU_GYRO_Y_BIAS)) 88 | imu_bp.set_attribute('noise_gyro_bias_z', str(NOISE_IMU_GYRO_Z_BIAS)) 89 | gnss_bp.set_attribute('noise_alt_bias', str(NOISE_GNSS_ALT_BIAS)) 90 | gnss_bp.set_attribute('noise_alt_stddev', str(NOISE_GNSS_ALT_STDDEV)) 91 | gnss_bp.set_attribute('noise_lat_bias', str(NOISE_GNSS_LAT_BIAS)) 92 | gnss_bp.set_attribute('noise_lat_stddev', str(NOISE_GNSS_LAT_STDDEV)) 93 | gnss_bp.set_attribute('noise_lon_bias', str(NOISE_GNSS_LON_BIAS)) 94 | gnss_bp.set_attribute('noise_lon_stddev', str(NOISE_GNSS_LON_STDDEV)) 95 | 96 | # Sensor sampling frequency 97 | IMU_FREQ = 200 98 | GNSS_FREQ = 10 99 | imu_bp.set_attribute('sensor_tick', str(1.0 / IMU_FREQ)) 100 | gnss_bp.set_attribute('sensor_tick', str(1.0 / GNSS_FREQ)) 101 | 102 | self.vehicle = world.spawn_actor(vehicle_bp, spawn_point) 103 | self.vehicle.set_autopilot(False) 104 | 105 | self.camera = world.spawn_actor( 106 | blueprint=camera_bp, 107 | transform=carla.Transform(carla.Location(x=1.6, z=1.6)), 108 | attach_to=self.vehicle 109 | ) 110 | 111 | self.imu = world.spawn_actor( 112 | blueprint=imu_bp, 113 | transform=carla.Transform(carla.Location(x=0, z=0)), 114 | attach_to=self.vehicle 115 | ) 116 | self.gnss = world.spawn_actor( 117 | blueprint=gnss_bp, 118 | transform=carla.Transform(carla.Location(x=0, z=0)), 119 | attach_to=self.vehicle 120 | ) 121 | 122 | self.actor_list = [self.vehicle, self.camera, self.imu, self.gnss] 123 | 124 | # Build the K projection matrix: 125 | image_w = camera_bp.get_attribute("image_size_x").as_int() 126 | image_h = camera_bp.get_attribute("image_size_y").as_int() 127 | fov = camera_bp.get_attribute("fov").as_float() 128 | focal = image_w / (2.0 * np.tan(fov * np.pi / 360.0)) 129 | 130 | # In this case Fx and Fy are the same since the pixel aspect 131 | # ratio is 1 132 | self.K = np.identity(3) 133 | self.K[0, 0] = self.K[1, 1] = focal 134 | self.K[0, 2] = image_w / 2.0 135 | self.K[1, 2] = image_h / 2.0 136 | 137 | # Keep the sensor readings from the callback to queues 138 | self.image_queue = Queue() 139 | self.imu_queue = Queue() 140 | self.gnss_queue = Queue() 141 | 142 | # Hook sensor readings to callback methods 143 | weak_self = weakref.ref(self) 144 | self.camera.listen(lambda data : Car.sensor_callback(weak_self, data, self.image_queue)) 145 | self.imu.listen(lambda data : Car.sensor_callback(weak_self, data, self.imu_queue)) 146 | self.gnss.listen(lambda data : Car.sensor_callback(weak_self, data, self.gnss_queue)) 147 | 148 | # Reference latitude and longitude (GNSS) 149 | self.gnss_lat_ref, self.gnss_long_ref = self._get_latlon_ref() 150 | 151 | def destroy(self): 152 | self.camera.destroy() 153 | self.imu.destroy() 154 | self.gnss.destroy() 155 | 156 | self.client.apply_batch([carla.command.DestroyActor(x) 157 | for x in self.actor_list if x is not None]) 158 | 159 | destroy_queue(self.image_queue) 160 | destroy_queue(self.imu_queue) 161 | destroy_queue(self.gnss_queue) 162 | 163 | 164 | def get_location(self): 165 | return self.vehicle.get_location() 166 | 167 | @staticmethod 168 | def sensor_callback(weak_self, data, queue): 169 | self = weak_self() 170 | if not self: 171 | return 172 | queue.put(data) 173 | 174 | def get_sensor_readings(self, frame): 175 | """Return a dict containing the sensor readings 176 | at the particular frame 177 | 178 | :param frame: unique frame at the current world frame 179 | :type frame: int 180 | """ 181 | sensors = {'image': None, 182 | 'imu': None, 183 | 'gnss': None} 184 | 185 | while not self.image_queue.empty(): 186 | image_data = self.image_queue.get() 187 | 188 | if image_data.frame == frame: 189 | # Get the raw BGRA buffer and convert it to an array of RGB of 190 | # shape (image_data.height, image_data.width, 3). 191 | im_array = np.copy(np.frombuffer(image_data.raw_data, dtype=np.dtype("uint8"))) 192 | im_array = np.reshape(im_array, (image_data.height, image_data.width, 4)) 193 | im_array = im_array[:, :, :3] # [:, :, ::-1] 194 | sensors['image'] = im_array 195 | 196 | self.image_queue.task_done() 197 | break 198 | 199 | self.image_queue.task_done() 200 | 201 | 202 | while not self.imu_queue.empty(): 203 | imu_data = self.imu_queue.get() 204 | 205 | if imu_data.frame == frame: 206 | sensors['imu'] = imu_data 207 | 208 | self.imu_queue.task_done() 209 | break 210 | 211 | self.imu_queue.task_done() 212 | 213 | while not self.gnss_queue.empty(): 214 | gnss_data = self.gnss_queue.get() 215 | 216 | if gnss_data.frame == frame: 217 | 218 | alt = gnss_data.altitude 219 | lat = gnss_data.latitude 220 | long = gnss_data.longitude 221 | 222 | gps_xyz = self.gnss_to_xyz(lat, long, alt) 223 | 224 | sensors['gnss'] = gps_xyz 225 | 226 | self.gnss_queue.task_done() 227 | break 228 | 229 | self.gnss_queue.task_done() 230 | 231 | 232 | return sensors 233 | 234 | def gnss_to_xyz(self, latitude, longitude, altitude): 235 | """Creates Location from GPS (latitude, longitude, altitude). 236 | This is the inverse of the _location_to_gps method found in 237 | https://github.com/carla-simulator/scenario_runner/blob/master/srunner/tools/route_manipulation.py 238 | 239 | Modified from: 240 | https://github.com/erdos-project/pylot/blob/master/pylot/utils.py 241 | """ 242 | EARTH_RADIUS_EQUA = 6378137.0 243 | 244 | scale = math.cos(self.gnss_lat_ref * math.pi / 180.0) 245 | basex = scale * math.pi * EARTH_RADIUS_EQUA / 180.0 * self.gnss_long_ref 246 | basey = scale * EARTH_RADIUS_EQUA * math.log( 247 | math.tan((90.0 + self.gnss_lat_ref) * math.pi / 360.0)) 248 | 249 | x = scale * math.pi * EARTH_RADIUS_EQUA / 180.0 * longitude - basex 250 | y = scale * EARTH_RADIUS_EQUA * math.log( 251 | math.tan((90.0 + latitude) * math.pi / 360.0)) - basey 252 | 253 | # This wasn't in the original method, but seems to be necessary. 254 | y *= -1 255 | 256 | return Gnss(x, y, altitude) 257 | 258 | def _get_latlon_ref(self): 259 | """ 260 | Convert from waypoints world coordinates to CARLA GPS coordinates 261 | :return: tuple with lat and lon coordinates 262 | https://github.com/carla-simulator/scenario_runner/blob/master/srunner/tools/route_manipulation.py 263 | """ 264 | xodr = self.world.get_map().to_opendrive() 265 | tree = ET.ElementTree(ET.fromstring(xodr)) 266 | 267 | # default reference 268 | lat_ref = 42.0 269 | lon_ref = 2.0 270 | 271 | for opendrive in tree.iter("OpenDRIVE"): 272 | for header in opendrive.iter("header"): 273 | for georef in header.iter("geoReference"): 274 | if georef.text: 275 | str_list = georef.text.split(' ') 276 | for item in str_list: 277 | if '+lat_0' in item: 278 | lat_ref = float(item.split('=')[1]) 279 | if '+lon_0' in item: 280 | lon_ref = float(item.split('=')[1]) 281 | return lat_ref, lon_ref -------------------------------------------------------------------------------- /requirements.txt: -------------------------------------------------------------------------------- 1 | setuptools~=65.5.1 2 | scipy~=1.10.0 3 | torch~=1.12.1 4 | sympy~=1.11.1 5 | sklearn~=0.0.post1 6 | scikit-learn~=1.2.1 7 | numpy~=1.24.2 8 | matplotlib~=3.6.3 9 | auto-LiRPA~=0.3 10 | torchvision~=0.13.1 11 | pandas~=1.4.1 12 | carla~=0.9.12 13 | filterpy~=1.4.5 -------------------------------------------------------------------------------- /util.py: -------------------------------------------------------------------------------- 1 | # Author: Shing-Yan Loo (lsyan@ualberta.ca) 2 | # helper functions 3 | 4 | import multiprocessing 5 | 6 | def destroy_queue(queue): 7 | """destroy the queue 8 | 9 | :param queue: queue object 10 | :type queue: multiprocessing.Queue or Queue 11 | """ 12 | # Clear the queues 13 | while not queue.empty(): 14 | queue.get() 15 | 16 | if type(queue) != multiprocessing.queues.Queue: 17 | queue.task_done() 18 | 19 | if type(queue) == multiprocessing.queues.Queue: 20 | queue.close() 21 | queue.join_thread() 22 | else: 23 | queue.join() 24 | --------------------------------------------------------------------------------