diff --git a/README.md b/README.md index 0358e4b..59c6d5d 100644 --- a/README.md +++ b/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) | | --- | --- | -| | | +| | | | MPC-CBF (N=1) | MPC-CBF (N=8) | | --- | --- | -| | | +| | | and also the safety performance for different numbers of horizon and hyperparameters | Different hyperparameter | Different horizon | | --- | --- | -| | +| | #### Dependencies The packages needed for running the code are [Yalmip](https://yalmip.github.io/) and [IPOPT](https://projects.coin-or.org/Ipopt/wiki/MatlabInterface). diff --git a/double-integrator-2D/DCLF_DCBF.m b/examples/DCLFDCBF.m similarity index 95% rename from double-integrator-2D/DCLF_DCBF.m rename to examples/DCLFDCBF.m index 2e8e0d8..6c2c42b 100644 --- a/double-integrator-2D/DCLF_DCBF.m +++ b/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); diff --git a/double-integrator-2D/MPC_CBF.m b/examples/MPCCBF.m similarity index 96% rename from double-integrator-2D/MPC_CBF.m rename to examples/MPCCBF.m index 45299ee..0f53342 100644 --- a/double-integrator-2D/MPC_CBF.m +++ b/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 diff --git a/double-integrator-2D/MPC_DC.m b/examples/MPCDC.m similarity index 96% rename from double-integrator-2D/MPC_DC.m rename to examples/MPCDC.m index 738b24b..468117b 100644 --- a/double-integrator-2D/MPC_DC.m +++ b/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 diff --git a/double-integrator-2D/figures/benchmark-gamma.eps b/examples/figures/benchmark-gamma.eps similarity index 100% rename from double-integrator-2D/figures/benchmark-gamma.eps rename to examples/figures/benchmark-gamma.eps diff --git a/double-integrator-2D/figures/benchmark-gamma.png b/examples/figures/benchmark-gamma.png similarity index 100% rename from double-integrator-2D/figures/benchmark-gamma.png rename to examples/figures/benchmark-gamma.png diff --git a/double-integrator-2D/figures/benchmark-horizon-computational-time.eps b/examples/figures/benchmark-horizon-computational-time.eps similarity index 100% rename from double-integrator-2D/figures/benchmark-horizon-computational-time.eps rename to examples/figures/benchmark-horizon-computational-time.eps diff --git a/double-integrator-2D/figures/benchmark-horizon-computational-time.png b/examples/figures/benchmark-horizon-computational-time.png similarity index 100% rename from double-integrator-2D/figures/benchmark-horizon-computational-time.png rename to examples/figures/benchmark-horizon-computational-time.png diff --git a/double-integrator-2D/figures/benchmark-horizon.eps b/examples/figures/benchmark-horizon.eps similarity index 100% rename from double-integrator-2D/figures/benchmark-horizon.eps rename to examples/figures/benchmark-horizon.eps diff --git a/double-integrator-2D/figures/benchmark-horizon.png b/examples/figures/benchmark-horizon.png similarity index 100% rename from double-integrator-2D/figures/benchmark-horizon.png rename to examples/figures/benchmark-horizon.png diff --git a/double-integrator-2D/figures/dclf-dcbf-avoidance.eps b/examples/figures/dclf-dcbf-avoidance.eps similarity index 100% rename from double-integrator-2D/figures/dclf-dcbf-avoidance.eps rename to examples/figures/dclf-dcbf-avoidance.eps diff --git a/double-integrator-2D/figures/dclf-dcbf-avoidance.png b/examples/figures/dclf-dcbf-avoidance.png similarity index 100% rename from double-integrator-2D/figures/dclf-dcbf-avoidance.png rename to examples/figures/dclf-dcbf-avoidance.png diff --git a/double-integrator-2D/figures/mpc-cbf-avoidance-one-step.eps b/examples/figures/mpc-cbf-avoidance-one-step.eps similarity index 100% rename from double-integrator-2D/figures/mpc-cbf-avoidance-one-step.eps rename to examples/figures/mpc-cbf-avoidance-one-step.eps diff --git a/double-integrator-2D/figures/mpc-cbf-avoidance-one-step.png b/examples/figures/mpc-cbf-avoidance-one-step.png similarity index 100% rename from double-integrator-2D/figures/mpc-cbf-avoidance-one-step.png rename to examples/figures/mpc-cbf-avoidance-one-step.png diff --git a/double-integrator-2D/figures/mpc-cbf-avoidance-several-steps.eps b/examples/figures/mpc-cbf-avoidance-several-steps.eps similarity index 100% rename from double-integrator-2D/figures/mpc-cbf-avoidance-several-steps.eps rename to examples/figures/mpc-cbf-avoidance-several-steps.eps diff --git a/double-integrator-2D/figures/mpc-cbf-avoidance-several-steps.png b/examples/figures/mpc-cbf-avoidance-several-steps.png similarity index 100% rename from double-integrator-2D/figures/mpc-cbf-avoidance-several-steps.png rename to examples/figures/mpc-cbf-avoidance-several-steps.png diff --git a/double-integrator-2D/figures/mpc-dc-avoidance.eps b/examples/figures/mpc-dc-avoidance.eps similarity index 100% rename from double-integrator-2D/figures/mpc-dc-avoidance.eps rename to examples/figures/mpc-dc-avoidance.eps diff --git a/double-integrator-2D/figures/mpc-dc-avoidance.png b/examples/figures/mpc-dc-avoidance.png similarity index 100% rename from double-integrator-2D/figures/mpc-dc-avoidance.png rename to examples/figures/mpc-dc-avoidance.png diff --git a/double-integrator-2D/main.m b/examples/test.m similarity index 96% rename from double-integrator-2D/main.m rename to examples/test.m index 77e874a..196364f 100644 --- a/double-integrator-2D/main.m +++ b/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 diff --git a/double-integrator-2D/analysis_benchmark.m b/examples/testBenchmark.m similarity index 95% rename from double-integrator-2D/analysis_benchmark.m rename to examples/testBenchmark.m index 816f219..f0cdcfe 100644 --- a/double-integrator-2D/analysis_benchmark.m +++ b/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; diff --git a/double-integrator-2D/analysis_gamma.m b/examples/testGamma.m similarity index 92% rename from double-integrator-2D/analysis_gamma.m rename to examples/testGamma.m index e081b75..b924514 100644 --- a/double-integrator-2D/analysis_gamma.m +++ b/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); diff --git a/double-integrator-2D/analysis_horizon.m b/examples/testHorizon.m similarity index 95% rename from double-integrator-2D/analysis_horizon.m rename to examples/testHorizon.m index 51f687a..d3b7a09 100644 --- a/double-integrator-2D/analysis_horizon.m +++ b/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);