├── DiscKernelAlgo
    ├── Algo
    │   ├── algo.cpp
    │   └── algo.h
    ├── CMakeLists.txt
    ├── Data
    │   └── LoadKN.m
    ├── Grid
    │   └── grid.h
    ├── Model
    │   ├── model.cpp
    │   └── model.h
    ├── README.md
    ├── car.json
    ├── install.sh
    ├── main.cpp
    ├── run.sh
    ├── types.cpp
    └── types.h
├── Images
    ├── AdverseRoad.png
    └── Disc285.png
├── LICENSE
├── LearningSafeSets
    ├── Data
    │   ├── Disc-10.bin
    │   ├── Disc-100.bin
    │   ├── Disc-1000.bin
    │   ├── Disc-20.bin
    │   ├── Disc-200.bin
    │   ├── Disc-25.bin
    │   ├── Disc-250.bin
    │   ├── Disc-33.bin
    │   ├── Disc-333.bin
    │   ├── Disc-50.bin
    │   ├── Disc-500.bin
    │   ├── Disc-666.bin
    │   ├── Disc-800.bin
    │   ├── Disc-Test-285.bin
    │   └── Disc-Test-66.bin
    ├── DataLoader
    │   └── DataLoader.py
    ├── Model
    │   └── SafeSet.py
    ├── Trainer
    │   └── Trainer.py
    ├── Validation
    │   └── Validation.py
    ├── config.json
    ├── run.sh
    └── train.py
└── README.md


/DiscKernelAlgo/Algo/algo.cpp:
--------------------------------------------------------------------------------
  1 | // Copyright 2020 Alexander Liniger
  2 | 
  3 | // Licensed under the Apache License, Version 2.0 (the "License");
  4 | // you may not use this file except in compliance with the License.
  5 | // You may obtain a copy of the License at
  6 | 
  7 | //     http://www.apache.org/licenses/LICENSE-2.0
  8 | 
  9 | // Unless required by applicable law or agreed to in writing, software
 10 | // distributed under the License is distributed on an "AS IS" BASIS,
 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 12 | // See the License for the specific language governing permissions and
 13 | // limitations under the License.
 14 | ///////////////////////////////////////////////////////////////////////////
 15 | ///////////////////////////////////////////////////////////////////////////
 16 | 
 17 | #include "algo.h"
 18 | 
 19 | void Algo::initGrid(Grid<bool> *grid) const
 20 | {
 21 | 
 22 |     for (int i = 0; i < NGD; i++) {
 23 |         for (int j = 0; j < NGMU; j++) {
 24 | 
 25 |             State x = grid->indexToState({i,j,0});
 26 |             double tightening = car_.l_car*std::sin(std::fabs(x.mu)) + car_.w_car*std::cos(x.mu);
 27 |             bool safe = x.d + car_.L / 2.0 * std::sin(x.mu) <=  road_.w_road - tightening
 28 |                      && x.d + car_.L / 2.0 * std::sin(x.mu) >= -road_.w_road + tightening;
 29 | 
 30 |             for (int k=0; k<NGV ; k++)
 31 |             {
 32 |                 grid->setValue({i,j,k},safe);
 33 |             }
 34 |         }
 35 |     }
 36 | }
 37 | 
 38 | bool Algo::isDiscSafe(const Model &model,const State &x,const Grid<bool> &grid) const
 39 | {
 40 |     Eigen::VectorXd kappa_vec = Eigen::VectorXd::LinSpaced(NGKAPPA,-road_.kappa_max,road_.kappa_max);
 41 | 
 42 |     std::vector<Input> allowed_inputs = model.possibleInputs(x.v);
 43 |     bool safe = false;
 44 | 
 45 |     for(int i = 0;i<NGKAPPA;i++)
 46 |     {
 47 |         safe = false;
 48 |         for (auto allowed_input : allowed_inputs) {
 49 |             ModelOutput next_state = model.nextState(x, allowed_input,kappa_vec(i));
 50 |             if(next_state.feasible)
 51 |             {
 52 |                 Index idx = grid.projectOntoGrid(next_state.x);
 53 |                 if(idx.isValid() && grid.getValue(idx))
 54 |                 {
 55 |                     safe = true;
 56 |                     break;
 57 |                 }
 58 |             }
 59 |         }
 60 |         if(!safe)
 61 |             return false;
 62 |     }
 63 |     return true;
 64 | }
 65 | 
 66 | bool Algo::isRobustSafe(const Model &model,const State &x,const Grid<bool> &grid) const
 67 | {
 68 |     Eigen::VectorXd kappa_vec = Eigen::VectorXd::LinSpaced(NGKAPPA,-road_.kappa_max,road_.kappa_max);
 69 | 
 70 |     std::vector<Input> allowed_inputs = model.possibleInputs(x.v);
 71 |     bool safe = false;
 72 |     for (auto allowed_input : allowed_inputs)
 73 |     {
 74 |         safe = true;
 75 |         for(int i = 0;i<NGKAPPA;i++)
 76 |         {
 77 |             ModelOutput next_state = model.nextState(x, allowed_input,kappa_vec(i));
 78 |             if(next_state.feasible)
 79 |             {
 80 |                 Index idx = grid.projectOntoGrid(next_state.x);
 81 |                 if(!idx.isValid()||!grid.getValue(idx))
 82 |                 {
 83 |                     safe = false;
 84 |                     break;
 85 |                 }
 86 |             }
 87 |         }
 88 |         if(safe)
 89 |             return true;
 90 |     }
 91 |     return false;
 92 | }
 93 | 
 94 | DiscReturn Algo::runDiscAlgo(const Model &model, Grid<bool> *grid) const
 95 | {
 96 |     initGrid(grid);
 97 |     std::cout << "Initial grid nnz: " << grid->nonZeros() << std::endl;
 98 |     int changes = 0;
 99 |     for(int it = 0; it < 200; it++)
100 |     {
101 |         std::cout << it << std::endl;
102 |         changes = 0;
103 |         for (int i = 0; i<NGD; i++)
104 |         {
105 |             for (int j = 0; j<NGMU; j++)
106 |             {
107 |                 for (int k = 0; k<NGV; k++)
108 |                 {
109 |                     if(grid->getValue({i,j,k}))
110 |                     {
111 |                         State x = grid->indexToState({i,j,k});
112 |                         if(!isDiscSafe(model_,x,*grid))
113 |                         {
114 |                             changes++;
115 |                             grid->setValue({i,j,k},false);
116 | 
117 |                         }
118 |                     }
119 | 
120 |                 }
121 |             }
122 |         }
123 |         std::cout << changes << std::endl;
124 | 
125 |         std::cout << "non zeros in grid: " << grid->nonZeros() << std::endl;
126 |         if(changes == 0)
127 |         {
128 |             return {true,it+1};
129 |         }
130 |     }
131 |     return {true,0};
132 | }
133 | 
134 | DiscReturn Algo::runRobustAlgo(const Model &model, Grid<bool> *grid) const
135 | {
136 |     initGrid(grid);
137 | 
138 |     for(int it = 0; it < 10; it++)
139 |     {
140 |         std::cout << it << std::endl;
141 |         int changes = 0;
142 |         for (int i = 0; i<NGD; i++)
143 |         {
144 |             for (int j = 0; j<NGMU; j++)
145 |             {
146 |                 for (int k = 0; k<NGV; k++)
147 |                 {
148 |                     if(grid->getValue({i,j,k}))
149 |                     {
150 |                         State x = grid->indexToState({i,j,k});
151 |                         if(!isRobustSafe(model_,x,*grid))
152 |                         {
153 |                             changes++;
154 |                             grid->setValue({i,j,k},false);
155 |                         }
156 |                     }
157 | 
158 |                 }
159 |             }
160 |         }
161 |         std::cout << changes << std::endl;
162 |         if(changes == 0)
163 |         {
164 |             return {true,it+1};
165 |         }
166 |     }
167 |     return {true,0};
168 | }


--------------------------------------------------------------------------------
/DiscKernelAlgo/Algo/algo.h:
--------------------------------------------------------------------------------
 1 | // Copyright 2020 Alexander Liniger
 2 | 
 3 | // Licensed under the Apache License, Version 2.0 (the "License");
 4 | // you may not use this file except in compliance with the License.
 5 | // You may obtain a copy of the License at
 6 | 
 7 | //     http://www.apache.org/licenses/LICENSE-2.0
 8 | 
 9 | // Unless required by applicable law or agreed to in writing, software
10 | // distributed under the License is distributed on an "AS IS" BASIS,
11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12 | // See the License for the specific language governing permissions and
13 | // limitations under the License.
14 | ///////////////////////////////////////////////////////////////////////////
15 | ///////////////////////////////////////////////////////////////////////////
16 | 
17 | #ifndef DISC_ALGO_H
18 | #define DISC_ALGO_H
19 | 
20 | #include "types.h"
21 | #include "Grid/grid.h"
22 | #include "Model/model.h"
23 | 
24 | 
25 | class Algo {
26 | public:
27 |     DiscReturn runDiscAlgo(const Model &model,Grid<bool> *grid) const;
28 |     DiscReturn runRobustAlgo(const Model &model,Grid<bool> *grid) const;
29 | private:
30 |     void initGrid(Grid<bool> *grid) const;
31 |     bool isDiscSafe(const Model &model,const State &x,const Grid<bool> &grid) const;
32 |     bool isRobustSafe(const Model &model,const State &x,const Grid<bool> &grid) const;
33 | 
34 |     Model model_;
35 |     CarData car_;
36 |     RoadData road_;
37 | };
38 | 
39 | 
40 | #endif //DISC_ALGO_H
41 | 


--------------------------------------------------------------------------------
/DiscKernelAlgo/CMakeLists.txt:
--------------------------------------------------------------------------------
 1 | cmake_minimum_required(VERSION 3.13)
 2 | project(DiscKernelAlgo)
 3 | 
 4 | set(CMAKE_CXX_STANDARD 14)
 5 | 
 6 | include_directories(.)
 7 | include_directories(External/Eigen/)
 8 | include_directories(External/Json/include)
 9 | 
10 | 
11 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -O3")
12 | 
13 | add_executable(DiscKernelAlgo
14 |         Model/model.cpp
15 |         Algo/algo.cpp
16 |         main.cpp
17 |         types.h)
18 | 


--------------------------------------------------------------------------------
/DiscKernelAlgo/Data/LoadKN.m:
--------------------------------------------------------------------------------
 1 | %% Copyright 2020 Alexander Liniger
 2 | 
 3 | %% Licensed under the Apache License, Version 2.0 (the "License");
 4 | %% you may not use this file except in compliance with the License.
 5 | %% You may obtain a copy of the License at
 6 | 
 7 | %%     http://www.apache.org/licenses/LICENSE-2.0
 8 | 
 9 | %% Unless required by applicable law or agreed to in writing, software
10 | %% distributed under the License is distributed on an "AS IS" BASIS,
11 | %% WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12 | %% See the License for the specific language governing permissions and
13 | %% limitations under the License.
14 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%/
15 | %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%/
16 | 
17 | clear
18 | close all
19 | clc
20 | %%
21 | fileID = fopen('Disc_Test-285.bin');
22 | KN_CM = fread(fileID,'bool');
23 | r_max = 285
24 | a_max = 1.6
25 | 
26 | % NGD = 201;
27 | % NGMU = 161;
28 | % NGVX = 270;
29 | % 
30 | NGD = 101;
31 | NGMU = 81;
32 | NGVX = 135;
33 | 
34 | 
35 | KN = zeros(NGD+2,NGMU+2,NGVX+2);
36 | 
37 | for k = 1:NGVX
38 |     for j = 1:NGMU
39 |         for i = 1:NGD
40 |             KN(i+1,j+1,k+1) = KN_CM(k + (j-1)*NGVX + (i-1)*NGVX*NGMU);
41 |         end
42 |     end
43 | end
44 | 
45 | %%
46 | d_start = -0.3415;
47 | d_end = 0.3415;
48 | d_diff = (d_end - d_start)/(NGD-1);
49 | d_vec = (d_start - d_diff):d_diff:(d_end + d_diff);
50 | 
51 | mu_start = -0.2;
52 | mu_end = 0.2;
53 | mu_diff = (mu_end - mu_start)/(NGMU-1);
54 | mu_vec = (mu_start - mu_diff):mu_diff:(mu_end + mu_diff);
55 | 
56 | v_start = 0;
57 | v_end = min(sqrt(kappa_max*a_max),35);
58 | v_diff = (v_end - v_start)/(NGVX-1);
59 | v_vec = (v_start - v_diff):v_diff:(v_end + v_diff);
60 | 
61 | [d1,mu1,vx1] = meshgrid(mu_vec,d_vec,v_vec);
62 | %%
63 | 
64 | figure(3)
65 | v = KN;
66 | p = patch(isosurface(d1,mu1,vx1,v,0));
67 | isonormals(d1,mu1,vx1,v,p)
68 | set(p,'FaceColor','y','EdgeColor','none');
69 | daspect([1,1,15])
70 | view(3); axis tight
71 | camlight 
72 | % lighting gouraud
73 | lighting phong;
74 | % axis on
75 | grid on
76 | xlim([-.25,0.25])
77 | ylim([-.4,0.4])
78 | zlim([0,v_end+1])
79 | xlabel('\mu [rad]','FontSize',12)
80 | ylabel('d [m]','FontSize',12)
81 | zlabel('v [m/s]','FontSize',12)
82 | 
83 | 
84 | 


--------------------------------------------------------------------------------
/DiscKernelAlgo/Grid/grid.h:
--------------------------------------------------------------------------------
  1 | // Copyright 2020 Alexander Liniger
  2 | 
  3 | // Licensed under the Apache License, Version 2.0 (the "License");
  4 | // you may not use this file except in compliance with the License.
  5 | // You may obtain a copy of the License at
  6 | 
  7 | //     http://www.apache.org/licenses/LICENSE-2.0
  8 | 
  9 | // Unless required by applicable law or agreed to in writing, software
 10 | // distributed under the License is distributed on an "AS IS" BASIS,
 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 12 | // See the License for the specific language governing permissions and
 13 | // limitations under the License.
 14 | ///////////////////////////////////////////////////////////////////////////
 15 | ///////////////////////////////////////////////////////////////////////////
 16 | 
 17 | #ifndef DISC_GRID_H
 18 | #define DISC_GRID_H
 19 | 
 20 | #include <types.h>
 21 | 
 22 | template <class T>
 23 | class Grid{
 24 | public:
 25 |     Grid(const RoadData road, const CarData &car)
 26 |     {
 27 |         start_d_ = -road.w_road + car.w_car;
 28 |         end_d_   =  road.w_road - car.w_car;
 29 |         diff_d_  = (end_d_-start_d_)/(double)(NGD-1);
 30 | 
 31 |         start_mu_ = -road.heading_max;
 32 |         end_mu_   =  road.heading_max;
 33 |         diff_mu_  = (end_mu_-start_mu_)/(double)(NGMU-1);
 34 | 
 35 |         start_v_ =  0.0;
 36 |         end_v_   =  road.velo_max; //or based on kappa_max
 37 |         diff_v_  = (end_v_-start_v_)/(double)(NGV-1);
 38 |     };
 39 | 
 40 |     ~Grid(){
 41 |         std::cout << "deleting grid\n";
 42 |     };
 43 | 
 44 |     Index projectOntoGrid(const State &x) const
 45 |     {
 46 |         int dIndex  = (int)round((x.d -start_d_) /diff_d_);
 47 |         int muIndex = (int)round((x.mu-start_mu_)/diff_mu_);
 48 |         int vIndex  = (int)round((x.v -start_v_) /diff_v_);
 49 | 
 50 |         return {dIndex,muIndex,vIndex};
 51 |     }
 52 | 
 53 |     T getValue(const Index &index) const
 54 |     {
 55 |         return grid[index.columnMajorIndex()];
 56 |     };
 57 | 
 58 |     void setValue(const Index &index,T value)
 59 |     {
 60 |         grid[index.columnMajorIndex()] = value;
 61 |     };
 62 | 
 63 |     State indexToState(const Index &ind) const
 64 |     {
 65 |         return {start_d_ +ind.d *diff_d_,
 66 |                 start_mu_+ind.mu*diff_mu_,
 67 |                 start_v_ +ind.v *diff_v_};
 68 |     }
 69 | 
 70 |     void saveGrid(const std::string &name,double kappa) const
 71 |     {
 72 |         std::string name_file = "Data/" + name + "-" + std::to_string((int)(1./kappa)) + ".bin";
 73 |         auto myfile = std::fstream(name_file, std::ios::out | std::ios::binary);
 74 |         myfile.write((char*)&grid[0], sizeof(T)*NGD*NGMU*NGV);
 75 |         myfile.close();
 76 |     }
 77 |     int nonZeros() const
 78 |     {
 79 |         int non_zeros = 0;
 80 |         for(int i=0;i < NGD*NGMU*NGV;i++){
 81 |             if(grid[i])
 82 |                 non_zeros++;
 83 |         }
 84 |         return non_zeros;
 85 |     }
 86 | private:
 87 |     std::unique_ptr<T[]> grid = std::unique_ptr<T[]>(new T[NGD*NGMU*NGV]);
 88 | 
 89 |     double diff_d_;
 90 |     double diff_mu_;
 91 |     double diff_v_;
 92 | 
 93 |     double start_d_;
 94 |     double start_mu_;
 95 |     double start_v_;
 96 | 
 97 |     double end_d_;
 98 |     double end_mu_;
 99 |     double end_v_;
100 | };
101 | 
102 | #endif //DISC_GRID_H
103 | 


--------------------------------------------------------------------------------
/DiscKernelAlgo/Model/model.cpp:
--------------------------------------------------------------------------------
 1 | // Copyright 2020 Alexander Liniger
 2 | 
 3 | // Licensed under the Apache License, Version 2.0 (the "License");
 4 | // you may not use this file except in compliance with the License.
 5 | // You may obtain a copy of the License at
 6 | 
 7 | //     http://www.apache.org/licenses/LICENSE-2.0
 8 | 
 9 | // Unless required by applicable law or agreed to in writing, software
10 | // distributed under the License is distributed on an "AS IS" BASIS,
11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12 | // See the License for the specific language governing permissions and
13 | // limitations under the License.
14 | ///////////////////////////////////////////////////////////////////////////
15 | ///////////////////////////////////////////////////////////////////////////
16 | 
17 | #include "model.h"
18 | 
19 | StateVector Model::dx(const StateVector &x_vec,const Input &u,double kappa) const
20 | {
21 |     StateVector dx;
22 | 
23 |     double d = x_vec(0);
24 |     double mu= x_vec(1);
25 |     double v = x_vec(2);
26 | 
27 |     dx(0) = v*std::sin(mu);
28 |     dx(1) = v*std::tan(u.delta)/car_.L - (v*std::cos(mu))/(1.0-d*kappa)*kappa;
29 |     dx(2) = u.a;
30 | 
31 |     return dx;
32 | }
33 | 
34 | State Model::RK4(const State &x,const Input &u,double kappa) const
35 | {
36 |     StateVector x_vec = x.toVector();
37 |     StateVector k1 = dx(x_vec,u,kappa);
38 |     StateVector k2 = dx(x_vec+TS/2.*k1,u,kappa);
39 |     StateVector k3 = dx(x_vec+TS/2.*k2,u,kappa);
40 |     StateVector k4 = dx(x_vec+TS*k3,u,kappa);
41 | 
42 |     x_vec = x_vec + TS*(k1/6.+k2/3.+k3/3.+k4/6.);
43 | 
44 |     return {x_vec(0),x_vec(1),x_vec(2)};
45 | }
46 | 
47 | ModelOutput Model::nextState(const State &x,const Input &u,double kappa) const
48 | {
49 | 
50 |     return {RK4(x,u,kappa),true};
51 | 
52 | 
53 | //    State next_state = x;
54 | //    for(int i = 0;i<5;i++)
55 | //    {
56 | //        next_state = RK4(next_state,u,kappa);
57 | //        if(!roadConstraints(next_state))
58 | //            return {next_state,false};
59 | //    }
60 | //    return {next_state,true};
61 | 
62 | 
63 | }
64 | 
65 | bool Model::roadConstraints(const State &x) const
66 | {
67 |     double tightening = car_.l_car*std::sin(std::fabs(x.mu)) + car_.w_car*std::cos(x.mu);
68 | 
69 |     return x.d + car_.L / 2.0 * std::sin(x.mu) <= road_.w_road - tightening &&
70 |            x.d + car_.L / 2.0 * std::sin(x.mu) >= -road_.w_road + tightening &&
71 |            x.mu >= -road_.heading_max &&
72 |            x.mu <= road_.heading_max;
73 | }
74 | 
75 | std::vector<Input> Model::possibleInputs(double v) const
76 | {
77 |     std::vector<Input> inputs;
78 | 
79 |     double delta_high = std::min(car_.delta_max,std::atan(car_.a_lat_max*car_.L/std::pow(v,2.0)));
80 |     double a_high = car_.a_long_max;
81 | 
82 |     Eigen::VectorXd a_vec;
83 |     Eigen::VectorXd delta_vec;
84 |     delta_vec.setLinSpaced(NGDELTA,-delta_high,delta_high);
85 |     a_vec.setLinSpaced(NGA,-a_high,a_high);
86 | 
87 |     for(int i=0;i<NGDELTA;i++)
88 |     {
89 |         for(int j=0;j<NGA;j++)
90 |         {
91 |             if(std::pow(std::pow(v,2.0)*std::tan(delta_vec(i))/car_.L,2.0) + std::pow(a_vec(j),2.0) <= std::pow(car_.a_lat_max,2.0))
92 |                 inputs.push_back({a_vec(j),delta_vec(i)});
93 |         }
94 |     }
95 | 
96 |     return inputs;
97 | }
98 | 


--------------------------------------------------------------------------------
/DiscKernelAlgo/Model/model.h:
--------------------------------------------------------------------------------
 1 | // Copyright 2020 Alexander Liniger
 2 | 
 3 | // Licensed under the Apache License, Version 2.0 (the "License");
 4 | // you may not use this file except in compliance with the License.
 5 | // You may obtain a copy of the License at
 6 | 
 7 | //     http://www.apache.org/licenses/LICENSE-2.0
 8 | 
 9 | // Unless required by applicable law or agreed to in writing, software
10 | // distributed under the License is distributed on an "AS IS" BASIS,
11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12 | // See the License for the specific language governing permissions and
13 | // limitations under the License.
14 | ///////////////////////////////////////////////////////////////////////////
15 | ///////////////////////////////////////////////////////////////////////////
16 | 
17 | #ifndef DISC_MODEL_H
18 | #define DISC_MODEL_H
19 | 
20 | #include "types.h"
21 | #include "Grid/grid.h"
22 | 
23 | 
24 | class Model {
25 | public:
26 |     ModelOutput nextState(const State &x,const Input &u,double kappa) const;
27 |     std::vector<Input> possibleInputs(double v) const;
28 | private:
29 | 
30 |     StateVector dx(const StateVector &x_vec,const Input &u,double kappa) const;
31 |     State RK4(const State &x,const Input &u,double kappa) const;
32 |     bool roadConstraints(const State &x) const;
33 | 
34 |     RoadData road_;
35 |     CarData car_;
36 | };
37 | 
38 | 
39 | #endif //DISC_MODEL_H
40 | 


--------------------------------------------------------------------------------
/DiscKernelAlgo/README.md:
--------------------------------------------------------------------------------
 1 | # Discriminating Kernel Algorithm
 2 | This is a C++ implementation of the discriminating kernel algorithm used to compute the safe set using the idea of an adversarial road as proposed in "Safe Motion Planning for Autonomous Driving using an Adversarial Road Model".
 3 | 
 4 | ## Installation 
 5 | 
 6 | To install all the dependencies run
 7 | ```
 8 | bash install.sh
 9 | ```
10 | this clones `nlohmann/json`, and `eigen`, from their git repo, and saves them in a folder External. 
11 | 
12 | Once all dependencies are installed `cmake` can be used to build the project
13 | ```
14 | cmake CMakeLists.txt
15 | make
16 | ```
17 | To run the code simply execute the `DiscKernelAlgo`
18 | ```
19 | ./DiscKernelAlgo
20 | ```
21 | 
22 | ## Running Code
23 | 
24 | ### Change Car Parameters
25 | 
26 | The parameters of the car can be easily changed in `car.json`, including the maximum curvature `kappa_max`.
27 | 
28 | ### Chang Grid Parameters
29 | 
30 | The number of grid points and the sampling time can be changed `types.h`. Note that the sampling time and the grid resolution are generally coupled, if the sampling time is too small relative to the grid spacing the results are not representative.
31 | 
32 | ### Generating Training Set
33 | 
34 | If the goal is to generate the training set used in the paper the script `run.sh` can be executed which automatically computes the discriminating kernel for all the maximum curvatures.
35 | 


--------------------------------------------------------------------------------
/DiscKernelAlgo/car.json:
--------------------------------------------------------------------------------
 1 | {
 2 |   "l_car": 2.26,
 3 |   "w_car": 0.9085,
 4 |   "L": 2.68,
 5 |   "a_lat_max": 1.6,
 6 |   "a_long_max": 1.6,
 7 |   "delta_max": 0.6,
 8 |   "w_road": 1.25,
 9 |   "heading_max": 0.2,
10 |   "velo_max": 35,
11 |   "kappa_max": 0.001
12 | }
13 | 


--------------------------------------------------------------------------------
/DiscKernelAlgo/install.sh:
--------------------------------------------------------------------------------
 1 | ## Copyright 2020 Alexander Liniger
 2 | 
 3 | ## Licensed under the Apache License, Version 2.0 (the "License");
 4 | ## you may not use this file except in compliance with the License.
 5 | ## You may obtain a copy of the License at
 6 | 
 7 | ##     http://www.apache.org/licenses/LICENSE-2.0
 8 | 
 9 | ## Unless required by applicable law or agreed to in writing, software
10 | ## distributed under the License is distributed on an "AS IS" BASIS,
11 | ## WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12 | ## See the License for the specific language governing permissions and
13 | ## limitations under the License.
14 | ###########################################################################
15 | ###########################################################################
16 | ## install dependincies
17 | 
18 | #!/bin/bash
19 | ## clone eigne
20 | repository_eigen="https://gitlab.com/libeigen/eigen.git"
21 | localFolder_eigen="External/Eigen"
22 | git clone "$repository_eigen" "$localFolder_eigen"
23 | ## clone json
24 | repository_json="https://github.com/nlohmann/json.git"
25 | localFolder_json="External/Json"
26 | git clone "$repository_json" "$localFolder_json"


--------------------------------------------------------------------------------
/DiscKernelAlgo/main.cpp:
--------------------------------------------------------------------------------
 1 | // Copyright 2020 Alexander Liniger
 2 | 
 3 | // Licensed under the Apache License, Version 2.0 (the "License");
 4 | // you may not use this file except in compliance with the License.
 5 | // You may obtain a copy of the License at
 6 | 
 7 | //     http://www.apache.org/licenses/LICENSE-2.0
 8 | 
 9 | // Unless required by applicable law or agreed to in writing, software
10 | // distributed under the License is distributed on an "AS IS" BASIS,
11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12 | // See the License for the specific language governing permissions and
13 | // limitations under the License.
14 | ///////////////////////////////////////////////////////////////////////////
15 | ///////////////////////////////////////////////////////////////////////////
16 | 
17 | #include "Algo/algo.h"
18 | 
19 | int main() {
20 | 
21 |     Model model;
22 |     RoadData road;
23 |     CarData car;
24 | 
25 |     std::cout << road.velo_max << std::endl;
26 | 
27 |     Grid<bool> grid(road,car);
28 | 
29 |     Algo algo;
30 |    // algo.runRobustAlgo(model,&grid);
31 |     algo.runDiscAlgo(model,&grid);
32 | 
33 |     grid.saveGrid("Disc",road.kappa_max);
34 |     // grid.saveGrid("Disc-Test",road.kappa_max);
35 | 
36 |     return 0;
37 | }
38 | 


--------------------------------------------------------------------------------
/DiscKernelAlgo/run.sh:
--------------------------------------------------------------------------------
 1 | ## Copyright 2020 Alexander Liniger
 2 | 
 3 | ## Licensed under the Apache License, Version 2.0 (the "License");
 4 | ## you may not use this file except in compliance with the License.
 5 | ## You may obtain a copy of the License at
 6 | 
 7 | ##     http://www.apache.org/licenses/LICENSE-2.0
 8 | 
 9 | ## Unless required by applicable law or agreed to in writing, software
10 | ## distributed under the License is distributed on an "AS IS" BASIS,
11 | ## WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12 | ## See the License for the specific language governing permissions and
13 | ## limitations under the License.
14 | ###########################################################################
15 | ###########################################################################
16 | #!/bin/bash
17 | for i in 0.1 0.05 0.04 0.03 0.02 0.01 0.005 0.004 0.003 0.002 0.0015 0.00125 0.001
18 | do
19 |   echo "Running Disc Kernel Algo for kappa_max = $i"
20 |   jq '.kappa_max = $newVal' --argjson newVal $i car.json > tmp.$$.json && mv tmp.$$.json car.json
21 |   ./DiscKernelAlgo
22 | done
23 | 


--------------------------------------------------------------------------------
/DiscKernelAlgo/types.cpp:
--------------------------------------------------------------------------------
 1 | // Copyright 2020 Alexander Liniger
 2 | 
 3 | // Licensed under the Apache License, Version 2.0 (the "License");
 4 | // you may not use this file except in compliance with the License.
 5 | // You may obtain a copy of the License at
 6 | 
 7 | //     http://www.apache.org/licenses/LICENSE-2.0
 8 | 
 9 | // Unless required by applicable law or agreed to in writing, software
10 | // distributed under the License is distributed on an "AS IS" BASIS,
11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12 | // See the License for the specific language governing permissions and
13 | // limitations under the License.
14 | ///////////////////////////////////////////////////////////////////////////
15 | ///////////////////////////////////////////////////////////////////////////
16 | 
17 | #include "types.h"
18 | 


--------------------------------------------------------------------------------
/DiscKernelAlgo/types.h:
--------------------------------------------------------------------------------
  1 | // Copyright 2020 Alexander Liniger
  2 | 
  3 | // Licensed under the Apache License, Version 2.0 (the "License");
  4 | // you may not use this file except in compliance with the License.
  5 | // You may obtain a copy of the License at
  6 | 
  7 | //     http://www.apache.org/licenses/LICENSE-2.0
  8 | 
  9 | // Unless required by applicable law or agreed to in writing, software
 10 | // distributed under the License is distributed on an "AS IS" BASIS,
 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 12 | // See the License for the specific language governing permissions and
 13 | // limitations under the License.
 14 | ///////////////////////////////////////////////////////////////////////////
 15 | ///////////////////////////////////////////////////////////////////////////
 16 | 
 17 | #ifndef DISC_TYPES_H
 18 | #define DISC_TYPES_H
 19 | 
 20 | #include <Eigen/Dense>
 21 | 
 22 | #include <iostream>
 23 | #include <fstream>
 24 | #include <nlohmann/json.hpp>
 25 | 
 26 | #include <vector>
 27 | #include <memory>
 28 | #include <string>
 29 | #include <algorithm>
 30 | 
 31 | #define NX 3
 32 | #define NU 2
 33 | 
 34 | #define TS 0.2
 35 | 
 36 | // training grid
 37 | #define NGD 101
 38 | #define NGMU 81
 39 | #define NGV 135
 40 | // test set grid
 41 | // #define NGD 201
 42 | // #define NGMU 161
 43 | // #define NGV 270
 44 | 
 45 | #define NGA 9
 46 | #define NGDELTA 9
 47 | 
 48 | #define NGKAPPA 5
 49 | 
 50 | typedef Eigen::Matrix<double,NX,1> StateVector;
 51 | typedef Eigen::Matrix<double,NU,1> InputVector;
 52 | 
 53 | struct State{
 54 |     double d;
 55 |     double mu;
 56 |     double v;
 57 | 
 58 |     StateVector toVector() const{
 59 |         StateVector xk;
 60 |         xk << d,mu,v;
 61 |         return xk;
 62 |     }
 63 | 
 64 |     void setZero(){
 65 |         d = 0;
 66 |         mu = 0;
 67 |         v = 0;
 68 |     };
 69 | 
 70 |     State(){
 71 |         d = 0;
 72 |         mu = 0;
 73 |         v = 0;
 74 |     }
 75 | 
 76 |     State(StateVector x){
 77 |         d = x(0);
 78 |         mu = x(1);
 79 |         v = x(2);
 80 |     }
 81 |     State(double d_in,double mu_in,double v_in):
 82 |     d(d_in),mu(mu_in),v(v_in) {}
 83 | 
 84 | };
 85 | 
 86 | struct Input{
 87 |     double a;
 88 |     double delta;
 89 | };
 90 | 
 91 | struct ModelOutput{
 92 |     const State x;
 93 |     const bool feasible;
 94 | };
 95 | 
 96 | struct CarData{
 97 |     double l_car;
 98 |     double w_car;
 99 |     double L;
100 |     double a_lat_max;
101 |     double a_long_max;
102 |     double delta_max;
103 | 
104 |     CarData()
105 |     {
106 |         using json = nlohmann::json;
107 |         std::ifstream iCar("car.json");
108 |         json jsonCar;
109 |         iCar >> jsonCar;
110 | 
111 |         l_car = jsonCar["l_car"];
112 |         w_car = jsonCar["w_car"];
113 |         L    = jsonCar["L"];
114 |         a_lat_max = jsonCar["a_lat_max"];
115 |         a_long_max = jsonCar["a_lat_max"];
116 |         delta_max = jsonCar["delta_max"];
117 |     }
118 | };
119 | 
120 | struct RoadData{
121 |     double w_road;
122 |     double heading_max;
123 |     double velo_max;
124 |     double kappa_max;
125 | 
126 |     RoadData()
127 |     {
128 |         using json = nlohmann::json;
129 |         std::ifstream iCar("car.json");
130 |         json jsonCar;
131 |         iCar >> jsonCar;
132 | 
133 |         w_road = jsonCar["w_road"];
134 |         heading_max = jsonCar["heading_max"];
135 |         velo_max = jsonCar["velo_max"];
136 |         kappa_max = jsonCar["kappa_max"];
137 | 
138 |         velo_max = std::min(std::sqrt((double)jsonCar["a_lat_max"]/kappa_max),velo_max);
139 |     }
140 | };
141 | 
142 | struct Index{
143 |     int d;
144 |     int mu;
145 |     int v;
146 | 
147 |     int columnMajorIndex() const{
148 |         return d + NGD*mu + NGD*NGMU*v;
149 |     }
150 | 
151 |     bool isValid() const {
152 |         if(d<0 || d>=NGD)
153 |             return false;
154 |         else if(mu<0 || mu>=NGMU)
155 |             return false;
156 |         else if(v<0 || v>=NGV)
157 |             return false;
158 |         else
159 |             return true;
160 |     }
161 | };
162 | 
163 | struct DiscReturn{
164 |     bool converged;
165 |     int n_interation;
166 | };
167 | 
168 | 
169 | 
170 | 
171 | #endif //DISC_TYPES_H
172 | 


--------------------------------------------------------------------------------
/Images/AdverseRoad.png:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/alexliniger/AdversarialRoadModel/14157760687c22acc8b91c39128875005ada7563/Images/AdverseRoad.png


--------------------------------------------------------------------------------
/Images/Disc285.png:
--------------------------------------------------------------------------------
https://raw.githubusercontent.com/alexliniger/AdversarialRoadModel/14157760687c22acc8b91c39128875005ada7563/Images/Disc285.png


--------------------------------------------------------------------------------
/LICENSE:
--------------------------------------------------------------------------------
  1 |                                  Apache License
  2 |                            Version 2.0, January 2004
  3 |                         http://www.apache.org/licenses/
  4 | 
  5 |    TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION
  6 | 
  7 |    1. Definitions.
  8 | 
  9 |       "License" shall mean the terms and conditions for use, reproduction,
 10 |       and distribution as defined by Sections 1 through 9 of this document.
 11 | 
 12 |       "Licensor" shall mean the copyright owner or entity authorized by
 13 |       the copyright owner that is granting the License.
 14 | 
 15 |       "Legal Entity" shall mean the union of the acting entity and all
 16 |       other entities that control, are controlled by, or are under common
 17 |       control with that entity. For the purposes of this definition,
 18 |       "control" means (i) the power, direct or indirect, to cause the
 19 |       direction or management of such entity, whether by contract or
 20 |       otherwise, or (ii) ownership of fifty percent (50%) or more of the
 21 |       outstanding shares, or (iii) beneficial ownership of such entity.
 22 | 
 23 |       "You" (or "Your") shall mean an individual or Legal Entity
 24 |       exercising permissions granted by this License.
 25 | 
 26 |       "Source" form shall mean the preferred form for making modifications,
 27 |       including but not limited to software source code, documentation
 28 |       source, and configuration files.
 29 | 
 30 |       "Object" form shall mean any form resulting from mechanical
 31 |       transformation or translation of a Source form, including but
 32 |       not limited to compiled object code, generated documentation,
 33 |       and conversions to other media types.
 34 | 
 35 |       "Work" shall mean the work of authorship, whether in Source or
 36 |       Object form, made available under the License, as indicated by a
 37 |       copyright notice that is included in or attached to the work
 38 |       (an example is provided in the Appendix below).
 39 | 
 40 |       "Derivative Works" shall mean any work, whether in Source or Object
 41 |       form, that is based on (or derived from) the Work and for which the
 42 |       editorial revisions, annotations, elaborations, or other modifications
 43 |       represent, as a whole, an original work of authorship. For the purposes
 44 |       of this License, Derivative Works shall not include works that remain
 45 |       separable from, or merely link (or bind by name) to the interfaces of,
 46 |       the Work and Derivative Works thereof.
 47 | 
 48 |       "Contribution" shall mean any work of authorship, including
 49 |       the original version of the Work and any modifications or additions
 50 |       to that Work or Derivative Works thereof, that is intentionally
 51 |       submitted to Licensor for inclusion in the Work by the copyright owner
 52 |       or by an individual or Legal Entity authorized to submit on behalf of
 53 |       the copyright owner. For the purposes of this definition, "submitted"
 54 |       means any form of electronic, verbal, or written communication sent
 55 |       to the Licensor or its representatives, including but not limited to
 56 |       communication on electronic mailing lists, source code control systems,
 57 |       and issue tracking systems that are managed by, or on behalf of, the
 58 |       Licensor for the purpose of discussing and improving the Work, but
 59 |       excluding communication that is conspicuously marked or otherwise
 60 |       designated in writing by the copyright owner as "Not a Contribution."
 61 | 
 62 |       "Contributor" shall mean Licensor and any individual or Legal Entity
 63 |       on behalf of whom a Contribution has been received by Licensor and
 64 |       subsequently incorporated within the Work.
 65 | 
 66 |    2. Grant of Copyright License. Subject to the terms and conditions of
 67 |       this License, each Contributor hereby grants to You a perpetual,
 68 |       worldwide, non-exclusive, no-charge, royalty-free, irrevocable
 69 |       copyright license to reproduce, prepare Derivative Works of,
 70 |       publicly display, publicly perform, sublicense, and distribute the
 71 |       Work and such Derivative Works in Source or Object form.
 72 | 
 73 |    3. Grant of Patent License. Subject to the terms and conditions of
 74 |       this License, each Contributor hereby grants to You a perpetual,
 75 |       worldwide, non-exclusive, no-charge, royalty-free, irrevocable
 76 |       (except as stated in this section) patent license to make, have made,
 77 |       use, offer to sell, sell, import, and otherwise transfer the Work,
 78 |       where such license applies only to those patent claims licensable
 79 |       by such Contributor that are necessarily infringed by their
 80 |       Contribution(s) alone or by combination of their Contribution(s)
 81 |       with the Work to which such Contribution(s) was submitted. If You
 82 |       institute patent litigation against any entity (including a
 83 |       cross-claim or counterclaim in a lawsuit) alleging that the Work
 84 |       or a Contribution incorporated within the Work constitutes direct
 85 |       or contributory patent infringement, then any patent licenses
 86 |       granted to You under this License for that Work shall terminate
 87 |       as of the date such litigation is filed.
 88 | 
 89 |    4. Redistribution. You may reproduce and distribute copies of the
 90 |       Work or Derivative Works thereof in any medium, with or without
 91 |       modifications, and in Source or Object form, provided that You
 92 |       meet the following conditions:
 93 | 
 94 |       (a) You must give any other recipients of the Work or
 95 |           Derivative Works a copy of this License; and
 96 | 
 97 |       (b) You must cause any modified files to carry prominent notices
 98 |           stating that You changed the files; and
 99 | 
100 |       (c) You must retain, in the Source form of any Derivative Works
101 |           that You distribute, all copyright, patent, trademark, and
102 |           attribution notices from the Source form of the Work,
103 |           excluding those notices that do not pertain to any part of
104 |           the Derivative Works; and
105 | 
106 |       (d) If the Work includes a "NOTICE" text file as part of its
107 |           distribution, then any Derivative Works that You distribute must
108 |           include a readable copy of the attribution notices contained
109 |           within such NOTICE file, excluding those notices that do not
110 |           pertain to any part of the Derivative Works, in at least one
111 |           of the following places: within a NOTICE text file distributed
112 |           as part of the Derivative Works; within the Source form or
113 |           documentation, if provided along with the Derivative Works; or,
114 |           within a display generated by the Derivative Works, if and
115 |           wherever such third-party notices normally appear. The contents
116 |           of the NOTICE file are for informational purposes only and
117 |           do not modify the License. You may add Your own attribution
118 |           notices within Derivative Works that You distribute, alongside
119 |           or as an addendum to the NOTICE text from the Work, provided
120 |           that such additional attribution notices cannot be construed
121 |           as modifying the License.
122 | 
123 |       You may add Your own copyright statement to Your modifications and
124 |       may provide additional or different license terms and conditions
125 |       for use, reproduction, or distribution of Your modifications, or
126 |       for any such Derivative Works as a whole, provided Your use,
127 |       reproduction, and distribution of the Work otherwise complies with
128 |       the conditions stated in this License.
129 | 
130 |    5. Submission of Contributions. Unless You explicitly state otherwise,
131 |       any Contribution intentionally submitted for inclusion in the Work
132 |       by You to the Licensor shall be under the terms and conditions of
133 |       this License, without any additional terms or conditions.
134 |       Notwithstanding the above, nothing herein shall supersede or modify
135 |       the terms of any separate license agreement you may have executed
136 |       with Licensor regarding such Contributions.
137 | 
138 |    6. Trademarks. This License does not grant permission to use the trade
139 |       names, trademarks, service marks, or product names of the Licensor,
140 |       except as required for reasonable and customary use in describing the
141 |       origin of the Work and reproducing the content of the NOTICE file.
142 | 
143 |    7. Disclaimer of Warranty. Unless required by applicable law or
144 |       agreed to in writing, Licensor provides the Work (and each
145 |       Contributor provides its Contributions) on an "AS IS" BASIS,
146 |       WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or
147 |       implied, including, without limitation, any warranties or conditions
148 |       of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A
149 |       PARTICULAR PURPOSE. You are solely responsible for determining the
150 |       appropriateness of using or redistributing the Work and assume any
151 |       risks associated with Your exercise of permissions under this License.
152 | 
153 |    8. Limitation of Liability. In no event and under no legal theory,
154 |       whether in tort (including negligence), contract, or otherwise,
155 |       unless required by applicable law (such as deliberate and grossly
156 |       negligent acts) or agreed to in writing, shall any Contributor be
157 |       liable to You for damages, including any direct, indirect, special,
158 |       incidental, or consequential damages of any character arising as a
159 |       result of this License or out of the use or inability to use the
160 |       Work (including but not limited to damages for loss of goodwill,
161 |       work stoppage, computer failure or malfunction, or any and all
162 |       other commercial damages or losses), even if such Contributor
163 |       has been advised of the possibility of such damages.
164 | 
165 |    9. Accepting Warranty or Additional Liability. While redistributing
166 |       the Work or Derivative Works thereof, You may choose to offer,
167 |       and charge a fee for, acceptance of support, warranty, indemnity,
168 |       or other liability obligations and/or rights consistent with this
169 |       License. However, in accepting such obligations, You may act only
170 |       on Your own behalf and on Your sole responsibility, not on behalf
171 |       of any other Contributor, and only if You agree to indemnify,
172 |       defend, and hold each Contributor harmless for any liability
173 |       incurred by, or claims asserted against, such Contributor by reason
174 |       of your accepting any such warranty or additional liability.
175 | 
176 |    END OF TERMS AND CONDITIONS
177 | 
178 |    APPENDIX: How to apply the Apache License to your work.
179 | 
180 |       To apply the Apache License to your work, attach the following
181 |       boilerplate notice, with the fields enclosed by brackets "[]"
182 |       replaced with your own identifying information. (Don't include
183 |       the brackets!)  The text should be enclosed in the appropriate
184 |       comment syntax for the file format. We also recommend that a
185 |       file or class name and description of purpose be included on the
186 |       same "printed page" as the copyright notice for easier
187 |       identification within third-party archives.
188 | 
189 |    Copyright [2020] [Alexander Liniger]
190 | 
191 |    Licensed under the Apache License, Version 2.0 (the "License");
192 |    you may not use this file except in compliance with the License.
193 |    You may obtain a copy of the License at
194 | 
195 |        http://www.apache.org/licenses/LICENSE-2.0
196 | 
197 |    Unless required by applicable law or agreed to in writing, software
198 |    distributed under the License is distributed on an "AS IS" BASIS,
199 |    WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
200 |    See the License for the specific language governing permissions and
201 |    limitations under the License.
202 | 


--------------------------------------------------------------------------------
/LearningSafeSets/DataLoader/DataLoader.py:
--------------------------------------------------------------------------------
  1 | ## Copyright 2020 Alexander Liniger
  2 | 
  3 | ## Licensed under the Apache License, Version 2.0 (the "License");
  4 | ## you may not use this file except in compliance with the License.
  5 | ## You may obtain a copy of the License at
  6 | 
  7 | ##     http://www.apache.org/licenses/LICENSE-2.0
  8 | 
  9 | ## Unless required by applicable law or agreed to in writing, software
 10 | ## distributed under the License is distributed on an "AS IS" BASIS,
 11 | ## WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 12 | ## See the License for the specific language governing permissions and
 13 | ## limitations under the License.
 14 | ###########################################################################
 15 | ###########################################################################
 16 | 
 17 | import numpy as np
 18 | import torch
 19 | 
 20 | 
 21 | class DataLoader:
 22 |     def __init__(self,config):
 23 |         self.path = config['path']
 24 | 
 25 |         self.n_d = config['NGD']
 26 |         self.n_mu = config['NGMU']
 27 |         self.n_v = config['NGV']
 28 |         self.n_kappa = config['NGKAPPA']
 29 | 
 30 |         self.n_batch = config['n_batch']
 31 |         self.train_val_ratio = config['train_val_ratio']
 32 | 
 33 |         self.n_state = config['n_states']
 34 |         self.n_points = self.n_d*self.n_mu*(self.n_v+1)*self.n_kappa
 35 |         self.n_train_batches = int(np.floor(self.train_val_ratio * self.n_points / self.n_batch))
 36 |         self.n_all_batches = int(np.floor(self.n_points/self.n_batch))
 37 | 
 38 |         self.middle = torch.zeros(self.n_state)
 39 |         self.half_range = torch.zeros(self.n_state)
 40 | 
 41 |         self.state = torch.zeros((0, self.n_state))
 42 |         self.safe = torch.zeros((0))
 43 | 
 44 |         np.random.seed(123)
 45 |         self.rand_index = np.random.choice(self.n_points, self.n_points, replace=False)
 46 | 
 47 |         # curvaturs in the training set
 48 |         curvature = np.array([0.1, 0.05, 0.04, 0.03, 0.02, 0.01, 0.005, 0.004, 0.003, 0.002, 0.0015, 0.00125, 0.001])
 49 | 
 50 |         for c in curvature:
 51 |             print(c)
 52 |             [state_r, safe_r] = self.loadData(1/c, c)
 53 |             self.state = torch.cat((self.state, state_r))
 54 |             self.safe = torch.cat((self.safe, safe_r))
 55 | 
 56 |         curvature = np.array([0.015, 0.0035])
 57 | 
 58 |         self.n_d_test = config['NGD_T']
 59 |         self.n_mu_test = config['NGMU_T']
 60 |         self.n_v_test = config['NGV_T']
 61 |         self.n_kappa_test = config['NGKAPPA_T']
 62 | 
 63 |         self.n_points_test = self.n_d_test * self.n_mu_test * self.n_v_test * self.n_kappa_test
 64 |         self.n_points_test_case = self.n_d_test * self.n_mu_test * self.n_v_test
 65 | 
 66 |         self.state_test = torch.zeros((0, self.n_state))
 67 |         self.safe_test = torch.zeros((0))
 68 | 
 69 |         for c in curvature:
 70 |             print(c)
 71 |             [state_r, safe_r] = self.loadDataTest(1 / c, c)
 72 |             self.state_test = torch.cat((self.state_test, state_r))
 73 |             self.safe_test = torch.cat((self.safe_test, safe_r))
 74 | 
 75 |         self.findNormalization()
 76 | 
 77 |     def loadData(self,radius,kappa):
 78 |         name = self.path + "/Disc-" + str(int(np.floor(radius))) + ".bin"
 79 |         value_fun_cm = torch.from_numpy(np.fromfile(name, dtype=np.bool, count=-1, sep="")).type(torch.FloatTensor)
 80 |         # stat values
 81 | 
 82 |         d = torch.linspace(-0.3415, 0.3415, self.n_d)
 83 |         mu = torch.linspace(-0.2, 0.2, self.n_mu)
 84 |         v = torch.linspace(0, min(np.sqrt(radius * 1.6),35), self.n_v)
 85 |         diff_v = v[2] - v[1]
 86 |         v_end = v[-1]
 87 |         v = np.append(v,  [v_end+diff_v])
 88 |         n_points_wo_kappa = self.n_d*self.n_mu*(self.n_v+1)
 89 |         # initialize data
 90 |         state = torch.zeros((n_points_wo_kappa, self.n_state))
 91 |         safe = torch.zeros((n_points_wo_kappa))
 92 | 
 93 |         # init running variables
 94 |         o = 0
 95 |         # transfer column major value function into points
 96 |         for i in range(self.n_d):
 97 |             for j in range(self.n_mu):
 98 |                 for k in range(self.n_v+1):
 99 | 
100 |                     if k >= self.n_v:
101 |                         value = 0
102 |                     else:
103 |                         value = value_fun_cm[i + self.n_d * j + self.n_d * self.n_mu * k]
104 | 
105 |                     state[o, :] = torch.tensor([d[i], mu[j], v[k], kappa])
106 |                     safe[o] = value
107 |                     o = o + 1
108 | 
109 | 
110 |         return [state, safe]
111 | 
112 |     def loadDataTest(self, radius, kappa):
113 |         name = self.path + "/Disc-Test-" + str(int(np.floor(radius))) + ".bin"
114 |         value_fun_cm = torch.from_numpy(np.fromfile(name, dtype=np.bool, count=-1, sep="")).type(torch.FloatTensor)
115 |         # stat values
116 | 
117 |         d = torch.linspace(-0.3415, 0.3415, self.n_d_test)
118 |         mu = torch.linspace(-0.2, 0.2, self.n_mu_test)
119 |         v = torch.linspace(0, min(np.sqrt(radius * 1.6), 35), self.n_v_test)
120 |         n_points_wo_kappa = self.n_d_test * self.n_mu_test * self.n_v_test
121 |         # initialize data
122 |         state = torch.zeros((n_points_wo_kappa, self.n_state))
123 |         safe = torch.zeros((n_points_wo_kappa))
124 | 
125 |         # init running variables
126 |         o = 0
127 |         # transfer column major value function into points
128 |         for i in range(self.n_d_test):
129 |             for j in range(self.n_mu_test):
130 |                 for k in range(self.n_v_test):
131 |                     value = value_fun_cm[i + self.n_d_test * j + self.n_d_test * self.n_mu_test * k]
132 |                     state[o, :] = torch.tensor([d[i], mu[j], v[k], kappa])
133 |                     safe[o] = value
134 |                     o = o + 1
135 | 
136 |         return [state, safe]
137 | 
138 |     def findNormalization(self):
139 |         max_state = np.max(self.state.numpy(), 0)
140 |         min_state = np.min(self.state.numpy(), 0)
141 |         self.middle = torch.from_numpy(min_state + (max_state - min_state) / 2)
142 |         self.half_range = torch.from_numpy((max_state - min_state) / 2)
143 | 
144 |     def normalize(self,state):
145 |         return (state - self.middle)/self.half_range
146 | 
147 |     def denomralize(self,state):
148 |         return state*self.half_range + self.middle
149 | 
150 |     def setBatchSize(self,n_batch,train_val_ratio):
151 |         self.n_batch = n_batch
152 |         self.train_val_ratio = train_val_ratio
153 |         self.n_train_batches = int(np.floor(self.train_val_ratio * self.n_points / self.n_batch))
154 |         self.n_all_batches = int(np.floor(self.n_points / self.n_batch))
155 | 
156 |     def giveBatch(self, i):
157 |         batch_index = range(i * self.n_batch, (i + 1) * self.n_batch)
158 |         rand_batch_index = self.rand_index[batch_index]
159 |         return self.normalize(self.state[rand_batch_index, :]),self.safe[rand_batch_index]
160 | 
161 |     def giveTest(self, i):
162 |         batch_index = range(i * self.n_points_test_case, (i + 1) * self.n_points_test_case)
163 |         return self.normalize(self.state_test[batch_index, :]),self.safe_test[batch_index]
164 | 
165 |     def shuffleTrainSet(self):
166 |         train_data_points = self.rand_index[range(self.n_train_batches*self.n_batch)]
167 |         np.random.shuffle(train_data_points)
168 |         self.rand_index[range(self.n_train_batches*self.n_batch)] = train_data_points
169 | 
170 | 


--------------------------------------------------------------------------------
/LearningSafeSets/Model/SafeSet.py:
--------------------------------------------------------------------------------
  1 | ## Copyright 2020 Alexander Liniger
  2 | 
  3 | ## Licensed under the Apache License, Version 2.0 (the "License");
  4 | ## you may not use this file except in compliance with the License.
  5 | ## You may obtain a copy of the License at
  6 | 
  7 | ##     http://www.apache.org/licenses/LICENSE-2.0
  8 | 
  9 | ## Unless required by applicable law or agreed to in writing, software
 10 | ## distributed under the License is distributed on an "AS IS" BASIS,
 11 | ## WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 12 | ## See the License for the specific language governing permissions and
 13 | ## limitations under the License.
 14 | ###########################################################################
 15 | ###########################################################################
 16 | 
 17 | import torch
 18 | import torch.nn as nn
 19 | import torch.nn.functional as F
 20 | 
 21 | 
 22 | class SafeSet(nn.Module):
 23 |     def __init__(self,config):
 24 |         super(SafeSet, self).__init__()
 25 | 
 26 |         self.n_neurons = config['n_neurons']
 27 | 
 28 |         self.n_batch = config['n_batch']
 29 |         self.n_inputs = config['n_states']
 30 | 
 31 | 
 32 |         if config['activation'] == "Softplus":
 33 |             self.model = nn.Sequential(
 34 |                 nn.Linear(self.n_inputs, self.n_neurons),
 35 |                 nn.Softplus(beta=1),
 36 |                 nn.Linear(self.n_neurons, self.n_neurons),
 37 |                 nn.Softplus(beta=1),
 38 |                 nn.Linear(self.n_neurons, self.n_neurons),
 39 |                 nn.Softplus(beta=1),
 40 |                 nn.Linear(self.n_neurons, 1),
 41 |                 nn.Sigmoid()
 42 |             )
 43 |         elif config['activation'] == "Tanh":
 44 | 
 45 |             self.model = nn.Sequential(
 46 |                 nn.Linear(self.n_inputs, self.n_neurons),
 47 |                 nn.Tanh(),
 48 |                 nn.Linear(self.n_neurons, self.n_neurons),
 49 |                 nn.Tanh(),
 50 |                 nn.Linear(self.n_neurons, self.n_neurons),
 51 |                 nn.Tanh(),
 52 |                 nn.Linear(self.n_neurons, 1),
 53 |                 nn.Sigmoid()
 54 |             )
 55 |         elif config['activation'] == "ReLU":
 56 |             self.model = nn.Sequential(
 57 |                 nn.Linear(self.n_inputs, self.n_neurons),
 58 |                 nn.ReLU(),
 59 |                 nn.Linear(self.n_neurons, self.n_neurons),
 60 |                 nn.ReLU(),
 61 |                 nn.Linear(self.n_neurons, self.n_neurons),
 62 |                 nn.ReLU(),
 63 |                 nn.Linear(self.n_neurons, 1),
 64 |                 nn.Sigmoid()
 65 |             )
 66 |         elif config['activation'] == "ELU":
 67 |             self.model = nn.Sequential(
 68 |                 nn.Linear(self.n_inputs, self.n_neurons),
 69 |                 nn.ELU(),
 70 |                 nn.Linear(self.n_neurons, self.n_neurons),
 71 |                 nn.ELU(),
 72 |                 nn.Linear(self.n_neurons, self.n_neurons),
 73 |                 nn.Dropout(0.5),
 74 |                 nn.ELU(),
 75 |                 nn.Linear(self.n_neurons, 1),
 76 |                 nn.Sigmoid()
 77 |             )
 78 |         elif config['activation'] == "ELU2":
 79 |             self.model = nn.Sequential(
 80 |                 nn.Linear(self.n_inputs, self.n_neurons),
 81 |                 nn.ELU(),
 82 |                 nn.Linear(self.n_neurons, self.n_neurons),
 83 |                 nn.ELU(),
 84 |                 nn.Linear(self.n_neurons, 1),
 85 |                 nn.Sigmoid()
 86 |             )
 87 |         elif config['activation'] == "ELU6":
 88 |             self.model = nn.Sequential(
 89 |                 nn.Linear(self.n_inputs, self.n_neurons),
 90 |                 nn.ELU(),
 91 |                 nn.Linear(self.n_neurons, self.n_neurons),
 92 |                 nn.ELU(),
 93 |                 nn.Linear(self.n_neurons, self.n_neurons),
 94 |                 nn.ELU(),
 95 |                 nn.Linear(self.n_neurons, self.n_neurons),
 96 |                 nn.ELU(),
 97 |                 nn.Linear(self.n_neurons, self.n_neurons),
 98 |                 nn.ELU(),
 99 |                 nn.Linear(self.n_neurons, self.n_neurons),
100 |                 nn.ELU(),
101 |                 nn.Linear(self.n_neurons, 1),
102 |                 nn.Sigmoid()
103 |             )
104 |         else:
105 |             self.model = nn.Sequential(
106 |                 nn.Linear(self.n_inputs, self.n_neurons),
107 |                 nn.Linear(self.n_neurons, 1),
108 |                 nn.Sigmoid()
109 |             )
110 | 
111 |     def forward(self, input):
112 |         return self.model(input)


--------------------------------------------------------------------------------
/LearningSafeSets/Trainer/Trainer.py:
--------------------------------------------------------------------------------
 1 | ## Copyright 2020 Alexander Liniger
 2 | 
 3 | ## Licensed under the Apache License, Version 2.0 (the "License");
 4 | ## you may not use this file except in compliance with the License.
 5 | ## You may obtain a copy of the License at
 6 | 
 7 | ##     http://www.apache.org/licenses/LICENSE-2.0
 8 | 
 9 | ## Unless required by applicable law or agreed to in writing, software
10 | ## distributed under the License is distributed on an "AS IS" BASIS,
11 | ## WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12 | ## See the License for the specific language governing permissions and
13 | ## limitations under the License.
14 | ###########################################################################
15 | ###########################################################################
16 | 
17 | import torch
18 | import torch.nn as nn
19 | import torch.nn.functional as F
20 | import torch.optim as optim
21 | import numpy as np
22 | from Validation import Validation
23 | 
24 | 
25 | class Trainer:
26 |     def __init__(self,config):
27 | 
28 |         self.n_epochs = config['epochs']
29 |         self.learning_rate = config['lr']
30 | 
31 |         self.validation = Validation.Validation(config)
32 | 
33 | 
34 |     def train(self,model,data):
35 | 
36 |         criterion = nn.BCELoss()
37 | 
38 |         metric_mse = nn.MSELoss()
39 |         optimizer = optim.Adam(model.parameters(), self.learning_rate)
40 |         scheduler = optim.lr_scheduler.StepLR(optimizer, step_size=3, gamma=0.1)
41 |         model.train()
42 | 
43 |         avg_loss = 0
44 |         avg_acc = 0
45 |         for epoch in range(self.n_epochs):
46 |             data.shuffleTrainSet()
47 |             for i in range(data.n_train_batches):
48 |                 state, safe = data.giveBatch(i)
49 |                 model.zero_grad()
50 | 
51 |                 safe_model = model(state)
52 | 
53 |                 loss = criterion(safe_model.view(-1), safe)
54 | 
55 |                 loss.backward()
56 |                 optimizer.step()
57 | 
58 |                 metric = metric_mse(safe_model.view(-1), safe).item()
59 |                 safe_model_max = (safe_model.view(-1).detach() >= 0.5).type(torch.FloatTensor)
60 |                 acc = (safe_model_max == safe).sum().item()/data.n_batch
61 | 
62 |                 running_loss = loss.mean().item()
63 |                 l_filter = 0.01
64 |                 avg_loss = (1 - l_filter) * avg_loss + l_filter * running_loss
65 |                 avg_acc =  (1 - l_filter) * avg_acc + l_filter * acc
66 | 
67 | 
68 |                 if i % 50 == 0:
69 |                     print('[%d/%d][%d/%d] \tLoss: %.4f AvgLoss: %.4f, MSE: %.4f, Acc: %.4f, Avg_Acc: %.4f'
70 |                           % (epoch + 1, self.n_epochs, i+1, data.n_train_batches, running_loss, avg_loss,metric,acc,avg_acc))
71 | 
72 | 
73 |             scheduler.step()
74 |             print(optimizer.param_groups[0]['lr'])
75 |             # self.validation.validateTest(model,data)


--------------------------------------------------------------------------------
/LearningSafeSets/Validation/Validation.py:
--------------------------------------------------------------------------------
  1 | ## Copyright 2020 Alexander Liniger
  2 | 
  3 | ## Licensed under the Apache License, Version 2.0 (the "License");
  4 | ## you may not use this file except in compliance with the License.
  5 | ## You may obtain a copy of the License at
  6 | 
  7 | ##     http://www.apache.org/licenses/LICENSE-2.0
  8 | 
  9 | ## Unless required by applicable law or agreed to in writing, software
 10 | ## distributed under the License is distributed on an "AS IS" BASIS,
 11 | ## WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 12 | ## See the License for the specific language governing permissions and
 13 | ## limitations under the License.
 14 | ###########################################################################
 15 | ###########################################################################
 16 | 
 17 | import torch
 18 | import torch.nn as nn
 19 | import torch.nn.functional as F
 20 | import numpy as np
 21 | import json
 22 | 
 23 | 
 24 | class Validation:
 25 |     def __init__(self,config):
 26 |         self.config = config
 27 |         self.cut_off = config["cut_off"]
 28 |         self.data = {}
 29 |         self.result_dir = config["result_dir"]
 30 | 
 31 |     def validate(self,model,data):
 32 |         # model.to(device)
 33 |         criterion_bce = nn.BCELoss()
 34 |         # criterion = nn.BCEWithLogitsLoss()
 35 |         criterion_mse = nn.MSELoss()
 36 |         model.eval()
 37 | 
 38 |         correct = 0
 39 |         # false_safe = 0
 40 |         under_approx = 0
 41 |         over_approx = 0
 42 |         total = 0
 43 |         metric_mse = []
 44 |         metric_bce = []
 45 | 
 46 |         for i in range(data.n_all_batches): 
 47 |             state, safe = data.giveBatch(i)
 48 | 
 49 |             safe_model = model(state).view(-1)
 50 |             safe_model_max = (safe_model >= self.cut_off).type(torch.FloatTensor)
 51 | 
 52 |             metric_mse.append(criterion_mse(safe_model, safe).item())
 53 |             metric_bce.append(criterion_bce(safe_model, safe).item())
 54 |             total += safe.size(0)
 55 | 
 56 |             correct += (safe_model_max == safe).sum().item()
 57 |             under_approx += (safe_model_max < safe).sum().item()
 58 |             over_approx += (safe_model_max > safe).sum().item()
 59 | 
 60 |         print('\tMSE: %.4f, BCE: %.4f, Acc: %.4f, UnderApprox: %.4f, OverApprox: %.4f'
 61 |               % (np.mean(metric_mse), np.mean(metric_bce), correct / total, under_approx / total, over_approx / total))
 62 | 
 63 |         self.data['full_set'] = []
 64 |         self.data['full_set'].append({
 65 |             'acc': correct / total,
 66 |             'under': under_approx / total,
 67 |             'over': over_approx / total,
 68 |             'total': total,
 69 |             'correct': correct,
 70 |             'mse': np.mean(metric_mse),
 71 |             'bce': np.mean(metric_bce)
 72 |         })
 73 | 
 74 | 
 75 |     def validateTest(self,model,data):
 76 | 
 77 |         criterion_bce = nn.BCELoss()
 78 |         criterion_mse = nn.MSELoss()
 79 |         model.eval()
 80 | 
 81 |         correct = 0
 82 |         # false_safe = 0
 83 |         under_approx = 0
 84 |         over_approx = 0
 85 |         total = 0
 86 |         metric_mse = []
 87 |         metric_bce = []
 88 | 
 89 |         for i in range(data.n_train_batches,data.n_all_batches): 
 90 |             state, safe = data.giveBatch(i)
 91 | 
 92 |             safe_model = model(state).view(-1)
 93 |             safe_model_max = (safe_model >= self.cut_off).type(torch.FloatTensor)
 94 | 
 95 |             metric_mse.append(criterion_mse(safe_model, safe).item())
 96 |             metric_bce.append(criterion_bce(safe_model, safe).item())
 97 |             total += safe.size(0)
 98 |   
 99 |             correct += (safe_model_max == safe).sum().item()
100 |             under_approx += (safe_model_max < safe).sum().item()
101 |             over_approx += (safe_model_max > safe).sum().item()
102 | 
103 |         print('\tMSE: %.4f, BCE: %.4f, Acc: %.4f, UnderApprox: %.4f, OverApprox: %.4f'
104 |               % (np.mean(metric_mse), np.mean(metric_bce), correct / total, under_approx / total, over_approx / total))
105 | 
106 |         self.data['val_set'] = []
107 |         self.data['val_set'].append({
108 |             'acc': correct / total,
109 |             'under': under_approx / total,
110 |             'over': over_approx / total,
111 |             'total': total,
112 |             'correct': correct,
113 |             'mse': np.mean(metric_mse),
114 |             'bce': np.mean(metric_bce)
115 |         })
116 | 
117 |     def validateTestUnseen(self,model,data):
118 | 
119 |         criterion_bce = nn.BCELoss()
120 |         criterion_mse = nn.MSELoss()
121 |         model.eval()
122 | 
123 |         correct = 0
124 |         false_safe = 0
125 |         under_approx = 0
126 |         over_approx = 0
127 |         total = 0
128 |         metric_mse = []
129 |         metric_bce = []
130 | 
131 |         for i in range(self.config['NGKAPPA_T']):
132 |             state, safe = data.giveTest(i)
133 | 
134 |             safe_model = model(state).view(-1)
135 |             safe_model_max = (safe_model >= self.cut_off).type(torch.FloatTensor)
136 | 
137 |             metric_mse.append(criterion_mse(safe_model, safe).item())
138 |             metric_bce.append(criterion_bce(safe_model, safe).item())
139 |             total += safe.size(0)
140 | 
141 |             correct += (safe_model_max == safe).sum().item()
142 |             under_approx += (safe_model_max < safe).sum().item()
143 |             over_approx += (safe_model_max > safe).sum().item()
144 | 
145 |             name = self.result_dir+"/RobustInv-Pred-"+str(i)+".bin"
146 |             fh = open(name, "bw")
147 |             safe_model_max.detach().numpy().astype(bool).tofile(fh)
148 | 
149 | 
150 |         print('\tMSE: %.4f, BCE: %.4f, Acc: %.4f, UnderApprox: %.4f, OverApprox: %.4f'
151 |                 % (np.mean(metric_mse), np.mean(metric_bce),correct/total,under_approx/total,over_approx/total))
152 | 
153 |         self.data['test_set'] = []
154 |         self.data['test_set'].append({
155 |             'acc': correct / total,
156 |             'under': under_approx / total,
157 |             'over': over_approx / total,
158 |             'total': total,
159 |             'correct': correct,
160 |             'mse': np.mean(metric_mse),
161 |             'bce': np.mean(metric_bce)
162 |         })
163 | 
164 |     def save_val(self):
165 |         with open(self.result_dir + '/val.txt', 'w') as outfile:
166 |             json.dump(self.data, outfile, indent=4)
167 | 
168 |     def save_model(self,model):
169 |         model_dict = {}
170 |         k = 0
171 |         for i in range(len(model._modules['model']._modules)):
172 |             if len(model._modules['model']._modules[str(i)]._parameters) > 0:
173 |                 W = model._modules['model']._modules[str(i)]._parameters['weight'].data.detach().numpy().tolist()
174 |                 b = model._modules['model']._modules[str(i)]._parameters['bias'].data.detach().numpy().tolist()
175 |                 model_dict[str(k)] = []
176 |                 model_dict[str(k)].append({
177 |                     'W': W,
178 |                     'b': b })
179 |                 k+=1
180 | 
181 |         model_dict["length"] = k
182 | 
183 |         with open(self.result_dir+'/model.txt', 'w') as outfile:
184 |             json.dump(model_dict, outfile, indent=4)
185 | 


--------------------------------------------------------------------------------
/LearningSafeSets/config.json:
--------------------------------------------------------------------------------
 1 | {
 2 |   "path": "Data",
 3 |   "seed": 1,
 4 |   "NGD":101,
 5 |   "NGMU": 81,
 6 |   "NGV": 135,
 7 |   "NGKAPPA": 13,
 8 | 
 9 |   "NGD_T":201,
10 |   "NGMU_T": 161,
11 |   "NGV_T": 270,
12 |   "NGKAPPA_T": 2,
13 | 
14 | 
15 |   "n_batch": 1500,
16 |   "train_val_ratio": 0.95,
17 |   "n_neurons": 16,
18 |   "n_states": 4,
19 | 
20 |   "epochs": 9,
21 |   "lr": 0.01,
22 | 
23 |   "cut_off": 0.25,
24 | 
25 |   "load_data": false,
26 |   "load_model": false,
27 |   "activation": "ELU"
28 | }


--------------------------------------------------------------------------------
/LearningSafeSets/run.sh:
--------------------------------------------------------------------------------
 1 | #!/usr/bin/env bash
 2 | 
 3 | min=$(date +%Y%m%d_%H%M%S)
 4 | 
 5 | save_dir="Data/$min"
 6 | mkdir -p $save_dir
 7 | 
 8 | code_dir="$save_dir/code"
 9 | rm -rf $code_dir
10 | mkdir -p $code_dir
11 | rsync -av --progress . $code_dir --exclude Data --exclude .idea/
12 | 
13 | result_dir="$save_dir/results"
14 | rm -rf $result_dir
15 | mkdir -p $result_dir
16 | 
17 | python train.py --path $result_dir


--------------------------------------------------------------------------------
/LearningSafeSets/train.py:
--------------------------------------------------------------------------------
 1 | ## Copyright 2020 Alexander Liniger
 2 | 
 3 | ## Licensed under the Apache License, Version 2.0 (the "License");
 4 | ## you may not use this file except in compliance with the License.
 5 | ## You may obtain a copy of the License at
 6 | 
 7 | ##     http://www.apache.org/licenses/LICENSE-2.0
 8 | 
 9 | ## Unless required by applicable law or agreed to in writing, software
10 | ## distributed under the License is distributed on an "AS IS" BASIS,
11 | ## WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12 | ## See the License for the specific language governing permissions and
13 | ## limitations under the License.
14 | ###########################################################################
15 | ###########################################################################
16 | 
17 | import json
18 | import argparse
19 | 
20 | from DataLoader import DataLoader
21 | from Model import SafeSet
22 | from Trainer import Trainer
23 | from Validation import Validation
24 | 
25 | import torch
26 | import matplotlib.pyplot as plt
27 | import random
28 | import numpy as np
29 | 
30 | def main(config,args):
31 | 
32 |     config["result_dir"] = args.path
33 |     print(config["result_dir"])
34 |     torch.manual_seed(config["seed"])
35 |     random.seed(config["seed"])
36 |     np.random.seed(config["seed"])
37 |     path_data = config['path'] + "/safe_set_data.pth"
38 |     if config['load_data']:
39 |         data_loader = torch.load(path_data)
40 |         data_loader.setBatchSize(config['n_batch'],config['train_val_ratio'])
41 |     else:
42 |         data_loader = DataLoader.DataLoader(config)
43 |         torch.save(data_loader, path_data)
44 | 
45 |     print(data_loader.n_all_batches)
46 |     model = SafeSet.SafeSet(config)
47 | 
48 |     path_model = config['path'] + "/safe_set_model.pth"
49 |     path_model_results = config["result_dir"] + "/safe_set_model.pth"
50 |     if not config['load_model']:
51 |         trainer = Trainer.Trainer(config)
52 |         trainer.train(model,data_loader)
53 | 
54 |         torch.save(model, path_model)
55 |         torch.save(model, path_model_results)
56 |     else:
57 |         model = torch.load(path_model)
58 | 
59 | 
60 |     validation = Validation.Validation(config)
61 |     validation.validate(model,data_loader)
62 |     validation.validateTest(model, data_loader)
63 |     validation.validateTestUnseen(model, data_loader)
64 |     validation.save_val()
65 |     validation.save_model(model)
66 | 
67 | 
68 | 
69 | if __name__ == '__main__':
70 |     parser = argparse.ArgumentParser(description='PyTorch Template')
71 |     parser.add_argument('-c', '--config', default=None, type=str,
72 |                         help='config file path (default: None)')
73 |     parser.add_argument('-p', '--path', default=Data, type=str,
74 |                         help='run save directory path (default: None)')
75 | 
76 |     args = parser.parse_args()
77 |     print(args)
78 |     config = json.load(open('config.json'))
79 |     main(config, args)


--------------------------------------------------------------------------------
/README.md:
--------------------------------------------------------------------------------
 1 | # AdversarialRoadModel
 2 | Discriminating kernel algorithm implementation and neural network approximation, as presented in "Safe Motion Planning for Autonomous Driving using an Adversarial Road Model"
 3 | 
 4 | ## Idea
 5 | In our RSS 2020 paper "Safe Motion Planning for Autonomous Driving using an Adversarial Road Model" we show how we can establish a safe predictive path following controller. For this task one big issue is what happens after our prediction horizon since we can plan a trajectory that remains within the constraints until the end of the horizon but depending on how the road evolves, we may not be able to stay within the road indefinitely. For example, if there is a sharp turn directly after the prediction horizon, we may not have enough time to react once we see the turn. 
 6 | 
 7 | In this paper, we propose to treat the road after the prediction horizon as an adversarial player, and we establish safe sets, for which we can show that whatever the adversarial road looks like we can follow the road while staying safely inside. 
 8 | 
 9 | <p align="center">
10 | <img src="https://github.com/alexliniger/AdversarialRoadModel/blob/master/Images/AdverseRoad.png" width="700" />
11 | </p>
12 | 
13 | We formalize this idea by using a kinematic bicycle model in curvilinear coordinates and considering the road curvature as an adversarial player
14 | 
15 | ## Discriminating Kernel
16 | 
17 | In this setting, the largest safe set is equivalent to the discriminating kernel or the maximum robust control invariant set (depending on the feedback structure of the game). In our paper we focused on the discriminating kernel as it results in a larger set, using the reasonable assumption that the adversarial player plays first, which boils down to the assumption that curvature of the road can be observed.
18 | 
19 | ## Code
20 | 
21 | In this repo, you can find a C++ implementation that computes the discriminating kernel using a discrete space discriminating kernel algorithm as presented in the paper. But you can also compute the robust control invariant set. Additionally we also included a PyTorch implementation that learns the safe set from the several discrete space discriminating kernels computed for different "maximum curvatures". The resulting network is able to interpolate over a large range of "maximum curvatures" using a small multi-layer perceptron that can be included in an MPC as a terminal constraint.
22 | 
23 | <p align="center">
24 | <img src="https://github.com/alexliniger/AdversarialRoadModel/blob/master/Images/Disc285.png" width="700" class="center" />
25 | </p>
26 | 
27 | Visualization of the discriminating kernel, left neural network apporximiation, and right discrete space discriminating kernel, for a test set example where the "maximum curvatures" is 0.0035 1/m.
28 | 


--------------------------------------------------------------------------------