Add examples

README.md

@@ -1,14 +1,14 @@
 # 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:
 ### 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](https://github.com/HybridRobotics/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*
 
 #### Citing
 If you find this code useful in your work, please consider citing:
 ```shell
-@article{zeng2020mpc-cbf,
+@article{zeng2020mpccbf,
   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},
@@ -17,26 +17,27 @@ If you find this code useful in your work, please consider citing:
 ```
 
 #### Instructions
-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.
+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:
-* `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.
+* `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="double-integrator-2D/figures/dclf-dcbf-avoidance.png" width="200" height="200"> | <img src="double-integrator-2D/figures/mpc-dc-avoidance.png" width="200" height="200"> |
+| <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="double-integrator-2D/figures/mpc-cbf-avoidance-one-step.png" width="200" height="200"> | <img src="double-integrator-2D/figures/mpc-cbf-avoidance-several-steps.png" width="200" height="200"> |
+| <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="double-integrator-2D/figures/benchmark-gamma.png" width="200" height="200"> | <img src="double-integrator-2D/figures/benchmark-horizon.png" width="200" height="200">
+| <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).

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);
 
@@ -62,23 +62,23 @@ controller_mpc_cbf_3.sim(time_total);
 
 % The problem is infeasible at N=5
 % params.N = 5;
-% controller_mpc_dc_0 = MPC_DC(x0, system, params);
+% 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);
 
 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);