README.md

@@ -1,63 +1,28 @@
-**Status:** This repository is archived. For latest work about discrete-time CBF, please refer to the [collection repository](https://github.com/HybridRobotics/NMPC-DCLF-DCBF).
-
 # MPC-CBF
-We propose a control framework which unifies the model predictive control and control barrier functions, where terminal cost function serves as control Lyapunov functions for stability. This is the reference implementation of our paper:
+We propose a control framework which unifies the model predictive control and control barrier functions. This is the reference implementation of our paper:
 ### Safety-Critical Model Predictive Control with Discrete-Time Control Barrier Function
-[PDF](https://arxiv.org/abs/2007.11718) | [Code: Double Integratror](examples) | [Code: Car Racing](https://github.com/HybridRobotics/Car-Racing)
+[PDF](https://arxiv.org/abs/2007.11718) | [Code: Double Integratror](double-integrator-2D) | [Code: Car Racing](car-racing)
 
 *Jun Zeng, Bike Zhang and Koushil Sreenath*
 
+<img src="car-racing/demo.gif" height="200"/>
+
 #### Citing
-If you find this project useful in your work, please consider citing following work:
-```
-@inproceedings{zeng2021mpccbf,
+If you find this code useful in your work, please consider citing:
+```shell
+@article{zeng2020mpccbf,
   title={Safety-critical model predictive control with discrete-time control barrier function},
   author={Zeng, Jun and Zhang, Bike and Sreenath, Koushil},
-  booktitle={2021 American Control Conference (ACC)},
-  year={2021},
-  volume={},
-  number={},
-  pages={3882-3889}
+  journal={arXiv preprint arXiv:2007.11718},
+  year={2020}
 }
 ```
 
-For analysis of feasibility, safety and computational complexity, please check out the following paper:
-```
-@inproceedings{zeng2021mpccbf-feasibility,
-  title={Enhancing feasibility and safety of nonlinear model predictive control with discrete-time control barrier functions},
-  author={Zeng, Jun and Li, Zhongyu and Sreenath, Koushil},
-  booktitle={2021 Conference on Decision and Control (CDC)},
-  year={2021},
-  volume={},
-  number={},
-  pages={6137-6144}
-}
-``` 
+### Instructions
+Two independent markdown files are written to illustrate numerical examples of [double integrator](double-integrator-2D/README.md) and [racing car](car-racing/README.md).
 
-#### Instructions
-The 2D double integrator is assigned to reach the target position at origin while avoiding obstacles. We have three classes for different controllers: `DCLFDCBF.m` (DCLF-DCBF), `MPCCBF.m` (MPC-CBF) and `MPCDC` (MPC-DC), respectively.
-
-Moreover, to illustrate the performance among them, we have:
-* `test.m`: Run DCLF-DCBF/MPC-CBF/MPC-DC respectively.
-* `testGamma.m`: Run analysis for different hyperparameter $\gamma$.
-* `testHorizon.m`: Run analysis for different horizon.
-* `testBenchmark.m`: Run analysis for some benchmark.
-
-We illustrate the performance between DCLF-DCBF/MPC-DC/MPC-CBF
-| DCLF-DCBF  | MPC-DC (N=8) |
-| --- | --- |
-| <img src="examples/figures/dclf-dcbf-avoidance.png" width="200" height="200"> | <img src="examples/figures/mpc-dc-avoidance.png" width="200" height="200"> |
-
-| MPC-CBF (N=1) | MPC-CBF (N=8) |
-| --- | --- |
-| <img src="examples/figures/mpc-cbf-avoidance-one-step.png" width="200" height="200"> | <img src="examples/figures/mpc-cbf-avoidance-several-steps.png" width="200" height="200"> |
-
-and also the safety performance for different numbers of horizon and hyperparameters
-| Different hyperparameter | Different horizon |
-| --- | --- |
-| <img src="examples/figures/benchmark-gamma.png" width="200" height="200"> | <img src="examples/figures/benchmark-horizon.png" width="200" height="200">
-
-#### Dependencies
+### Dependencies
+#### Matlab
 The packages needed for running the code are [Yalmip](https://yalmip.github.io/) and [IPOPT](https://projects.coin-or.org/Ipopt/wiki/MatlabInterface).
 
-We also provide the zipped version of precompiled .mex files for IPOPT in the folder `packages` in case you don't have it. Unzip that file and add those .mex files into your MATLAB path.
+We also provide the zipped version of precompiled .mex files for IPOPT in the folder `packages` in case you don't have it. Unzip that file and add those .mex files into your MATLAB path.

car-racing/README.md

@@ -0,0 +1,12 @@
+### Car racing competition
+The source codes are mainly adapted from Ugo's LMPC code and there are some lagacy features which are not used in our paper.
+
+We simulate a car racing competition between several cars, and ego car's speed profile and control input are shown as follow,  
+
+<img src="lmpc-speed-norm-profile.png" width="400">
+
+<img src="lmpc-deviation-profile.png" width="400">
+
+<img src="lmpc-input-profile.png" width="400">
+
+The animation can be found on the top of this readme, we will release full code after the paper is accepted.

double-integrator-2D/DCLF_DCBF.m

@@ -1,4 +1,4 @@
-classdef DCLFDCBF < handle
+classdef DCLF_DCBF < handle
     properties
         system
         params
@@ -12,7 +12,7 @@ classdef DCLFDCBF < handle
     end
     
     methods
-        function self = DCLFDCBF(x0, system, params)
+        function self = DCLF_DCBF(x0, system, params)
             % Define DCLF-DCBF controller
             self.x0 = x0;
             self.x_curr = x0;
@@ -25,7 +25,7 @@ classdef DCLFDCBF < handle
             xk = self.x_curr;
             while self.time_curr <= time
                 % Solve DCLF-DCBF
-                uk = self.solveDCLFDCBF();
+                uk = self.solve_dclf_dcbf();
                 xk = self.system.A * xk + self.system.B * uk;
                 % update system
                 self.x_curr = xk;
@@ -36,7 +36,7 @@ classdef DCLFDCBF < handle
             end
         end
         
-        function uopt = solveDCLFDCBF(self)
+        function uopt = solve_dclf_dcbf(self)
             % Solve DCLF-DCBF
             x = self.x_curr;
             u = sdpvar(2,1);

double-integrator-2D/MPC_CBF.m

@@ -1,4 +1,4 @@
-classdef MPCCBF < handle
+classdef MPC_CBF < handle
     % MPC with distance constraints
     properties
         system
@@ -16,7 +16,7 @@ classdef MPCCBF < handle
         obs
     end
     methods
-        function self = MPCCBF(x0, system, params)
+        function self = MPC_CBF(x0, system, params)
             % Define MPC_CBF controller
             self.x0 = x0;
             self.x_curr = x0;
@@ -29,7 +29,7 @@ classdef MPCCBF < handle
             xk = self.x_curr;
             while self.time_curr <= time
                 % Solve CFTOC
-                [~, uk] = self.solveMPCCBF(self.x_curr);
+                [~, uk] = self.solve_mpc_dc(self.x_curr);
                 xk = self.system.A * xk + self.system.B * uk;
                 % update system
                 self.x_curr = xk;
@@ -40,7 +40,7 @@ classdef MPCCBF < handle
             end
         end
         
-        function [xopt, uopt] = solveMPCCBF(self, xk)
+        function [xopt, uopt] = solve_mpc_dc(self, xk)
             % Solve MPC-CBF
             [feas, x, u, J] = self.solve_cftoc(xk);
             if ~feas

double-integrator-2D/MPC_DC.m

@@ -1,4 +1,4 @@
-classdef MPCDC < handle
+classdef MPC_DC < handle
     % MPC with distance constraints
     properties
         system
@@ -16,7 +16,7 @@ classdef MPCDC < handle
         obs
     end
     methods
-        function self = MPCDC(x0, system, params)
+        function self = MPC_DC(x0, system, params)
             % Define MPC_DC controller
             self.x0 = x0;
             self.x_curr = x0;
@@ -29,7 +29,7 @@ classdef MPCDC < handle
             xk = self.x_curr;
             while self.time_curr <= time
                 % Solve CFTOC
-                [~, uk] = self.solveMPCDC(self.x_curr);
+                [~, uk] = self.solve_mpc_dc(self.x_curr);
                 xk = self.system.A * xk + self.system.B * uk;
                 % update system
                 self.x_curr = xk;
@@ -40,7 +40,7 @@ classdef MPCDC < handle
             end
         end
         
-        function [xopt, uopt] = solveMPCDC(self, xk)
+        function [xopt, uopt] = solve_mpc_dc(self, xk)
             % Solve MPC-DC
             [feas, x, u, J] = self.solve_cftoc(xk);
             if ~feas

double-integrator-2D/README.md

@@ -0,0 +1,21 @@
+### 2D double integrator
+The 2D double integrator is assigned to reach the target position at origin while avoiding obstacles. We have three classes for different controllers: `DCLF_DCBF.m` (DCLF-DCBF), `MPC_CBF.m` (MPC-CBF) and `MPC_DC` (MPC-DC), respectively.
+
+Moreover, to illustrate the performance among them, we have:
+* `main.m`: Run DCLF-DCBF/MPC-CBF/MPC-DC respectively.
+* `analysis_gamma.m`: Run analysis for different hyperparameter $\gamma$.
+* `analysis_horizon.m`: Run analysis for different horizon.
+
+We illustrate the performance between DCLF-DCBF/MPC-DC/MPC-CBF
+| DCLF-DCBF  | MPC-DC (N=8) |
+| --- | --- |
+| <img src="figures/dclf-dcbf-avoidance.png" width="200" height="200"> | <img src="figures/mpc-dc-avoidance.png" width="200" height="200"> |
+
+| MPC-CBF (N=1) | MPC-CBF (N=8) |
+| --- | --- |
+| <img src="figures/mpc-cbf-avoidance-one-step.png" width="200" height="200"> | <img src="figures/mpc-cbf-avoidance-several-steps.png" width="200" height="200"> |
+
+and also the safety performance for different numbers of horizon and hyperparameters
+| Different hyperparameter | Different horizon |
+| --- | --- |
+| <img src="figures/benchmark-gamma.png" width="200" height="200"> | <img src="figures/benchmark-horizon.png" width="200" height="200">

double-integrator-2D/analysis_benchmark.m

@@ -9,10 +9,10 @@ P = 100*eye(4);
 Q = 10*eye(4);
 R = eye(2);
 N = 8;
-xmin = [-5; -5; -5; -5];
-xmax = [5; 5; 5; 5];
 umin = [-1; -1];
 umax = [1; 1];
+xmin = [-5; -5; -5; -5];
+xmax = [5; 5; 5; 5];
 
 %% Discrete-time double integrator 2D
 system.dt = dt;
@@ -47,7 +47,7 @@ for i = 1:size(gamma_list, 2)
     new_params = params;
     new_params.N = 5;
     new_params.gamma = gamma_list(i);
-    controller_mpc_cbf = MPCCBF(x0, system, new_params);
+    controller_mpc_cbf = MPC_CBF(x0, system, new_params);
     controller_mpc_cbf.obs = obs;
     controller_mpc_cbf.sim(time_total);
     controller_mpc_cbf_list{i} = controller_mpc_cbf;

double-integrator-2D/analysis_gamma.m

@@ -9,10 +9,10 @@ P = 100*eye(4);
 Q = 10*eye(4);
 R = eye(2);
 N = 8;
-xmin = [-5; -5; -5; -5];
-xmax = [5; 5; 5; 5];
 umin = [-1; -1];
 umax = [1; 1];
+xmin = [-5; -5; -5; -5];
+xmax = [5; 5; 5; 5];
 
 %% Discrete-time double integrator 2D
 system.dt = dt;
@@ -45,14 +45,14 @@ gamma_list = [0.1, 0.2, 0.3, 1.0];
 for ind = 1:size(gamma_list, 2)
     fprintf('Run MPC-CBF with gamma %f\n', gamma_list(ind));
     params_mpc_cbf.gamma = gamma_list(ind);
-    controller_mpc_cbf_list{ind} = MPCCBF(x0, system, params_mpc_cbf);
+    controller_mpc_cbf_list{ind} = MPC_CBF(x0, system, params_mpc_cbf);
     controller_mpc_cbf_list{ind}.obs = obs;
     controller_mpc_cbf_list{ind}.sim(time_total);
 end
 
 %% Simulate MPC-DC
 params_mpc_dc = params_mpc_cbf;
-controller_mpc_dc = MPCDC(x0, system, params_mpc_cbf);
+controller_mpc_dc = MPC_DC(x0, system, params_mpc_cbf);
 controller_mpc_dc.obs = obs;
 controller_mpc_dc.sim(time_total);
 

double-integrator-2D/analysis_horizon.m

@@ -9,10 +9,10 @@ P = 100*eye(4);
 Q = 10*eye(4);
 R = eye(2);
 N = 8;
-xmin = [-5; -5; -5; -5];
-xmax = [5; 5; 5; 5];
 umin = [-1; -1];
 umax = [1; 1];
+xmin = [-5; -5; -5; -5];
+xmax = [5; 5; 5; 5];
 
 %% Discrete-time double integrator 2D
 system.dt = dt;
@@ -43,13 +43,13 @@ obs.r = 1.5;
 %% Simulate MPC-CBF
 params.N = 5;
 params.gamma = 0.25;
-controller_mpc_cbf_1 = MPCCBF(x0, system, params);
+controller_mpc_cbf_1 = MPC_CBF(x0, system, params);
 controller_mpc_cbf_1.obs = obs;
 controller_mpc_cbf_1.sim(time_total);
 
 %% Simulate MPC-CBF with lower gamma
 params.gamma = 0.20;
-controller_mpc_cbf_2 = MPCCBF(x0, system, params);
+controller_mpc_cbf_2 = MPC_CBF(x0, system, params);
 controller_mpc_cbf_2.obs = obs;
 controller_mpc_cbf_2.sim(time_total);
 
@@ -59,26 +59,20 @@ controller_mpc_cbf_3.obs = obs;
 controller_mpc_cbf_3.sim(time_total);
 
 %% Simulate MPC-DC
-
-% The problem is infeasible at N=5
-% params.N = 5;
-% controller_mpc_dc_0 = MPCDC(x0, system, params);
-% controller_mpc_dc_0.obs = obs;
-% controller_mpc_dc_0.sim(time_total);
-
 params.N = 7;
-controller_mpc_dc_1 = MPCDC(x0, system, params);
+controller_mpc_dc_1 = MPC_DC(x0, system, params);
 controller_mpc_dc_1.obs = obs;
 controller_mpc_dc_1.sim(time_total);
 
+%% Simulate MPC-DC with larger horizon
 params.N = 15;
-controller_mpc_dc_2 = MPCDC(x0, system, params);
+controller_mpc_dc_2 = MPC_DC(x0, system, params);
 controller_mpc_dc_2.obs = obs;
 controller_mpc_dc_2.sim(time_total);
 
 %%
 params.N = 30;
-controller_mpc_dc_3 = MPCDC(x0, system, params);
+controller_mpc_dc_3 = MPC_DC(x0, system, params);
 controller_mpc_dc_3.obs = obs;
 controller_mpc_dc_3.sim(time_total);
 
@@ -179,14 +173,6 @@ fprintf('Computational time for MPC-CBF1: mean %.3f, std %.3f, min %.3f, max %.3
     controller_mpc_cbf_1.u_cost,...
     min(controller_mpc_cbf_1.distlog)]);
 
-% fprintf('Computational time for MPC-DC0: mean %.3f, std %.3f, min %.3f, max %.3f, input cost %.3f, min dist %f\n',...
-%     [mean(controller_mpc_dc_0.solvertime),...
-%     std(controller_mpc_dc_0.solvertime),...
-%     min(controller_mpc_dc_0.solvertime),...
-%     max(controller_mpc_dc_0.solvertime),...
-%     controller_mpc_dc_0.u_cost,...
-%     min(controller_mpc_dc_0.distlog)]);
-
 fprintf('Computational time for MPC-DC1: mean %.3f, std %.3f, min %.3f, max %.3f, input cost %.3f, min dist %f\n',...
     [mean(controller_mpc_dc_1.solvertime),...
     std(controller_mpc_dc_1.solvertime),...

double-integrator-2D/main.m

@@ -19,10 +19,10 @@ P = 100*eye(4);
 Q = 10*eye(4);
 R = eye(2);
 N = 8;
-xmin = [-5; -5; -5; -5];
-xmax = [5; 5; 5; 5];
 umin = [-1; -1];
 umax = [1; 1];
+xmin = [-5; -5; -5; -5];
+xmax = [5; 5; 5; 5];
 
 %% Discrete-time double integrator 2D
 system.dt = dt;
@@ -67,7 +67,7 @@ obs.r = 1.5;
 %% Simulate DCLF-DCBF
 if run_dclf_dcbf
     fprintf('Run DCLF-DCBF\n');
-    controller_dclf_dcbf = DCLFDCBF(x0, system, params_dclf_dcbf);
+    controller_dclf_dcbf = DCLF_DCBF(x0, system, params_dclf_dcbf);
     controller_dclf_dcbf.obs = obs;
     controller_dclf_dcbf.sim(time_total);
 end
@@ -110,7 +110,7 @@ end
 params_mpc_cbf.N = 1;
 if run_mpc_cbf_one
     fprintf('Run MPC-CBF\n');
-    controller_mpc_cbf_one = MPCCBF(x0, system, params_mpc_cbf);
+    controller_mpc_cbf_one = MPC_CBF(x0, system, params_mpc_cbf);
     controller_mpc_cbf_one.obs = obs;
     controller_mpc_cbf_one.sim(time_total);
 end
@@ -153,7 +153,7 @@ end
 params_mpc_cbf.N = 8;
 if run_mpc_cbf_one
     fprintf('Run MPC-CBF\n');
-    controller_mpc_cbf_multiple = MPCCBF(x0, system, params_mpc_cbf);
+    controller_mpc_cbf_multiple = MPC_CBF(x0, system, params_mpc_cbf);
     controller_mpc_cbf_multiple.obs = obs;
     controller_mpc_cbf_multiple.sim(time_total);
 end
@@ -195,7 +195,7 @@ end
 %% Simulate MPC-DC
 if run_mpc_dc
     fprintf('Run MPC-DC\n');
-    controller_mpc_dc = MPCDC(x0, system, params_mpc_dc);
+    controller_mpc_dc = MPC_DC(x0, system, params_mpc_dc);
     controller_mpc_dc.obs = obs;
     controller_mpc_dc.sim(time_total);
 end