feat: first commit

Archive the repository.

README.md

@@ -1,28 +1,63 @@
+**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. This is the reference implementation of our paper:
+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:
 ### Safety-Critical Model Predictive Control with Discrete-Time Control Barrier Function
-[PDF](https://arxiv.org/abs/2007.11718) | [Code: Double Integratror](double-integrator-2D) | [Code: Car Racing](car-racing)
+[PDF](https://arxiv.org/abs/2007.11718) | [Code: Double Integratror](examples) | [Code: Car Racing](https://github.com/HybridRobotics/Car-Racing)
 
 *Jun Zeng, Bike Zhang and Koushil Sreenath*
 
-<img src="car-racing/demo.gif" height="200"/>
-
 #### Citing
-If you find this code useful in your work, please consider citing:
-```shell
-@article{zeng2020mpccbf,
+If you find this project useful in your work, please consider citing following work:
+```
+@inproceedings{zeng2021mpccbf,
   title={Safety-critical model predictive control with discrete-time control barrier function},
   author={Zeng, Jun and Zhang, Bike and Sreenath, Koushil},
-  journal={arXiv preprint arXiv:2007.11718},
-  year={2020}
+  booktitle={2021 American Control Conference (ACC)},
+  year={2021},
+  volume={},
+  number={},
+  pages={3882-3889}
 }
 ```
 
-### 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).
+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}
+}
+``` 
 
-### Dependencies
-#### Matlab
+#### 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
 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

@@ -1,12 +0,0 @@
-### 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/README.md

@@ -1,21 +0,0 @@
-### 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">

examples/DCLFDCBF.m

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

examples/MPCCBF.m

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

examples/MPCDC.m

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

examples/test.m

@@ -19,10 +19,10 @@ P = 100*eye(4);
 Q = 10*eye(4);
 R = eye(2);
 N = 8;
-umin = [-1; -1];
-umax = [1; 1];
 xmin = [-5; -5; -5; -5];
 xmax = [5; 5; 5; 5];
+umin = [-1; -1];
+umax = [1; 1];
 
 %% 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 = DCLF_DCBF(x0, system, params_dclf_dcbf);
+    controller_dclf_dcbf = DCLFDCBF(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 = MPC_CBF(x0, system, params_mpc_cbf);
+    controller_mpc_cbf_one = MPCCBF(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 = MPC_CBF(x0, system, params_mpc_cbf);
+    controller_mpc_cbf_multiple = MPCCBF(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 = MPC_DC(x0, system, params_mpc_dc);
+    controller_mpc_dc = MPCDC(x0, system, params_mpc_dc);
     controller_mpc_dc.obs = obs;
     controller_mpc_dc.sim(time_total);
 end

examples/testBenchmark.m

@@ -9,10 +9,10 @@ P = 100*eye(4);
 Q = 10*eye(4);
 R = eye(2);
 N = 8;
-umin = [-1; -1];
-umax = [1; 1];
 xmin = [-5; -5; -5; -5];
 xmax = [5; 5; 5; 5];
+umin = [-1; -1];
+umax = [1; 1];
 
 %% 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 = MPC_CBF(x0, system, new_params);
+    controller_mpc_cbf = MPCCBF(x0, system, new_params);
     controller_mpc_cbf.obs = obs;
     controller_mpc_cbf.sim(time_total);
     controller_mpc_cbf_list{i} = controller_mpc_cbf;

examples/testGamma.m

@@ -9,10 +9,10 @@ P = 100*eye(4);
 Q = 10*eye(4);
 R = eye(2);
 N = 8;
-umin = [-1; -1];
-umax = [1; 1];
 xmin = [-5; -5; -5; -5];
 xmax = [5; 5; 5; 5];
+umin = [-1; -1];
+umax = [1; 1];
 
 %% 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} = MPC_CBF(x0, system, params_mpc_cbf);
+    controller_mpc_cbf_list{ind} = MPCCBF(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 = MPC_DC(x0, system, params_mpc_cbf);
+controller_mpc_dc = MPCDC(x0, system, params_mpc_cbf);
 controller_mpc_dc.obs = obs;
 controller_mpc_dc.sim(time_total);
 

examples/testHorizon.m

@@ -9,10 +9,10 @@ P = 100*eye(4);
 Q = 10*eye(4);
 R = eye(2);
 N = 8;
-umin = [-1; -1];
-umax = [1; 1];
 xmin = [-5; -5; -5; -5];
 xmax = [5; 5; 5; 5];
+umin = [-1; -1];
+umax = [1; 1];
 
 %% 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 = MPC_CBF(x0, system, params);
+controller_mpc_cbf_1 = MPCCBF(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 = MPC_CBF(x0, system, params);
+controller_mpc_cbf_2 = MPCCBF(x0, system, params);
 controller_mpc_cbf_2.obs = obs;
 controller_mpc_cbf_2.sim(time_total);
 
@@ -59,20 +59,26 @@ 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 = MPC_DC(x0, system, params);
+controller_mpc_dc_1 = MPCDC(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 = MPC_DC(x0, system, params);
+controller_mpc_dc_2 = MPCDC(x0, system, params);
 controller_mpc_dc_2.obs = obs;
 controller_mpc_dc_2.sim(time_total);
 
 %%
 params.N = 30;
-controller_mpc_dc_3 = MPC_DC(x0, system, params);
+controller_mpc_dc_3 = MPCDC(x0, system, params);
 controller_mpc_dc_3.obs = obs;
 controller_mpc_dc_3.sim(time_total);
 
@@ -173,6 +179,14 @@ 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),...