Showing preview only (1,371K chars total). Download the full file or copy to clipboard to get everything.
Repository: HybridRobotics/NMPC-DCLF-DCBF
Branch: master
Commit: 3f40c67578f4
Files: 184
Total size: 1.3 MB
Directory structure:
gitextract_hxxrxdvy/
├── .gitignore
├── LICENSE
├── README.md
├── bibtex/
│ ├── acc2021_nmpc_dcbf.md
│ ├── acc2023_impc_dhocbf.md
│ ├── cdc2022_nmpc_dcbf_feasibility.md
│ ├── icra2022_nmpc_dcbf_polytope.md
│ └── rss2022_nmpc_dcbf_legged_robots.md
└── matlab/
├── acc2021/
│ ├── DCLFDCBF.m
│ ├── MPCCBF.m
│ ├── MPCDC.m
│ ├── README.md
│ ├── figures/
│ │ └── .gitignore
│ ├── test.m
│ ├── testBenchmark.m
│ ├── testGamma.m
│ └── testHorizon.m
├── acc2023/
│ ├── README.md
│ ├── benchmark/
│ │ ├── InitialState.m
│ │ ├── InitialStateData.mat
│ │ ├── gamma1-0p6/
│ │ │ ├── InitialStateData.mat
│ │ │ ├── NMPCDCBF1.m
│ │ │ ├── feasibility12.mat
│ │ │ ├── feasibility16.mat
│ │ │ ├── feasibility20.mat
│ │ │ ├── feasibility24.mat
│ │ │ ├── feasibility4.mat
│ │ │ ├── feasibility8.mat
│ │ │ ├── test_comprehensive.m
│ │ │ ├── test_each_horizon/
│ │ │ │ ├── InitialStateData.mat
│ │ │ │ ├── NMPCDCBF1.m
│ │ │ │ ├── tabledata.m
│ │ │ │ ├── test_N12.m
│ │ │ │ ├── test_N16.m
│ │ │ │ ├── test_N20.m
│ │ │ │ ├── test_N24.m
│ │ │ │ ├── test_N4.m
│ │ │ │ └── test_N8.m
│ │ │ ├── timecom12.mat
│ │ │ ├── timecom16.mat
│ │ │ ├── timecom20.mat
│ │ │ ├── timecom24.mat
│ │ │ ├── timecom4.mat
│ │ │ └── timecom8.mat
│ │ ├── gamma1-0p6gamma2-0p6/
│ │ │ ├── InitialStateData.mat
│ │ │ ├── NMPCDCBF2.m
│ │ │ ├── feasibility12.mat
│ │ │ ├── feasibility16.mat
│ │ │ ├── feasibility20.mat
│ │ │ ├── feasibility24.mat
│ │ │ ├── feasibility4.mat
│ │ │ ├── feasibility8.mat
│ │ │ ├── test_comprehensive.m
│ │ │ ├── test_each_horizon/
│ │ │ │ ├── InitialStateData.mat
│ │ │ │ ├── NMPCDCBF2.m
│ │ │ │ ├── tabledata.m
│ │ │ │ ├── test_N12.m
│ │ │ │ ├── test_N16.m
│ │ │ │ ├── test_N20.m
│ │ │ │ ├── test_N24.m
│ │ │ │ ├── test_N4.m
│ │ │ │ └── test_N8.m
│ │ │ ├── timecom12.mat
│ │ │ ├── timecom16.mat
│ │ │ ├── timecom20.mat
│ │ │ ├── timecom24.mat
│ │ │ ├── timecom4.mat
│ │ │ └── timecom8.mat
│ │ ├── gamma1_0p4/
│ │ │ ├── InitialStateData.mat
│ │ │ ├── NMPCDCBF1.m
│ │ │ ├── feasibility12.mat
│ │ │ ├── feasibility16.mat
│ │ │ ├── feasibility20.mat
│ │ │ ├── feasibility24.mat
│ │ │ ├── feasibility4.mat
│ │ │ ├── feasibility8.mat
│ │ │ ├── test_comprehensive.m
│ │ │ ├── test_each_horizon/
│ │ │ │ ├── InitialStateData.mat
│ │ │ │ ├── NMPCDCBF1.m
│ │ │ │ ├── tabledata.m
│ │ │ │ ├── test_N12.m
│ │ │ │ ├── test_N16.m
│ │ │ │ ├── test_N20.m
│ │ │ │ ├── test_N24.m
│ │ │ │ ├── test_N4.m
│ │ │ │ └── test_N8.m
│ │ │ ├── timecom12.mat
│ │ │ ├── timecom16.mat
│ │ │ ├── timecom20.mat
│ │ │ ├── timecom24.mat
│ │ │ ├── timecom4.mat
│ │ │ └── timecom8.mat
│ │ └── gamma1_0p4gamma2_0p4/
│ │ ├── InitialStateData.mat
│ │ ├── NMPCDCBF2.m
│ │ ├── feasibility12.mat
│ │ ├── feasibility16.mat
│ │ ├── feasibility20.mat
│ │ ├── feasibility24.mat
│ │ ├── feasibility4.mat
│ │ ├── feasibility8.mat
│ │ ├── test_comprehensive.m
│ │ ├── test_each_horizon/
│ │ │ ├── InitialStateData.mat
│ │ │ ├── NMPCDCBF2.m
│ │ │ ├── tabledata.m
│ │ │ ├── test_N12.m
│ │ │ ├── test_N16.m
│ │ │ ├── test_N20.m
│ │ │ ├── test_N24.m
│ │ │ ├── test_N4.m
│ │ │ └── test_N8.m
│ │ ├── timecom12.mat
│ │ ├── timecom16.mat
│ │ ├── timecom20.mat
│ │ ├── timecom24.mat
│ │ ├── timecom4.mat
│ │ └── timecom8.mat
│ └── closedloop_performance/
│ ├── Closedloop_Trajectories_Hyperparameters.m
│ ├── FigureGenerate.m
│ ├── Iterative_Convergence.m
│ ├── Maximum_Iterations.m
│ ├── NMPCDCBF1.m
│ ├── NMPCDCBF2.m
│ ├── figures/
│ │ ├── J-converge-1.eps
│ │ ├── J-converge-2.eps
│ │ ├── J-converge-3.eps
│ │ ├── J-converge-4.eps
│ │ ├── closedloop-snapshots1.eps
│ │ ├── closedloop-snapshots2.eps
│ │ ├── closedloop-snapshots3.eps
│ │ ├── openloop-snapshots.eps
│ │ ├── state-converge-1.eps
│ │ ├── state-converge-2.eps
│ │ ├── state-converge-3.eps
│ │ └── state-converge-4.eps
│ ├── impc_N16_gamma4_4.mat
│ ├── impc_N16_gamma6_6.mat
│ ├── impc_N24_Gamma4.mat
│ ├── impc_N24_Gamma6.mat
│ ├── impc_N24_gamma4_4.mat
│ ├── impc_N24_gamma6_6.mat
│ ├── iteration_theta.mat
│ ├── iteration_v.mat
│ ├── iteration_x.mat
│ ├── iteration_y.mat
│ ├── jtconv_Gamma4_4.mat
│ ├── jtconv_Gamma4_6.mat
│ ├── jtconv_Gamma6_4.mat
│ ├── jtconv_Gamma6_6.mat
│ ├── nmpc_N16_gamma4_4.mat
│ ├── nmpc_N16_gamma6_6.mat
│ ├── nmpc_N24_gamma4.mat
│ ├── nmpc_N24_gamma4_4.mat
│ ├── nmpc_N24_gamma6.mat
│ ├── nmpc_N24_gamma6_6.mat
│ └── trajectory.mat
└── cdc2021/
├── .gitignore
├── CBFDT.m
├── ParamDCLFDCBF.m
├── ParamMPCDCBF.m
├── ParamMPCGCBF.m
├── ParamNMPCDCBF.m
├── ParamNMPCDCLFDCBF.m
├── README.md
├── data/
│ ├── feasibility-dclfdcbf1.mat
│ ├── feasibility-dclfdcbf2.mat
│ ├── feasibility-dclfdcbf3.mat
│ ├── feasibility-dclfdcbf4.mat
│ ├── feasibility-mpcdcbf1.mat
│ ├── feasibility-mpcdcbf2.mat
│ ├── feasibility-mpcdcbf3.mat
│ ├── feasibility-mpcdcbf4.mat
│ ├── feasibility-mpcgcbf1.mat
│ ├── feasibility-mpcgcbf2.mat
│ ├── feasibility-mpcgcbf3.mat
│ ├── feasibility-mpcgcbf4.mat
│ ├── safety-mpcdcbf.mat
│ ├── safety-mpcgcbf.mat
│ └── safety-nmpccbf.mat
├── test.m
├── testComplexity.m
├── testFeasibilityDCLFDCBF.m
├── testFeasibilityMPCDCBF.m
├── testFeasibilityMPCGCBF.m
└── testSafety.m
================================================
FILE CONTENTS
================================================
================================================
FILE: .gitignore
================================================
## MATLAB files to ignore ##
# Windows default autosave extension
*.asv
# OSX / *nix default autosave extension
*.m~
# Compiled MEX binaries (all platforms)
*.mex*
# Packaged app and toolbox files
*.mlappinstall
*.mltbx
# Generated helpsearch folders
helpsearch*/
# Simulink code generation folders
slprj/
sccprj/
# Matlab code generation folders
codegen/
# Simulink autosave extension
*.autosave
# Simulink cache files
*.slxc
# Octave session info
octave-workspace
================================================
FILE: LICENSE
================================================
MIT License
Copyright (c) 2021 Jun Zeng
Permission is hereby granted, free of charge, to any person obtaining a copy
of this software and associated documentation files (the "Software"), to deal
in the Software without restriction, including without limitation the rights
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
copies of the Software, and to permit persons to whom the Software is
furnished to do so, subject to the following conditions:
The above copyright notice and this permission notice shall be included in all
copies or substantial portions of the Software.
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
SOFTWARE.
================================================
FILE: README.md
================================================
**Status**: The implementation code for corresponding papers will be merged here and new papers will be added in an inverse order of submission.
### Introduction
In this repository, a collection of our work is presented where nonlinear model predictive control (NMPC) with control Lyapunov functions (CLFs) and control barrier functions (CBFs) are applied.
### 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 the file based on the operating system and add those .mex files into your MATLAB path.
### Citing
#### Theoretical Publications
If you find this project useful in your work, please consider citing following work:
* S. Liu, J. Zeng, K. Sreenath and C. Belta. "Iterative Convex Optimization for Model Predictive Control with Discrete-Time High-Order Control Barrier Functions." *2023 IEEE American Control Conference (ACC)*. [[arXiv]](https://arxiv.org/abs/2210.04361) [[Docs]](matlab/acc2023/README.md) [[Code]](matlab/acc2023) [[BibTex]](bibtex/acc2023_impc_dhocbf.md)
* A. Thirugnanam, J. Zeng, K. Sreenath. "Safety-Critical Control and Planning for Obstacle Avoidance between Polytopes with Control Barrier Functions." *2022 IEEE International Conference on Robotics and Automation (ICRA)*. [[IEEE]](https://ieeexplore.ieee.org/document/9812334) [[arXiv]](https://arxiv.org/abs/2109.12313) [[Video]](https://youtu.be/wucophROPRY) [[BibTex]](bibtex/icra2022_nmpc_dcbf_polytope.md)
* J. Zeng, Z. Li, K. Sreenath. "Enhancing Feasibility and Safety of Nonlinear Model Predictive Control with Discrete-Time Control Barrier Functions." *2021 IEEE Conference on Decision and Control (CDC)*. [[IEEE]](https://ieeexplore.ieee.org/document/9683174) [[arXiv]](https://arxiv.org/abs/2105.10596) [[Docs]](matlab/cdc2021/README.md) [[Code]](matlab/cdc2021) [[BibTex]](bibtex/cdc2022_nmpc_dcbf_feasibility.md)
* J. Zeng, B. Zhang and K. Sreenath. "Safety-Critical Model Predictive Control with Discrete-Time Control Barrier Function." *2021 IEEE American Control Conference (ACC)*. [[IEEE]](https://ieeexplore.ieee.org/document/9483029) [[arXiv]](https://arxiv.org/abs/2007.11718) [[Docs]](matlab/acc2021/README.md) [[Code]](matlab/acc2021) [[BibTex]](bibtex/acc2021_nmpc_dcbf.md)
#### Applicational Publications
* Z. Li, J. Zeng, A. Thirugnanam, K. Sreenath. "Bridging Model-based Safety and Model-free Reinforcement Learning through System Identification of Low Dimensional Linear Models." *2022 Proceedings of Robotics: Science and Systems (RSS)*. [[RSS]](http://www.roboticsproceedings.org/rss18/p033.html) [[arXiv]](https://arxiv.org/abs/2205.05787) [[BibTex]](bibtex/rss2022_nmpc_dcbf_legged_robots.md) [[Webpage]](https://sites.google.com/berkeley.edu/rl-sysid-rss2022/home)
================================================
FILE: bibtex/acc2021_nmpc_dcbf.md
================================================
```
@inproceedings{zeng2021mpccbf,
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}
}
```
================================================
FILE: bibtex/acc2023_impc_dhocbf.md
================================================
```
@inproceedings{liu2023iterative,
title={Iterative Convex Optimization for Model Predictive Control with Discrete-Time High-Order Control Barrier Functions},
author={Liu, Shuo and Zeng, Jun and Sreenath, Koushil and Belta, Calin A},
booktitle={2023 American Control Conference (ACC)},
year={2023}
}
```
================================================
FILE: bibtex/cdc2022_nmpc_dcbf_feasibility.md
================================================
```
@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}
}
```
================================================
FILE: bibtex/icra2022_nmpc_dcbf_polytope.md
================================================
```
@inproceedings{thirugnanam2021safetycritical,
author={Thirugnanam, Akshay and Zeng, Jun and Sreenath, Koushil},
booktitle={2022 International Conference on Robotics and Automation (ICRA)},
title={Safety-Critical Control and Planning for Obstacle Avoidance between Polytopes with Control Barrier Functions},
year={2022},
volume={},
number={},
pages={286-292},
}
```
================================================
FILE: bibtex/rss2022_nmpc_dcbf_legged_robots.md
================================================
```
@inproceedings{li2022bridging,
author = {Li, Zhongyu and Zeng, Jun and Thirugnanam, Akshay and Sreenath, Koushil},
title = {{Bridging Model-based Safety and Model-free Reinforcement Learning through System Identification of Low Dimensional Linear Models}},
booktitle = {Proceedings of Robotics: Science and Systems},
year = {2022},
address = {New York City, NY, USA},
month = {June},
}
```
================================================
FILE: matlab/acc2021/DCLFDCBF.m
================================================
classdef DCLFDCBF < handle
properties
system
params
x0
x_curr
time_curr = 0.0
xlog = []
ulog = []
u_cost = 0
obs
end
methods
function self = DCLFDCBF(x0, system, params)
% Define DCLF-DCBF controller
self.x0 = x0;
self.x_curr = x0;
self.system = system;
self.params = params;
end
function sim(self, time)
% Simulate the system until a given time
xk = self.x_curr;
while self.time_curr <= time
% Solve DCLF-DCBF
uk = self.solveDCLFDCBF();
xk = self.system.A * xk + self.system.B * uk;
% update system
self.x_curr = xk;
self.time_curr = self.time_curr + self.system.dt;
self.xlog = [self.xlog, xk];
self.ulog = [self.ulog, uk];
self.u_cost = self.u_cost + uk'*uk*self.system.dt;
end
end
function uopt = solveDCLFDCBF(self)
% Solve DCLF-DCBF
x = self.x_curr;
u = sdpvar(2,1);
delta = sdpvar(1,1);
constraints = [];
% add DCLF constraints
x_next = self.system.A * x + self.system.B * u;
v = x' * self.params.P * x;
v_next = x_next' * self.params.P * x_next;
constraints = [constraints; v_next - v + self.params.alpha * v <= delta];
% add DCBF constraints
pos = self.obs.pos;
r = self.obs.r;
b = (x(1:2)-pos)'*(x(1:2)-pos) - r^2;
b_next = (x_next(1:2)-pos)'*(x_next(1:2)-pos) - r^2;
constraints = [constraints; b_next - b + self.params.gamma*b >= 0];
% input constraints
constraints = [constraints; self.system.ul <= u <= self.system.uu];
% cost
cost = u'*self.params.H*u + delta'*self.params.p*delta;
% solve optimization
ops = sdpsettings('solver','ipopt','verbose',0);
diagnostics = optimize(constraints, cost, ops);
if diagnostics.problem == 0
feas = true;
uopt = value(u);
else
feas = false;
uopt = [];
end
fprintf('solver time: %f\n', diagnostics.solvertime);
end
function plot(self,figure_name)
% Plot simulation
figure('Renderer', 'painters', 'Position', [0 0 400 400]);
set(gca,'LooseInset',get(gca,'TightInset'));
hold on;
% plot trajectory
plot(self.xlog(1,:), self.xlog(2,:), 'ko-',...
'LineWidth', 1.0,'MarkerSize',4);
% plot obstacle
pos = self.obs.pos;
r = self.obs.r;
th = linspace(0,2*pi*100);
x = cos(th) ; y = sin(th) ;
plot(pos(1) + r*x, pos(2) + r*y, 'Color', [0.8500, 0.3250, 0.0980], 'LineWidth', 2);
plot(pos(1), pos(2), 'Color', [0.8500, 0.3250, 0.0980], 'MarkerSize', 5, 'LineWidth', 2);
% plot target position
plot(self.x0(1), self.x0(2), 'db', 'LineWidth', 1);
plot(0.0, 0.0, 'dr', 'LineWidth', 1);
h=get(gca,'Children');
h_legend = legend(h([end]), {'DCLF-DCBF'}, 'Location', 'SouthEast');
set(h_legend, 'Interpreter','latex');
set(gca,'LineWidth', 0.2, 'FontSize', 15);
grid on
xlabel('$x (m)$','interpreter','latex','FontSize',20);
ylabel('$y (m)$','interpreter','latex','FontSize',20);
xticks(-5:0);
yticks(-5:0);
xlim([-5,0.2]);
ylim([-5,0.2]);
print(gcf,strcat('figures/',figure_name), '-depsc');
end
end
end
================================================
FILE: matlab/acc2021/MPCCBF.m
================================================
classdef MPCCBF < handle
% MPC with distance constraints
properties
system
params
x0
x_curr
time_curr = 0.0
xlog = []
ulog = []
distlog = []
solvertime = []
xopenloop = {}
uopenloop = {}
u_cost = 0
obs
end
methods
function self = MPCCBF(x0, system, params)
% Define MPC_CBF controller
self.x0 = x0;
self.x_curr = x0;
self.system = system;
self.params = params;
end
function sim(self, time)
% Simulate the system until a given time
xk = self.x_curr;
while self.time_curr <= time
% Solve CFTOC
[~, uk] = self.solveMPCCBF(self.x_curr);
xk = self.system.A * xk + self.system.B * uk;
% update system
self.x_curr = xk;
self.time_curr = self.time_curr + self.system.dt;
self.xlog = [self.xlog, xk];
self.ulog = [self.ulog, uk];
self.u_cost = self.u_cost + uk'*uk*self.system.dt;
end
end
function [xopt, uopt] = solveMPCCBF(self, xk)
% Solve MPC-CBF
[feas, x, u, J] = self.solve_cftoc(xk);
if ~feas
xopt = [];
uopt = [];
return
else
xopt = x(:,2);
uopt = u(:,1);
end
end
function [feas, xopt, uopt, Jopt] = solve_cftoc(self, xk)
% Solve CFTOC
% extract variables
N = self.params.N;
% define variables and cost
x = sdpvar(4, N+1);
u = sdpvar(2, N);
constraints = [];
cost = 0;
% initial constraint
constraints = [constraints; x(:,1) == xk];
% add constraints and costs
for i = 1:N
constraints = [constraints;
self.system.xl <= x(:,i) <= self.system.xu;
self.system.ul <= u(:,i) <= self.system.uu
x(:,i+1) == self.system.A * x(:,i) + self.system.B * u(:,i)];
cost = cost + x(:,i)'*self.params.Q*x(:,i) + u(:,i)'*self.params.R*u(:,i);
end
% add CBF constraints
for i = 1:N
pos = self.obs.pos;
r = self.obs.r ;
b = (x([1:2],i)-pos)'*((x([1:2],i)-pos)) - r^2;
b_next = (x([1:2],i+1)-pos)'*((x([1:2],i+1)-pos)) - r^2;
constraints = [constraints; b_next - b + self.params.gamma * b >= 0];
end
% add terminal cost
cost = cost + x(:,N+1)'*self.params.P*x(:,N+1);
ops = sdpsettings('solver','ipopt','verbose',0);
% solve optimization
diagnostics = optimize(constraints, cost, ops);
if diagnostics.problem == 0
feas = true;
xopt = value(x);
uopt = value(u);
Jopt = value(cost);
else
feas = false;
xopt = [];
uopt = [];
Jopt = [];
end
self.distlog = [self.distlog, sqrt((xk(1:2)-pos)'*(xk(1:2)-pos)-r^2)];
self.xopenloop{size(self.xopenloop,2)+1} = xopt;
self.uopenloop{size(self.uopenloop,2)+1} = uopt;
self.solvertime = [self.solvertime, diagnostics.solvertime];
fprintf('solver time: %f\n', diagnostics.solvertime);
end
function plot(self, figure_name)
% Plot simulation
figure('Renderer', 'painters', 'Position', [0 0 400 400]);
set(gca,'LooseInset',get(gca,'TightInset'));
hold on;
% plot closed-loop trajectory
plot(self.xlog(1,:), self.xlog(2,:), 'ko-',...
'LineWidth', 1.0, 'MarkerSize', 4);
% plot open-loop trajectory
for i = 1:size(self.xopenloop, 2)
plot(self.xopenloop{i}(1,:), self.xopenloop{i}(2,:),...
'k*-.', 'LineWidth', 0.5,'MarkerSize',0.5)
end
% plot obstacle
pos = self.obs.pos;
r = self.obs.r;
th = linspace(0,2*pi*100);
x = cos(th) ; y = sin(th) ;
plot(pos(1) + r*x, pos(2) + r*y, 'Color', [0.8500, 0.3250, 0.0980],...
'LineWidth', 2);
plot(pos(1), pos(2), 'Color', [0.8500, 0.3250, 0.0980],...
'MarkerSize', 5, 'LineWidth', 2);
% plot target position
plot(self.x0(1), self.x0(2), 'db', 'LineWidth', 1);
plot(0.0, 0.0, 'dr', 'LineWidth', 1);
h=get(gca,'Children');
h_legend = legend(h([end]), {'MPC-CBF'}, 'Location', 'SouthEast');
set(h_legend, 'Interpreter','latex');
set(gca,'LineWidth', 0.2, 'FontSize', 15);
grid on
xlabel('$x (m)$','interpreter','latex','FontSize',20);
ylabel('$y (m)$','interpreter','latex','FontSize',20);
xticks(-5:0);
yticks(-5:0);
xlim([-5,0.2]);
ylim([-5,0.2]);
print(gcf,strcat('figures/',figure_name), '-depsc');
end
end
end
================================================
FILE: matlab/acc2021/MPCDC.m
================================================
classdef MPCDC < handle
% MPC with distance constraints
properties
system
params
x0
x_curr
time_curr = 0.0
xlog = []
ulog = []
distlog = []
solvertime = []
xopenloop = {}
uopenloop = {}
u_cost = 0
obs
end
methods
function self = MPCDC(x0, system, params)
% Define MPC_DC controller
self.x0 = x0;
self.x_curr = x0;
self.system = system;
self.params = params;
end
function sim(self, time)
% Simulate the system until a given time
xk = self.x_curr;
while self.time_curr <= time
% Solve CFTOC
[~, uk] = self.solveMPCDC(self.x_curr);
xk = self.system.A * xk + self.system.B * uk;
% update system
self.x_curr = xk;
self.time_curr = self.time_curr + self.system.dt;
self.xlog = [self.xlog, xk];
self.ulog = [self.ulog, uk];
self.u_cost = self.u_cost + uk'*uk*self.system.dt;
end
end
function [xopt, uopt] = solveMPCDC(self, xk)
% Solve MPC-DC
[feas, x, u, J] = self.solve_cftoc(xk);
if ~feas
xopt = [];
uopt = [];
return
else
xopt = x(:,2);
uopt = u(:,1);
end
end
function [feas, xopt, uopt, Jopt] = solve_cftoc(self, xk)
% Solve CFTOC
% extract variables
N = self.params.N;
% define variables and cost
x = sdpvar(4, N+1);
u = sdpvar(2, N);
constraints = [];
cost = 0;
% initial constraint
constraints = [constraints; x(:,1) == xk];
% add constraints and costs
for i = 1:N
constraints = [constraints;
self.system.xl <= x(:,i) <= self.system.xu;
self.system.ul <= u(:,i) <= self.system.uu
x(:,i+1) == self.system.A * x(:,i) + self.system.B * u(:,i)];
cost = cost + x(:,i)'*self.params.Q*x(:,i) + u(:,i)'*self.params.R*u(:,i);
end
% add CBF constraints
for i = 1:N
pos = self.obs.pos;
r = self.obs.r ;
b = (x([1:2],i)-pos)'*((x([1:2],i)-pos)) - r^2;
constraints = [constraints; b >= 0];
end
% add terminal cost
cost = cost + x(:,N+1)'*self.params.P*x(:,N+1);
ops = sdpsettings('solver','ipopt','verbose',0);
% solve optimization
diagnostics = optimize(constraints, cost, ops);
if diagnostics.problem == 0
feas = true;
xopt = value(x);
uopt = value(u);
Jopt = value(cost);
else
feas = false;
xopt = [];
uopt = [];
Jopt = [];
end
self.distlog = [self.distlog, sqrt((xk(1:2)-pos)'*(xk(1:2)-pos)-r^2)];
self.xopenloop{size(self.xopenloop,2)+1} = xopt;
self.uopenloop{size(self.uopenloop,2)+1} = uopt;
self.solvertime = [self.solvertime, diagnostics.solvertime];
fprintf('solver time: %f\n', diagnostics.solvertime);
end
function plot(self, figure_name)
% Plot simulation
figure('Renderer', 'painters', 'Position', [0 0 400 400]);
set(gca,'LooseInset',get(gca,'TightInset'));
hold on;
% plot closed-loop trajectory
plot(self.xlog(1,:), self.xlog(2,:), 'ko-',...
'LineWidth', 1.0, 'MarkerSize', 4);
% plot open-loop trajectory
for i = 1:size(self.xopenloop, 2)
plot(self.xopenloop{i}(1,:), self.xopenloop{i}(2,:),...
'k*-.', 'LineWidth', 0.5,'MarkerSize',0.5)
end
% plot obstacle
pos = self.obs.pos;
r = self.obs.r;
th = linspace(0,2*pi*100);
x = cos(th) ; y = sin(th) ;
plot(pos(1) + r*x, pos(2) + r*y, 'Color', [0.8500, 0.3250, 0.0980],...
'LineWidth', 2);
plot(pos(1), pos(2), 'Color', [0.8500, 0.3250, 0.0980],...
'MarkerSize', 5, 'LineWidth', 2);
% plot target position
plot(self.x0(1), self.x0(2), 'db', 'LineWidth', 1);
plot(0.0, 0.0, 'dr', 'LineWidth', 1);
h=get(gca,'Children');
h_legend = legend(h([end]), {'MPC-DC'}, 'Location', 'SouthEast');
set(h_legend, 'Interpreter','latex');
set(gca,'LineWidth', 0.2, 'FontSize', 15);
grid on
xlabel('$x (m)$','interpreter','latex','FontSize',20);
ylabel('$y (m)$','interpreter','latex','FontSize',20);
xticks(-5:0);
yticks(-5:0);
xlim([-5,0.2]);
ylim([-5,0.2]);
print(gcf,strcat('figures/',figure_name), '-depsc');
end
end
end
================================================
FILE: matlab/acc2021/README.md
================================================
## MPC-CBF-ACC2021
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:
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},
booktitle={2021 American Control Conference (ACC)},
year={2021}
}
```
### 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.
================================================
FILE: matlab/acc2021/figures/.gitignore
================================================
*.eps
================================================
FILE: matlab/acc2021/test.m
================================================
close all
clear
%% General Flags
run_dclf_dcbf = true;
display_dclf_dcbf = true;
run_mpc_cbf_one = true;
display_mpc_cbf_one = true;
run_mpc_cbf_multiple = true;
run_mpc_cbf_multiple = true;
run_mpc_dc = true;
display_mpc_dc = true;
%% Setup and Parameters
x0 = [-5; -5; 0; 0];
time_total = 30.0;
dt = 0.2;
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];
%% Discrete-time double integrator 2D
system.dt = dt;
system.A = [1 0 dt 0;
0 1 0 dt;
0 0 1 0;
0 0 0 1];
system.B = [0.5*dt^2 0;
0 0.5*dt^2;
dt 0;
0 dt];
system.xl = xmin;
system.xu = xmax;
system.ul = umin;
system.uu = umax;
%% DCLF-DCBF parameters
p = 1e3;
params_dclf_dcbf.P = P;
params_dclf_dcbf.H = R;
params_dclf_dcbf.p = p;
params_dclf_dcbf.alpha = 1.0;
params_dclf_dcbf.gamma = 0.4;
%% MPC-CBF parameters
params_mpc_cbf.Q = Q;
params_mpc_cbf.R = R;
params_mpc_cbf.P = P;
params_mpc_cbf.N = N;
params_mpc_cbf.gamma = 0.4;
%% MPC-DC parameters
params_mpc_dc.Q = Q;
params_mpc_dc.R = R;
params_mpc_dc.P = P;
params_mpc_dc.N = N;
%% Obstacle
obs.pos = [-2; -2.25];
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.obs = obs;
controller_dclf_dcbf.sim(time_total);
end
%% Display DCLF-DCBF simulation
if display_dclf_dcbf
% Plot simulation
figure('Renderer', 'painters', 'Position', [0 0 400 400]);
set(gca,'LooseInset',get(gca,'TightInset'));
hold on;
% plot trajectory
plot(controller_dclf_dcbf.xlog(1,:), controller_dclf_dcbf.xlog(2,:), 'ko-',...
'LineWidth', 1.0,'MarkerSize',4);
% plot obstacle
pos = controller_dclf_dcbf.obs.pos;
r = controller_dclf_dcbf.obs.r;
th = linspace(0,2*pi*100);
x = cos(th) ; y = sin(th) ;
plot(pos(1) + r*x, pos(2) + r*y, 'Color', [0.8500, 0.3250, 0.0980], 'LineWidth', 2);
plot(pos(1), pos(2), 'Color', [0.8500, 0.3250, 0.0980], 'MarkerSize', 5, 'LineWidth', 2);
% plot target position
plot(controller_dclf_dcbf.x0(1), controller_dclf_dcbf.x0(2), 'db', 'LineWidth', 1);
plot(0.0, 0.0, 'dr', 'LineWidth', 1);
h=get(gca,'Children');
h_legend = legend(h([end]), {'DCLF-DCBF'}, 'Location', 'SouthEast');
set(h_legend, 'Interpreter','latex');
set(gca,'LineWidth', 0.2, 'FontSize', 15);
grid on
xlabel('$x (m)$','interpreter','latex','FontSize',20);
ylabel('$y (m)$','interpreter','latex','FontSize',20);
xticks(-5:0);
yticks(-5:0);
xlim([-5,0.2]);
ylim([-5,0.2]);
print(gcf,'figures/dclf-dcbf-avoidance.eps', '-depsc');
print(gcf,'figures/dclf-dcbf-avoidance.png', '-dpng', '-r800');
end
%% Simulate MPC-CBF with N=1;
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.obs = obs;
controller_mpc_cbf_one.sim(time_total);
end
%% Display MPC-CBF simulation with N=1
if display_mpc_cbf_one
% Plot simulation
figure('Renderer', 'painters', 'Position', [0 0 400 400]);
set(gca,'LooseInset',get(gca,'TightInset'));
hold on;
% plot trajectory
plot(controller_mpc_cbf_one.xlog(1,:), controller_mpc_cbf_one.xlog(2,:), 'ko-',...
'LineWidth', 1.0,'MarkerSize',4);
% plot obstacle
pos = controller_mpc_cbf_one.obs.pos;
r = controller_mpc_cbf_one.obs.r;
th = linspace(0,2*pi*100);
x = cos(th) ; y = sin(th) ;
plot(pos(1) + r*x, pos(2) + r*y, 'Color', [0.8500, 0.3250, 0.0980], 'LineWidth', 2);
plot(pos(1), pos(2), 'Color', [0.8500, 0.3250, 0.0980], 'MarkerSize', 5, 'LineWidth', 2);
% plot target position
plot(controller_mpc_cbf_one.x0(1), controller_mpc_cbf_one.x0(2), 'db', 'LineWidth', 1);
plot(0.0, 0.0, 'dr', 'LineWidth', 1);
h=get(gca,'Children');
h_legend = legend(h([end]), {'MPC-CBF ($N=1$)'}, 'Location', 'SouthEast');
set(h_legend, 'Interpreter','latex');
set(gca,'LineWidth', 0.2, 'FontSize', 15);
grid on
xlabel('$x (m)$','interpreter','latex','FontSize',20);
ylabel('$y (m)$','interpreter','latex','FontSize',20);
xticks(-5:0);
yticks(-5:0);
xlim([-5,0.2]);
ylim([-5,0.2]);
print(gcf, 'figures/mpc-cbf-avoidance-one-step.eps', '-depsc');
print(gcf, 'figures/mpc-cbf-avoidance-one-step.png', '-dpng', '-r800');
end
%% Simulate MPC-CBF with other N
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.obs = obs;
controller_mpc_cbf_multiple.sim(time_total);
end
%% Display MPC-CBF simulation with other N
if display_mpc_cbf_one
% Plot simulation
figure('Renderer', 'painters', 'Position', [0 0 400 400]);
set(gca,'LooseInset',get(gca,'TightInset'));
hold on;
% plot trajectory
plot(controller_mpc_cbf_multiple.xlog(1,:), controller_mpc_cbf_multiple.xlog(2,:), 'ko-',...
'LineWidth', 1.0,'MarkerSize',4);
% plot obstacle
pos = controller_mpc_cbf_multiple.obs.pos;
r = controller_mpc_cbf_multiple.obs.r;
th = linspace(0,2*pi*100);
x = cos(th) ; y = sin(th) ;
plot(pos(1) + r*x, pos(2) + r*y, 'Color', [0.8500, 0.3250, 0.0980], 'LineWidth', 2);
plot(pos(1), pos(2), 'Color', [0.8500, 0.3250, 0.0980], 'MarkerSize', 5, 'LineWidth', 2);
% plot target position
plot(controller_mpc_cbf_multiple.x0(1), controller_mpc_cbf_multiple.x0(2), 'db', 'LineWidth', 1);
plot(0.0, 0.0, 'dr', 'LineWidth', 1);
h=get(gca,'Children');
h_legend = legend(h([end]), {'MPC-CBF ($N=8$)'}, 'Location', 'SouthEast');
set(h_legend, 'Interpreter','latex');
set(gca,'LineWidth', 0.2, 'FontSize', 15);
grid on
xlabel('$x (m)$','interpreter','latex','FontSize',20);
ylabel('$y (m)$','interpreter','latex','FontSize',20);
xticks(-5:0);
yticks(-5:0);
xlim([-5,0.2]);
ylim([-5,0.2]);
print(gcf, 'figures/mpc-cbf-avoidance-several-steps.eps', '-depsc');
print(gcf, 'figures/mpc-cbf-avoidance-several-steps.png', '-dpng', '-r800');
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.obs = obs;
controller_mpc_dc.sim(time_total);
end
%% Display MPC-DC simulation
if display_mpc_dc
% Plot simulation
figure('Renderer', 'painters', 'Position', [0 0 400 400]);
set(gca,'LooseInset',get(gca,'TightInset'));
hold on;
% plot trajectory
plot(controller_mpc_dc.xlog(1,:), controller_mpc_dc.xlog(2,:), 'ko-',...
'LineWidth', 1.0,'MarkerSize',4);
% plot obstacle
pos = controller_mpc_dc.obs.pos;
r = controller_mpc_dc.obs.r;
th = linspace(0,2*pi*100);
x = cos(th) ; y = sin(th) ;
plot(pos(1) + r*x, pos(2) + r*y, 'Color', [0.8500, 0.3250, 0.0980], 'LineWidth', 2);
plot(pos(1), pos(2), 'Color', [0.8500, 0.3250, 0.0980], 'MarkerSize', 5, 'LineWidth', 2);
% plot target position
plot(controller_mpc_dc.x0(1), controller_mpc_dc.x0(2), 'db', 'LineWidth', 1);
plot(0.0, 0.0, 'dr', 'LineWidth', 1);
h=get(gca,'Children');
h_legend = legend(h([end]), {'MPC-DC ($N=8$)'}, 'Location', 'SouthEast');
set(h_legend, 'Interpreter','latex');
set(gca,'LineWidth', 0.2, 'FontSize', 15);
grid on
xlabel('$x (m)$','interpreter','latex','FontSize',20);
ylabel('$y (m)$','interpreter','latex','FontSize',20);
xticks(-5:0);
yticks(-5:0);
xlim([-5,0.2]);
ylim([-5,0.2]);
print(gcf, 'figures/mpc-dc-avoidance.eps', '-depsc');
print(gcf, 'figures/mpc-dc-avoidance.png', '-dpng', '-r800');
end
================================================
FILE: matlab/acc2021/testBenchmark.m
================================================
close all
clear
%% Setup and Parameters
x0 = [-5; -5; 0; 0];
time_total = 20.0;
dt = 0.2;
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];
%% Discrete-time double integrator 2D
system.dt = dt;
system.A = [1 0 dt 0;
0 1 0 dt;
0 0 1 0;
0 0 0 1];
system.B = [0.5*dt^2 0;
0 0.5*dt^2;
dt 0;
0 dt];
system.xl = xmin;
system.xu = xmax;
system.ul = umin;
system.uu = umax;
%% MPC-CBF parameters
params.Q = Q;
params.R = R;
params.P = P;
params.N = N;
params.gamma = 0.5;
%% Obstacle
obs.pos = [-2; -2.25];
obs.r = 1.5;
%% Simulate MPC-CBF
gamma_list = linspace(0.1, 0.5, 5);
controller_mpc_cbf_list = {};
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.obs = obs;
controller_mpc_cbf.sim(time_total);
controller_mpc_cbf_list{i} = controller_mpc_cbf;
end
%% Computational time benchmark
for i = 1:size(gamma_list, 2)
controller_mpc_cbf = controller_mpc_cbf_list{i};
fprintf('Computational time for MPC-CBF3: mean %.3f, std %.3f, min %.3f, max %.3f, input cost %.3f, min dist %f\n',...
[mean(controller_mpc_cbf.solvertime),...
std(controller_mpc_cbf.solvertime),...
min(controller_mpc_cbf.solvertime),...
max(controller_mpc_cbf.solvertime),...
controller_mpc_cbf.u_cost,...
min(controller_mpc_cbf.distlog)]);
end
================================================
FILE: matlab/acc2021/testGamma.m
================================================
close all
clear
%% Setup and Parameters
x0 = [-5; -5; 0; 0];
time_total = 20.0;
dt = 0.2;
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];
%% Discrete-time double integrator 2D
system.dt = dt;
system.A = [1 0 dt 0;
0 1 0 dt;
0 0 1 0;
0 0 0 1];
system.B = [0.5*dt^2 0;
0 0.5*dt^2;
dt 0;
0 dt];
system.xl = xmin;
system.xu = xmax;
system.ul = umin;
system.uu = umax;
%% MPC-CBF parameters
params_mpc_cbf.Q = Q;
params_mpc_cbf.R = R;
params_mpc_cbf.P = P;
params_mpc_cbf.N = N;
params_mpc_cbf.gamma = 0.5;
%% Obstacle
obs.pos = [-2; -2.25];
obs.r = 1.5;
%% Simulate MPC-CBF
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}.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.obs = obs;
controller_mpc_dc.sim(time_total);
%% Visualization benchmark
figure('Renderer', 'painters', 'Position', [0 0 400 400]);
set(gca,'LooseInset',get(gca,'TightInset'));
hold on
for ind = 1:size(gamma_list, 2)
plot(controller_mpc_cbf_list{ind}.xlog(1,:), controller_mpc_cbf_list{ind}.xlog(2,:),...
'-', 'LineWidth', 1.0, 'MarkerSize', 4);
end
plot(controller_mpc_dc.xlog(1,:), controller_mpc_dc.xlog(2,:),...
'k--', 'LineWidth', 1.0, 'MarkerSize', 4);
% plot obstacle
pos = obs.pos;
r = obs.r;
th = linspace(0,2*pi*100);
x = cos(th) ; y = sin(th) ;
plot(pos(1) + r*x, pos(2) + r*y, 'Color', [0.8500, 0.3250, 0.0980],...
'LineWidth', 2);
plot(pos(1), pos(2), 'Color', [0.8500, 0.3250, 0.0980],...
'MarkerSize', 5, 'LineWidth', 2);
% plot target position
plot(x0(1), x0(2), 'db', 'LineWidth', 1);
plot(0.0, 0.0, 'dr', 'LineWidth', 1);
h=get(gca,'Children');
h_legend = legend(h([end, end-1, end-2, end-3, end-4]),...
{'MPC-CBF ($\gamma=0.1$)', 'MPC-CBF ($\gamma=0.2$)',...
'MPC-CBF ($\gamma=0.3$)', 'MPC-CBF ($\gamma=1.0$)', 'MPC-DC'}, 'Location', 'SouthEast');
set(h_legend, 'Interpreter','latex');
set(gca,'LineWidth', 0.2, 'FontSize', 15);
grid on
xlabel('$x (m)$','interpreter','latex','FontSize',20);
ylabel('$y (m)$','interpreter','latex','FontSize',20);
xticks(-5:1);
yticks(-5:1);
xlim([-5,0.2]);
ylim([-5,0.2]);
print(gcf,'figures/benchmark-gamma.eps', '-depsc');
print(gcf,'figures/benchmark-gamma.png', '-dpng', '-r800');
================================================
FILE: matlab/acc2021/testHorizon.m
================================================
close all
clear
%% Setup and Parameters
x0 = [-5; -5; 0; 0];
time_total = 20.0;
dt = 0.2;
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];
%% Discrete-time double integrator 2D
system.dt = dt;
system.A = [1 0 dt 0;
0 1 0 dt;
0 0 1 0;
0 0 0 1];
system.B = [0.5*dt^2 0;
0 0.5*dt^2;
dt 0;
0 dt];
system.xl = xmin;
system.xu = xmax;
system.ul = umin;
system.uu = umax;
%% MPC-CBF parameters
params.Q = Q;
params.R = R;
params.P = P;
params.N = N;
params.gamma = 0.5;
%% Obstacle
obs.pos = [-2; -2.25];
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.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.obs = obs;
controller_mpc_cbf_2.sim(time_total);
params.gamma = 0.15;
controller_mpc_cbf_3 = MPC_CBF(x0, system, params);
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.obs = obs;
controller_mpc_dc_1.sim(time_total);
params.N = 15;
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 = MPCDC(x0, system, params);
controller_mpc_dc_3.obs = obs;
controller_mpc_dc_3.sim(time_total);
%% Visualization benchmark
figure('Renderer', 'painters', 'Position', [0 0 400 400]);
set(gca,'LooseInset',get(gca,'TightInset'));
hold on
plot(controller_mpc_cbf_3.xlog(1,:), controller_mpc_cbf_3.xlog(2,:),...
'-', 'Color', [0, 0.4470, 0.7410], 'LineWidth', 1.0, 'MarkerSize', 4);
plot(controller_mpc_cbf_2.xlog(1,:), controller_mpc_cbf_2.xlog(2,:),...
'-', 'Color', [0.8500, 0.3250, 0.0980], 'LineWidth', 1.0, 'MarkerSize', 4);
plot(controller_mpc_cbf_1.xlog(1,:), controller_mpc_cbf_1.xlog(2,:),...
'-', 'Color', [0.9290, 0.6940, 0.1250], 'LineWidth', 1.0, 'MarkerSize', 4);
plot(controller_mpc_dc_1.xlog(1,:), controller_mpc_dc_1.xlog(2,:),...
'--', 'Color', [0.4940, 0.1840, 0.5560], 'LineWidth', 1.0, 'MarkerSize', 4);
plot(controller_mpc_dc_2.xlog(1,:), controller_mpc_dc_2.xlog(2,:),...
'--', 'Color', [0.4660, 0.6740, 0.1880], 'LineWidth', 1.0, 'MarkerSize', 4);
plot(controller_mpc_dc_3.xlog(1,:), controller_mpc_dc_3.xlog(2,:),...
'--', 'Color', [0.3010, 0.7450, 0.9330], 'LineWidth', 1.0, 'MarkerSize', 4);
% plot obstacle
pos = obs.pos;
r = obs.r;
th = linspace(0,2*pi*100);
x = cos(th) ; y = sin(th) ;
plot(pos(1) + r*x, pos(2) + r*y, 'Color', [0.8500, 0.3250, 0.0980],...
'LineWidth', 2);
plot(pos(1), pos(2), 'Color', [0.8500, 0.3250, 0.0980],...
'MarkerSize', 5, 'LineWidth', 2);
% plot target position
plot(x0(1), x0(2), 'db', 'LineWidth', 1);
plot(0.0, 0.0, 'dr', 'LineWidth', 1);
h=get(gca,'Children');
h_legend = legend(h([end, end-1, end-2, end-3, end-4, end-5]),...
{'MPC-CBF ($N = 5, \gamma = 0.15$)', 'MPC-CBF ($N = 5, \gamma = 0.20$)',...
'MPC-CBF ($N = 5, \gamma = 0.25$)', 'MPC-DC ($N = 7$)', 'MPC-DC ($N = 15$)',...
'MPC-DC ($N = 30$)'}, 'Location', 'SouthEast');
set(h_legend, 'Interpreter','latex', 'FontSize', 10);
set(gca,'LineWidth', 0.2, 'FontSize', 15);
grid on
xlabel('$x (m)$','interpreter','latex','FontSize',20);
ylabel('$y (m)$','interpreter','latex','FontSize',20);
xticks(-5:0);
yticks(-5:0);
xlim([-5,0.2]);
ylim([-5,0.2]);
print(gcf,'figures/benchmark-horizon.eps', '-depsc');
print(gcf,'figures/benchmark-horizon.png', '-dpng', '-r800');
%% Computational time benchmark
figure('Renderer', 'painters', 'Position', [0 0 800 400]);
xlabel('Computational time (s)', 'interpreter', 'latex', 'FontSize', 16);
ylabel('Percentage', 'interpreter', 'latex', 'FontSize', 16);
hold on
% [N_mpc_3, edges_mpc_3] = histcounts(controller_mpc_cbf_3.solvertime, 10, 'Normalization', 'probability');
% plot(edges_mpc_3(1:end-1), N_mpc_3, '-', 'Color', [0, 0.4470, 0.7410], 'LineWidth', 1);
% [N_mpc_2, edges_mpc_2] = histcounts(controller_mpc_cbf_2.solvertime, 10, 'Normalization', 'probability');
% plot(edges_mpc_2(1:end-1), N_mpc_2, '-', 'Color', [0.8500, 0.3250, 0.0980], 'LineWidth', 1);
[N_mpc_1, edges_mpc_1] = histcounts(controller_mpc_cbf_1.solvertime, 10, 'Normalization', 'probability');
plot(edges_mpc_1(1:end-1), N_mpc_1, '-', 'Color', [0.9290, 0.6940, 0.1250], 'LineWidth', 1);
[N_dc_1, edges_dc_1] = histcounts(controller_mpc_dc_1.solvertime, 10, 'Normalization', 'probability');
plot(edges_dc_1(1:end-1), N_dc_1, '--', 'Color', [0.4940, 0.1840, 0.5560], 'LineWidth', 1);
[N_dc_2, edges_dc_2] = histcounts(controller_mpc_dc_2.solvertime, 10, 'Normalization', 'probability');
plot(edges_dc_2(1:end-1), N_dc_2, '--', 'Color', [0.4660, 0.6740, 0.1880], 'LineWidth', 1);
[N_dc_3, edges_dc_3] = histcounts(controller_mpc_dc_3.solvertime, 10, 'Normalization', 'probability');
plot(edges_dc_3(1:end-1), N_dc_3, '--', 'Color', [0.3010, 0.7450, 0.9330], 'LineWidth', 1);
h=get(gca,'Children');
h_legend = legend(h([end, end-1, end-2, end-3]),...
{'MPC-CBF ($N = 5, \gamma = 0.25$)',...
'MPC-DC ($N = 7$)',...
'MPC-DC ($N = 15$)',...
'MPC-DC ($N = 30$)'}, 'Location', 'NorthEast');
set(h_legend, 'Interpreter','latex');
set(gca,'LineWidth', 0.2, 'FontSize', 15);
print(gcf,'figures/benchmark-horizon-computational-time.eps', '-depsc');
print(gcf,'figures/benchmark-horizon-computational-time.png', '-dpng', '-r800');
%% Computational time table
fprintf('Computational time for MPC-CBF3: mean %.3f, std %.3f, min %.3f, max %.3f, input cost %.3f, min dist %f\n',...
[mean(controller_mpc_cbf_3.solvertime),...
std(controller_mpc_cbf_3.solvertime),...
min(controller_mpc_cbf_3.solvertime),...
max(controller_mpc_cbf_3.solvertime),...
controller_mpc_cbf_3.u_cost,...
min(controller_mpc_cbf_3.distlog)]);
fprintf('Computational time for MPC-CBF2: mean %.3f, std %.3f, min %.3f, max %.3f, input cost %.3f, min dist %f\n',...
[mean(controller_mpc_cbf_2.solvertime),...
std(controller_mpc_cbf_2.solvertime),...
min(controller_mpc_cbf_2.solvertime),...
max(controller_mpc_cbf_2.solvertime),...
controller_mpc_cbf_2.u_cost,...
min(controller_mpc_cbf_2.distlog)]);
fprintf('Computational time for MPC-CBF1: mean %.3f, std %.3f, min %.3f, max %.3f, input cost %.3f, min dist %f\n',...
[mean(controller_mpc_cbf_1.solvertime),...
std(controller_mpc_cbf_1.solvertime),...
min(controller_mpc_cbf_1.solvertime),...
max(controller_mpc_cbf_1.solvertime),...
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),...
min(controller_mpc_dc_1.solvertime),...
max(controller_mpc_dc_1.solvertime),...
controller_mpc_dc_1.u_cost,...
min(controller_mpc_dc_1.distlog)]);
fprintf('Computational time for MPC-DC2: mean %.3f, std %.3f, min %.3f, max %.3f, input cost %.3f, min dist %f\n',...
[mean(controller_mpc_dc_2.solvertime),...
std(controller_mpc_dc_2.solvertime),...
min(controller_mpc_dc_2.solvertime),...
max(controller_mpc_dc_2.solvertime),...
controller_mpc_dc_2.u_cost,...
min(controller_mpc_dc_2.distlog)]);
fprintf('Computational time for MPC-DC3: mean %.3f, std %.3f, min %.3f, max %.3f, input cost %.3f, min dist %f\n',...
[mean(controller_mpc_dc_3.solvertime),...
std(controller_mpc_dc_3.solvertime),...
min(controller_mpc_dc_3.solvertime),...
max(controller_mpc_dc_3.solvertime),...
controller_mpc_dc_3.u_cost,...
min(controller_mpc_dc_3.distlog)]);
================================================
FILE: matlab/acc2023/README.md
================================================
# Iterative-MPC-DHOCBF
Matlab code for the paper *"Iterative Convex Optimization for Model Predictive Control with Discrete-Time High-Order Control Barrier Functions"*, accepted by IEEE American Control Conference (ACC) 2023, Authors: *Shuo Liu, Jun Zeng, Koushil Sreenath and Calin Belta* [[PDF]](https://arxiv.org/pdf/2210.04361.pdf)
In this paper, we propose a framework that solves the safety critical MPC problem in an iterative optimization, which is applicable for any relative-degree control barrier functions. In the proposed formulation, the nonlinear system dynamics as well as the safety constraints modeled as discrete-time high order control barrier functions (DHOCBF) are linearized at each time step. Our formulation is generally valid for any control barrier function with an arbitrary relative-degree. The advantages of fast computational performance with safety guarantee are analyzed and validated with numerical results.
## Citing
If you find this project useful in your work, please consider citing following work:
```
@inproceedings{liu2023iterative,
title={Iterative Convex Optimization for Model Predictive Control with Discrete-Time High-Order Control Barrier Functions},
author={Liu, Shuo and Zeng, Jun and Sreenath, Koushil and Belta, Calin A},
booktitle={2023 American Control Conference (ACC)},
year={2023}
}
```
## Instruction
There are two subfolders `closedloop_performance` and `benchmark`. `closedloop_performance` contains all information to generate figure 2, 3, 4 in paper and `benchmark` conatins code to generate data for table 1,2 in paper.
## Subfolder `closedloop_performance`
### Code Descriptions
* `Closedloop_Trajectories_Hyperparameters.m` includes codes to generate necessary data for figure 2-a and figure 3; *"Iterative_Convergence"* includes codes to generate necessary data for figure 2-b,c,d.
* `Maximum_Iterations.m` includes codes to generate necessary data for figure 4.
* `NMPCDCBF1.m` and `NMPCDCBF2.m` include codes related to NMPC-DHOCBF with `mcbf=1` and `mcbf=2` respectively and `FigureGenerate.m` includes codes to transfer the data into figures in paper.
### Usage
* Run `Closedloop_Trajectories_Hyperparameters.m`, `Iterative_Convergence.m` and `Maximum_Iterations.m` and the data files are generated as MATLAB mat files.
* Run `FigureGenerate.m` to generate all figures in paper, which will be saved in folder `closedloop_performance/figures`.
## Subfolder `benchmark`
### Code Descriptions
There are four folders including the codes to generate necessary dada for table 1,2 in paper.
* `gamma1_0p4gamma2_0p4` includes the codes for iMPC-DHOCBF and NMPC-DHOCBF with decay rate parameters `gamma1=0.4, gamma2=0.4, mcbf=2`.
* `gamma1_0p4` includes the codes for iMPC-DHOCBF and NMPC-DHOCBF with decay rate parameters `gamma1=0.4, mcbf=1`.
* `gamma1_0p6gamma2_0p6` includes the codes for iMPC-DHOCBF and NMPC-DHOCBF with decay rate parameters `gamma1=0.6, gamma2=0.6, mcbf=2`.
* `gamma1_0p6` includes the codes for iMPC-DHOCBF and NMPC-DHOCBF with decay rate parameters `gamma1=0.6, mcbf=1`.
### Usage
* Run `InitialState.m` first to generate 1000 random initial states in the mat file `InitialStateData.mat`, then copy it to each folder.
* In each folder, run the file `test_comprehensive.m` and you will see corresponding data files generated as MATLAB mat files `feasibility_N.mat` and `timecom_N.mat` which includes the information about infeasible rate and mean/variance of computing time (stored in matrices `nmpcdata` and `impcdata`) in paper from generating one time-step trajectories for iMPC-DHOCBF and NMPC-DHOCBF.
### Warnings
* It may take a long time to run the file `test_comprehensive.m` (several hours to a day depending on your computer when the number of horizon reaches large). Instead, if you want to run the file for different number of horizon parallelly to speed up the running process, in folder `test_each_horizon.m`, there are six files called `test_N4.m`, `test_N8.m`, `test_N12.m`, `test_N16.m`, `test_N20.m`,`test_N24.m` which correspond to the number of horizon 4, 8, 12, 16, 20, 24 for iMPC-DHOCBF and NMPC-DHOCBF. By copying `InitialStateData.mat` to this folder then you can run these 6 files parallely, in order to generate same MATLAB mat files `feasibility_N.mat` and `timecom_N.mat` which includes the information about infeasible rate and mean/variance of computing time in paper from generating one time-step trajectories for iMPC-DHOCBF and NMPC-DHOCBF. You should run the file `tabledata.m` next and all useful data in table are stored in matrices `nmpcdata` and `impcdata`.
## Post Analysis
1. For comparison of capability of generating safe optimal trajectories for different numbers of horizon and hyperparameters, please see the figure below:
2. For comparison of infeasible rate and mean/variance of computing time from generating one time-step trajectories, please see the figure below:
For other figures please refer to the paper.
## Dependencies
The packages needed for running the code are [Yalmip](https://yalmip.github.io/) and [IPOPT](https://github.com/coin-or/Ipopt) for NMPC-DHOCBF and [OSQP](https://github.com/osqp/osqp) for iMPC-DHOCBF.
================================================
FILE: matlab/acc2023/benchmark/InitialState.m
================================================
samplen=1000;%Total number of initial states
InitialMat=zeros(4,samplen);%Store initialized x, y, theta, v into each row by sequence
for i=1:samplen
rr=0;
while rr<=1
xini1=(-10+rand(1)*(20));%-10~10
yini1=(-10+rand(1)*(20));%-10~10
rr = (xini1)^2+(yini1)^2;%Initialize location states which are inside safe region
end
thetaini1=(-10+rand(1)*(20));%-10~10
vini1=(-10+rand(1)*(20));%-10~10
InitialMat(1,i)=xini1;
InitialMat(2,i)=yini1;
InitialMat(3,i)=thetaini1;
InitialMat(4,i)=vini1;
end
save('InitialStateData','InitialMat');
================================================
FILE: matlab/acc2023/benchmark/gamma1-0p6/NMPCDCBF1.m
================================================
classdef NMPCDCBF1 < handle
% MPC with distance constraints
properties
system
params
x0
x_curr
time_curr = 0.0
xlog = []
ulog = []
tt = 0
distlog = []
solvertime = []
xopenloop = {}
uopenloop = {}
u_cost = 0
obs
end
methods
function self = NMPCDCBF1(x0, system, params)
% Define MPC_CBF controller
self.x0 = x0;
self.x_curr = x0;
self.system = system;
self.params = params;
end
function sim(self, time)
% Simulate the system until a given time
tic;
xk = self.x_curr;
while self.time_curr < time
[~, uk, tt] = self.solveNMPCDCBF1(self.x_curr);
if tt == -1 %if infeasiblility happens, stop
self.tt = tt;
return
end
xk = self.system.A * xk + [xk(4,1) * cos(xk(3,1))*self.system.dt;xk(4,1) * sin(xk(3,1))*self.system.dt;0;0] + self.system.C * uk;
% update system
self.x_curr = xk;
self.time_curr = self.time_curr + self.system.dt;
self.xlog = [self.xlog, xk];
self.ulog = [self.ulog, uk];
self.u_cost = self.u_cost + uk'*uk;
tt = tt + toc;%record computing time for each time-step
self.tt = tt;
return
end
end
function [xopt, uopt,tt] = solveNMPCDCBF1(self, xk)
% Solve NMPC-DCBF
[feas, x, u, J] = self.solve_cftoc1(xk);
if ~feas
xopt = [];
uopt = [];
tt = -1;
return
else
xopt = x(:,2);
uopt = u(:,1);
tt = 0;
end
end
function [feas, xopt, uopt, Jopt] = solve_cftoc1(self, xk)
% Solve CFTOC
% extract variables
N = self.params.N;
% define variables and cost
x = sdpvar(4, N+1);
u = sdpvar(4, N);
constraints = [];
cost = 0;
% initial constraint
constraints = [constraints; x(:,1) == xk];
% add constraints and costs
AA=self.system.A;
BB=self.system.B;
CC=self.system.C;
constraints = [constraints;
self.system.xl <= x(:,1) <= self.system.xu;
self.system.ul <= u(:,1) <= self.system.uu;
x(:,2) == AA * x(:,1) + [x(4,1)*cos(x(3,1))*self.system.dt;x(4,1)*sin(x(3,1))*self.system.dt;0; 0] + CC * u(:,1)];
cost = cost + (x(:,1)-[3;0.01;0;0])'*self.params.Q*(x(:,1)-[3;0.01;0;0]) + (u(:,1)-[0;0;1;1])'*self.params.R*(u(:,1)-[0;0;1;1]); % self.params.S*(w(:,1)-1)^2;
for i = 2:1:N
constraints = [constraints;
self.system.xl <= x(:,i) <= self.system.xu;
self.system.ul <= u(:,i) <= self.system.uu;
x(:,i+1) == AA * x(:,i) + [x(4,i)*cos(x(3,i))*self.system.dt;x(4,i)*sin(x(3,i))*self.system.dt;0; 0] + CC * u(:,i)];
cost = cost + (x(:,i)-[3;0.01;0;0])'*self.params.Q*(x(:,i)-[3;0.01;0;0]) + (u(:,i)-[0;0;1;1])'*self.params.R*(u(:,i)-[0;0;1;1]);% self.params.S*(w(:,i)-1)^2;
end
% add CBF constraints
for i = 1:N-1
pos = self.obs.pos1;
r = self.obs.r1 ;
b = (x([1:2],i)-pos)'*((x([1:2],i)-pos))-r^2;
b_next = (x([1:2],i+1)-pos)'*((x([1:2],i+1)-pos))-r^2;
constraints = [constraints; b_next >= u(3,i)*(1-self.params.gamma1)*b];%Highest order mcbf=1
end
% add terminal cost
cost = cost + (x(:,N+1)-[3;0.01;0;0])'*self.params.P*(x(:,N+1)-[3;0.01;0;0]);
ops = sdpsettings('solver','ipopt','verbose',0);
% solve optimization
diagnostics = optimize(constraints, cost, ops);
if diagnostics.problem == 0
feas = true;
xopt = value(x);
uopt = value(u);
Jopt = value(cost);
else
feas = false;
xopt = [];
uopt = [];
Jopt = [];
end
pos = self.obs.pos1;
r = self.obs.r1 ;
self.distlog = [self.distlog, (r^2-(xk(1:2)-pos)'*(xk(1:2)-pos))];
self.xopenloop{size(self.xopenloop,2)+1} = xopt;
self.uopenloop{size(self.uopenloop,2)+1} = uopt;
self.solvertime = [self.solvertime, diagnostics.solvertime];
fprintf('solver time: %f\n', diagnostics.solvertime);
end
end
end
================================================
FILE: matlab/acc2023/benchmark/gamma1-0p6/test_comprehensive.m
================================================
close all
clear
%------------------------------------------------------------------------------------------
nmpcdata=zeros(3,6);%Store data in matrix for nmpc and impc, first row: mean of computing time; second row: standard deviation of computing time; third row: infeasible rate
impcdata=zeros(3,6);
for ii = 1:6
nb = 1000;%Total number of initial states
nmpcd=zeros(1,nb);%Store computing time for one time-step for NMPC-DCBF
impcd=zeros(1,nb);%Store computing time for one time-step for iMPC-DCBF
nmpcinf=zeros(1,1);%Store infeasible rate for one time-step for NMPC-DCBF
impcinf=zeros(1,1);%Store infeasible rate for one time-step for iMPC-DCBF
i=ii*4;%i is number of horozon
ts = 1;%Total time steps
load(gamma1_0p6\InitialStateData');%Load the generated 1000 initial states
[a, b, c, d]=compare(i,ts,nb,InitialMat);%Involves NMPC-DCBF and iMPC-DCBF
nmpcd(1,:)=a;
impcd(1,:)=b;
nmpcinf(1,1)=c;
impcinf(1,1)=d;
nmpcplot1=nmpcd(1,:);
nmpcplot1(nmpcplot1==-1)=[];%Eliminate infeasible one time-step trajectories
impcplot1=impcd(1,:);
impcplot1(impcplot1==-1)=[];%Eliminate infeasible one time-step trajectories
filenm1 = ['timecom' num2str(i) '.mat'];
filenm2 = ['feasibility' num2str(i) '.mat'];
save(filenm1,'nmpcplot1','impcplot1');
save(filenm2,'nmpcinf','impcinf');
distnmpc = fitdist(nmpcplot1','Normal');
distimpc = fitdist(impcplot1','Normal');
mu1=distnmpc.mu;%Get mean of sample of computing time for NMPC-DCBF
mu2=distimpc.mu;%Get mean of sample of computing time for iMPC-DCBF
sigma1=distnmpc.sigma;%Get variance of sample of computing time for NMPC-DCBF
sigma2=distimpc.sigma;%Get variance of sample of computing time for iMPC-DCBF
nmpcdata(1,ii)=mu1;
impcdata(1,ii)=mu2;
nmpcdata(2,ii)=sigma1;
impcdata(2,ii)=sigma2;
nmpcdata(3,ii)=nmpcinf;
impcdata(3,ii)=impcinf;
end
%Initialize atmosphere parameters
function [tnmpc, timpc, ratiotnmpc, ratiotimpc]=compare(N11,ttsim,samplen,InitialMat)
tnmpc=[];
timpc=[];
for i=1:samplen
N1=N11;
tsim=ttsim;
xini1=InitialMat(1,i);
yini1=InitialMat(2,i);
thetaini1=InitialMat(3,i);
vini1=InitialMat(4,i);
x01=[xini1;yini1;thetaini1;vini1];
x02=[xini1 yini1 thetaini1 vini1];
t1 = nmpcdcbf(x01, N1, tsim);
t2 = impcdcbf(x02, N1, tsim);
tindex=[N11 i];
disp(tindex);
tnmpc=[tnmpc t1];%Computing time for NMPC-DCBF
timpc=[timpc t2];%Computing time for iMPC-DCBF
end
nnmpc1 = length(tnmpc);
nimpc1 = length(timpc);
tnmpcs = tnmpc;
timpcs = timpc;
tnmpcs(tnmpcs==-1)=[];
timpcs(timpcs==-1)=[];
nnmpc2 = length(tnmpcs);
nimpc2 = length(timpcs);
ratiotnmpc = (nnmpc1-nnmpc2)/nnmpc1;%Infeasible rate for NMPC-DCBF
ratiotimpc = (nimpc1-nimpc2)/nimpc1;%Infeasible rate for iMPC-DCBF
end
function t1 = nmpcdcbf(x00, N1, ttsim)
%% General Flags
run_nmpc_dcbf_one = true;
run_nmpc_dcbf_multiple = true;
%% Setup and Parameters
x0 = x00;
dt = 0.1;
time_total = ttsim *dt;
P = zeros(4,4);%Weight matrix P
P(1,1) = 10; P(2,2) = 10; P(3,3) = 10;P(4,4) = 10;
Q = zeros(4,4);%Weight matrix Q
Q(1,1) = 10; Q(2,2) = 10; Q(3,3) = 10;Q(4,4) = 10;
R = zeros(4,4);%Weight matrix R
R(1,1) = 1; R(2,2) = 1;R(3,3) = 1000;R(4,4) = 1000;
%Variables range as below
xmin = [-10; -10; -10;-10];
xmax = [10; 10; 10;10];
umin = [-7; -5;-inf;-inf];
umax = [7; 5;inf;inf];
%% Discrete-time unicycle model
system.dt = dt;
system.A = [1 0 0 0;
0 1 0 0;
0 0 1 0;
0 0 0 1];
system.B = [x0(4,1)*cos(x0(3,1))*dt;
x0(4,1)*sin(x0(3,1))*dt;
0;
0];
system.C =[0 0 0 0;
0 0 0 0;
1*dt 0 0 0;
0 1*dt 0 0];
system.xl = xmin;
system.xu = xmax;
system.ul = umin;
system.uu = umax;
%% NMPC-DCBF parameters
params_nmpc_dcbf.Q = Q;
params_nmpc_dcbf.R = R;
params_nmpc_dcbf.P = P;
params_nmpc_dcbf.gamma1 = 0.6;
params_nmpc_dcbf.gamma2 = 0.6;
%% Obstacle
obs.pos1 = [0; 0];
obs.r1 = 1;
%% Simulate NMPC-DCBF with other N
params_nmpc_dcbf.N = N1;
if run_nmpc_dcbf_one
fprintf('Run NMPC-DCBF-4\n');
controller_nmpc_dcbf_multiple4 = NMPCDCBF1(x0, system, params_nmpc_dcbf);%Highest order mcbf=2
controller_nmpc_dcbf_multiple4.obs = obs;
controller_nmpc_dcbf_multiple4.sim(time_total);
end
t1=controller_nmpc_dcbf_multiple4.tt;
end
function t2=impcdcbf(x00, N1, ttsim)
t2=0;
xo = 0;
yo = 0;
r = 1;
N = N1;%Number of horizon
K1 = 1000;%Maximum iteration times, jmax
K = 1000;%Maximum iteration times, jmax
dt = 0.1;
gamma1 = 6;
gamma2 = 6;
nsim = ttsim;
tic;
% Constraints
umin = [-7; -5; -inf; -inf;];
umax = [7; 5; Inf; Inf];
xmin = [-10; -10; -10; -10];
xmax = [ 10; 10; 10; 10];
% Objective function
Q = diag([10 10 10 10]);
QN = Q;
R = 1 * eye(4);
R(3,3) = 1000;
R(4,4) = 1000;
% Initial and reference states
x0 = x00;
xr = [3; 0.01; 0; 0];
ur = [0; 0; 1; 1];
% Dynamic system initialization
BB = [0 0 0 0;0 0 0 0;dt 0 0 0;0 dt 0 0];
% Convex MPC frame
[nx, nu] = size(BB);
% Initialize states set (x00,x01,,,x0N）
x_0 = [];
x0 = x0';
u_0 = zeros(nu, N);
u0 = zeros(nu, 1);% we may need to change this for a better warm-up start
impc = zeros(nx, nsim + 1);
impc(:, 1) = x0;
x0new = x0;
for i=1 : (N+1)
x_0 = [x_0 x0new];
x0new = [dt*x0new(4)*cos(x0new(3))+x0new(1);dt*x0new(4)*sin(x0new(3))+x0new(2);dt*u0(1)+x0new(3);dt*u0(2)+x0new(4)];
end
abc = tangentline(x_0, r);
Acbf = cbfmat(abc, nx, nu, dt, gamma1, x0);%First order CBF constraints
lcbf = -abc(3, :);
lcbf(1) = [];
lcbf = lcbf';
ucbf = inf * ones(N, 1);
A2cbf = cbfmat2(abc, nx, nu, dt, gamma1, gamma2, x0);%Second order CBF constraints
lcbf2 = getlcbf2(abc, dt, gamma1, gamma2);
ucbf2 = inf * ones(N-1, 1);
% Cast MPC problem to a QP: x = (x(0),x(1),...,x(N),u(0),...,u(N-1))
% - quadratic objective
P = blkdiag( kron(speye(N), Q), QN, kron(speye(N), R) );
% - linear objective
q = [repmat(-Q*xr, N, 1); -QN*xr; repmat(-R*ur, N, 1)];
% - linear dynamics
Ax = getax(x_0, dt, N, nx);
Bu = kron([sparse(1, N); speye(N)], BB);
Cx = getcx(x_0, u_0, dt, N, nx);
Aeq = [Ax, Bu];
leq = [-x0; -Cx];
ueq = leq;
% - input and state constraints
Aineq = speye((N+1)*nx + N*nu);
lineq = [repmat(xmin, N+1, 1); repmat(umin, N, 1)];
uineq = [repmat(xmax, N+1, 1); repmat(umax, N, 1)];
% - OSQP constraints
A = [Aeq; Aineq; Acbf; A2cbf];
l = [leq; lineq; lcbf; lcbf2];
u = [ueq; uineq; ucbf; ucbf2];
% Create an OSQP object
prob = osqp;
% Setup workspace
prob.setup(P, q, A, l, u, 'warm_start', true);
% Solve
res = prob.solve();
% Check solver status
if ~strcmp(res.info.status, 'solved')
% error('OSQP did not solve the problem!')
t2=-1;
return
end
ctrlx = res.x(1:(N+1)*nx);
ctrlu = res.x((N+1)*nx+1:(N+1)*nx+N*nu);
for j = 1 : (K1-1)%Iterations for the first time-step
storagex = ctrlx;%store the state variables and inputs in order to compare them with the next iteration
storageu = ctrlu;
x_0 = trans(x0, ctrlx, nx, N+1);
u_0 = transu(ctrlu, nu, N);
abc = tangentline(x_0, r);
Acbf = cbfmat(abc, nx, nu, dt, gamma1, x0);
lcbf = -abc(3, :);
lcbf(1) = [];
lcbf = lcbf';
ucbf = inf * ones(N, 1);
A2cbf = cbfmat2(abc, nx, nu, dt, gamma1, gamma2, x0);
lcbf2 = getlcbf2(abc, dt, gamma1, gamma2);
ucbf2 = inf * ones(N-1, 1);
% Cast MPC problem to a QP: x = (x(0),x(1),...,x(N),u(0),...,u(N-1))
% - quadratic objective
P = blkdiag( kron(speye(N), Q), QN, kron(speye(N), R) );
% - linear objective
q = [repmat(-Q*xr, N, 1); -QN*xr; repmat(-R*ur, N, 1)];
% - linear dynamics
Ax = getax(x_0, dt, N, nx);
Bu = kron([sparse(1, N); speye(N)], BB);
Cx = getcx(x_0, u_0, dt, N, nx);
Aeq = [Ax, Bu];
leq = [-x0; -Cx];
ueq = leq;
% - input and state constraints
Aineq = speye((N+1)*nx + N*nu);
lineq = [repmat(xmin, N+1, 1); repmat(umin, N, 1)];
uineq = [repmat(xmax, N+1, 1); repmat(umax, N, 1)];
% - OSQP constraints
A = [Aeq; Aineq; Acbf; A2cbf];
l = [leq; lineq; lcbf; lcbf2];
u = [ueq; uineq; ucbf; ucbf2];
% Create an OSQP object
prob = osqp;
% Setup workspace
prob.setup(P, q, A, l, u, 'warm_start', true);
% Solve
res = prob.solve();
% Check solver status
if ~strcmp(res.info.status, 'solved')
% error('OSQP did not solve the problem!')
t2=-1;
return
end
ctrlx = res.x(1:(N+1)*nx);
ctrlu = res.x((N+1)*nx+1:(N+1)*nx+N*nu);
testx = (storagex - ctrlx)'*(storagex - ctrlx);
testu = (storageu - ctrlu)'*(storageu - ctrlu);
test = (testx)/(storagex'*storagex);
if (test)^(0.5)<=10^(-2)&& (testx/((N+1)*nx))^(0.5)<=10^(-4)%Convergence criterion
break
end
end % Move for the first step
ctrl = ctrlu(1:nu);
x0 = [dt*x0(4)*cos(x0(3))+x0(1);dt*x0(4)*sin(x0(3))+x0(2);dt*ctrl(1)+x0(3);dt*ctrl(2)+x0(4)];
ctrlu = rewrite(ctrlu, nu, N);
t2=t2+toc;
return
x_0 = newinit(x0, ctrlu, nx, nu, N, dt);
u_0 = transu(ctrlu, nu, N);
impc(:, 2) = x0;
storagex = ctrlx;
storageu = ctrlu;
for i = 1 : (nsim-1)%Iterations for the left time steps
for j = 1 : K
abc = tangentline(x_0, r);
Acbf = cbfmat(abc, nx, nu, dt, gamma1, x0);
lcbf = -abc(3, :);
lcbf(1) = [];
lcbf = lcbf';
ucbf = inf * ones(N, 1);
A2cbf = cbfmat2(abc, nx, nu, dt, gamma1, gamma2, x0);
lcbf2 = getlcbf2(abc, dt, gamma1, gamma2);
ucbf2 = inf * ones(N-1, 1);
% Cast MPC problem to a QP: x = (x(0),x(1),...,x(N),u(0),...,u(N-1))
% - quadratic objective
P = blkdiag( kron(speye(N), Q), QN, kron(speye(N), R) );
% - linear objective
q = [repmat(-Q*xr, N, 1); -QN*xr; repmat(-R*ur, N, 1)];
% - linear dynamics
Ax = getax(x_0, dt, N, nx);
Bu = kron([sparse(1, N); speye(N)], BB);
Cx = getcx(x_0, u_0, dt, N, nx);
Aeq = [Ax, Bu];
leq = [-x0; -Cx];
ueq = leq;
% - input and state constraints
Aineq = speye((N+1)*nx + N*nu);
lineq = [repmat(xmin, N+1, 1); repmat(umin, N, 1)];
uineq = [repmat(xmax, N+1, 1); repmat(umax, N, 1)];
% - OSQP constraints
A = [Aeq; Aineq; Acbf; A2cbf];
l = [leq; lineq; lcbf; lcbf2];
u = [ueq; uineq; ucbf; ucbf2];
% Create an OSQP object
prob = osqp;
% Setup workspace
prob.setup(P, q, A, l, u, 'warm_start', true);
% Solve
res = prob.solve();
% Check solver status
if ~strcmp(res.info.status, 'solved')
% error('OSQP did not solve the problem!')
t2=-1;
return
end
ctrlx = res.x(1:(N+1)*nx);
ctrlu = res.x((N+1)*nx+1:(N+1)*nx+N*nu);
x0 = ctrlx(1:nx);
testx0 = [testx0 x0];
x_0 = trans(x0, ctrlx, nx, N+1);
u_0 = transu(ctrlu, nu, N);
testx = (storagex - ctrlx)'*(storagex - ctrlx);
testu = (storageu - ctrlu)'*(storageu - ctrlu);
test = (testx)/(storagex'*storagex);
if (test)^(0.5)<=10^(-2)&& (testx/((N+1)*nx))^(0.5)<=10^(-4)%Convergence criterion
break
end
storagex = ctrlx;
storageu = ctrlu;
end
ctrl = ctrlu(1:nu);
x0 = [dt*x0(4)*cos(x0(3))+x0(1);dt*x0(4)*sin(x0(3))+x0(2);dt*ctrl(1)+x0(3);dt*ctrl(2)+x0(4)];
ctrlu = rewrite(ctrlu, nu, N);
x_0 = newinit(x0, ctrlu, nx, nu, N, dt);
u_0 = transu(ctrlu, nu, N);
impc(:, i+2) = x0;
end
end
% Linerize the CBF constraints (get a, b, c for lines)
function abc = tangentline(xy, r)% x and y from initialize states set, abc are coeeficients for linear equation a*x+b*y+c=0
[xx, ~] = size(xy);%xx=2,yy=N+1
xy(xx,:) = []; % this part should be changed for other case
xy((xx-1),:) = []; % this part should be changed other case
[xx, yy] = size(xy);%xx=2,yy=N+1
xyjiao = zeros(xx, yy);%intersection points
for i = 1 : xx
for j = 1 : yy
xyjiao(i, j) = r * xy(i, j) * (1 / (xy(:, j)' * xy(:, j)))^(0.5);%calculate coordinates of intersection points
end
end
cc = -r^2 * ones(1, yy);
abc = [xyjiao; cc];
end
% Get CBF constraints matrix
function Acbf = cbfmat(abc, nx, nu, dt, gamma, x0)
[~, yy] = size(abc);
Acbfx = zeros((yy-1), yy*nx);
Acbfu = zeros((yy-1), (yy-1)*nu);
for i = 1 : (yy-1)
Acbfx(i, (i*nx)+1) = abc(1, (i+1));
Acbfx(i, (i*nx)+2) = abc(2, (i+1));
end
for i = 1 : (yy-1)
Acbfu(i, ((i-1)*nu+3)) = - (1 - dt * gamma)^(i) * (abc(1, 1) * x0(1, 1) + abc(2, 1) * x0(2, 1) + abc(3, 1));
end
Acbf = [Acbfx Acbfu];
end
% Transfer vector x into matrix
function res = trans(x0, vector, nxx, nyy)%nxx=nx,nyy=N+1
res = zeros(nxx, nyy);
res(:,1) = x0;
for i = 1 : (nyy -1)
res(:,i+1)= vector(((i)*nxx+1):(i+1)*nxx);
end
end
% Transfer vector u into matrix
function resu = transu(vector, nxx, nyy)%nxx=nu,nyy=N
resu = zeros(nxx, nyy);
for i = 1 : (nyy)
resu(:,i)= vector(((i-1)*nxx+1):(i)*nxx);
end
end
% Rewrite u vector
function reu = rewrite(vector, nu, N)
append = vector((N-1)*nu+1:N*nu);
vector(1:nu) = [];
reu = [vector;append];
end
% Get new x_0
function x_0 = newinit(x0, ctrlu, nx, nu, N, dt)
x_0 = zeros(nx, N+1);
x_0(:, 1) = x0;
for i=1 : N
u0 = ctrlu((i-1)*nu+1:i*nu);
x0 = [dt*x0(4)*cos(x0(3))+x0(1);dt*x0(4)*sin(x0(3))+x0(2);dt*u0(1)+x0(3);dt*u0(2)+x0(4)];
x_0(:, i + 1) = x0;
end
end
% Get AA matrix
function AA = getaa(x0, dt)
AA = [1 0 -x0(4)*sin(x0(3))*dt cos(x0(3))*dt;0 1 x0(4)*cos(x0(3))*dt sin(x0(3))*dt;0 0 1 0;0 0 0 1];
end
% Get CC matrix
function CC = getcc(x0, x1, u0, dt)
CC = [x0(4)*sin(x0(3))*x0(3)*dt-x0(4)*cos(x0(3))*dt+x1(1)-x0(1);-x0(4)*cos(x0(3))*x0(3)*dt-x0(4)*sin(x0(3))*dt+x1(2)-x0(2);-u0(1)*dt+x1(3)-x0(3);-u0(2)*dt+x1(4)-x0(4)];
end
% Get Ax matrix
function Ax = getax(x_0, dt, N, nx)
x0 = x_0(:,1);
AA = getaa(x0, dt);
Ax = kron(speye(N+1), -speye(nx)) + kron(sparse(diag(ones(N, 1), -1)), AA);
for i = 1 : (N-1)
x0 = x_0(:,i+1);
AA = getaa(x0, dt);
Ax(nx*(i+1)+1:nx*(i+1)+nx,nx*i+1:nx*i+nx) = AA;
end
end
% Get Cx matrix
function Cx = getcx(x_0, u_0, dt, N, nx)
Cx = zeros(N*nx, 1);
for i = 1 : N
u0 = u_0(:,i);
x0 = x_0(:,i);
x1 = x_0(:,i+1);
CC = getcc(x0, x1, u0, dt);
Cx((i-1)*nx+1:(i-1)*nx+nx) = CC;
end
end
% Get A2cbf
function A2cbf = cbfmat2(abc, nx, nu, dt, gamma1, gamma2, x0)
[~, yy] = size(abc);
Acbfx2 = zeros((yy-2), yy*nx);
Acbfx22 = zeros((yy-2), yy*nx);
Acbfu2 = zeros((yy-2), (yy-1)*nu);
for i = 1 : (yy-2)
Acbfx2(i, (i*nx)+1) = (gamma1-1/dt)*abc(1, (i+1));
Acbfx2(i, (i*nx)+2) = (gamma1-1/dt)*abc(2, (i+1));
Acbfx2(i, ((i+1)*nx)+1) = (1/dt)*abc(1, (i+2));
Acbfx2(i, ((i+1)*nx)+2) = (1/dt)*abc(2, (i+2));
end
for i = 1 : (yy-2)
Acbfx22(i, (1*nx)+1) = -(1-dt*gamma2)^(i)/dt*abc(1, (1+1));
Acbfx22(i, (1*nx)+2) = -(1-dt*gamma2)^(i)/dt*abc(2, (1+1));
end
Acbfx2 = Acbfx2 + Acbfx22;
for i = 1 : (yy-2)
Acbfu2(i, ((i-1)*nu+4)) = - (1 - dt * gamma2)^(i)*(gamma1-1/dt)*(abc(1, 1) * x0(1, 1) + abc(2, 1) * x0(2, 1) + abc(3, 1));
end
A2cbf = [Acbfx2 Acbfu2];
end
% Get lcbf2
function lcbf2 = getlcbf2(abc, dt, gamma1, gamma2)
[~, yy] = size(abc);
lcbf2 = zeros((yy-2),1);
for i = 1 : (yy-2)
lcbf2(i, 1) = -abc(3, (i+2))/dt-(gamma1-1/dt)*abc(3, (i+1))+(1 - dt * gamma2)^(i)*abc(3, (1+1))/dt;
end
end
================================================
FILE: matlab/acc2023/benchmark/gamma1-0p6/test_each_horizon/NMPCDCBF1.m
================================================
classdef NMPCDCBF1 < handle
% MPC with distance constraints
properties
system
params
x0
x_curr
time_curr = 0.0
xlog = []
ulog = []
tt = 0
distlog = []
solvertime = []
xopenloop = {}
uopenloop = {}
u_cost = 0
obs
end
methods
function self = NMPCDCBF1(x0, system, params)
% Define MPC_CBF controller
self.x0 = x0;
self.x_curr = x0;
self.system = system;
self.params = params;
end
function sim(self, time)
% Simulate the system until a given time
tic;
xk = self.x_curr;
while self.time_curr < time
[~, uk, tt] = self.solveNMPCDCBF1(self.x_curr);
if tt == -1 %if infeasiblility happens, stop
self.tt = tt;
return
end
xk = self.system.A * xk + [xk(4,1) * cos(xk(3,1))*self.system.dt;xk(4,1) * sin(xk(3,1))*self.system.dt;0;0] + self.system.C * uk;
% update system
self.x_curr = xk;
self.time_curr = self.time_curr + self.system.dt;
self.xlog = [self.xlog, xk];
self.ulog = [self.ulog, uk];
self.u_cost = self.u_cost + uk'*uk;
tt = tt + toc;%record computing time for each time-step
self.tt = tt;
return
end
end
function [xopt, uopt,tt] = solveNMPCDCBF1(self, xk)
% Solve NMPC-DCBF
[feas, x, u, J] = self.solve_cftoc1(xk);
if ~feas
xopt = [];
uopt = [];
tt = -1;
return
else
xopt = x(:,2);
uopt = u(:,1);
tt = 0;
end
end
function [feas, xopt, uopt, Jopt] = solve_cftoc1(self, xk)
% Solve CFTOC
% extract variables
N = self.params.N;
% define variables and cost
x = sdpvar(4, N+1);
u = sdpvar(4, N);
constraints = [];
cost = 0;
% initial constraint
constraints = [constraints; x(:,1) == xk];
% add constraints and costs
AA=self.system.A;
BB=self.system.B;
CC=self.system.C;
constraints = [constraints;
self.system.xl <= x(:,1) <= self.system.xu;
self.system.ul <= u(:,1) <= self.system.uu;
x(:,2) == AA * x(:,1) + [x(4,1)*cos(x(3,1))*self.system.dt;x(4,1)*sin(x(3,1))*self.system.dt;0; 0] + CC * u(:,1)];
cost = cost + (x(:,1)-[3;0.01;0;0])'*self.params.Q*(x(:,1)-[3;0.01;0;0]) + (u(:,1)-[0;0;1;1])'*self.params.R*(u(:,1)-[0;0;1;1]); % self.params.S*(w(:,1)-1)^2;
for i = 2:1:N
constraints = [constraints;
self.system.xl <= x(:,i) <= self.system.xu;
self.system.ul <= u(:,i) <= self.system.uu;
x(:,i+1) == AA * x(:,i) + [x(4,i)*cos(x(3,i))*self.system.dt;x(4,i)*sin(x(3,i))*self.system.dt;0; 0] + CC * u(:,i)];
cost = cost + (x(:,i)-[3;0.01;0;0])'*self.params.Q*(x(:,i)-[3;0.01;0;0]) + (u(:,i)-[0;0;1;1])'*self.params.R*(u(:,i)-[0;0;1;1]);% self.params.S*(w(:,i)-1)^2;
end
% add CBF constraints
for i = 1:N-1
pos = self.obs.pos1;
r = self.obs.r1 ;
b = (x([1:2],i)-pos)'*((x([1:2],i)-pos))-r^2;
b_next = (x([1:2],i+1)-pos)'*((x([1:2],i+1)-pos))-r^2;
constraints = [constraints; b_next >= u(3,i)*(1-self.params.gamma1)*b];%Highest order mcbf=1
end
% add terminal cost
cost = cost + (x(:,N+1)-[3;0.01;0;0])'*self.params.P*(x(:,N+1)-[3;0.01;0;0]);
ops = sdpsettings('solver','ipopt','verbose',0);
% solve optimization
diagnostics = optimize(constraints, cost, ops);
if diagnostics.problem == 0
feas = true;
xopt = value(x);
uopt = value(u);
Jopt = value(cost);
else
feas = false;
xopt = [];
uopt = [];
Jopt = [];
end
pos = self.obs.pos1;
r = self.obs.r1 ;
self.distlog = [self.distlog, (r^2-(xk(1:2)-pos)'*(xk(1:2)-pos))];
self.xopenloop{size(self.xopenloop,2)+1} = xopt;
self.uopenloop{size(self.uopenloop,2)+1} = uopt;
self.solvertime = [self.solvertime, diagnostics.solvertime];
fprintf('solver time: %f\n', diagnostics.solvertime);
end
end
end
================================================
FILE: matlab/acc2023/benchmark/gamma1-0p6/test_each_horizon/tabledata.m
================================================
nmpcdata=zeros(3,6);%Store data in matrix for nmpc and impc, first row: mean of computing time; second row: standard deviation of computing time; third row: infeasible rate
impcdata=zeros(3,6);
for ii = 1:6
i=ii*4;
filenm1 = ['timecom' num2str(i) '.mat'];
filenm2 = ['feasibility' num2str(i) '.mat'];
load(filenm1);
load(filenm2);
distnmpc = fitdist(nmpcplot1','Normal');
distimpc = fitdist(impcplot1','Normal');
mu1=distnmpc.mu;%Get mean of sample of computing time for NMPC-DCBF
mu2=distimpc.mu;%Get mean of sample of computing time for iMPC-DCBF
sigma1=distnmpc.sigma;%Get variance of sample of computing time for NMPC-DCBF
sigma2=distimpc.sigma;%Get variance of sample of computing time for iMPC-DCBF
nmpcdata(1,ii)=mu1;
impcdata(1,ii)=mu2;
nmpcdata(2,ii)=sigma1;
impcdata(2,ii)=sigma2;
nmpcdata(3,ii)=nmpcinf;
impcdata(3,ii)=impcinf;
end
================================================
FILE: matlab/acc2023/benchmark/gamma1-0p6/test_each_horizon/test_N12.m
================================================
close all
clear
%------------------------------------------------------------------------------------------
nb = 1000;%Total number of initial states
nmpcd=zeros(1,nb);%Store computing time for one time-step for NMPC-DCBF
impcd=zeros(1,nb);%Store computing time for one time-step for iMPC-DCBF
nmpcinf=zeros(1,1);%Store infeasible rate for one time-step for NMPC-DCBF
impcinf=zeros(1,1);%Store infeasible rate for one time-step for iMPC-DCBF
i=12;%i is number of horozon
ts = 1;%Total time steps
load(gamma1_0p6\InitialStateData');%Load the generated 1000 initial states
[a, b, c, d]=compare(i,ts,nb,InitialMat);%Involves NMPC-DCBF and iMPC-DCBF
nmpcd(1,:)=a;
impcd(1,:)=b;
nmpcinf(1,1)=c;
impcinf(1,1)=d;
nmpcplot1=nmpcd(1,:);
nmpcplot1(nmpcplot1==-1)=[];%Eliminate infeasible one time-step trajectories
impcplot1=impcd(1,:);
impcplot1(impcplot1==-1)=[];%Eliminate infeasible one time-step trajectories
save('timecom12','nmpcplot1','impcplot1');
save('feasibility12','nmpcinf','impcinf');
distnmpc = fitdist(nmpcplot1','Normal');
distimpc = fitdist(impcplot1','Normal');
mu1=distnmpc.mu;%Get mean of sample of computing time for NMPC-DCBF
mu2=distimpc.mu;%Get mean of sample of computing time for iMPC-DCBF
sigma1=distnmpc.sigma;%Get variance of sample of computing time for NMPC-DCBF
sigma2=distimpc.sigma;%Get variance of sample of computing time for iMPC-DCBF
%Initialize atmosphere parameters
function [tnmpc, timpc, ratiotnmpc, ratiotimpc]=compare(N11,ttsim,samplen,InitialMat)
tnmpc=[];
timpc=[];
for i=1:samplen
N1=N11;
tsim=ttsim;
xini1=InitialMat(1,i);
yini1=InitialMat(2,i);
thetaini1=InitialMat(3,i);
vini1=InitialMat(4,i);
x01=[xini1;yini1;thetaini1;vini1];
x02=[xini1 yini1 thetaini1 vini1];
t1 = nmpcdcbf(x01, N1, tsim);
t2 = impcdcbf(x02, N1, tsim);
tindex=[N11 i];
disp(tindex);
tnmpc=[tnmpc t1];%Computing time for NMPC-DCBF
timpc=[timpc t2];%Computing time for iMPC-DCBF
end
nnmpc1 = length(tnmpc);
nimpc1 = length(timpc);
tnmpcs = tnmpc;
timpcs = timpc;
tnmpcs(tnmpcs==-1)=[];
timpcs(timpcs==-1)=[];
nnmpc2 = length(tnmpcs);
nimpc2 = length(timpcs);
ratiotnmpc = (nnmpc1-nnmpc2)/nnmpc1;%Infeasible rate for NMPC-DCBF
ratiotimpc = (nimpc1-nimpc2)/nimpc1;%Infeasible rate for iMPC-DCBF
end
function t1 = nmpcdcbf(x00, N1, ttsim)
%% General Flags
run_nmpc_dcbf_one = true;
run_nmpc_dcbf_multiple = true;
%% Setup and Parameters
x0 = x00;
dt = 0.1;
time_total = ttsim *dt;
P = zeros(4,4);%Weight matrix P
P(1,1) = 10; P(2,2) = 10; P(3,3) = 10;P(4,4) = 10;
Q = zeros(4,4);%Weight matrix Q
Q(1,1) = 10; Q(2,2) = 10; Q(3,3) = 10;Q(4,4) = 10;
R = zeros(4,4);%Weight matrix R
R(1,1) = 1; R(2,2) = 1;R(3,3) = 1000;R(4,4) = 1000;
%Variables range as below
xmin = [-10; -10; -10;-10];
xmax = [10; 10; 10;10];
umin = [-7; -5;-inf;-inf];
umax = [7; 5;inf;inf];
%% Discrete-time unicycle model
system.dt = dt;
system.A = [1 0 0 0;
0 1 0 0;
0 0 1 0;
0 0 0 1];
system.B = [x0(4,1)*cos(x0(3,1))*dt;
x0(4,1)*sin(x0(3,1))*dt;
0;
0];
system.C =[0 0 0 0;
0 0 0 0;
1*dt 0 0 0;
0 1*dt 0 0];
system.xl = xmin;
system.xu = xmax;
system.ul = umin;
system.uu = umax;
%% NMPC-DCBF parameters
params_nmpc_dcbf.Q = Q;
params_nmpc_dcbf.R = R;
params_nmpc_dcbf.P = P;
params_nmpc_dcbf.gamma1 = 0.6;
params_nmpc_dcbf.gamma2 = 0.6;
%% Obstacle
obs.pos1 = [0; 0];
obs.r1 = 1;
%% Simulate NMPC-DCBF with other N
params_nmpc_dcbf.N = N1;
if run_nmpc_dcbf_one
fprintf('Run NMPC-DCBF-12\n');
controller_nmpc_dcbf_multiple12 = NMPCDCBF1(x0, system, params_nmpc_dcbf);%Highest order mcbf=2
controller_nmpc_dcbf_multiple12.obs = obs;
controller_nmpc_dcbf_multiple12.sim(time_total);
end
t1=controller_nmpc_dcbf_multiple12.tt;
end
function t2=impcdcbf(x00, N1, ttsim)
t2=0;
xo = 0;
yo = 0;
r = 1;
N = N1;%Number of horizon
K1 = 1000;%Maximum iteration times, jmax
K = 1000;%Maximum iteration times, jmax
dt = 0.1;
gamma1 = 6;
gamma2 = 6;
nsim = ttsim;
tic;
% Constraints
umin = [-7; -5; -inf; -inf;];
umax = [7; 5; Inf; Inf];
xmin = [-10; -10; -10; -10];
xmax = [ 10; 10; 10; 10];
% Objective function
Q = diag([10 10 10 10]);
QN = Q;
R = 1 * eye(4);
R(3,3) = 1000;
R(4,4) = 1000;
% Initial and reference states
x0 = x00;
xr = [3; 0.01; 0; 0];
ur = [0; 0; 1; 1];
% Dynamic system initialization
BB = [0 0 0 0;0 0 0 0;dt 0 0 0;0 dt 0 0];
% Convex MPC frame
[nx, nu] = size(BB);
% Initialize states set (x00,x01,,,x0N）
x_0 = [];
x0 = x0';
u_0 = zeros(nu, N);
u0 = zeros(nu, 1);% we may need to change this for a better warm-up start
impc = zeros(nx, nsim + 1);
impc(:, 1) = x0;
x0new = x0;
for i=1 : (N+1)
x_0 = [x_0 x0new];
x0new = [dt*x0new(4)*cos(x0new(3))+x0new(1);dt*x0new(4)*sin(x0new(3))+x0new(2);dt*u0(1)+x0new(3);dt*u0(2)+x0new(4)];
end
abc = tangentline(x_0, r);
Acbf = cbfmat(abc, nx, nu, dt, gamma1, x0);%First order CBF constraints
lcbf = -abc(3, :);
lcbf(1) = [];
lcbf = lcbf';
ucbf = inf * ones(N, 1);
A2cbf = cbfmat2(abc, nx, nu, dt, gamma1, gamma2, x0);%Second order CBF constraints
lcbf2 = getlcbf2(abc, dt, gamma1, gamma2);
ucbf2 = inf * ones(N-1, 1);
% Cast MPC problem to a QP: x = (x(0),x(1),...,x(N),u(0),...,u(N-1))
% - quadratic objective
P = blkdiag( kron(speye(N), Q), QN, kron(speye(N), R) );
% - linear objective
q = [repmat(-Q*xr, N, 1); -QN*xr; repmat(-R*ur, N, 1)];
% - linear dynamics
Ax = getax(x_0, dt, N, nx);
Bu = kron([sparse(1, N); speye(N)], BB);
Cx = getcx(x_0, u_0, dt, N, nx);
Aeq = [Ax, Bu];
leq = [-x0; -Cx];
ueq = leq;
% - input and state constraints
Aineq = speye((N+1)*nx + N*nu);
lineq = [repmat(xmin, N+1, 1); repmat(umin, N, 1)];
uineq = [repmat(xmax, N+1, 1); repmat(umax, N, 1)];
% - OSQP constraints
A = [Aeq; Aineq; Acbf; A2cbf];
l = [leq; lineq; lcbf; lcbf2];
u = [ueq; uineq; ucbf; ucbf2];
% Create an OSQP object
prob = osqp;
% Setup workspace
prob.setup(P, q, A, l, u, 'warm_start', true);
% Solve
res = prob.solve();
% Check solver status
if ~strcmp(res.info.status, 'solved')
% error('OSQP did not solve the problem!')
t2=-1;
return
end
ctrlx = res.x(1:(N+1)*nx);
ctrlu = res.x((N+1)*nx+1:(N+1)*nx+N*nu);
for j = 1 : (K1-1)%Iterations for the first time-step
storagex = ctrlx;%store the state variables and inputs in order to compare them with the next iteration
storageu = ctrlu;
x_0 = trans(x0, ctrlx, nx, N+1);
u_0 = transu(ctrlu, nu, N);
abc = tangentline(x_0, r);
Acbf = cbfmat(abc, nx, nu, dt, gamma1, x0);
lcbf = -abc(3, :);
lcbf(1) = [];
lcbf = lcbf';
ucbf = inf * ones(N, 1);
A2cbf = cbfmat2(abc, nx, nu, dt, gamma1, gamma2, x0);
lcbf2 = getlcbf2(abc, dt, gamma1, gamma2);
ucbf2 = inf * ones(N-1, 1);
% Cast MPC problem to a QP: x = (x(0),x(1),...,x(N),u(0),...,u(N-1))
% - quadratic objective
P = blkdiag( kron(speye(N), Q), QN, kron(speye(N), R) );
% - linear objective
q = [repmat(-Q*xr, N, 1); -QN*xr; repmat(-R*ur, N, 1)];
% - linear dynamics
Ax = getax(x_0, dt, N, nx);
Bu = kron([sparse(1, N); speye(N)], BB);
Cx = getcx(x_0, u_0, dt, N, nx);
Aeq = [Ax, Bu];
leq = [-x0; -Cx];
ueq = leq;
% - input and state constraints
Aineq = speye((N+1)*nx + N*nu);
lineq = [repmat(xmin, N+1, 1); repmat(umin, N, 1)];
uineq = [repmat(xmax, N+1, 1); repmat(umax, N, 1)];
% - OSQP constraints
A = [Aeq; Aineq; Acbf; A2cbf];
l = [leq; lineq; lcbf; lcbf2];
u = [ueq; uineq; ucbf; ucbf2];
% Create an OSQP object
prob = osqp;
% Setup workspace
prob.setup(P, q, A, l, u, 'warm_start', true);
% Solve
res = prob.solve();
% Check solver status
if ~strcmp(res.info.status, 'solved')
% error('OSQP did not solve the problem!')
t2=-1;
return
end
ctrlx = res.x(1:(N+1)*nx);
ctrlu = res.x((N+1)*nx+1:(N+1)*nx+N*nu);
testx = (storagex - ctrlx)'*(storagex - ctrlx);
testu = (storageu - ctrlu)'*(storageu - ctrlu);
test = (testx)/(storagex'*storagex);
if (test)^(0.5)<=10^(-2)&& (testx/((N+1)*nx))^(0.5)<=10^(-4)%Convergence criterion
break
end
end % Move for the first step
ctrl = ctrlu(1:nu);
x0 = [dt*x0(4)*cos(x0(3))+x0(1);dt*x0(4)*sin(x0(3))+x0(2);dt*ctrl(1)+x0(3);dt*ctrl(2)+x0(4)];
ctrlu = rewrite(ctrlu, nu, N);
t2=t2+toc;
return
x_0 = newinit(x0, ctrlu, nx, nu, N, dt);
u_0 = transu(ctrlu, nu, N);
impc(:, 2) = x0;
storagex = ctrlx;
storageu = ctrlu;
for i = 1 : (nsim-1)%Iterations for the left time steps
for j = 1 : K
abc = tangentline(x_0, r);
Acbf = cbfmat(abc, nx, nu, dt, gamma1, x0);
lcbf = -abc(3, :);
lcbf(1) = [];
lcbf = lcbf';
ucbf = inf * ones(N, 1);
A2cbf = cbfmat2(abc, nx, nu, dt, gamma1, gamma2, x0);
lcbf2 = getlcbf2(abc, dt, gamma1, gamma2);
ucbf2 = inf * ones(N-1, 1);
% Cast MPC problem to a QP: x = (x(0),x(1),...,x(N),u(0),...,u(N-1))
% - quadratic objective
P = blkdiag( kron(speye(N), Q), QN, kron(speye(N), R) );
% - linear objective
q = [repmat(-Q*xr, N, 1); -QN*xr; repmat(-R*ur, N, 1)];
% - linear dynamics
Ax = getax(x_0, dt, N, nx);
Bu = kron([sparse(1, N); speye(N)], BB);
Cx = getcx(x_0, u_0, dt, N, nx);
Aeq = [Ax, Bu];
leq = [-x0; -Cx];
ueq = leq;
% - input and state constraints
Aineq = speye((N+1)*nx + N*nu);
lineq = [repmat(xmin, N+1, 1); repmat(umin, N, 1)];
uineq = [repmat(xmax, N+1, 1); repmat(umax, N, 1)];
% - OSQP constraints
A = [Aeq; Aineq; Acbf; A2cbf];
l = [leq; lineq; lcbf; lcbf2];
u = [ueq; uineq; ucbf; ucbf2];
% Create an OSQP object
prob = osqp;
% Setup workspace
prob.setup(P, q, A, l, u, 'warm_start', true);
% Solve
res = prob.solve();
% Check solver status
if ~strcmp(res.info.status, 'solved')
% error('OSQP did not solve the problem!')
t2=-1;
return
end
ctrlx = res.x(1:(N+1)*nx);
ctrlu = res.x((N+1)*nx+1:(N+1)*nx+N*nu);
x0 = ctrlx(1:nx);
testx0 = [testx0 x0];
x_0 = trans(x0, ctrlx, nx, N+1);
u_0 = transu(ctrlu, nu, N);
testx = (storagex - ctrlx)'*(storagex - ctrlx);
testu = (storageu - ctrlu)'*(storageu - ctrlu);
test = (testx)/(storagex'*storagex);
if (test)^(0.5)<=10^(-2)&& (testx/((N+1)*nx))^(0.5)<=10^(-4)%Convergence criterion
break
end
storagex = ctrlx;
storageu = ctrlu;
end
ctrl = ctrlu(1:nu);
x0 = [dt*x0(4)*cos(x0(3))+x0(1);dt*x0(4)*sin(x0(3))+x0(2);dt*ctrl(1)+x0(3);dt*ctrl(2)+x0(4)];
ctrlu = rewrite(ctrlu, nu, N);
x_0 = newinit(x0, ctrlu, nx, nu, N, dt);
u_0 = transu(ctrlu, nu, N);
impc(:, i+2) = x0;
end
end
% Linerize the CBF constraints (get a, b, c for lines)
function abc = tangentline(xy, r)% x and y from initialize states set, abc are coeeficients for linear equation a*x+b*y+c=0
[xx, ~] = size(xy);%xx=2,yy=N+1
xy(xx,:) = []; % this part should be changed for other case
xy((xx-1),:) = []; % this part should be changed other case
[xx, yy] = size(xy);%xx=2,yy=N+1
xyjiao = zeros(xx, yy);%intersection points
for i = 1 : xx
for j = 1 : yy
xyjiao(i, j) = r * xy(i, j) * (1 / (xy(:, j)' * xy(:, j)))^(0.5);%calculate coordinates of intersection points
end
end
cc = -r^2 * ones(1, yy);
abc = [xyjiao; cc];
end
% Get CBF constraints matrix
function Acbf = cbfmat(abc, nx, nu, dt, gamma, x0)
[~, yy] = size(abc);
Acbfx = zeros((yy-1), yy*nx);
Acbfu = zeros((yy-1), (yy-1)*nu);
for i = 1 : (yy-1)
Acbfx(i, (i*nx)+1) = abc(1, (i+1));
Acbfx(i, (i*nx)+2) = abc(2, (i+1));
end
for i = 1 : (yy-1)
Acbfu(i, ((i-1)*nu+3)) = - (1 - dt * gamma)^(i) * (abc(1, 1) * x0(1, 1) + abc(2, 1) * x0(2, 1) + abc(3, 1));
end
Acbf = [Acbfx Acbfu];
end
% Transfer vector x into matrix
function res = trans(x0, vector, nxx, nyy)%nxx=nx,nyy=N+1
res = zeros(nxx, nyy);
res(:,1) = x0;
for i = 1 : (nyy -1)
res(:,i+1)= vector(((i)*nxx+1):(i+1)*nxx);
end
end
% Transfer vector u into matrix
function resu = transu(vector, nxx, nyy)%nxx=nu,nyy=N
resu = zeros(nxx, nyy);
for i = 1 : (nyy)
resu(:,i)= vector(((i-1)*nxx+1):(i)*nxx);
end
end
% Rewrite u vector
function reu = rewrite(vector, nu, N)
append = vector((N-1)*nu+1:N*nu);
vector(1:nu) = [];
reu = [vector;append];
end
% Get new x_0
function x_0 = newinit(x0, ctrlu, nx, nu, N, dt)
x_0 = zeros(nx, N+1);
x_0(:, 1) = x0;
for i=1 : N
u0 = ctrlu((i-1)*nu+1:i*nu);
x0 = [dt*x0(4)*cos(x0(3))+x0(1);dt*x0(4)*sin(x0(3))+x0(2);dt*u0(1)+x0(3);dt*u0(2)+x0(4)];
x_0(:, i + 1) = x0;
end
end
% Get AA matrix
function AA = getaa(x0, dt)
AA = [1 0 -x0(4)*sin(x0(3))*dt cos(x0(3))*dt;0 1 x0(4)*cos(x0(3))*dt sin(x0(3))*dt;0 0 1 0;0 0 0 1];
end
% Get CC matrix
function CC = getcc(x0, x1, u0, dt)
CC = [x0(4)*sin(x0(3))*x0(3)*dt-x0(4)*cos(x0(3))*dt+x1(1)-x0(1);-x0(4)*cos(x0(3))*x0(3)*dt-x0(4)*sin(x0(3))*dt+x1(2)-x0(2);-u0(1)*dt+x1(3)-x0(3);-u0(2)*dt+x1(4)-x0(4)];
end
% Get Ax matrix
function Ax = getax(x_0, dt, N, nx)
x0 = x_0(:,1);
AA = getaa(x0, dt);
Ax = kron(speye(N+1), -speye(nx)) + kron(sparse(diag(ones(N, 1), -1)), AA);
for i = 1 : (N-1)
x0 = x_0(:,i+1);
AA = getaa(x0, dt);
Ax(nx*(i+1)+1:nx*(i+1)+nx,nx*i+1:nx*i+nx) = AA;
end
end
% Get Cx matrix
function Cx = getcx(x_0, u_0, dt, N, nx)
Cx = zeros(N*nx, 1);
for i = 1 : N
u0 = u_0(:,i);
x0 = x_0(:,i);
x1 = x_0(:,i+1);
CC = getcc(x0, x1, u0, dt);
Cx((i-1)*nx+1:(i-1)*nx+nx) = CC;
end
end
% Get A2cbf
function A2cbf = cbfmat2(abc, nx, nu, dt, gamma1, gamma2, x0)
[~, yy] = size(abc);
Acbfx2 = zeros((yy-2), yy*nx);
Acbfx22 = zeros((yy-2), yy*nx);
Acbfu2 = zeros((yy-2), (yy-1)*nu);
for i = 1 : (yy-2)
Acbfx2(i, (i*nx)+1) = (gamma1-1/dt)*abc(1, (i+1));
Acbfx2(i, (i*nx)+2) = (gamma1-1/dt)*abc(2, (i+1));
Acbfx2(i, ((i+1)*nx)+1) = (1/dt)*abc(1, (i+2));
Acbfx2(i, ((i+1)*nx)+2) = (1/dt)*abc(2, (i+2));
end
for i = 1 : (yy-2)
Acbfx22(i, (1*nx)+1) = -(1-dt*gamma2)^(i)/dt*abc(1, (1+1));
Acbfx22(i, (1*nx)+2) = -(1-dt*gamma2)^(i)/dt*abc(2, (1+1));
end
Acbfx2 = Acbfx2 + Acbfx22;
for i = 1 : (yy-2)
Acbfu2(i, ((i-1)*nu+4)) = - (1 - dt * gamma2)^(i)*(gamma1-1/dt)*(abc(1, 1) * x0(1, 1) + abc(2, 1) * x0(2, 1) + abc(3, 1));
end
A2cbf = [Acbfx2 Acbfu2];
end
% Get lcbf2
function lcbf2 = getlcbf2(abc, dt, gamma1, gamma2)
[~, yy] = size(abc);
lcbf2 = zeros((yy-2),1);
for i = 1 : (yy-2)
lcbf2(i, 1) = -abc(3, (i+2))/dt-(gamma1-1/dt)*abc(3, (i+1))+(1 - dt * gamma2)^(i)*abc(3, (1+1))/dt;
end
end
================================================
FILE: matlab/acc2023/benchmark/gamma1-0p6/test_each_horizon/test_N16.m
================================================
close all
clear
%------------------------------------------------------------------------------------------
nb = 1000;%Total number of initial states
nmpcd=zeros(1,nb);%Store computing time for one time-step for NMPC-DCBF
impcd=zeros(1,nb);%Store computing time for one time-step for iMPC-DCBF
nmpcinf=zeros(1,1);%Store infeasible rate for one time-step for NMPC-DCBF
impcinf=zeros(1,1);%Store infeasible rate for one time-step for iMPC-DCBF
i=16;%i is number of horozon
ts = 1;%Total time steps
load(gamma1_0p6\InitialStateData');%Load the generated 1000 initial states
[a, b, c, d]=compare(i,ts,nb,InitialMat);%Involves NMPC-DCBF and iMPC-DCBF
nmpcd(1,:)=a;
impcd(1,:)=b;
nmpcinf(1,1)=c;
impcinf(1,1)=d;
nmpcplot1=nmpcd(1,:);
nmpcplot1(nmpcplot1==-1)=[];%Eliminate infeasible one time-step trajectories
impcplot1=impcd(1,:);
impcplot1(impcplot1==-1)=[];%Eliminate infeasible one time-step trajectories
save('timecom16','nmpcplot1','impcplot1');
save('feasibility16','nmpcinf','impcinf');
distnmpc = fitdist(nmpcplot1','Normal');
distimpc = fitdist(impcplot1','Normal');
mu1=distnmpc.mu;%Get mean of sample of computing time for NMPC-DCBF
mu2=distimpc.mu;%Get mean of sample of computing time for iMPC-DCBF
sigma1=distnmpc.sigma;%Get variance of sample of computing time for NMPC-DCBF
sigma2=distimpc.sigma;%Get variance of sample of computing time for iMPC-DCBF
%Initialize atmosphere parameters
function [tnmpc, timpc, ratiotnmpc, ratiotimpc]=compare(N11,ttsim,samplen,InitialMat)
tnmpc=[];
timpc=[];
for i=1:samplen
N1=N11;
tsim=ttsim;
xini1=InitialMat(1,i);
yini1=InitialMat(2,i);
thetaini1=InitialMat(3,i);
vini1=InitialMat(4,i);
x01=[xini1;yini1;thetaini1;vini1];
x02=[xini1 yini1 thetaini1 vini1];
t1 = nmpcdcbf(x01, N1, tsim);
t2 = impcdcbf(x02, N1, tsim);
tindex=[N11 i];
disp(tindex);
tnmpc=[tnmpc t1];%Computing time for NMPC-DCBF
timpc=[timpc t2];%Computing time for iMPC-DCBF
end
nnmpc1 = length(tnmpc);
nimpc1 = length(timpc);
tnmpcs = tnmpc;
timpcs = timpc;
tnmpcs(tnmpcs==-1)=[];
timpcs(timpcs==-1)=[];
nnmpc2 = length(tnmpcs);
nimpc2 = length(timpcs);
ratiotnmpc = (nnmpc1-nnmpc2)/nnmpc1;%Infeasible rate for NMPC-DCBF
ratiotimpc = (nimpc1-nimpc2)/nimpc1;%Infeasible rate for iMPC-DCBF
end
function t1 = nmpcdcbf(x00, N1, ttsim)
%% General Flags
run_nmpc_dcbf_one = true;
run_nmpc_dcbf_multiple = true;
%% Setup and Parameters
x0 = x00;
dt = 0.1;
time_total = ttsim *dt;
P = zeros(4,4);%Weight matrix P
P(1,1) = 10; P(2,2) = 10; P(3,3) = 10;P(4,4) = 10;
Q = zeros(4,4);%Weight matrix Q
Q(1,1) = 10; Q(2,2) = 10; Q(3,3) = 10;Q(4,4) = 10;
R = zeros(4,4);%Weight matrix R
R(1,1) = 1; R(2,2) = 1;R(3,3) = 1000;R(4,4) = 1000;
%Variables range as below
xmin = [-10; -10; -10;-10];
xmax = [10; 10; 10;10];
umin = [-7; -5;-inf;-inf];
umax = [7; 5;inf;inf];
%% Discrete-time unicycle model
system.dt = dt;
system.A = [1 0 0 0;
0 1 0 0;
0 0 1 0;
0 0 0 1];
system.B = [x0(4,1)*cos(x0(3,1))*dt;
x0(4,1)*sin(x0(3,1))*dt;
0;
0];
system.C =[0 0 0 0;
0 0 0 0;
1*dt 0 0 0;
0 1*dt 0 0];
system.xl = xmin;
system.xu = xmax;
system.ul = umin;
system.uu = umax;
%% NMPC-DCBF parameters
params_nmpc_dcbf.Q = Q;
params_nmpc_dcbf.R = R;
params_nmpc_dcbf.P = P;
params_nmpc_dcbf.gamma1 = 0.6;
params_nmpc_dcbf.gamma2 = 0.6;
%% Obstacle
obs.pos1 = [0; 0];
obs.r1 = 1;
%% Simulate NMPC-DCBF with other N
params_nmpc_dcbf.N = N1;
if run_nmpc_dcbf_one
fprintf('Run NMPC-DCBF-16\n');
controller_nmpc_dcbf_multiple16 = NMPCDCBF1(x0, system, params_nmpc_dcbf);%Highest order mcbf=2
controller_nmpc_dcbf_multiple16.obs = obs;
controller_nmpc_dcbf_multiple16.sim(time_total);
end
t1=controller_nmpc_dcbf_multiple16.tt;
end
function t2=impcdcbf(x00, N1, ttsim)
t2=0;
xo = 0;
yo = 0;
r = 1;
N = N1;%Number of horizon
K1 = 1000;%Maximum iteration times, jmax
K = 1000;%Maximum iteration times, jmax
dt = 0.1;
gamma1 = 6;
gamma2 = 6;
nsim = ttsim;
tic;
% Constraints
umin = [-7; -5; -inf; -inf;];
umax = [7; 5; Inf; Inf];
xmin = [-10; -10; -10; -10];
xmax = [ 10; 10; 10; 10];
% Objective function
Q = diag([10 10 10 10]);
QN = Q;
R = 1 * eye(4);
R(3,3) = 1000;
R(4,4) = 1000;
% Initial and reference states
x0 = x00;
xr = [3; 0.01; 0; 0];
ur = [0; 0; 1; 1];
% Dynamic system initialization
BB = [0 0 0 0;0 0 0 0;dt 0 0 0;0 dt 0 0];
% Convex MPC frame
[nx, nu] = size(BB);
% Initialize states set (x00,x01,,,x0N）
x_0 = [];
x0 = x0';
u_0 = zeros(nu, N);
u0 = zeros(nu, 1);% we may need to change this for a better warm-up start
impc = zeros(nx, nsim + 1);
impc(:, 1) = x0;
x0new = x0;
for i=1 : (N+1)
x_0 = [x_0 x0new];
x0new = [dt*x0new(4)*cos(x0new(3))+x0new(1);dt*x0new(4)*sin(x0new(3))+x0new(2);dt*u0(1)+x0new(3);dt*u0(2)+x0new(4)];
end
abc = tangentline(x_0, r);
Acbf = cbfmat(abc, nx, nu, dt, gamma1, x0);%First order CBF constraints
lcbf = -abc(3, :);
lcbf(1) = [];
lcbf = lcbf';
ucbf = inf * ones(N, 1);
A2cbf = cbfmat2(abc, nx, nu, dt, gamma1, gamma2, x0);%Second order CBF constraints
lcbf2 = getlcbf2(abc, dt, gamma1, gamma2);
ucbf2 = inf * ones(N-1, 1);
% Cast MPC problem to a QP: x = (x(0),x(1),...,x(N),u(0),...,u(N-1))
% - quadratic objective
P = blkdiag( kron(speye(N), Q), QN, kron(speye(N), R) );
% - linear objective
q = [repmat(-Q*xr, N, 1); -QN*xr; repmat(-R*ur, N, 1)];
% - linear dynamics
Ax = getax(x_0, dt, N, nx);
Bu = kron([sparse(1, N); speye(N)], BB);
Cx = getcx(x_0, u_0, dt, N, nx);
Aeq = [Ax, Bu];
leq = [-x0; -Cx];
ueq = leq;
% - input and state constraints
Aineq = speye((N+1)*nx + N*nu);
lineq = [repmat(xmin, N+1, 1); repmat(umin, N, 1)];
uineq = [repmat(xmax, N+1, 1); repmat(umax, N, 1)];
% - OSQP constraints
A = [Aeq; Aineq; Acbf; A2cbf];
l = [leq; lineq; lcbf; lcbf2];
u = [ueq; uineq; ucbf; ucbf2];
% Create an OSQP object
prob = osqp;
% Setup workspace
prob.setup(P, q, A, l, u, 'warm_start', true);
% Solve
res = prob.solve();
% Check solver status
if ~strcmp(res.info.status, 'solved')
% error('OSQP did not solve the problem!')
t2=-1;
return
end
ctrlx = res.x(1:(N+1)*nx);
ctrlu = res.x((N+1)*nx+1:(N+1)*nx+N*nu);
for j = 1 : (K1-1)%Iterations for the first time-step
storagex = ctrlx;%store the state variables and inputs in order to compare them with the next iteration
storageu = ctrlu;
x_0 = trans(x0, ctrlx, nx, N+1);
u_0 = transu(ctrlu, nu, N);
abc = tangentline(x_0, r);
Acbf = cbfmat(abc, nx, nu, dt, gamma1, x0);
lcbf = -abc(3, :);
lcbf(1) = [];
lcbf = lcbf';
ucbf = inf * ones(N, 1);
A2cbf = cbfmat2(abc, nx, nu, dt, gamma1, gamma2, x0);
lcbf2 = getlcbf2(abc, dt, gamma1, gamma2);
ucbf2 = inf * ones(N-1, 1);
% Cast MPC problem to a QP: x = (x(0),x(1),...,x(N),u(0),...,u(N-1))
% - quadratic objective
P = blkdiag( kron(speye(N), Q), QN, kron(speye(N), R) );
% - linear objective
q = [repmat(-Q*xr, N, 1); -QN*xr; repmat(-R*ur, N, 1)];
% - linear dynamics
Ax = getax(x_0, dt, N, nx);
Bu = kron([sparse(1, N); speye(N)], BB);
Cx = getcx(x_0, u_0, dt, N, nx);
Aeq = [Ax, Bu];
leq = [-x0; -Cx];
ueq = leq;
% - input and state constraints
Aineq = speye((N+1)*nx + N*nu);
lineq = [repmat(xmin, N+1, 1); repmat(umin, N, 1)];
uineq = [repmat(xmax, N+1, 1); repmat(umax, N, 1)];
% - OSQP constraints
A = [Aeq; Aineq; Acbf; A2cbf];
l = [leq; lineq; lcbf; lcbf2];
u = [ueq; uineq; ucbf; ucbf2];
% Create an OSQP object
prob = osqp;
% Setup workspace
prob.setup(P, q, A, l, u, 'warm_start', true);
% Solve
res = prob.solve();
% Check solver status
if ~strcmp(res.info.status, 'solved')
% error('OSQP did not solve the problem!')
t2=-1;
return
end
ctrlx = res.x(1:(N+1)*nx);
ctrlu = res.x((N+1)*nx+1:(N+1)*nx+N*nu);
testx = (storagex - ctrlx)'*(storagex - ctrlx);
testu = (storageu - ctrlu)'*(storageu - ctrlu);
test = (testx)/(storagex'*storagex);
if (test)^(0.5)<=10^(-2)&& (testx/((N+1)*nx))^(0.5)<=10^(-4)%Convergence criterion
break
end
end % Move for the first step
ctrl = ctrlu(1:nu);
x0 = [dt*x0(4)*cos(x0(3))+x0(1);dt*x0(4)*sin(x0(3))+x0(2);dt*ctrl(1)+x0(3);dt*ctrl(2)+x0(4)];
ctrlu = rewrite(ctrlu, nu, N);
t2=t2+toc;
return
x_0 = newinit(x0, ctrlu, nx, nu, N, dt);
u_0 = transu(ctrlu, nu, N);
impc(:, 2) = x0;
storagex = ctrlx;
storageu = ctrlu;
for i = 1 : (nsim-1)%Iterations for the left time steps
for j = 1 : K
abc = tangentline(x_0, r);
Acbf = cbfmat(abc, nx, nu, dt, gamma1, x0);
lcbf = -abc(3, :);
lcbf(1) = [];
lcbf = lcbf';
ucbf = inf * ones(N, 1);
A2cbf = cbfmat2(abc, nx, nu, dt, gamma1, gamma2, x0);
lcbf2 = getlcbf2(abc, dt, gamma1, gamma2);
ucbf2 = inf * ones(N-1, 1);
% Cast MPC problem to a QP: x = (x(0),x(1),...,x(N),u(0),...,u(N-1))
% - quadratic objective
P = blkdiag( kron(speye(N), Q), QN, kron(speye(N), R) );
% - linear objective
q = [repmat(-Q*xr, N, 1); -QN*xr; repmat(-R*ur, N, 1)];
% - linear dynamics
Ax = getax(x_0, dt, N, nx);
Bu = kron([sparse(1, N); speye(N)], BB);
Cx = getcx(x_0, u_0, dt, N, nx);
Aeq = [Ax, Bu];
leq = [-x0; -Cx];
ueq = leq;
% - input and state constraints
Aineq = speye((N+1)*nx + N*nu);
lineq = [repmat(xmin, N+1, 1); repmat(umin, N, 1)];
uineq = [repmat(xmax, N+1, 1); repmat(umax, N, 1)];
% - OSQP constraints
A = [Aeq; Aineq; Acbf; A2cbf];
l = [leq; lineq; lcbf; lcbf2];
u = [ueq; uineq; ucbf; ucbf2];
% Create an OSQP object
prob = osqp;
% Setup workspace
prob.setup(P, q, A, l, u, 'warm_start', true);
% Solve
res = prob.solve();
% Check solver status
if ~strcmp(res.info.status, 'solved')
% error('OSQP did not solve the problem!')
t2=-1;
return
end
ctrlx = res.x(1:(N+1)*nx);
ctrlu = res.x((N+1)*nx+1:(N+1)*nx+N*nu);
x0 = ctrlx(1:nx);
testx0 = [testx0 x0];
x_0 = trans(x0, ctrlx, nx, N+1);
u_0 = transu(ctrlu, nu, N);
testx = (storagex - ctrlx)'*(storagex - ctrlx);
testu = (storageu - ctrlu)'*(storageu - ctrlu);
test = (testx)/(storagex'*storagex);
if (test)^(0.5)<=10^(-2)&& (testx/((N+1)*nx))^(0.5)<=10^(-4)%Convergence criterion
break
end
storagex = ctrlx;
storageu = ctrlu;
end
ctrl = ctrlu(1:nu);
x0 = [dt*x0(4)*cos(x0(3))+x0(1);dt*x0(4)*sin(x0(3))+x0(2);dt*ctrl(1)+x0(3);dt*ctrl(2)+x0(4)];
ctrlu = rewrite(ctrlu, nu, N);
x_0 = newinit(x0, ctrlu, nx, nu, N, dt);
u_0 = transu(ctrlu, nu, N);
impc(:, i+2) = x0;
end
end
% Linerize the CBF constraints (get a, b, c for lines)
function abc = tangentline(xy, r)% x and y from initialize states set, abc are coeeficients for linear equation a*x+b*y+c=0
[xx, ~] = size(xy);%xx=2,yy=N+1
xy(xx,:) = []; % this part should be changed for other case
xy((xx-1),:) = []; % this part should be changed other case
[xx, yy] = size(xy);%xx=2,yy=N+1
xyjiao = zeros(xx, yy);%intersection points
for i = 1 : xx
for j = 1 : yy
xyjiao(i, j) = r * xy(i, j) * (1 / (xy(:, j)' * xy(:, j)))^(0.5);%calculate coordinates of intersection points
end
end
cc = -r^2 * ones(1, yy);
abc = [xyjiao; cc];
end
% Get CBF constraints matrix
function Acbf = cbfmat(abc, nx, nu, dt, gamma, x0)
[~, yy] = size(abc);
Acbfx = zeros((yy-1), yy*nx);
Acbfu = zeros((yy-1), (yy-1)*nu);
for i = 1 : (yy-1)
Acbfx(i, (i*nx)+1) = abc(1, (i+1));
Acbfx(i, (i*nx)+2) = abc(2, (i+1));
end
for i = 1 : (yy-1)
Acbfu(i, ((i-1)*nu+3)) = - (1 - dt * gamma)^(i) * (abc(1, 1) * x0(1, 1) + abc(2, 1) * x0(2, 1) + abc(3, 1));
end
Acbf = [Acbfx Acbfu];
end
% Transfer vector x into matrix
function res = trans(x0, vector, nxx, nyy)%nxx=nx,nyy=N+1
res = zeros(nxx, nyy);
res(:,1) = x0;
for i = 1 : (nyy -1)
res(:,i+1)= vector(((i)*nxx+1):(i+1)*nxx);
end
end
% Transfer vector u into matrix
function resu = transu(vector, nxx, nyy)%nxx=nu,nyy=N
resu = zeros(nxx, nyy);
for i = 1 : (nyy)
resu(:,i)= vector(((i-1)*nxx+1):(i)*nxx);
end
end
% Rewrite u vector
function reu = rewrite(vector, nu, N)
append = vector((N-1)*nu+1:N*nu);
vector(1:nu) = [];
reu = [vector;append];
end
% Get new x_0
function x_0 = newinit(x0, ctrlu, nx, nu, N, dt)
x_0 = zeros(nx, N+1);
x_0(:, 1) = x0;
for i=1 : N
u0 = ctrlu((i-1)*nu+1:i*nu);
x0 = [dt*x0(4)*cos(x0(3))+x0(1);dt*x0(4)*sin(x0(3))+x0(2);dt*u0(1)+x0(3);dt*u0(2)+x0(4)];
x_0(:, i + 1) = x0;
end
end
% Get AA matrix
function AA = getaa(x0, dt)
AA = [1 0 -x0(4)*sin(x0(3))*dt cos(x0(3))*dt;0 1 x0(4)*cos(x0(3))*dt sin(x0(3))*dt;0 0 1 0;0 0 0 1];
end
% Get CC matrix
function CC = getcc(x0, x1, u0, dt)
CC = [x0(4)*sin(x0(3))*x0(3)*dt-x0(4)*cos(x0(3))*dt+x1(1)-x0(1);-x0(4)*cos(x0(3))*x0(3)*dt-x0(4)*sin(x0(3))*dt+x1(2)-x0(2);-u0(1)*dt+x1(3)-x0(3);-u0(2)*dt+x1(4)-x0(4)];
end
% Get Ax matrix
function Ax = getax(x_0, dt, N, nx)
x0 = x_0(:,1);
AA = getaa(x0, dt);
Ax = kron(speye(N+1), -speye(nx)) + kron(sparse(diag(ones(N, 1), -1)), AA);
for i = 1 : (N-1)
x0 = x_0(:,i+1);
AA = getaa(x0, dt);
Ax(nx*(i+1)+1:nx*(i+1)+nx,nx*i+1:nx*i+nx) = AA;
end
end
% Get Cx matrix
function Cx = getcx(x_0, u_0, dt, N, nx)
Cx = zeros(N*nx, 1);
for i = 1 : N
u0 = u_0(:,i);
x0 = x_0(:,i);
x1 = x_0(:,i+1);
CC = getcc(x0, x1, u0, dt);
Cx((i-1)*nx+1:(i-1)*nx+nx) = CC;
end
end
% Get A2cbf
function A2cbf = cbfmat2(abc, nx, nu, dt, gamma1, gamma2, x0)
[~, yy] = size(abc);
Acbfx2 = zeros((yy-2), yy*nx);
Acbfx22 = zeros((yy-2), yy*nx);
Acbfu2 = zeros((yy-2), (yy-1)*nu);
for i = 1 : (yy-2)
Acbfx2(i, (i*nx)+1) = (gamma1-1/dt)*abc(1, (i+1));
Acbfx2(i, (i*nx)+2) = (gamma1-1/dt)*abc(2, (i+1));
Acbfx2(i, ((i+1)*nx)+1) = (1/dt)*abc(1, (i+2));
Acbfx2(i, ((i+1)*nx)+2) = (1/dt)*abc(2, (i+2));
end
for i = 1 : (yy-2)
Acbfx22(i, (1*nx)+1) = -(1-dt*gamma2)^(i)/dt*abc(1, (1+1));
Acbfx22(i, (1*nx)+2) = -(1-dt*gamma2)^(i)/dt*abc(2, (1+1));
end
Acbfx2 = Acbfx2 + Acbfx22;
for i = 1 : (yy-2)
Acbfu2(i, ((i-1)*nu+4)) = - (1 - dt * gamma2)^(i)*(gamma1-1/dt)*(abc(1, 1) * x0(1, 1) + abc(2, 1) * x0(2, 1) + abc(3, 1));
end
A2cbf = [Acbfx2 Acbfu2];
end
% Get lcbf2
function lcbf2 = getlcbf2(abc, dt, gamma1, gamma2)
[~, yy] = size(abc);
lcbf2 = zeros((yy-2),1);
for i = 1 : (yy-2)
lcbf2(i, 1) = -abc(3, (i+2))/dt-(gamma1-1/dt)*abc(3, (i+1))+(1 - dt * gamma2)^(i)*abc(3, (1+1))/dt;
end
end
================================================
FILE: matlab/acc2023/benchmark/gamma1-0p6/test_each_horizon/test_N20.m
================================================
close all
clear
%------------------------------------------------------------------------------------------
nb = 1000;%Total number of initial states
nmpcd=zeros(1,nb);%Store computing time for one time-step for NMPC-DCBF
impcd=zeros(1,nb);%Store computing time for one time-step for iMPC-DCBF
nmpcinf=zeros(1,1);%Store infeasible rate for one time-step for NMPC-DCBF
impcinf=zeros(1,1);%Store infeasible rate for one time-step for iMPC-DCBF
i=20;%i is number of horozon
ts = 1;%Total time steps
load(gamma1_0p6\InitialStateData');%Load the generated 1000 initial states
[a, b, c, d]=compare(i,ts,nb,InitialMat);%Involves NMPC-DCBF and iMPC-DCBF
nmpcd(1,:)=a;
impcd(1,:)=b;
nmpcinf(1,1)=c;
impcinf(1,1)=d;
nmpcplot1=nmpcd(1,:);
nmpcplot1(nmpcplot1==-1)=[];%Eliminate infeasible one time-step trajectories
impcplot1=impcd(1,:);
impcplot1(impcplot1==-1)=[];%Eliminate infeasible one time-step trajectories
save('timecom20','nmpcplot1','impcplot1');
save('feasibility20','nmpcinf','impcinf');
distnmpc = fitdist(nmpcplot1','Normal');
distimpc = fitdist(impcplot1','Normal');
mu1=distnmpc.mu;%Get mean of sample of computing time for NMPC-DCBF
mu2=distimpc.mu;%Get mean of sample of computing time for iMPC-DCBF
sigma1=distnmpc.sigma;%Get variance of sample of computing time for NMPC-DCBF
sigma2=distimpc.sigma;%Get variance of sample of computing time for iMPC-DCBF
%Initialize atmosphere parameters
function [tnmpc, timpc, ratiotnmpc, ratiotimpc]=compare(N11,ttsim,samplen,InitialMat)
tnmpc=[];
timpc=[];
for i=1:samplen
N1=N11;
tsim=ttsim;
xini1=InitialMat(1,i);
yini1=InitialMat(2,i);
thetaini1=InitialMat(3,i);
vini1=InitialMat(4,i);
x01=[xini1;yini1;thetaini1;vini1];
x02=[xini1 yini1 thetaini1 vini1];
t1 = nmpcdcbf(x01, N1, tsim);
t2 = impcdcbf(x02, N1, tsim);
tindex=[N11 i];
disp(tindex);
tnmpc=[tnmpc t1];%Computing time for NMPC-DCBF
timpc=[timpc t2];%Computing time for iMPC-DCBF
end
nnmpc1 = length(tnmpc);
nimpc1 = length(timpc);
tnmpcs = tnmpc;
timpcs = timpc;
tnmpcs(tnmpcs==-1)=[];
timpcs(timpcs==-1)=[];
nnmpc2 = length(tnmpcs);
nimpc2 = length(timpcs);
ratiotnmpc = (nnmpc1-nnmpc2)/nnmpc1;%Infeasible rate for NMPC-DCBF
ratiotimpc = (nimpc1-nimpc2)/nimpc1;%Infeasible rate for iMPC-DCBF
end
function t1 = nmpcdcbf(x00, N1, ttsim)
%% General Flags
run_nmpc_dcbf_one = true;
run_nmpc_dcbf_multiple = true;
%% Setup and Parameters
x0 = x00;
dt = 0.1;
time_total = ttsim *dt;
P = zeros(4,4);%Weight matrix P
P(1,1) = 10; P(2,2) = 10; P(3,3) = 10;P(4,4) = 10;
Q = zeros(4,4);%Weight matrix Q
Q(1,1) = 10; Q(2,2) = 10; Q(3,3) = 10;Q(4,4) = 10;
R = zeros(4,4);%Weight matrix R
R(1,1) = 1; R(2,2) = 1;R(3,3) = 1000;R(4,4) = 1000;
%Variables range as below
xmin = [-10; -10; -10;-10];
xmax = [10; 10; 10;10];
umin = [-7; -5;-inf;-inf];
umax = [7; 5;inf;inf];
%% Discrete-time unicycle model
system.dt = dt;
system.A = [1 0 0 0;
0 1 0 0;
0 0 1 0;
0 0 0 1];
system.B = [x0(4,1)*cos(x0(3,1))*dt;
x0(4,1)*sin(x0(3,1))*dt;
0;
0];
system.C =[0 0 0 0;
0 0 0 0;
1*dt 0 0 0;
0 1*dt 0 0];
system.xl = xmin;
system.xu = xmax;
system.ul = umin;
system.uu = umax;
%% NMPC-DCBF parameters
params_nmpc_dcbf.Q = Q;
params_nmpc_dcbf.R = R;
params_nmpc_dcbf.P = P;
params_nmpc_dcbf.gamma1 = 0.6;
params_nmpc_dcbf.gamma2 = 0.6;
%% Obstacle
obs.pos1 = [0; 0];
obs.r1 = 1;
%% Simulate NMPC-DCBF with other N
params_nmpc_dcbf.N = N1;
if run_nmpc_dcbf_one
fprintf('Run NMPC-DCBF-20\n');
controller_nmpc_dcbf_multiple20 = NMPCDCBF1(x0, system, params_nmpc_dcbf);%Highest order mcbf=2
controller_nmpc_dcbf_multiple20.obs = obs;
controller_nmpc_dcbf_multiple20.sim(time_total);
end
t1=controller_nmpc_dcbf_multiple20.tt;
end
function t2=impcdcbf(x00, N1, ttsim)
t2=0;
xo = 0;
yo = 0;
r = 1;
N = N1;%Number of horizon
K1 = 1000;%Maximum iteration times, jmax
K = 1000;%Maximum iteration times, jmax
dt = 0.1;
gamma1 = 6;
gamma2 = 6;
nsim = ttsim;
tic;
% Constraints
umin = [-7; -5; -inf; -inf;];
umax = [7; 5; Inf; Inf];
xmin = [-10; -10; -10; -10];
xmax = [ 10; 10; 10; 10];
% Objective function
Q = diag([10 10 10 10]);
QN = Q;
R = 1 * eye(4);
R(3,3) = 1000;
R(4,4) = 1000;
% Initial and reference states
x0 = x00;
xr = [3; 0.01; 0; 0];
ur = [0; 0; 1; 1];
% Dynamic system initialization
BB = [0 0 0 0;0 0 0 0;dt 0 0 0;0 dt 0 0];
% Convex MPC frame
[nx, nu] = size(BB);
% Initialize states set (x00,x01,,,x0N）
x_0 = [];
x0 = x0';
u_0 = zeros(nu, N);
u0 = zeros(nu, 1);% we may need to change this for a better warm-up start
impc = zeros(nx, nsim + 1);
impc(:, 1) = x0;
x0new = x0;
for i=1 : (N+1)
x_0 = [x_0 x0new];
x0new = [dt*x0new(4)*cos(x0new(3))+x0new(1);dt*x0new(4)*sin(x0new(3))+x0new(2);dt*u0(1)+x0new(3);dt*u0(2)+x0new(4)];
end
abc = tangentline(x_0, r);
Acbf = cbfmat(abc, nx, nu, dt, gamma1, x0);%First order CBF constraints
lcbf = -abc(3, :);
lcbf(1) = [];
lcbf = lcbf';
ucbf = inf * ones(N, 1);
A2cbf = cbfmat2(abc, nx, nu, dt, gamma1, gamma2, x0);%Second order CBF constraints
lcbf2 = getlcbf2(abc, dt, gamma1, gamma2);
ucbf2 = inf * ones(N-1, 1);
% Cast MPC problem to a QP: x = (x(0),x(1),...,x(N),u(0),...,u(N-1))
% - quadratic objective
P = blkdiag( kron(speye(N), Q), QN, kron(speye(N), R) );
% - linear objective
q = [repmat(-Q*xr, N, 1); -QN*xr; repmat(-R*ur, N, 1)];
% - linear dynamics
Ax = getax(x_0, dt, N, nx);
Bu = kron([sparse(1, N); speye(N)], BB);
Cx = getcx(x_0, u_0, dt, N, nx);
Aeq = [Ax, Bu];
leq = [-x0; -Cx];
ueq = leq;
% - input and state constraints
Aineq = speye((N+1)*nx + N*nu);
lineq = [repmat(xmin, N+1, 1); repmat(umin, N, 1)];
uineq = [repmat(xmax, N+1, 1); repmat(umax, N, 1)];
% - OSQP constraints
A = [Aeq; Aineq; Acbf; A2cbf];
l = [leq; lineq; lcbf; lcbf2];
u = [ueq; uineq; ucbf; ucbf2];
% Create an OSQP object
prob = osqp;
% Setup workspace
prob.setup(P, q, A, l, u, 'warm_start', true);
% Solve
res = prob.solve();
% Check solver status
if ~strcmp(res.info.status, 'solved')
% error('OSQP did not solve the problem!')
t2=-1;
return
end
ctrlx = res.x(1:(N+1)*nx);
ctrlu = res.x((N+1)*nx+1:(N+1)*nx+N*nu);
for j = 1 : (K1-1)%Iterations for the first time-step
storagex = ctrlx;%store the state variables and inputs in order to compare them with the next iteration
storageu = ctrlu;
x_0 = trans(x0, ctrlx, nx, N+1);
u_0 = transu(ctrlu, nu, N);
abc = tangentline(x_0, r);
Acbf = cbfmat(abc, nx, nu, dt, gamma1, x0);
lcbf = -abc(3, :);
lcbf(1) = [];
lcbf = lcbf';
ucbf = inf * ones(N, 1);
A2cbf = cbfmat2(abc, nx, nu, dt, gamma1, gamma2, x0);
lcbf2 = getlcbf2(abc, dt, gamma1, gamma2);
ucbf2 = inf * ones(N-1, 1);
% Cast MPC problem to a QP: x = (x(0),x(1),...,x(N),u(0),...,u(N-1))
% - quadratic objective
P = blkdiag( kron(speye(N), Q), QN, kron(speye(N), R) );
% - linear objective
q = [repmat(-Q*xr, N, 1); -QN*xr; repmat(-R*ur, N, 1)];
% - linear dynamics
Ax = getax(x_0, dt, N, nx);
Bu = kron([sparse(1, N); speye(N)], BB);
Cx = getcx(x_0, u_0, dt, N, nx);
Aeq = [Ax, Bu];
leq = [-x0; -Cx];
ueq = leq;
% - input and state constraints
Aineq = speye((N+1)*nx + N*nu);
lineq = [repmat(xmin, N+1, 1); repmat(umin, N, 1)];
uineq = [repmat(xmax, N+1, 1); repmat(umax, N, 1)];
% - OSQP constraints
A = [Aeq; Aineq; Acbf; A2cbf];
l = [leq; lineq; lcbf; lcbf2];
u = [ueq; uineq; ucbf; ucbf2];
% Create an OSQP object
prob = osqp;
% Setup workspace
prob.setup(P, q, A, l, u, 'warm_start', true);
% Solve
res = prob.solve();
% Check solver status
if ~strcmp(res.info.status, 'solved')
% error('OSQP did not solve the problem!')
t2=-1;
return
end
ctrlx = res.x(1:(N+1)*nx);
ctrlu = res.x((N+1)*nx+1:(N+1)*nx+N*nu);
testx = (storagex - ctrlx)'*(storagex - ctrlx);
testu = (storageu - ctrlu)'*(storageu - ctrlu);
test = (testx)/(storagex'*storagex);
if (test)^(0.5)<=10^(-2)&& (testx/((N+1)*nx))^(0.5)<=10^(-4)%Convergence criterion
break
end
end % Move for the first step
ctrl = ctrlu(1:nu);
x0 = [dt*x0(4)*cos(x0(3))+x0(1);dt*x0(4)*sin(x0(3))+x0(2);dt*ctrl(1)+x0(3);dt*ctrl(2)+x0(4)];
ctrlu = rewrite(ctrlu, nu, N);
t2=t2+toc;
return
x_0 = newinit(x0, ctrlu, nx, nu, N, dt);
u_0 = transu(ctrlu, nu, N);
impc(:, 2) = x0;
storagex = ctrlx;
storageu = ctrlu;
for i = 1 : (nsim-1)%Iterations for the left time steps
for j = 1 : K
abc = tangentline(x_0, r);
Acbf = cbfmat(abc, nx, nu, dt, gamma1, x0);
lcbf = -abc(3, :);
lcbf(1) = [];
lcbf = lcbf';
ucbf = inf * ones(N, 1);
A2cbf = cbfmat2(abc, nx, nu, dt, gamma1, gamma2, x0);
lcbf2 = getlcbf2(abc, dt, gamma1, gamma2);
ucbf2 = inf * ones(N-1, 1);
% Cast MPC problem to a QP: x = (x(0),x(1),...,x(N),u(0),...,u(N-1))
% - quadratic objective
P = blkdiag( kron(speye(N), Q), QN, kron(speye(N), R) );
% - linear objective
q = [repmat(-Q*xr, N, 1); -QN*xr; repmat(-R*ur, N, 1)];
% - linear dynamics
Ax = getax(x_0, dt, N, nx);
Bu = kron([sparse(1, N); speye(N)], BB);
Cx = getcx(x_0, u_0, dt, N, nx);
Aeq = [Ax, Bu];
leq = [-x0; -Cx];
ueq = leq;
% - input and state constraints
Aineq = speye((N+1)*nx + N*nu);
lineq = [repmat(xmin, N+1, 1); repmat(umin, N, 1)];
uineq = [repmat(xmax, N+1, 1); repmat(umax, N, 1)];
% - OSQP constraints
A = [Aeq; Aineq; Acbf; A2cbf];
l = [leq; lineq; lcbf; lcbf2];
u = [ueq; uineq; ucbf; ucbf2];
% Create an OSQP object
prob = osqp;
% Setup workspace
prob.setup(P, q, A, l, u, 'warm_start', true);
% Solve
res = prob.solve();
% Check solver status
if ~strcmp(res.info.status, 'solved')
% error('OSQP did not solve the problem!')
t2=-1;
return
end
ctrlx = res.x(1:(N+1)*nx);
ctrlu = res.x((N+1)*nx+1:(N+1)*nx+N*nu);
x0 = ctrlx(1:nx);
testx0 = [testx0 x0];
x_0 = trans(x0, ctrlx, nx, N+1);
u_0 = transu(ctrlu, nu, N);
testx = (storagex - ctrlx)'*(storagex - ctrlx);
testu = (storageu - ctrlu)'*(storageu - ctrlu);
test = (testx)/(storagex'*storagex);
if (test)^(0.5)<=10^(-2)&& (testx/((N+1)*nx))^(0.5)<=10^(-4)%Convergence criterion
break
end
storagex = ctrlx;
storageu = ctrlu;
end
ctrl = ctrlu(1:nu);
x0 = [dt*x0(4)*cos(x0(3))+x0(1);dt*x0(4)*sin(x0(3))+x0(2);dt*ctrl(1)+x0(3);dt*ctrl(2)+x0(4)];
ctrlu = rewrite(ctrlu, nu, N);
x_0 = newinit(x0, ctrlu, nx, nu, N, dt);
u_0 = transu(ctrlu, nu, N);
impc(:, i+2) = x0;
end
end
% Linerize the CBF constraints (get a, b, c for lines)
function abc = tangentline(xy, r)% x and y from initialize states set, abc are coeeficients for linear equation a*x+b*y+c=0
[xx, ~] = size(xy);%xx=2,yy=N+1
xy(xx,:) = []; % this part should be changed for other case
xy((xx-1),:) = []; % this part should be changed other case
[xx, yy] = size(xy);%xx=2,yy=N+1
xyjiao = zeros(xx, yy);%intersection points
for i = 1 : xx
for j = 1 : yy
xyjiao(i, j) = r * xy(i, j) * (1 / (xy(:, j)' * xy(:, j)))^(0.5);%calculate coordinates of intersection points
end
end
cc = -r^2 * ones(1, yy);
abc = [xyjiao; cc];
end
% Get CBF constraints matrix
function Acbf = cbfmat(abc, nx, nu, dt, gamma, x0)
[~, yy] = size(abc);
Acbfx = zeros((yy-1), yy*nx);
Acbfu = zeros((yy-1), (yy-1)*nu);
for i = 1 : (yy-1)
Acbfx(i, (i*nx)+1) = abc(1, (i+1));
Acbfx(i, (i*nx)+2) = abc(2, (i+1));
end
for i = 1 : (yy-1)
Acbfu(i, ((i-1)*nu+3)) = - (1 - dt * gamma)^(i) * (abc(1, 1) * x0(1, 1) + abc(2, 1) * x0(2, 1) + abc(3, 1));
end
Acbf = [Acbfx Acbfu];
end
% Transfer vector x into matrix
function res = trans(x0, vector, nxx, nyy)%nxx=nx,nyy=N+1
res = zeros(nxx, nyy);
res(:,1) = x0;
for i = 1 : (nyy -1)
res(:,i+1)= vector(((i)*nxx+1):(i+1)*nxx);
end
end
% Transfer vector u into matrix
function resu = transu(vector, nxx, nyy)%nxx=nu,nyy=N
resu = zeros(nxx, nyy);
for i = 1 : (nyy)
resu(:,i)= vector(((i-1)*nxx+1):(i)*nxx);
end
end
% Rewrite u vector
function reu = rewrite(vector, nu, N)
append = vector((N-1)*nu+1:N*nu);
vector(1:nu) = [];
reu = [vector;append];
end
% Get new x_0
function x_0 = newinit(x0, ctrlu, nx, nu, N, dt)
x_0 = zeros(nx, N+1);
x_0(:, 1) = x0;
for i=1 : N
u0 = ctrlu((i-1)*nu+1:i*nu);
x0 = [dt*x0(4)*cos(x0(3))+x0(1);dt*x0(4)*sin(x0(3))+x0(2);dt*u0(1)+x0(3);dt*u0(2)+x0(4)];
x_0(:, i + 1) = x0;
end
end
% Get AA matrix
function AA = getaa(x0, dt)
AA = [1 0 -x0(4)*sin(x0(3))*dt cos(x0(3))*dt;0 1 x0(4)*cos(x0(3))*dt sin(x0(3))*dt;0 0 1 0;0 0 0 1];
end
% Get CC matrix
function CC = getcc(x0, x1, u0, dt)
CC = [x0(4)*sin(x0(3))*x0(3)*dt-x0(4)*cos(x0(3))*dt+x1(1)-x0(1);-x0(4)*cos(x0(3))*x0(3)*dt-x0(4)*sin(x0(3))*dt+x1(2)-x0(2);-u0(1)*dt+x1(3)-x0(3);-u0(2)*dt+x1(4)-x0(4)];
end
% Get Ax matrix
function Ax = getax(x_0, dt, N, nx)
x0 = x_0(:,1);
AA = getaa(x0, dt);
Ax = kron(speye(N+1), -speye(nx)) + kron(sparse(diag(ones(N, 1), -1)), AA);
for i = 1 : (N-1)
x0 = x_0(:,i+1);
AA = getaa(x0, dt);
Ax(nx*(i+1)+1:nx*(i+1)+nx,nx*i+1:nx*i+nx) = AA;
end
end
% Get Cx matrix
function Cx = getcx(x_0, u_0, dt, N, nx)
Cx = zeros(N*nx, 1);
for i = 1 : N
u0 = u_0(:,i);
x0 = x_0(:,i);
x1 = x_0(:,i+1);
CC = getcc(x0, x1, u0, dt);
Cx((i-1)*nx+1:(i-1)*nx+nx) = CC;
end
end
% Get A2cbf
function A2cbf = cbfmat2(abc, nx, nu, dt, gamma1, gamma2, x0)
[~, yy] = size(abc);
Acbfx2 = zeros((yy-2), yy*nx);
Acbfx22 = zeros((yy-2), yy*nx);
Acbfu2 = zeros((yy-2), (yy-1)*nu);
for i = 1 : (yy-2)
Acbfx2(i, (i*nx)+1) = (gamma1-1/dt)*abc(1, (i+1));
Acbfx2(i, (i*nx)+2) = (gamma1-1/dt)*abc(2, (i+1));
Acbfx2(i, ((i+1)*nx)+1) = (1/dt)*abc(1, (i+2));
Acbfx2(i, ((i+1)*nx)+2) = (1/dt)*abc(2, (i+2));
end
for i = 1 : (yy-2)
Acbfx22(i, (1*nx)+1) = -(1-dt*gamma2)^(i)/dt*abc(1, (1+1));
Acbfx22(i, (1*nx)+2) = -(1-dt*gamma2)^(i)/dt*abc(2, (1+1));
end
Acbfx2 = Acbfx2 + Acbfx22;
for i = 1 : (yy-2)
Acbfu2(i, ((i-1)*nu+4)) = - (1 - dt * gamma2)^(i)*(gamma1-1/dt)*(abc(1, 1) * x0(1, 1) + abc(2, 1) * x0(2, 1) + abc(3, 1));
end
A2cbf = [Acbfx2 Acbfu2];
end
% Get lcbf2
function lcbf2 = getlcbf2(abc, dt, gamma1, gamma2)
[~, yy] = size(abc);
lcbf2 = zeros((yy-2),1);
for i = 1 : (yy-2)
lcbf2(i, 1) = -abc(3, (i+2))/dt-(gamma1-1/dt)*abc(3, (i+1))+(1 - dt * gamma2)^(i)*abc(3, (1+1))/dt;
end
end
================================================
FILE: matlab/acc2023/benchmark/gamma1-0p6/test_each_horizon/test_N24.m
================================================
close all
clear
%------------------------------------------------------------------------------------------
nb = 1000;%Total number of initial states
nmpcd=zeros(1,nb);%Store computing time for one time-step for NMPC-DCBF
impcd=zeros(1,nb);%Store computing time for one time-step for iMPC-DCBF
nmpcinf=zeros(1,1);%Store infeasible rate for one time-step for NMPC-DCBF
impcinf=zeros(1,1);%Store infeasible rate for one time-step for iMPC-DCBF
i=24;%i is number of horozon
ts = 1;%Total time steps
load(gamma1_0p6\InitialStateData');%Load the generated 1000 initial states
[a, b, c, d]=compare(i,ts,nb,InitialMat);%Involves NMPC-DCBF and iMPC-DCBF
nmpcd(1,:)=a;
impcd(1,:)=b;
nmpcinf(1,1)=c;
impcinf(1,1)=d;
nmpcplot1=nmpcd(1,:);
nmpcplot1(nmpcplot1==-1)=[];%Eliminate infeasible one time-step trajectories
impcplot1=impcd(1,:);
impcplot1(impcplot1==-1)=[];%Eliminate infeasible one time-step trajectories
save('timecom24','nmpcplot1','impcplot1');
save('feasibility24','nmpcinf','impcinf');
distnmpc = fitdist(nmpcplot1','Normal');
distimpc = fitdist(impcplot1','Normal');
mu1=distnmpc.mu;%Get mean of sample of computing time for NMPC-DCBF
mu2=distimpc.mu;%Get mean of sample of computing time for iMPC-DCBF
sigma1=distnmpc.sigma;%Get variance of sample of computing time for NMPC-DCBF
sigma2=distimpc.sigma;%Get variance of sample of computing time for iMPC-DCBF
%Initialize atmosphere parameters
function [tnmpc, timpc, ratiotnmpc, ratiotimpc]=compare(N11,ttsim,samplen,InitialMat)
tnmpc=[];
timpc=[];
for i=1:samplen
N1=N11;
tsim=ttsim;
xini1=InitialMat(1,i);
yini1=InitialMat(2,i);
thetaini1=InitialMat(3,i);
vini1=InitialMat(4,i);
x01=[xini1;yini1;thetaini1;vini1];
x02=[xini1 yini1 thetaini1 vini1];
t1 = nmpcdcbf(x01, N1, tsim);
t2 = impcdcbf(x02, N1, tsim);
tindex=[N11 i];
disp(tindex);
tnmpc=[tnmpc t1];%Computing time for NMPC-DCBF
timpc=[timpc t2];%Computing time for iMPC-DCBF
end
nnmpc1 = length(tnmpc);
nimpc1 = length(timpc);
tnmpcs = tnmpc;
timpcs = timpc;
tnmpcs(tnmpcs==-1)=[];
timpcs(timpcs==-1)=[];
nnmpc2 = length(tnmpcs);
nimpc2 = length(timpcs);
ratiotnmpc = (nnmpc1-nnmpc2)/nnmpc1;%Infeasible rate for NMPC-DCBF
ratiotimpc = (nimpc1-nimpc2)/nimpc1;%Infeasible rate for iMPC-DCBF
end
function t1 = nmpcdcbf(x00, N1, ttsim)
%% General Flags
run_nmpc_dcbf_one = true;
run_nmpc_dcbf_multiple = true;
%% Setup and Parameters
x0 = x00;
dt = 0.1;
time_total = ttsim *dt;
P = zeros(4,4);%Weight matrix P
P(1,1) = 10; P(2,2) = 10; P(3,3) = 10;P(4,4) = 10;
Q = zeros(4,4);%Weight matrix Q
Q(1,1) = 10; Q(2,2) = 10; Q(3,3) = 10;Q(4,4) = 10;
R = zeros(4,4);%Weight matrix R
R(1,1) = 1; R(2,2) = 1;R(3,3) = 1000;R(4,4) = 1000;
%Variables range as below
xmin = [-10; -10; -10;-10];
xmax = [10; 10; 10;10];
umin = [-7; -5;-inf;-inf];
umax = [7; 5;inf;inf];
%% Discrete-time unicycle model
system.dt = dt;
system.A = [1 0 0 0;
0 1 0 0;
0 0 1 0;
0 0 0 1];
system.B = [x0(4,1)*cos(x0(3,1))*dt;
x0(4,1)*sin(x0(3,1))*dt;
0;
0];
system.C =[0 0 0 0;
0 0 0 0;
1*dt 0 0 0;
0 1*dt 0 0];
system.xl = xmin;
system.xu = xmax;
system.ul = umin;
system.uu = umax;
%% NMPC-DCBF parameters
params_nmpc_dcbf.Q = Q;
params_nmpc_dcbf.R = R;
params_nmpc_dcbf.P = P;
params_nmpc_dcbf.gamma1 = 0.6;
params_nmpc_dcbf.gamma2 = 0.6;
%% Obstacle
obs.pos1 = [0; 0];
obs.r1 = 1;
%% Simulate NMPC-DCBF with other N
params_nmpc_dcbf.N = N1;
if run_nmpc_dcbf_one
fprintf('Run NMPC-DCBF-24\n');
controller_nmpc_dcbf_multiple24 = NMPCDCBF1(x0, system, params_nmpc_dcbf);%Highest order mcbf=2
controller_nmpc_dcbf_multiple24.obs = obs;
controller_nmpc_dcbf_multiple24.sim(time_total);
end
t1=controller_nmpc_dcbf_multiple24.tt;
end
function t2=impcdcbf(x00, N1, ttsim)
t2=0;
xo = 0;
yo = 0;
r = 1;
N = N1;%Number of horizon
K1 = 1000;%Maximum iteration times, jmax
K = 1000;%Maximum iteration times, jmax
dt = 0.1;
gamma1 = 6;
gamma2 = 6;
nsim = ttsim;
tic;
% Constraints
umin = [-7; -5; -inf; -inf;];
umax = [7; 5; Inf; Inf];
xmin = [-10; -10; -10; -10];
xmax = [ 10; 10; 10; 10];
% Objective function
Q = diag([10 10 10 10]);
QN = Q;
R = 1 * eye(4);
R(3,3) = 1000;
R(4,4) = 1000;
% Initial and reference states
x0 = x00;
xr = [3; 0.01; 0; 0];
ur = [0; 0; 1; 1];
% Dynamic system initialization
BB = [0 0 0 0;0 0 0 0;dt 0 0 0;0 dt 0 0];
% Convex MPC frame
[nx, nu] = size(BB);
% Initialize states set (x00,x01,,,x0N）
x_0 = [];
x0 = x0';
u_0 = zeros(nu, N);
u0 = zeros(nu, 1);% we may need to change this for a better warm-up start
impc = zeros(nx, nsim + 1);
impc(:, 1) = x0;
x0new = x0;
for i=1 : (N+1)
x_0 = [x_0 x0new];
x0new = [dt*x0new(4)*cos(x0new(3))+x0new(1);dt*x0new(4)*sin(x0new(3))+x0new(2);dt*u0(1)+x0new(3);dt*u0(2)+x0new(4)];
end
abc = tangentline(x_0, r);
Acbf = cbfmat(abc, nx, nu, dt, gamma1, x0);%First order CBF constraints
lcbf = -abc(3, :);
lcbf(1) = [];
lcbf = lcbf';
ucbf = inf * ones(N, 1);
A2cbf = cbfmat2(abc, nx, nu, dt, gamma1, gamma2, x0);%Second order CBF constraints
lcbf2 = getlcbf2(abc, dt, gamma1, gamma2);
ucbf2 = inf * ones(N-1, 1);
% Cast MPC problem to a QP: x = (x(0),x(1),...,x(N),u(0),...,u(N-1))
% - quadratic objective
P = blkdiag( kron(speye(N), Q), QN, kron(speye(N), R) );
% - linear objective
q = [repmat(-Q*xr, N, 1); -QN*xr; repmat(-R*ur, N, 1)];
% - linear dynamics
Ax = getax(x_0, dt, N, nx);
Bu = kron([sparse(1, N); speye(N)], BB);
Cx = getcx(x_0, u_0, dt, N, nx);
Aeq = [Ax, Bu];
leq = [-x0; -Cx];
ueq = leq;
% - input and state constraints
Aineq = speye((N+1)*nx + N*nu);
lineq = [repmat(xmin, N+1, 1); repmat(umin, N, 1)];
uineq = [repmat(xmax, N+1, 1); repmat(umax, N, 1)];
% - OSQP constraints
A = [Aeq; Aineq; Acbf; A2cbf];
l = [leq; lineq; lcbf; lcbf2];
u = [ueq; uineq; ucbf; ucbf2];
% Create an OSQP object
prob = osqp;
% Setup workspace
prob.setup(P, q, A, l, u, 'warm_start', true);
% Solve
res = prob.solve();
% Check solver status
if ~strcmp(res.info.status, 'solved')
% error('OSQP did not solve the problem!')
t2=-1;
return
end
ctrlx = res.x(1:(N+1)*nx);
ctrlu = res.x((N+1)*nx+1:(N+1)*nx+N*nu);
for j = 1 : (K1-1)%Iterations for the first time-step
storagex = ctrlx;%store the state variables and inputs in order to compare them with the next iteration
storageu = ctrlu;
x_0 = trans(x0, ctrlx, nx, N+1);
u_0 = transu(ctrlu, nu, N);
abc = tangentline(x_0, r);
Acbf = cbfmat(abc, nx, nu, dt, gamma1, x0);
lcbf = -abc(3, :);
lcbf(1) = [];
lcbf = lcbf';
ucbf = inf * ones(N, 1);
A2cbf = cbfmat2(abc, nx, nu, dt, gamma1, gamma2, x0);
lcbf2 = getlcbf2(abc, dt, gamma1, gamma2);
ucbf2 = inf * ones(N-1, 1);
% Cast MPC problem to a QP: x = (x(0),x(1),...,x(N),u(0),...,u(N-1))
% - quadratic objective
P = blkdiag( kron(speye(N), Q), QN, kron(speye(N), R) );
% - linear objective
q = [repmat(-Q*xr, N, 1); -QN*xr; repmat(-R*ur, N, 1)];
% - linear dynamics
Ax = getax(x_0, dt, N, nx);
Bu = kron([sparse(1, N); speye(N)], BB);
Cx = getcx(x_0, u_0, dt, N, nx);
Aeq = [Ax, Bu];
leq = [-x0; -Cx];
ueq = leq;
% - input and state constraints
Aineq = speye((N+1)*nx + N*nu);
lineq = [repmat(xmin, N+1, 1); repmat(umin, N, 1)];
uineq = [repmat(xmax, N+1, 1); repmat(umax, N, 1)];
% - OSQP constraints
A = [Aeq; Aineq; Acbf; A2cbf];
l = [leq; lineq; lcbf; lcbf2];
u = [ueq; uineq; ucbf; ucbf2];
% Create an OSQP object
prob = osqp;
% Setup workspace
prob.setup(P, q, A, l, u, 'warm_start', true);
% Solve
res = prob.solve();
% Check solver status
if ~strcmp(res.info.status, 'solved')
% error('OSQP did not solve the problem!')
t2=-1;
return
end
ctrlx = res.x(1:(N+1)*nx);
ctrlu = res.x((N+1)*nx+1:(N+1)*nx+N*nu);
testx = (storagex - ctrlx)'*(storagex - ctrlx);
testu = (storageu - ctrlu)'*(storageu - ctrlu);
test = (testx)/(storagex'*storagex);
if (test)^(0.5)<=10^(-2)&& (testx/((N+1)*nx))^(0.5)<=10^(-4)%Convergence criterion
break
end
end % Move for the first step
ctrl = ctrlu(1:nu);
x0 = [dt*x0(4)*cos(x0(3))+x0(1);dt*x0(4)*sin(x0(3))+x0(2);dt*ctrl(1)+x0(3);dt*ctrl(2)+x0(4)];
ctrlu = rewrite(ctrlu, nu, N);
t2=t2+toc;
return
x_0 = newinit(x0, ctrlu, nx, nu, N, dt);
u_0 = transu(ctrlu, nu, N);
impc(:, 2) = x0;
storagex = ctrlx;
storageu = ctrlu;
for i = 1 : (nsim-1)%Iterations for the left time steps
for j = 1 : K
abc = tangentline(x_0, r);
Acbf = cbfmat(abc, nx, nu, dt, gamma1, x0);
lcbf = -abc(3, :);
lcbf(1) = [];
lcbf = lcbf';
ucbf = inf * ones(N, 1);
A2cbf = cbfmat2(abc, nx, nu, dt, gamma1, gamma2, x0);
lcbf2 = getlcbf2(abc, dt, gamma1, gamma2);
ucbf2 = inf * ones(N-1, 1);
% Cast MPC problem to a QP: x = (x(0),x(1),...,x(N),u(0),...,u(N-1))
% - quadratic objective
P = blkdiag( kron(speye(N), Q), QN, kron(speye(N), R) );
% - linear objective
q = [repmat(-Q*xr, N, 1); -QN*xr; repmat(-R*ur, N, 1)];
% - linear dynamics
Ax = getax(x_0, dt, N, nx);
Bu = kron([sparse(1, N); speye(N)], BB);
Cx = getcx(x_0, u_0, dt, N, nx);
Aeq = [Ax, Bu];
leq = [-x0; -Cx];
ueq = leq;
% - input and state constraints
Aineq = speye((N+1)*nx + N*nu);
lineq = [repmat(xmin, N+1, 1); repmat(umin, N, 1)];
uineq = [repmat(xmax, N+1, 1); repmat(umax, N, 1)];
% - OSQP constraints
A = [Aeq; Aineq; Acbf; A2cbf];
l = [leq; lineq; lcbf; lcbf2];
u = [ueq; uineq; ucbf; ucbf2];
% Create an OSQP object
prob = osqp;
% Setup workspace
prob.setup(P, q, A, l, u, 'warm_start', true);
% Solve
res = prob.solve();
% Check solver status
if ~strcmp(res.info.status, 'solved')
% error('OSQP did not solve the problem!')
t2=-1;
return
end
ctrlx = res.x(1:(N+1)*nx);
ctrlu = res.x((N+1)*nx+1:(N+1)*nx+N*nu);
x0 = ctrlx(1:nx);
testx0 = [testx0 x0];
x_0 = trans(x0, ctrlx, nx, N+1);
u_0 = transu(ctrlu, nu, N);
testx = (storagex - ctrlx)'*(storagex - ctrlx);
testu = (storageu - ctrlu)'*(storageu - ctrlu);
test = (testx)/(storagex'*storagex);
if (test)^(0.5)<=10^(-2)&& (testx/((N+1)*nx))^(0.5)<=10^(-4)%Convergence criterion
break
end
storagex = ctrlx;
storageu = ctrlu;
end
ctrl = ctrlu(1:nu);
x0 = [dt*x0(4)*cos(x0(3))+x0(1);dt*x0(4)*sin(x0(3))+x0(2);dt*ctrl(1)+x0(3);dt*ctrl(2)+x0(4)];
ctrlu = rewrite(ctrlu, nu, N);
x_0 = newinit(x0, ctrlu, nx, nu, N, dt);
u_0 = transu(ctrlu, nu, N);
impc(:, i+2) = x0;
end
end
% Linerize the CBF constraints (get a, b, c for lines)
function abc = tangentline(xy, r)% x and y from initialize states set, abc are coeeficients for linear equation a*x+b*y+c=0
[xx, ~] = size(xy);%xx=2,yy=N+1
xy(xx,:) = []; % this part should be changed for other case
xy((xx-1),:) = []; % this part should be changed other case
[xx, yy] = size(xy);%xx=2,yy=N+1
xyjiao = zeros(xx, yy);%intersection points
for i = 1 : xx
for j = 1 : yy
xyjiao(i, j) = r * xy(i, j) * (1 / (xy(:, j)' * xy(:, j)))^(0.5);%calculate coordinates of intersection points
end
end
cc = -r^2 * ones(1, yy);
abc = [xyjiao; cc];
end
% Get CBF constraints matrix
function Acbf = cbfmat(abc, nx, nu, dt, gamma, x0)
[~, yy] = size(abc);
Acbfx = zeros((yy-1), yy*nx);
Acbfu = zeros((yy-1), (yy-1)*nu);
for i = 1 : (yy-1)
Acbfx(i, (i*nx)+1) = abc(1, (i+1));
Acbfx(i, (i*nx)+2) = abc(2, (i+1));
end
for i = 1 : (yy-1)
Acbfu(i, ((i-1)*nu+3)) = - (1 - dt * gamma)^(i) * (abc(1, 1) * x0(1, 1) + abc(2, 1) * x0(2, 1) + abc(3, 1));
end
Acbf = [Acbfx Acbfu];
end
% Transfer vector x into matrix
function res = trans(x0, vector, nxx, nyy)%nxx=nx,nyy=N+1
res = zeros(nxx, nyy);
res(:,1) = x0;
for i = 1 : (nyy -1)
res(:,i+1)= vector(((i)*nxx+1):(i+1)*nxx);
end
end
% Transfer vector u into matrix
function resu = transu(vector, nxx, nyy)%nxx=nu,nyy=N
resu = zeros(nxx, nyy);
for i = 1 : (nyy)
resu(:,i)= vector(((i-1)*nxx+1):(i)*nxx);
end
end
% Rewrite u vector
function reu = rewrite(vector, nu, N)
append = vector((N-1)*nu+1:N*nu);
vector(1:nu) = [];
reu = [vector;append];
end
% Get new x_0
function x_0 = newinit(x0, ctrlu, nx, nu, N, dt)
x_0 = zeros(nx, N+1);
x_0(:, 1) = x0;
for i=1 : N
u0 = ctrlu((i-1)*nu+1:i*nu);
x0 = [dt*x0(4)*cos(x0(3))+x0(1);dt*x0(4)*sin(x0(3))+x0(2);dt*u0(1)+x0(3);dt*u0(2)+x0(4)];
x_0(:, i + 1) = x0;
end
end
% Get AA matrix
function AA = getaa(x0, dt)
AA = [1 0 -x0(4)*sin(x0(3))*dt cos(x0(3))*dt;0 1 x0(4)*cos(x0(3))*dt sin(x0(3))*dt;0 0 1 0;0 0 0 1];
end
% Get CC matrix
function CC = getcc(x0, x1, u0, dt)
CC = [x0(4)*sin(x0(3))*x0(3)*dt-x0(4)*cos(x0(3))*dt+x1(1)-x0(1);-x0(4)*cos(x0(3))*x0(3)*dt-x0(4)*sin(x0(3))*dt+x1(2)-x0(2);-u0(1)*dt+x1(3)-x0(3);-u0(2)*dt+x1(4)-x0(4)];
end
% Get Ax matrix
function Ax = getax(x_0, dt, N, nx)
x0 = x_0(:,1);
AA = getaa(x0, dt);
Ax = kron(speye(N+1), -speye(nx)) + kron(sparse(diag(ones(N, 1), -1)), AA);
for i = 1 : (N-1)
x0 = x_0(:,i+1);
AA = getaa(x0, dt);
Ax(nx*(i+1)+1:nx*(i+1)+nx,nx*i+1:nx*i+nx) = AA;
end
end
% Get Cx matrix
function Cx = getcx(x_0, u_0, dt, N, nx)
Cx = zeros(N*nx, 1);
for i = 1 : N
u0 = u_0(:,i);
x0 = x_0(:,i);
x1 = x_0(:,i+1);
CC = getcc(x0, x1, u0, dt);
Cx((i-1)*nx+1:(i-1)*nx+nx) = CC;
end
end
% Get A2cbf
function A2cbf = cbfmat2(abc, nx, nu, dt, gamma1, gamma2, x0)
[~, yy] = size(abc);
Acbfx2 = zeros((yy-2), yy*nx);
Acbfx22 = zeros((yy-2), yy*nx);
Acbfu2 = zeros((yy-2), (yy-1)*nu);
for i = 1 : (yy-2)
Acbfx2(i, (i*nx)+1) = (gamma1-1/dt)*abc(1, (i+1));
Acbfx2(i, (i*nx)+2) = (gamma1-1/dt)*abc(2, (i+1));
Acbfx2(i, ((i+1)*nx)+1) = (1/dt)*abc(1, (i+2));
Acbfx2(i, ((i+1)*nx)+2) = (1/dt)*abc(2, (i+2));
end
for i = 1 : (yy-2)
Acbfx22(i, (1*nx)+1) = -(1-dt*gamma2)^(i)/dt*abc(1, (1+1));
Acbfx22(i, (1*nx)+2) = -(1-dt*gamma2)^(i)/dt*abc(2, (1+1));
end
Acbfx2 = Acbfx2 + Acbfx22;
for i = 1 : (yy-2)
Acbfu2(i, ((i-1)*nu+4)) = - (1 - dt * gamma2)^(i)*(gamma1-1/dt)*(abc(1, 1) * x0(1, 1) + abc(2, 1) * x0(2, 1) + abc(3, 1));
end
A2cbf = [Acbfx2 Acbfu2];
end
% Get lcbf2
function lcbf2 = getlcbf2(abc, dt, gamma1, gamma2)
[~, yy] = size(abc);
lcbf2 = zeros((yy-2),1);
for i = 1 : (yy-2)
lcbf2(i, 1) = -abc(3, (i+2))/dt-(gamma1-1/dt)*abc(3, (i+1))+(1 - dt * gamma2)^(i)*abc(3, (1+1))/dt;
end
end
================================================
FILE: matlab/acc2023/benchmark/gamma1-0p6/test_each_horizon/test_N4.m
================================================
close all
clear
%------------------------------------------------------------------------------------------
nb = 1000;%Total number of initial states
nmpcd=zeros(1,nb);%Store computing time for one time-step for NMPC-DCBF
impcd=zeros(1,nb);%Store computing time for one time-step for iMPC-DCBF
nmpcinf=zeros(1,1);%Store infeasible rate for one time-step for NMPC-DCBF
impcinf=zeros(1,1);%Store infeasible rate for one time-step for iMPC-DCBF
i=4;%i is number of horozon
ts = 1;%Total time steps
load(gamma1_0p6\InitialStateData');%Load the generated 1000 initial states
[a, b, c, d]=compare(i,ts,nb,InitialMat);%Involves NMPC-DCBF and iMPC-DCBF
nmpcd(1,:)=a;
impcd(1,:)=b;
nmpcinf(1,1)=c;
impcinf(1,1)=d;
nmpcplot1=nmpcd(1,:);
nmpcplot1(nmpcplot1==-1)=[];%Eliminate infeasible one time-step trajectories
impcplot1=impcd(1,:);
impcplot1(impcplot1==-1)=[];%Eliminate infeasible one time-step trajectories
save('timecom4','nmpcplot1','impcplot1');
save('feasibility4','nmpcinf','impcinf');
distnmpc = fitdist(nmpcplot1','Normal');
distimpc = fitdist(impcplot1','Normal');
mu1=distnmpc.mu;%Get mean of sample of computing time for NMPC-DCBF
mu2=distimpc.mu;%Get mean of sample of computing time for iMPC-DCBF
sigma1=distnmpc.sigma;%Get variance of sample of computing time for NMPC-DCBF
sigma2=distimpc.sigma;%Get variance of sample of computing time for iMPC-DCBF
%Initialize atmosphere parameters
function [tnmpc, timpc, ratiotnmpc, ratiotimpc]=compare(N11,ttsim,samplen,InitialMat)
tnmpc=[];
timpc=[];
for i=1:samplen
N1=N11;
tsim=ttsim;
xini1=InitialMat(1,i);
yini1=InitialMat(2,i);
thetaini1=InitialMat(3,i);
vini1=InitialMat(4,i);
x01=[xini1;yini1;thetaini1;vini1];
x02=[xini1 yini1 thetaini1 vini1];
t1 = nmpcdcbf(x01, N1, tsim);
t2 = impcdcbf(x02, N1, tsim);
tindex=[N11 i];
disp(tindex);
tnmpc=[tnmpc t1];%Computing time for NMPC-DCBF
timpc=[timpc t2];%Computing time for iMPC-DCBF
end
nnmpc1 = length(tnmpc);
nimpc1 = length(timpc);
tnmpcs = tnmpc;
timpcs = timpc;
tnmpcs(tnmpcs==-1)=[];
timpcs(timpcs==-1)=[];
nnmpc2 = length(tnmpcs);
nimpc2 = length(timpcs);
ratiotnmpc = (nnmpc1-nnmpc2)/nnmpc1;%Infeasible rate for NMPC-DCBF
ratiotimpc = (nimpc1-nimpc2)/nimpc1;%Infeasible rate for iMPC-DCBF
end
function t1 = nmpcdcbf(x00, N1, ttsim)
%% General Flags
run_nmpc_dcbf_one = true;
run_nmpc_dcbf_multiple = true;
%% Setup and Parameters
x0 = x00;
dt = 0.1;
time_total = ttsim *dt;
P = zeros(4,4);%Weight matrix P
P(1,1) = 10; P(2,2) = 10; P(3,3) = 10;P(4,4) = 10;
Q = zeros(4,4);%Weight matrix Q
Q(1,1) = 10; Q(2,2) = 10; Q(3,3) = 10;Q(4,4) = 10;
R = zeros(4,4);%Weight matrix R
R(1,1) = 1; R(2,2) = 1;R(3,3) = 1000;R(4,4) = 1000;
%Variables range as below
xmin = [-10; -10; -10;-10];
xmax = [10; 10; 10;10];
umin = [-7; -5;-inf;-inf];
umax = [7; 5;inf;inf];
%% Discrete-time unicycle model
system.dt = dt;
system.A = [1 0 0 0;
0 1 0 0;
0 0 1 0;
0 0 0 1];
system.B = [x0(4,1)*cos(x0(3,1))*dt;
x0(4,1)*sin(x0(3,1))*dt;
0;
0];
system.C =[0 0 0 0;
0 0 0 0;
1*dt 0 0 0;
0 1*dt 0 0];
system.xl = xmin;
system.xu = xmax;
system.ul = umin;
system.uu = umax;
%% NMPC-DCBF parameters
params_nmpc_dcbf.Q = Q;
params_nmpc_dcbf.R = R;
params_nmpc_dcbf.P = P;
params_nmpc_dcbf.gamma1 = 0.6;
params_nmpc_dcbf.gamma2 = 0.6;
%% Obstacle
obs.pos1 = [0; 0];
obs.r1 = 1;
%% Simulate NMPC-DCBF with other N
params_nmpc_dcbf.N = N1;
if run_nmpc_dcbf_one
fprintf('Run NMPC-DCBF-4\n');
controller_nmpc_dcbf_multiple4 = NMPCDCBF1(x0, system, params_nmpc_dcbf);%Highest order mcbf=2
controller_nmpc_dcbf_multiple4.obs = obs;
controller_nmpc_dcbf_multiple4.sim(time_total);
end
t1=controller_nmpc_dcbf_multiple4.tt;
end
function t2=impcdcbf(x00, N1, ttsim)
t2=0;
xo = 0;
yo = 0;
r = 1;
N = N1;%Number of horizon
K1 = 1000;%Maximum iteration times, jmax
K = 1000;%Maximum iteration times, jmax
dt = 0.1;
gamma1 = 6;
gamma2 = 6;
nsim = ttsim;
tic;
% Constraints
umin = [-7; -5; -inf; -inf;];
umax = [7; 5; Inf; Inf];
xmin = [-10; -10; -10; -10];
xmax = [ 10; 10; 10; 10];
% Objective function
Q = diag([10 10 10 10]);
QN = Q;
R = 1 * eye(4);
R(3,3) = 1000;
R(4,4) = 1000;
% Initial and reference states
x0 = x00;
xr = [3; 0.01; 0; 0];
ur = [0; 0; 1; 1];
% Dynamic system initialization
BB = [0 0 0 0;0 0 0 0;dt 0 0 0;0 dt 0 0];
% Convex MPC frame
[nx, nu] = size(BB);
% Initialize states set (x00,x01,,,x0N）
x_0 = [];
x0 = x0';
u_0 = zeros(nu, N);
u0 = zeros(nu, 1);% we may need to change this for a better warm-up start
impc = zeros(nx, nsim + 1);
impc(:, 1) = x0;
x0new = x0;
for i=1 : (N+1)
x_0 = [x_0 x0new];
x0new = [dt*x0new(4)*cos(x0new(3))+x0new(1);dt*x0new(4)*sin(x0new(3))+x0new(2);dt*u0(1)+x0new(3);dt*u0(2)+x0new(4)];
end
abc = tangentline(x_0, r);
Acbf = cbfmat(abc, nx, nu, dt, gamma1, x0);%First order CBF constraints
lcbf = -abc(3, :);
lcbf(1) = [];
lcbf = lcbf';
ucbf = inf * ones(N, 1);
A2cbf = cbfmat2(abc, nx, nu, dt, gamma1, gamma2, x0);%Second order CBF constraints
lcbf2 = getlcbf2(abc, dt, gamma1, gamma2);
ucbf2 = inf * ones(N-1, 1);
% Cast MPC problem to a QP: x = (x(0),x(1),...,x(N),u(0),...,u(N-1))
% - quadratic objective
P = blkdiag( kron(speye(N), Q), QN, kron(speye(N), R) );
% - linear objective
q = [repmat(-Q*xr, N, 1); -QN*xr; repmat(-R*ur, N, 1)];
% - linear dynamics
Ax = getax(x_0, dt, N, nx);
Bu = kron([sparse(1, N); speye(N)], BB);
Cx = getcx(x_0, u_0, dt, N, nx);
Aeq = [Ax, Bu];
leq = [-x0; -Cx];
ueq = leq;
% - input and state constraints
Aineq = speye((N+1)*nx + N*nu);
lineq = [repmat(xmin, N+1, 1); repmat(umin, N, 1)];
uineq = [repmat(xmax, N+1, 1); repmat(umax, N, 1)];
% - OSQP constraints
A = [Aeq; Aineq; Acbf; A2cbf];
l = [leq; lineq; lcbf; lcbf2];
u = [ueq; uineq; ucbf; ucbf2];
% Create an OSQP object
prob = osqp;
% Setup workspace
prob.setup(P, q, A, l, u, 'warm_start', true);
% Solve
res = prob.solve();
% Check solver status
if ~strcmp(res.info.status, 'solved')
% error('OSQP did not solve the problem!')
t2=-1;
return
end
ctrlx = res.x(1:(N+1)*nx);
ctrlu = res.x((N+1)*nx+1:(N+1)*nx+N*nu);
for j = 1 : (K1-1)%Iterations for the first time-step
storagex = ctrlx;%store the state variables and inputs in order to compare them with the next iteration
storageu = ctrlu;
x_0 = trans(x0, ctrlx, nx, N+1);
u_0 = transu(ctrlu, nu, N);
abc = tangentline(x_0, r);
Acbf = cbfmat(abc, nx, nu, dt, gamma1, x0);
lcbf = -abc(3, :);
lcbf(1) = [];
lcbf = lcbf';
ucbf = inf * ones(N, 1);
A2cbf = cbfmat2(abc, nx, nu, dt, gamma1, gamma2, x0);
lcbf2 = getlcbf2(abc, dt, gamma1, gamma2);
ucbf2 = inf * ones(N-1, 1);
% Cast MPC problem to a QP: x = (x(0),x(1),...,x(N),u(0),...,u(N-1))
% - quadratic objective
P = blkdiag( kron(speye(N), Q), QN, kron(speye(N), R) );
% - linear objective
q = [repmat(-Q*xr, N, 1); -QN*xr; repmat(-R*ur, N, 1)];
% - linear dynamics
Ax = getax(x_0, dt, N, nx);
Bu = kron([sparse(1, N); speye(N)], BB);
Cx = getcx(x_0, u_0, dt, N, nx);
Aeq = [Ax, Bu];
leq = [-x0; -Cx];
ueq = leq;
% - input and state constraints
Aineq = speye((N+1)*nx + N*nu);
lineq = [repmat(xmin, N+1, 1); repmat(umin, N, 1)];
uineq = [repmat(xmax, N+1, 1); repmat(umax, N, 1)];
% - OSQP constraints
A = [Aeq; Aineq; Acbf; A2cbf];
l = [leq; lineq; lcbf; lcbf2];
u = [ueq; uineq; ucbf; ucbf2];
% Create an OSQP object
prob = osqp;
% Setup workspace
prob.setup(P, q, A, l, u, 'warm_start', true);
% Solve
res = prob.solve();
% Check solver status
if ~strcmp(res.info.status, 'solved')
% error('OSQP did not solve the problem!')
t2=-1;
return
end
ctrlx = res.x(1:(N+1)*nx);
ctrlu = res.x((N+1)*nx+1:(N+1)*nx+N*nu);
testx = (storagex - ctrlx)'*(storagex - ctrlx);
testu = (storageu - ctrlu)'*(storageu - ctrlu);
test = (testx)/(storagex'*storagex);
if (test)^(0.5)<=10^(-2)&& (testx/((N+1)*nx))^(0.5)<=10^(-4)%Convergence criterion
break
end
end % Move for the first step
ctrl = ctrlu(1:nu);
x0 = [dt*x0(4)*cos(x0(3))+x0(1);dt*x0(4)*sin(x0(3))+x0(2);dt*ctrl(1)+x0(3);dt*ctrl(2)+x0(4)];
ctrlu = rewrite(ctrlu, nu, N);
t2=t2+toc;
return
x_0 = newinit(x0, ctrlu, nx, nu, N, dt);
u_0 = transu(ctrlu, nu, N);
impc(:, 2) = x0;
storagex = ctrlx;
storageu = ctrlu;
for i = 1 : (nsim-1)%Iterations for the left time steps
for j = 1 : K
abc = tangentline(x_0, r);
Acbf = cbfmat(abc, nx, nu, dt, gamma1, x0);
lcbf = -abc(3, :);
lcbf(1) = [];
lcbf = lcbf';
ucbf = inf * ones(N, 1);
A2cbf = cbfmat2(abc, nx, nu, dt, gamma1, gamma2, x0);
lcbf2 = getlcbf2(abc, dt, gamma1, gamma2);
ucbf2 = inf * ones(N-1, 1);
% Cast MPC problem to a QP: x = (x(0),x(1),...,x(N),u(0),...,u(N-1))
% - quadratic objective
P = blkdiag( kron(speye(N), Q), QN, kron(speye(N), R) );
% - linear objective
q = [repmat(-Q*xr, N, 1); -QN*xr; repmat(-R*ur, N, 1)];
% - linear dynamics
Ax = getax(x_0, dt, N, nx);
Bu = kron([sparse(1, N); speye(N)], BB);
Cx = getcx(x_0, u_0, dt, N, nx);
Aeq = [Ax, Bu];
leq = [-x0; -Cx];
ueq = leq;
% - input and state constraints
Aineq = speye((N+1)*nx + N*nu);
lineq = [repmat(xmin, N+1, 1); repmat(umin, N, 1)];
uineq = [repmat(xmax, N+1, 1); repmat(umax, N, 1)];
% - OSQP constraints
A = [Aeq; Aineq; Acbf; A2cbf];
l = [leq; lineq; lcbf; lcbf2];
u = [ueq; uineq; ucbf; ucbf2];
% Create an OSQP object
prob = osqp;
% Setup workspace
prob.setup(P, q, A, l, u, 'warm_start', true);
% Solve
res = prob.solve();
% Check solver status
if ~strcmp(res.info.status, 'solved')
% error('OSQP did not solve the problem!')
t2=-1;
return
end
ctrlx = res.x(1:(N+1)*nx);
ctrlu = res.x((N+1)*nx+1:(N+1)*nx+N*nu);
x0 = ctrlx(1:nx);
testx0 = [testx0 x0];
x_0 = trans(x0, ctrlx, nx, N+1);
u_0 = transu(ctrlu, nu, N);
testx = (storagex - ctrlx)'*(storagex - ctrlx);
testu = (storageu - ctrlu)'*(storageu - ctrlu);
test = (testx)/(storagex'*storagex);
if (test)^(0.5)<=10^(-2)&& (testx/((N+1)*nx))^(0.5)<=10^(-4)%Convergence criterion
break
end
storagex = ctrlx;
storageu = ctrlu;
end
ctrl = ctrlu(1:nu);
x0 = [dt*x0(4)*cos(x0(3))+x0(1);dt*x0(4)*sin(x0(3))+x0(2);dt*ctrl(1)+x0(3);dt*ctrl(2)+x0(4)];
ctrlu = rewrite(ctrlu, nu, N);
x_0 = newinit(x0, ctrlu, nx, nu, N, dt);
u_0 = transu(ctrlu, nu, N);
impc(:, i+2) = x0;
end
end
% Linerize the CBF constraints (get a, b, c for lines)
function abc = tangentline(xy, r)% x and y from initialize states set, abc are coeeficients for linear equation a*x+b*y+c=0
[xx, ~] = size(xy);%xx=2,yy=N+1
xy(xx,:) = []; % this part should be changed for other case
xy((xx-1),:) = []; % this part should be changed other case
[xx, yy] = size(xy);%xx=2,yy=N+1
xyjiao = zeros(xx, yy);%intersection points
for i = 1 : xx
for j = 1 : yy
xyjiao(i, j) = r * xy(i, j) * (1 / (xy(:, j)' * xy(:, j)))^(0.5);%calculate coordinates of intersection points
end
end
cc = -r^2 * ones(1, yy);
abc = [xyjiao; cc];
end
% Get CBF constraints matrix
function Acbf = cbfmat(abc, nx, nu, dt, gamma, x0)
[~, yy] = size(abc);
Acbfx = zeros((yy-1), yy*nx);
Acbfu = zeros((yy-1), (yy-1)*nu);
for i = 1 : (yy-1)
Acbfx(i, (i*nx)+1) = abc(1, (i+1));
Acbfx(i, (i*nx)+2) = abc(2, (i+1));
end
for i = 1 : (yy-1)
Acbfu(i, ((i-1)*nu+3)) = - (1 - dt * gamma)^(i) * (abc(1, 1) * x0(1, 1) + abc(2, 1) * x0(2, 1) + abc(3, 1));
end
Acbf = [Acbfx Acbfu];
end
% Transfer vector x into matrix
function res = trans(x0, vector, nxx, nyy)%nxx=nx,nyy=N+1
res = zeros(nxx, nyy);
res(:,1) = x0;
for i = 1 : (nyy -1)
res(:,i+1)= vector(((i)*nxx+1):(i+1)*nxx);
end
end
% Transfer vector u into matrix
function resu = transu(vector, nxx, nyy)%nxx=nu,nyy=N
resu = zeros(nxx, nyy);
for i = 1 : (nyy)
resu(:,i)= vector(((i-1)*nxx+1):(i)*nxx);
end
end
% Rewrite u vector
function reu = rewrite(vector, nu, N)
append = vector((N-1)*nu+1:N*nu);
vector(1:nu) = [];
reu = [vector;append];
end
% Get new x_0
function x_0 = newinit(x0, ctrlu, nx, nu, N, dt)
x_0 = zeros(nx, N+1);
x_0(:, 1) = x0;
for i=1 : N
u0 = ctrlu((i-1)*nu+1:i*nu);
x0 = [dt*x0(4)*cos(x0(3))+x0(1);dt*x0(4)*sin(x0(3))+x0(2);dt*u0(1)+x0(3);dt*u0(2)+x0(4)];
x_0(:, i + 1) = x0;
end
end
% Get AA matrix
function AA = getaa(x0, dt)
AA = [1 0 -x0(4)*sin(x0(3))*dt cos(x0(3))*dt;0 1 x0(4)*cos(x0(3))*dt sin(x0(3))*dt;0 0 1 0;0 0 0 1];
end
% Get CC matrix
function CC = getcc(x0, x1, u0, dt)
CC = [x0(4)*sin(x0(3))*x0(3)*dt-x0(4)*cos(x0(3))*dt+x1(1)-x0(1);-x0(4)*cos(x0(3))*x0(3)*dt-x0(4)*sin(x0(3))*dt+x1(2)-x0(2);-u0(1)*dt+x1(3)-x0(3);-u0(2)*dt+x1(4)-x0(4)];
end
% Get Ax matrix
function Ax = getax(x_0, dt, N, nx)
x0 = x_0(:,1);
AA = getaa(x0, dt);
Ax = kron(speye(N+1), -speye(nx)) + kron(sparse(diag(ones(N, 1), -1)), AA);
for i = 1 : (N-1)
x0 = x_0(:,i+1);
AA = getaa(x0, dt);
Ax(nx*(i+1)+1:nx*(i+1)+nx,nx*i+1:nx*i+nx) = AA;
end
end
% Get Cx matrix
function Cx = getcx(x_0, u_0, dt, N, nx)
Cx = zeros(N*nx, 1);
for i = 1 : N
u0 = u_0(:,i);
x0 = x_0(:,i);
x1 = x_0(:,i+1);
CC = getcc(x0, x1, u0, dt);
Cx((i-1)*nx+1:(i-1)*nx+nx) = CC;
end
end
% Get A2cbf
function A2cbf = cbfmat2(abc, nx, nu, dt, gamma1, gamma2, x0)
[~, yy] = size(abc);
Acbfx2 = zeros((yy-2), yy*nx);
Acbfx22 = zeros((yy-2), yy*nx);
Acbfu2 = zeros((yy-2), (yy-1)*nu);
for i = 1 : (yy-2)
Acbfx2(i, (i*nx)+1) = (gamma1-1/dt)*abc(1, (i+1));
Acbfx2(i, (i*nx)+2) = (gamma1-1/dt)*abc(2, (i+1));
Acbfx2(i, ((i+1)*nx)+1) = (1/dt)*abc(1, (i+2));
Acbfx2(i, ((i+1)*nx)+2) = (1/dt)*abc(2, (i+2));
end
for i = 1 : (yy-2)
Acbfx22(i, (1*nx)+1) = -(1-dt*gamma2)^(i)/dt*abc(1, (1+1));
Acbfx22(i, (1*nx)+2) = -(1-dt*gamma2)^(i)/dt*abc(2, (1+1));
end
Acbfx2 = Acbfx2 + Acbfx22;
for i = 1 : (yy-2)
Acbfu2(i, ((i-1)*nu+4)) = - (1 - dt * gamma2)^(i)*(gamma1-1/dt)*(abc(1, 1) * x0(1, 1) + abc(2, 1) * x0(2, 1) + abc(3, 1));
end
A2cbf = [Acbfx2 Acbfu2];
end
% Get lcbf2
function lcbf2 = getlcbf2(abc, dt, gamma1, gamma2)
[~, yy] = size(abc);
lcbf2 = zeros((yy-2),1);
for i = 1 : (yy-2)
lcbf2(i, 1) = -abc(3, (i+2))/dt-(gamma1-1/dt)*abc(3, (i+1))+(1 - dt * gamma2)^(i)*abc(3, (1+1))/dt;
end
end
================================================
FILE: matlab/acc2023/benchmark/gamma1-0p6/test_each_horizon/test_N8.m
================================================
close all
clear
%------------------------------------------------------------------------------------------
nb = 1000;%Total number of initial states
nmpcd=zeros(1,nb);%Store computing time for one time-step for NMPC-DCBF
impcd=zeros(1,nb);%Store computing time for one time-step for iMPC-DCBF
nmpcinf=zeros(1,1);%Store infeasible rate for one time-step for NMPC-DCBF
impcinf=zeros(1,1);%Store infeasible rate for one time-step for iMPC-DCBF
i=8;%i is number of horozon
ts = 1;%Total time steps
load(gamma1_0p6\InitialStateData');%Load the generated 1000 initial states
[a, b, c, d]=compare(i,ts,nb,InitialMat);%Involves NMPC-DCBF and iMPC-DCBF
nmpcd(1,:)=a;
impcd(1,:)=b;
nmpcinf(1,1)=c;
impcinf(1,1)=d;
nmpcplot1=nmpcd(1,:);
nmpcplot1(nmpcplot1==-1)=[];%Eliminate infeasible one time-step trajectories
impcplot1=impcd(1,:);
impcplot1(impcplot1==-1)=[];%Eliminate infeasible one time-step trajectories
save('timecom8','nmpcplot1','impcplot1');
save('feasibility8','nmpcinf','impcinf');
distnmpc = fitdist(nmpcplot1','Normal');
distimpc = fitdist(impcplot1','Normal');
mu1=distnmpc.mu;%Get mean of sample of computing time for NMPC-DCBF
mu2=distimpc.mu;%Get mean of sample of computing time for iMPC-DCBF
sigma1=distnmpc.sigma;%Get variance of sample of computing time for NMPC-DCBF
sigma2=distimpc.sigma;%Get variance of sample of computing time for iMPC-DCBF
%Initialize atmosphere parameters
function [tnmpc, timpc, ratiotnmpc, ratiotimpc]=compare(N11,ttsim,samplen,InitialMat)
tnmpc=[];
timpc=[];
for i=1:samplen
N1=N11;
tsim=ttsim;
xini1=InitialMat(1,i);
yini1=InitialMat(2,i);
thetaini1=InitialMat(3,i);
vini1=InitialMat(4,i);
x01=[xini1;yini1;thetaini1;vini1];
x02=[xini1 yini1 thetaini1 vini1];
t1 = nmpcdcbf(x01, N1, tsim);
t2 = impcdcbf(x02, N1, tsim);
tindex=[N11 i];
disp(tindex);
tnmpc=[tnmpc t1];%Computing time for NMPC-DCBF
timpc=[timpc t2];%Computing time for iMPC-DCBF
end
nnmpc1 = length(tnmpc);
nimpc1 = length(timpc);
tnmpcs = tnmpc;
timpcs = timpc;
tnmpcs(tnmpcs==-1)=[];
timpcs(timpcs==-1)=[];
nnmpc2 = length(tnmpcs);
nimpc2 = length(timpcs);
ratiotnmpc = (nnmpc1-nnmpc2)/nnmpc1;%Infeasible rate for NMPC-DCBF
ratiotimpc = (nimpc1-nimpc2)/nimpc1;%Infeasible rate for iMPC-DCBF
end
function t1 = nmpcdcbf(x00, N1, ttsim)
%% General Flags
run_nmpc_dcbf_one = true;
run_nmpc_dcbf_multiple = true;
%% Setup and Parameters
x0 = x00;
dt = 0.1;
time_total = ttsim *dt;
P = zeros(4,4);%Weight matrix P
P(1,1) = 10; P(2,2) = 10; P(3,3) = 10;P(4,4) = 10;
Q = zeros(4,4);%Weight matrix Q
Q(1,1) = 10; Q(2,2) = 10; Q(3,3) = 10;Q(4,4) = 10;
R = zeros(4,4);%Weight matrix R
R(1,1) = 1; R(2,2) = 1;R(3,3) = 1000;R(4,4) = 1000;
%Variables range as below
xmin = [-10; -10; -10;-10];
xmax = [10; 10; 10;10];
umin = [-7; -5;-inf;-inf];
umax = [7; 5;inf;inf];
%% Discrete-time unicycle model
system.dt = dt;
system.A = [1 0 0 0;
0 1 0 0;
0 0 1 0;
0 0 0 1];
system.B = [x0(4,1)*cos(x0(3,1))*dt;
x0(4,1)*sin(x0(3,1))*dt;
0;
0];
system.C =[0 0 0 0;
0 0 0 0;
1*dt 0 0 0;
0 1*dt 0 0];
system.xl = xmin;
system.xu = xmax;
system.ul = umin;
system.uu = umax;
%% NMPC-DCBF parameters
params_nmpc_dcbf.Q = Q;
params_nmpc_dcbf.R = R;
params_nmpc_dcbf.P = P;
params_nmpc_dcbf.gamma1 = 0.6;
params_nmpc_dcbf.gamma2 = 0.6;
%% Obstacle
obs.pos1 = [0; 0];
obs.r1 = 1;
%% Simulate NMPC-DCBF with other N
params_nmpc_dcbf.N = N1;
if run_nmpc_dcbf_one
fprintf('Run NMPC-DCBF-8\n');
controller_nmpc_dcbf_multiple8 = NMPCDCBF1(x0, system, params_nmpc_dcbf);%Highest order mcbf=2
controller_nmpc_dcbf_multiple8.obs = obs;
controller_nmpc_dcbf_multiple8.sim(time_total);
end
t1=controller_nmpc_dcbf_multiple8.tt;
end
function t2=impcdcbf(x00, N1, ttsim)
t2=0;
xo = 0;
yo = 0;
r = 1;
N = N1;%Number of horizon
K1 = 1000;%Maximum iteration times, jmax
K = 1000;%Maximum iteration times, jmax
dt = 0.1;
gamma1 = 6;
gamma2 = 6;
nsim = ttsim;
tic;
% Constraints
umin = [-7; -5; -inf; -inf;];
umax = [7; 5; Inf; Inf];
xmin = [-10; -10; -10; -10];
xmax = [ 10; 10; 10; 10];
% Objective function
Q = diag([10 10 10 10]);
QN = Q;
R = 1 * eye(4);
R(3,3) = 1000;
R(4,4) = 1000;
% Initial and reference states
x0 = x00;
xr = [3; 0.01; 0; 0];
ur = [0; 0; 1; 1];
% Dynamic system initialization
BB = [0 0 0 0;0 0 0 0;dt 0 0 0;0 dt 0 0];
% Convex MPC frame
[nx, nu] = size(BB);
% Initialize states set (x00,x01,,,x0N）
x_0 = [];
x0 = x0';
u_0 = zeros(nu, N);
u0 = zeros(nu, 1);% we may need to change this for a better warm-up start
impc = zeros(nx, nsim + 1);
impc(:, 1) = x0;
x0new = x0;
for i=1 : (N+1)
x_0 = [x_0 x0new];
x0new = [dt*x0new(4)*cos(x0new(3))+x0new(1);dt*x0new(4)*sin(x0new(3))+x0new(2);dt*u0(1)+x0new(3);dt*u0(2)+x0new(4)];
end
abc = tangentline(x_0, r);
Acbf = cbfmat(abc, nx, nu, dt, gamma1, x0);%First order CBF constraints
lcbf = -abc(3, :);
lcbf(1) = [];
lcbf = lcbf';
ucbf = inf * ones(N, 1);
A2cbf = cbfmat2(abc, nx, nu, dt, gamma1, gamma2, x0);%Second order CBF constraints
lcbf2 = getlcbf2(abc, dt, gamma1, gamma2);
ucbf2 = inf * ones(N-1, 1);
% Cast MPC problem to a QP: x = (x(0),x(1),...,x(N),u(0),...,u(N-1))
% - quadratic objective
P = blkdiag( kron(speye(N), Q), QN, kron(speye(N), R) );
% - linear objective
q = [repmat(-Q*xr, N, 1); -QN*xr; repmat(-R*ur, N, 1)];
% - linear dynamics
Ax = getax(x_0, dt, N, nx);
Bu = kron([sparse(1, N); speye(N)], BB);
Cx = getcx(x_0, u_0, dt, N, nx);
Aeq = [Ax, Bu];
leq = [-x0; -Cx];
ueq = leq;
% - input and state constraints
Aineq = speye((N+1)*nx + N*nu);
lineq = [repmat(xmin, N+1, 1); repmat(umin, N, 1)];
uineq = [repmat(xmax, N+1, 1); repmat(umax, N, 1)];
% - OSQP constraints
A = [Aeq; Aineq; Acbf; A2cbf];
l = [leq; lineq; lcbf; lcbf2];
u = [ueq; uineq; ucbf; ucbf2];
% Create an OSQP object
prob = osqp;
% Setup workspace
prob.setup(P, q, A, l, u, 'warm_start', true);
% Solve
res = prob.solve();
% Check solver status
if ~strcmp(res.info.status, 'solved')
% error('OSQP did not solve the problem!')
t2=-1;
return
end
ctrlx = res.x(1:(N+1)*nx);
ctrlu = res.x((N+1)*nx+1:(N+1)*nx+N*nu);
for j = 1 : (K1-1)%Iterations for the first time-step
storagex = ctrlx;%store the state variables and inputs in order to compare them with the next iteration
storageu = ctrlu;
x_0 = trans(x0, ctrlx, nx, N+1);
u_0 = transu(ctrlu, nu, N);
abc = tangentline(x_0, r);
Acbf = cbfmat(abc, nx, nu, dt, gamma1, x0);
lcbf = -abc(3, :);
lcbf(1) = [];
lcbf = lcbf';
ucbf = inf * ones(N, 1);
A2cbf = cbfmat2(abc, nx, nu, dt, gamma1, gamma2, x0);
lcbf2 = getlcbf2(abc, dt, gamma1, gamma2);
ucbf2 = inf * ones(N-1, 1);
% Cast MPC problem to a QP: x = (x(0),x(1),...,x(N),u(0),...,u(N-1))
% - quadratic objective
P = blkdiag( kron(speye(N), Q), QN, kron(speye(N), R) );
% - linear objective
q = [repmat(-Q*xr, N, 1); -QN*xr; repmat(-R*ur, N, 1)];
% - linear dynamics
Ax = getax(x_0, dt, N, nx);
Bu = kron([sparse(1, N); speye(N)], BB);
Cx = getcx(x_0, u_0, dt, N, nx);
Aeq = [Ax, Bu];
leq = [-x0; -Cx];
ueq = leq;
% - input and state constraints
Aineq = speye((N+1)*nx + N*nu);
lineq = [repmat(xmin, N+1, 1); repmat(umin, N, 1)];
uineq = [repmat(xmax, N+1, 1); repmat(umax, N, 1)];
% - OSQP constraints
A = [Aeq; Aineq; Acbf; A2cbf];
l = [leq; lineq; lcbf; lcbf2];
u = [ueq; uineq; ucbf; ucbf2];
% Create an OSQP object
prob = osqp;
% Setup workspace
prob.setup(P, q, A, l, u, 'warm_start', true);
% Solve
res = prob.solve();
% Check solver status
if ~strcmp(res.info.status, 'solved')
% error('OSQP did not solve the problem!')
t2=-1;
return
end
ctrlx = res.x(1:(N+1)*nx);
ctrlu = res.x((N+1)*nx+1:(N+1)*nx+N*nu);
testx = (storagex - ctrlx)'*(storagex - ctrlx);
testu = (storageu - ctrlu)'*(storageu - ctrlu);
test = (testx)/(storagex'*storagex);
if (test)^(0.5)<=10^(-2)&& (testx/((N+1)*nx))^(0.5)<=10^(-4)%Convergence criterion
break
end
end % Move for the first step
ctrl = ctrlu(1:nu);
x0 = [dt*x0(4)*cos(x0(3))+x0(1);dt*x0(4)*sin(x0(3))+x0(2);dt*ctrl(1)+x0(3);dt*ctrl(2)+x0(4)];
ctrlu = rewrite(ctrlu, nu, N);
t2=t2+toc;
return
x_0 = newinit(x0, ctrlu, nx, nu, N, dt);
u_0 = transu(ctrlu, nu, N);
impc(:, 2) = x0;
storagex = ctrlx;
storageu = ctrlu;
for i = 1 : (nsim-1)%Iterations for the left time steps
for j = 1 : K
abc = tangentline(x_0, r);
Acbf = cbfmat(abc, nx, nu, dt, gamma1, x0);
lcbf = -abc(3, :);
lcbf(1) = [];
lcbf = lcbf';
ucbf = inf * ones(N, 1);
A2cbf = cbfmat2(abc, nx, nu, dt, gamma1, gamma2, x0);
lcbf2 = getlcbf2(abc, dt, gamma1, gamma2);
ucbf2 = inf * ones(N-1, 1);
% Cast MPC problem to a QP: x = (x(0),x(1),...,x(N),u(0),...,u(N-1))
% - quadratic objective
P = blkdiag( kron(speye(N), Q), QN, kron(speye(N), R) );
% - linear objective
q = [repmat(-Q*xr, N, 1); -QN*xr; repmat(-R*ur, N, 1)];
% - linear dynamics
Ax = getax(x_0, dt, N, nx);
Bu = kron([sparse(1, N); speye(N)], BB);
Cx = getcx(x_0, u_0, dt, N, nx);
Aeq = [Ax, Bu];
leq = [-x0; -Cx];
ueq = leq;
% - input and state constraints
Aineq = speye((N+1)*nx + N*nu);
lineq = [repmat(xmin, N+1, 1); repmat(umin, N, 1)];
uineq = [repmat(xmax, N+1, 1); repmat(umax, N, 1)];
% - OSQP constraints
A = [Aeq; Aineq; Acbf; A2cbf];
l = [leq; lineq; lcbf; lcbf2];
u = [ueq; uineq; ucbf; ucbf2];
% Create an OSQP object
prob = osqp;
% Setup workspace
prob.setup(P, q, A, l, u, 'warm_start', true);
% Solve
res = prob.solve();
% Check solver status
if ~strcmp(res.info.status, 'solved')
% error('OSQP did not solve the problem!')
t2=-1;
return
end
ctrlx = res.x(1:(N+1)*nx);
ctrlu = res.x((N+1)*nx+1:(N+1)*nx+N*nu);
x0 = ctrlx(1:nx);
testx0 = [testx0 x0];
x_0 = trans(x0, ctrlx, nx, N+1);
u_0 = transu(ctrlu, nu, N);
testx = (storagex - ctrlx)'*(storagex - ctrlx);
testu = (storageu - ctrlu)'*(storageu - ctrlu);
test = (testx)/(storagex'*storagex);
if (test)^(0.5)<=10^(-2)&& (testx/((N+1)*nx))^(0.5)<=10^(-4)%Convergence criterion
break
end
storagex = ctrlx;
storageu = ctrlu;
end
ctrl = ctrlu(1:nu);
x0 = [dt*x0(4)*cos(x0(3))+x0(1);dt*x0(4)*sin(x0(3))+x0(2);dt*ctrl(1)+x0(3);dt*ctrl(2)+x0(4)];
ctrlu = rewrite(ctrlu, nu, N);
x_0 = newinit(x0, ctrlu, nx, nu, N, dt);
u_0 = transu(ctrlu, nu, N);
impc(:, i+2) = x0;
end
end
% Linerize the CBF constraints (get a, b, c for lines)
function abc = tangentline(xy, r)% x and y from initialize states set, abc are coeeficients for linear equation a*x+b*y+c=0
[xx, ~] = size(xy);%xx=2,yy=N+1
xy(xx,:) = []; % this part should be changed for other case
xy((xx-1),:) = []; % this part should be changed other case
[xx, yy] = size(xy);%xx=2,yy=N+1
xyjiao = zeros(xx, yy);%intersection points
for i = 1 : xx
for j = 1 : yy
xyjiao(i, j) = r * xy(i, j) * (1 / (xy(:, j)' * xy(:, j)))^(0.5);%calculate coordinates of intersection points
end
end
cc = -r^2 * ones(1, yy);
abc = [xyjiao; cc];
end
% Get CBF constraints matrix
function Acbf = cbfmat(abc, nx, nu, dt, gamma, x0)
[~, yy] = size(abc);
Acbfx = zeros((yy-1), yy*nx);
Acbfu = zeros((yy-1), (yy-1)*nu);
for i = 1 : (yy-1)
Acbfx(i, (i*nx)+1) = abc(1, (i+1));
Acbfx(i, (i*nx)+2) = abc(2, (i+1));
end
for i = 1 : (yy-1)
Acbfu(i, ((i-1)*nu+3)) = - (1 - dt * gamma)^(i) * (abc(1, 1) * x0(1, 1) + abc(2, 1) * x0(2, 1) + abc(3, 1));
end
Acbf = [Acbfx Acbfu];
end
% Transfer vector x into matrix
function res = trans(x0, vector, nxx, nyy)%nxx=nx,nyy=N+1
res = zeros(nxx, nyy);
res(:,1) = x0;
for i = 1 : (nyy -1)
res(:,i+1)= vector(((i)*nxx+1):(i+1)*nxx);
end
end
% Transfer vector u into matrix
function resu = transu(vector, nxx, nyy)%nxx=nu,nyy=N
resu = zeros(nxx, nyy);
for i = 1 : (nyy)
resu(:,i)= vector(((i-1)*nxx+1):(i)*nxx);
end
end
% Rewrite u vector
function reu = rewrite(vector, nu, N)
append = vector((N-1)*nu+1:N*nu);
vector(1:nu) = [];
reu = [vector;append];
end
% Get new x_0
function x_0 = newinit(x0, ctrlu, nx, nu, N, dt)
x_0 = zeros(nx, N+1);
x_0(:, 1) = x0;
for i=1 : N
u0 = ctrlu((i-1)*nu+1:i*nu);
x0 = [dt*x0(4)*cos(x0(3))+x0(1);dt*x0(4)*sin(x0(3))+x0(2);dt*u0(1)+x0(3);dt*u0(2)+x0(4)];
x_0(:, i + 1) = x0;
end
end
% Get AA matrix
function AA = getaa(x0, dt)
AA = [1 0 -x0(4)*sin(x0(3))*dt cos(x0(3))*dt;0 1 x0(4)*cos(x0(3))*dt sin(x0(3))*dt;0 0 1 0;0 0 0 1];
end
% Get CC matrix
function CC = getcc(x0, x1, u0, dt)
CC = [x0(4)*sin(x0(3))*x0(3)*dt-x0(4)*cos(x0(3))*dt+x1(1)-x0(1);-x0(4)*cos(x0(3))*x0(3)*dt-x0(4)*sin(x0(3))*dt+x1(2)-x0(2);-u0(1)*dt+x1(3)-x0(3);-u0(2)*dt+x1(4)-x0(4)];
end
% Get Ax matrix
function Ax = getax(x_0, dt, N, nx)
x0 = x_0(:,1);
AA = getaa(x0, dt);
Ax = kron(speye(N+1), -speye(nx)) + kron(sparse(diag(ones(N, 1), -1)), AA);
for i = 1 : (N-1)
x0 = x_0(:,i+1);
AA = getaa(x0, dt);
Ax(nx*(i+1)+1:nx*(i+1)+nx,nx*i+1:nx*i+nx) = AA;
end
end
% Get Cx matrix
function Cx = getcx(x_0, u_0, dt, N, nx)
Cx = zeros(N*nx, 1);
for i = 1 : N
u0 = u_0(:,i);
x0 = x_0(:,i);
x1 = x_0(:,i+1);
CC = getcc(x0, x1, u0, dt);
Cx((i-1)*nx+1:(i-1)*nx+nx) = CC;
end
end
% Get A2cbf
function A2cbf = cbfmat2(abc, nx, nu, dt, gamma1, gamma2, x0)
[~, yy] = size(abc);
Acbfx2 = zeros((yy-2), yy*nx);
Acbfx22 = zeros((yy-2), yy*nx);
Acbfu2 = zeros((yy-2), (yy-1)*nu);
for i = 1 : (yy-2)
Acbfx2(i, (i*nx)+1) = (gamma1-1/dt)*abc(1, (i+1));
Acbfx2(i, (i*nx)+2) = (gamma1-1/dt)*abc(2, (i+1));
Acbfx2(i, ((i+1)*nx)+1) = (1/dt)*abc(1, (i+2));
Acbfx2(i, ((i+1)*nx)+2) = (1/dt)*abc(2, (i+2));
end
for i = 1 : (yy-2)
Acbfx22(i, (1*nx)+1) = -(1-dt*gamma2)^(i)/dt*abc(1, (1+1));
Acbfx22(i, (1*nx)+2) = -(1-dt*gamma2)^(i)/dt*abc(2, (1+1));
end
Acbfx2 = Acbfx2 + Acbfx22;
for i = 1 : (yy-2)
Acbfu2(i, ((i-1)*nu+4)) = - (1 - dt * gamma2)^(i)*(gamma1-1/dt)*(abc(1, 1) * x0(1, 1) + abc(2, 1) * x0(2, 1) + abc(3, 1));
end
A2cbf = [Acbfx2 Acbfu2];
end
% Get lcbf2
function lcbf2 = getlcbf2(abc, dt, gamma1, gamma2)
[~, yy] = size(abc);
lcbf2 = zeros((yy-2),1);
for i = 1 : (yy-2)
lcbf2(i, 1) = -abc(3, (i+2))/dt-(gamma1-1/dt)*abc(3, (i+1))+(1 - dt * gamma2)^(i)*abc(3, (1+1))/dt;
end
end
================================================
FILE: matlab/acc2023/benchmark/gamma1-0p6gamma2-0p6/NMPCDCBF2.m
================================================
classdef NMPCDCBF2 < handle
% MPC with distance constraints
properties
system
params
x0
x_curr
time_curr = 0.0
xlog = []
ulog = []
tt = 0
distlog = []
solvertime = []
xopenloop = {}
uopenloop = {}
u_cost = 0
obs
end
methods
function self = NMPCDCBF2(x0, system, params)
% Define MPC_CBF controller
self.x0 = x0;
self.x_curr = x0;
self.system = system;
self.params = params;
end
function sim(self, time)
% Simulate the system until a given time
tic;
xk = self.x_curr;
while self.time_curr < time
[~, uk, tt] = self.solveNMPCDCBF1(self.x_curr);%if infeasiblility happens, stop
if tt == -1
self.tt = tt;%record computing time for each time-step
return
end
xk = self.system.A * xk + [xk(4,1) * cos(xk(3,1))*self.system.dt;xk(4,1) * sin(xk(3,1))*self.system.dt;0;0] + self.system.C * uk;
% update system
self.x_curr = xk;
self.time_curr = self.time_curr + self.system.dt;
self.xlog = [self.xlog, xk];
self.ulog = [self.ulog, uk];
self.u_cost = self.u_cost + uk'*uk;
tt = tt + toc;
self.tt = tt;
return
end
end
function [xopt, uopt,tt] = solveNMPCDCBF1(self, xk)
% Solve NMPC-DCBF
[feas, x, u, J] = self.solve_cftoc1(xk);
if ~feas
xopt = [];
uopt = [];
tt = -1;
return
else
xopt = x(:,2);
uopt = u(:,1);
tt = 0;
end
end
function [feas, xopt, uopt, Jopt] = solve_cftoc1(self, xk)
% Solve CFTOC
% extract variables
N = self.params.N;
% define variables and cost
x = sdpvar(4, N+1);%%%%%%%%%%%%%%%%%%%%%%%%%%%%
u = sdpvar(4, N);
constraints = [];
cost = 0;
% initial constraint
constraints = [constraints; x(:,1) == xk];
% add constraints and costs
AA=self.system.A;
BB=self.system.B;
CC=self.system.C;
constraints = [constraints;
self.system.xl <= x(:,1) <= self.system.xu;
self.system.ul <= u(:,1) <= self.system.uu;
x(:,2) == AA * x(:,1) + [x(4,1)*cos(x(3,1))*self.system.dt;x(4,1)*sin(x(3,1))*self.system.dt;0; 0] + CC * u(:,1)];
cost = cost + (x(:,1)-[3;0.01;0;0])'*self.params.Q*(x(:,1)-[3;0.01;0;0]) + (u(:,1)-[0;0;1;1])'*self.params.R*(u(:,1)-[0;0;1;1]); % self.params.S*(w(:,1)-1)^2;
for i = 2:1:N
constraints = [constraints;
self.system.xl <= x(:,i) <= self.system.xu;
self.system.ul <= u(:,i) <= self.system.uu;
x(:,i+1) == AA * x(:,i) + [x(4,i)*cos(x(3,i))*self.system.dt;x(4,i)*sin(x(3,i))*self.system.dt;0; 0] + CC * u(:,i)];
cost = cost + (x(:,i)-[3;0.01;0;0])'*self.params.Q*(x(:,i)-[3;0.01;0;0]) + (u(:,i)-[0;0;1;1])'*self.params.R*(u(:,i)-[0;0;1;1]);% self.params.S*(w(:,i)-1)^2;
end
% add CBF constraints
for i = 1:N-1
pos = self.obs.pos1;
r = self.obs.r1 ;
b = (x([1:2],i)-pos)'*((x([1:2],i)-pos))-r^2;
b_next = (x([1:2],i+1)-pos)'*((x([1:2],i+1)-pos))-r^2;
b_next_next = (x([1:2],i+2)-pos)'*((x([1:2],i+2)-pos))-r^2;
b1 = (b_next - b)/self.system.dt + self.params.gamma1/self.system.dt * (b);
b1_next = (b_next_next - b_next)/self.system.dt + self.params.gamma1/self.system.dt * (b_next);
constraints = [constraints; b1_next >=u(4,i)*(1-self.params.gamma2)*b1;b_next >= u(3,i)*(1-self.params.gamma1)*b];%Highest order mcbf=2
end
% add terminal cost
cost = cost + (x(:,N+1)-[3;0.01;0;0])'*self.params.P*(x(:,N+1)-[3;0.01;0;0]);
ops = sdpsettings('solver','ipopt','verbose',0);
% solve optimization
diagnostics = optimize(constraints, cost, ops);
if diagnostics.problem == 0
feas = true;
xopt = value(x);
uopt = value(u);
Jopt = value(cost);
else
feas = false;
xopt = [];
uopt = [];
Jopt = [];
end
pos = self.obs.pos1;
r = self.obs.r1 ;
self.distlog = [self.distlog, (r^2-(xk(1:2)-pos)'*(xk(1:2)-pos))];
self.xopenloop{size(self.xopenloop,2)+1} = xopt;
self.uopenloop{size(self.uopenloop,2)+1} = uopt;
self.solvertime = [self.solvertime, diagnostics.solvertime];
fprintf('solver time: %f\n', diagnostics.solvertime);
end
end
end
================================================
FILE: matlab/acc2023/benchmark/gamma1-0p6gamma2-0p6/test_comprehensive.m
================================================
close all
clear
%------------------------------------------------------------------------------------------
nmpcdata=zeros(3,6);%Store data in matrix for nmpc and impc, first row: mean of computing time; second row: standard deviation of computing time; third row: infeasible rate
impcdata=zeros(3,6);
for ii = 1:6
nb = 1000;%Total number of initial states
nmpcd=zeros(1,nb);%Store computing time for one time-step for NMPC-DCBF
impcd=zeros(1,nb);%Store computing time for one time-step for iMPC-DCBF
nmpcinf=zeros(1,1);%Store infeasible rate for one time-step for NMPC-DCBF
impcinf=zeros(1,1);%Store infeasible rate for one time-step for iMPC-DCBF
i=ii*4;%i is number of horozon
ts = 1;%Total time steps
load(gamma1_0p6gamma2_0p6\InitialStateData');%Load the generated 1000 initial states
[a, b, c, d]=compare(i,ts,nb,InitialMat);%Involves NMPC-DCBF and iMPC-DCBF
nmpcd(1,:)=a;
impcd(1,:)=b;
nmpcinf(1,1)=c;
impcinf(1,1)=d;
nmpcplot1=nmpcd(1,:);
nmpcplot1(nmpcplot1==-1)=[];%Eliminate infeasible one time-step trajectories
impcplot1=impcd(1,:);
impcplot1(impcplot1==-1)=[];%Eliminate infeasible one time-step trajectories
filenm1 = ['timecom' num2str(i) '.mat'];
filenm2 = ['feasibility' num2str(i) '.mat'];
save(filenm1,'nmpcplot1','impcplot1');
save(filenm2,'nmpcinf','impcinf');
distnmpc = fitdist(nmpcplot1','Normal');
distimpc = fitdist(impcplot1','Normal');
mu1=distnmpc.mu;%Get mean of sample of computing time for NMPC-DCBF
mu2=distimpc.mu;%Get mean of sample of computing time for iMPC-DCBF
sigma1=distnmpc.sigma;%Get variance of sample of computing time for NMPC-DCBF
sigma2=distimpc.sigma;%Get variance of sample of computing time for iMPC-DCBF
nmpcdata(1,ii)=mu1;
impcdata(1,ii)=mu2;
nmpcdata(2,ii)=sigma1;
impcdata(2,ii)=sigma2;
nmpcdata(3,ii)=nmpcinf;
impcdata(3,ii)=impcinf;
end
%Initialize atmosphere parameters
function [tnmpc, timpc, ratiotnmpc, ratiotimpc]=compare(N11,ttsim,samplen,InitialMat)
tnmpc=[];
timpc=[];
for i=1:samplen
N1=N11;
tsim=ttsim;
xini1=InitialMat(1,i);
yini1=InitialMat(2,i);
thetaini1=InitialMat(3,i);
vini1=InitialMat(4,i);
x01=[xini1;yini1;thetaini1;vini1];
x02=[xini1 yini1 thetaini1 vini1];
t1 = nmpcdcbf(x01, N1, tsim);
t2 = impcdcbf(x02, N1, tsim);
tindex=[N11 i];
disp(tindex);
tnmpc=[tnmpc t1];%Computing time for NMPC-DCBF
timpc=[timpc t2];%Computing time for iMPC-DCBF
end
nnmpc1 = length(tnmpc);
nimpc1 = length(timpc);
tnmpcs = tnmpc;
timpcs = timpc;
tnmpcs(tnmpcs==-1)=[];
timpcs(timpcs==-1)=[];
nnmpc2 = length(tnmpcs);
nimpc2 = length(timpcs);
ratiotnmpc = (nnmpc1-nnmpc2)/nnmpc1;%Infeasible rate for NMPC-DCBF
ratiotimpc = (nimpc1-nimpc2)/nimpc1;%Infeasible rate for iMPC-DCBF
end
function t1 = nmpcdcbf(x00, N1, ttsim)
%% General Flags
run_nmpc_dcbf_one = true;
run_nmpc_dcbf_multiple = true;
%% Setup and Parameters
x0 = x00;
dt = 0.1;
time_total = ttsim *dt;
P = zeros(4,4);%Weight matrix P
P(1,1) = 10; P(2,2) = 10; P(3,3) = 10;P(4,4) = 10;
Q = zeros(4,4);%Weight matrix Q
Q(1,1) = 10; Q(2,2) = 10; Q(3,3) = 10;Q(4,4) = 10;
R = zeros(4,4);%Weight matrix R
R(1,1) = 1; R(2,2) = 1;R(3,3) = 1000;R(4,4) = 1000;
%Variables range as below
xmin = [-10; -10; -10;-10];
xmax = [10; 10; 10;10];
umin = [-7; -5;-inf;-inf];
umax = [7; 5;inf;inf];
%% Discrete-time unicycle model
system.dt = dt;
system.A = [1 0 0 0;
0 1 0 0;
0 0 1 0;
0 0 0 1];
system.B = [x0(4,1)*cos(x0(3,1))*dt;
x0(4,1)*sin(x0(3,1))*dt;
0;
0];
system.C =[0 0 0 0;
0 0 0 0;
1*dt 0 0 0;
0 1*dt 0 0];
system.xl = xmin;
system.xu = xmax;
system.ul = umin;
system.uu = umax;
%% NMPC-DCBF parameters
params_nmpc_dcbf.Q = Q;
params_nmpc_dcbf.R = R;
params_nmpc_dcbf.P = P;
params_nmpc_dcbf.gamma1 = 0.6;
params_nmpc_dcbf.gamma2 = 0.6;
%% Obstacle
obs.pos1 = [0; 0];
obs.r1 = 1;
%% Simulate NMPC-DCBF with other N
params_nmpc_dcbf.N = N1;
if run_nmpc_dcbf_one
fprintf('Run NMPC-DCBF-4\n');
controller_nmpc_dcbf_multiple4 = NMPCDCBF2(x0, system, params_nmpc_dcbf);%Highest order mcbf=2
controller_nmpc_dcbf_multiple4.obs = obs;
controller_nmpc_dcbf_multiple4.sim(time_total);
end
t1=controller_nmpc_dcbf_multiple4.tt;
end
function t2=impcdcbf(x00, N1, ttsim)
t2=0;
xo = 0;
yo = 0;
r = 1;
N = N1;%Number of horizon
K1 = 1000;%Maximum iteration times, jmax
K = 1000;%Maximum iteration times, jmax
dt = 0.1;
gamma1 = 6;
gamma2 = 6;
nsim = ttsim;
tic;
% Constraints
umin = [-7; -5; -inf; -inf;];
umax = [7; 5; Inf; Inf];
xmin = [-10; -10; -10; -10];
xmax = [ 10; 10; 10; 10];
% Objective function
Q = diag([10 10 10 10]);
QN = Q;
R = 1 * eye(4);
R(3,3) = 1000;
R(4,4) = 1000;
% Initial and reference states
x0 = x00;
xr = [3; 0.01; 0; 0];
ur = [0; 0; 1; 1];
% Dynamic system initialization
BB = [0 0 0 0;0 0 0 0;dt 0 0 0;0 dt 0 0];
% Convex MPC frame
[nx, nu] = size(BB);
% Initialize states set (x00,x01,,,x0N）
x_0 = [];
x0 = x0';
u_0 = zeros(nu, N);
u0 = zeros(nu, 1);% we may need to change this for a better warm-up start
impc = zeros(nx, nsim + 1);
impc(:, 1) = x0;
x0new = x0;
for i=1 : (N+1)
x_0 = [x_0 x0new];
x0new = [dt*x0new(4)*cos(x0new(3))+x0new(1);dt*x0new(4)*sin(x0new(3))+x0new(2);dt*u0(1)+x0new(3);dt*u0(2)+x0new(4)];
end
abc = tangentline(x_0, r);
Acbf = cbfmat(abc, nx, nu, dt, gamma1, x0);%First order CBF constraints
lcbf = -abc(3, :);
lcbf(1) = [];
lcbf = lcbf';
ucbf = inf * ones(N, 1);
A2cbf = cbfmat2(abc, nx, nu, dt, gamma1, gamma2, x0);%Second order CBF constraints
lcbf2 = getlcbf2(abc, dt, gamma1, gamma2);
ucbf2 = inf * ones(N-1, 1);
% Cast MPC problem to a QP: x = (x(0),x(1),...,x(N),u(0),...,u(N-1))
% - quadratic objective
P = blkdiag( kron(speye(N), Q), QN, kron(speye(N), R) );
% - linear objective
q = [repmat(-Q*xr, N, 1); -QN*xr; repmat(-R*ur, N, 1)];
% - linear dynamics
Ax = getax(x_0, dt, N, nx);
Bu = kron([sparse(1, N); speye(N)], BB);
Cx = getcx(x_0, u_0, dt, N, nx);
Aeq = [Ax, Bu];
leq = [-x0; -Cx];
ueq = leq;
% - input and state constraints
Aineq = speye((N+1)*nx + N*nu);
lineq = [repmat(xmin, N+1, 1); repmat(umin, N, 1)];
uineq = [repmat(xmax, N+1, 1); repmat(umax, N, 1)];
% - OSQP constraints
A = [Aeq; Aineq; Acbf; A2cbf];
l = [leq; lineq; lcbf; lcbf2];
u = [ueq; uineq; ucbf; ucbf2];
% Create an OSQP object
prob = osqp;
% Setup workspace
prob.setup(P, q, A, l, u, 'warm_start', true);
% Solve
res = prob.solve();
% Check solver status
if ~strcmp(res.info.status, 'solved')
% error('OSQP did not solve the problem!')
t2=-1;
return
end
ctrlx = res.x(1:(N+1)*nx);
ctrlu = res.x((N+1)*nx+1:(N+1)*nx+N*nu);
for j = 1 : (K1-1)%Iterations for the first time-step
storagex = ctrlx;%store the state variables and inputs in order to compare them with the next iteration
storageu = ctrlu;
x_0 = trans(x0, ctrlx, nx, N+1);
u_0 = transu(ctrlu, nu, N);
abc = tangentline(x_0, r);
Acbf = cbfmat(abc, nx, nu, dt, gamma1, x0);
lcbf = -abc(3, :);
lcbf(1) = [];
lcbf = lcbf';
ucbf = inf * ones(N, 1);
A2cbf = cbfmat2(abc, nx, nu, dt, gamma1, gamma2, x0);
lcbf2 = getlcbf2(abc, dt, gamma1, gamma2);
ucbf2 = inf * ones(N-1, 1);
% Cast MPC problem to a QP: x = (x(0),x(1),...,x(N),u(0),...,u(N-1))
% - quadratic objective
P = blkdiag( kron(speye(N), Q), QN, kron(speye(N), R) );
% - linear objective
q = [repmat(-Q*xr, N, 1); -QN*xr; repmat(-R*ur, N, 1)];
% - linear dynamics
Ax = getax(x_0, dt, N, nx);
Bu = kron([sparse(1, N); speye(N)], BB);
Cx = getcx(x_0, u_0, dt, N, nx);
Aeq = [Ax, Bu];
leq = [-x0; -Cx];
ueq = leq;
% - input and state constraints
Aineq = speye((N+1)*nx + N*nu);
lineq = [repmat(xmin, N+1, 1); repmat(umin, N, 1)];
uineq = [repmat(xmax, N+1, 1); repmat(umax, N, 1)];
% - OSQP constraints
A = [Aeq; Aineq; Acbf; A2cbf];
l = [leq; lineq; lcbf; lcbf2];
u = [ueq; uineq; ucbf; ucbf2];
% Create an OSQP object
prob = osqp;
% Setup workspace
prob.setup(P, q, A, l, u, 'warm_start', true);
% Solve
res = prob.solve();
% Check solver status
if ~strcmp(res.info.status, 'solved')
% error('OSQP did not solve the problem!')
t2=-1;
return
end
ctrlx = res.x(1:(N+1)*nx);
ctrlu = res.x((N+1)*nx+1:(N+1)*nx+N*nu);
testx = (storagex - ctrlx)'*(storagex - ctrlx);
testu = (storageu - ctrlu)'*(storageu - ctrlu);
test = (testx)/(storagex'*storagex);
if (test)^(0.5)<=10^(-2)&& (testx/((N+1)*nx))^(0.5)<=10^(-4)%Convergence criterion
break
end
end % Move for the first step
ctrl = ctrlu(1:nu);
x0 = [dt*x0(4)*cos(x0(3))+x0(1);dt*x0(4)*sin(x0(3))+x0(2);dt*ctrl(1)+x0(3);dt*ctrl(2)+x0(4)];
ctrlu = rewrite(ctrlu, nu, N);
t2=t2+toc;
return
x_0 = newinit(x0, ctrlu, nx, nu, N, dt);
u_0 = transu(ctrlu, nu, N);
impc(:, 2) = x0;
storagex = ctrlx;
storageu = ctrlu;
for i = 1 : (nsim-1)%Iterations for the left time steps
for j = 1 : K
abc = tangentline(x_0, r);
Acbf = cbfmat(abc, nx, nu, dt, gamma1, x0);
lcbf = -abc(3, :);
lcbf(1) = [];
lcbf = lcbf';
ucbf = inf * ones(N, 1);
A2cbf = cbfmat2(abc, nx, nu, dt, gamma1, gamma2, x0);
lcbf2 = getlcbf2(abc, dt, gamma1, gamma2);
ucbf2 = inf * ones(N-1, 1);
% Cast MPC problem to a QP: x = (x(0),x(1),...,x(N),u(0),...,u(N-1))
% - quadratic objective
P = blkdiag( kron(speye(N), Q), QN, kron(speye(N), R) );
% - linear objective
q = [repmat(-Q*xr, N, 1); -QN*xr; repmat(-R*ur, N, 1)];
% - linear dynamics
Ax = getax(x_0, dt, N, nx);
Bu = kron([sparse(1, N); speye(N)], BB);
Cx = getcx(x_0, u_0, dt, N, nx);
Aeq = [Ax, Bu];
leq = [-x0; -Cx];
ueq = leq;
% - input and state constraints
Aineq = speye((N+1)*nx + N*nu);
lineq = [repmat(xmin, N+1, 1); repmat(umin, N, 1)];
uineq = [repmat(xmax, N+1, 1); repmat(umax, N, 1)];
% - OSQP constraints
A = [Aeq; Aineq; Acbf; A2cbf];
l = [leq; lineq; lcbf; lcbf2];
u = [ueq; uineq; ucbf; ucbf2];
% Create an OSQP object
prob = osqp;
% Setup workspace
prob.setup(P, q, A, l, u, 'warm_start', true);
% Solve
res = prob.solve();
% Check solver status
if ~strcmp(res.info.status, 'solved')
% error('OSQP did not solve the problem!')
t2=-1;
return
end
ctrlx = res.x(1:(N+1)*nx);
ctrlu = res.x((N+1)*nx+1:(N+1)*nx+N*nu);
x0 = ctrlx(1:nx);
testx0 = [testx0 x0];
x_0 = trans(x0, ctrlx, nx, N+1);
u_0 = transu(ctrlu, nu, N);
testx = (storagex - ctrlx)'*(storagex - ctrlx);
testu = (storageu - ctrlu)'*(storageu - ctrlu);
test = (testx)/(storagex'*storagex);
if (test)^(0.5)<=10^(-2)&& (testx/((N+1)*nx))^(0.5)<=10^(-4)%Convergence criterion
break
end
storagex = ctrlx;
storageu = ctrlu;
end
ctrl = ctrlu(1:nu);
x0 = [dt*x0(4)*cos(x0(3))+x0(1);dt*x0(4)*sin(x0(3))+x0(2);dt*ctrl(1)+x0(3);dt*ctrl(2)+x0(4)];
ctrlu = rewrite(ctrlu, nu, N);
x_0 = newinit(x0, ctrlu, nx, nu, N, dt);
u_0 = transu(ctrlu, nu, N);
impc(:, i+2) = x0;
end
end
% Linerize the CBF constraints (get a, b, c for lines)
function abc = tangentline(xy, r)% x and y from initialize states set, abc are coeeficients for linear equation a*x+b*y+c=0
[xx, ~] = size(xy);%xx=2,yy=N+1
xy(xx,:) = []; % this part should be changed for other case
xy((xx-1),:) = []; % this part should be changed other case
[xx, yy] = size(xy);%xx=2,yy=N+1
xyjiao = zeros(xx, yy);%intersection points
for i = 1 : xx
for j = 1 : yy
xyjiao(i, j) = r * xy(i, j) * (1 / (xy(:, j)' * xy(:, j)))^(0.5);%calculate coordinates of intersection points
end
end
cc = -r^2 * ones(1, yy);
abc = [xyjiao; cc];
end
% Get CBF constraints matrix
function Acbf = cbfmat(abc, nx, nu, dt, gamma, x0)
[~, yy] = size(abc);
Acbfx = zeros((yy-1), yy*nx);
Acbfu = zeros((yy-1), (yy-1)*nu);
for i = 1 : (yy-1)
Acbfx(i, (i*nx)+1) = abc(1, (i+1));
Acbfx(i, (i*nx)+2) = abc(2, (i+1));
end
for i = 1 : (yy-1)
Acbfu(i, ((i-1)*nu+3)) = - (1 - dt * gamma)^(i) * (abc(1, 1) * x0(1, 1) + abc(2, 1) * x0(2, 1) + abc(3, 1));
end
Acbf = [Acbfx Acbfu];
end
% Transfer vector x into matrix
function res = trans(x0, vector, nxx, nyy)%nxx=nx,nyy=N+1
res = zeros(nxx, nyy);
res(:,1) = x0;
for i = 1 : (nyy -1)
res(:,i+1)= vector(((i)*nxx+1):(i+1)*nxx);
end
end
% Transfer vector u into matrix
function resu = transu(vector, nxx, nyy)%nxx=nu,nyy=N
resu = zeros(nxx, nyy);
for i = 1 : (nyy)
resu(:,i)= vector(((i-1)*nxx+1):(i)*nxx);
end
end
% Rewrite u vector
function reu = rewrite(vector, nu, N)
append = vector((N-1)*nu+1:N*nu);
vector(1:nu) = [];
reu = [vector;append];
end
% Get new x_0
function x_0 = newinit(x0, ctrlu, nx, nu, N, dt)
x_0 = zeros(nx, N+1);
x_0(:, 1) = x0;
for i=1 : N
u0 = ctrlu((i-1)*nu+1:i*nu);
x0 = [dt*x0(4)*cos(x0(3))+x0(1);dt*x0(4)*sin(x0(3))+x0(2);dt*u0(1)+x0(3);dt*u0(2)+x0(4)];
x_0(:, i + 1) = x0;
end
end
% Get AA matrix
function AA = getaa(x0, dt)
AA = [1 0 -x0(4)*sin(x0(3))*dt cos(x0(3))*dt;0 1 x0(4)*cos(x0(3))*dt sin(x0(3))*dt;0 0 1 0;0 0 0 1];
end
% Get CC matrix
function CC = getcc(x0, x1, u0, dt)
CC = [x0(4)*sin(x0(3))*x0(3)*dt-x0(4)*cos(x0(3))*dt+x1(1)-x0(1);-x0(4)*cos(x0(3))*x0(3)*dt-x0(4)*sin(x0(3))*dt+x1(2)-x0(2);-u0(1)*dt+x1(3)-x0(3);-u0(2)*dt+x1(4)-x0(4)];
end
% Get Ax matrix
function Ax = getax(x_0, dt, N, nx)
x0 = x_0(:,1);
AA = getaa(x0, dt);
Ax = kron(speye(N+1), -speye(nx)) + kron(sparse(diag(ones(N, 1), -1)), AA);
for i = 1 : (N-1)
x0 = x_0(:,i+1);
AA = getaa(x0, dt);
Ax(nx*(i+1)+1:nx*(i+1)+nx,nx*i+1:nx*i+nx) = AA;
end
end
% Get Cx matrix
function Cx = getcx(x_0, u_0, dt, N, nx)
Cx = zeros(N*nx, 1);
for i = 1 : N
u0 = u_0(:,i);
x0 = x_0(:,i);
x1 = x_0(:,i+1);
CC = getcc(x0, x1, u0, dt);
Cx((i-1)*nx+1:(i-1)*nx+nx) = CC;
end
end
% Get A2cbf
function A2cbf = cbfmat2(abc, nx, nu, dt, gamma1, gamma2, x0)
[~, yy] = size(abc);
Acbfx2 = zeros((yy-2), yy*nx);
Acbfx22 = zeros((yy-2), yy*nx);
Acbfu2 = zeros((yy-2), (yy-1)*nu);
for i = 1 : (yy-2)
Acbfx2(i, (i*nx)+1) = (gamma1-1/dt)*abc(1, (i+1));
Acbfx2(i, (i*nx)+2) = (gamma1-1/dt)*abc(2, (i+1));
Acbfx2(i, ((i+1)*nx)+1) = (1/dt)*abc(1, (i+2));
Acbfx2(i, ((i+1)*nx)+2) = (1/dt)*abc(2, (i+2));
end
for i = 1 : (yy-2)
Acbfx22(i, (1*nx)+1) = -(1-dt*gamma2)^(i)/dt*abc(1, (1+1));
Acbfx22(i, (1*nx)+2) = -(1-dt*gamma2)^(i)/dt*abc(2, (1+1));
end
Acbfx2 = Acbfx2 + Acbfx22;
for i = 1 : (yy-2)
Acbfu2(i, ((i-1)*nu+4)) = - (1 - dt * gamma2)^(i)*(gamma1-1/dt)*(abc(1, 1) * x0(1, 1) + abc(2, 1) * x0(2, 1) + abc(3, 1));
end
A2cbf = [Acbfx2 Acbfu2];
end
% Get lcbf2
function lcbf2 = getlcbf2(abc, dt, gamma1, gamma2)
[~, yy] = size(abc);
lcbf2 = zeros((yy-2),1);
for i = 1 : (yy-2)
lcbf2(i, 1) = -abc(3, (i+2))/dt-(gamma1-1/dt)*abc(3, (i+1))+(1 - dt * gamma2)^(i)*abc(3, (1+1))/dt;
end
end
================================================
FILE: matlab/acc2023/benchmark/gamma1-0p6gamma2-0p6/test_each_horizon/NMPCDCBF2.m
================================================
classdef NMPCDCBF2 < handle
% MPC with distance constraints
properties
system
params
x0
x_curr
time_curr = 0.0
xlog = []
ulog = []
tt = 0
distlog = []
solvertime = []
xopenloop = {}
uopenloop = {}
u_cost = 0
obs
end
methods
function self = NMPCDCBF2(x0, system, params)
% Define MPC_CBF controller
self.x0 = x0;
self.x_curr = x0;
self.system = system;
self.params = params;
end
function sim(self, time)
% Simulate the system until a given time
tic;
xk = self.x_curr;
while self.time_curr < time
[~, uk, tt] = self.solveNMPCDCBF1(self.x_curr);%if infeasiblility happens, stop
if tt == -1
self.tt = tt;%record computing time for each time-step
return
end
xk = self.system.A * xk + [xk(4,1) * cos(xk(3,1))*self.system.dt;xk(4,1) * sin(xk(3,1))*self.system.dt;0;0] + self.system.C * uk;
% update system
self.x_curr = xk;
self.time_curr = self.time_curr + self.system.dt;
self.xlog = [self.xlog, xk];
self.ulog = [self.ulog, uk];
self.u_cost = self.u_cost + uk'*uk;
tt = tt + toc;
self.tt = tt;
return
end
end
function [xopt, uopt,tt] = solveNMPCDCBF1(self, xk)
% Solve NMPC-DCBF
[feas, x, u, J] = self.solve_cftoc1(xk);
if ~feas
xopt = [];
uopt = [];
tt = -1;
return
else
xopt = x(:,2);
uopt = u(:,1);
tt = 0;
end
end
function [feas, xopt, uopt, Jopt] = solve_cftoc1(self, xk)
% Solve CFTOC
% extract variables
N = self.params.N;
% define variables and cost
x = sdpvar(4, N+1);%%%%%%%%%%%%%%%%%%%%%%%%%%%%
u = sdpvar(4, N);
constraints = [];
cost = 0;
% initial constraint
constraints = [constraints; x(:,1) == xk];
% add constraints and costs
AA=self.system.A;
BB=self.system.B;
CC=self.system.C;
constraints = [constraints;
self.system.xl <= x(:,1) <= self.system.xu;
self.system.ul <= u(:,1) <= self.system.uu;
x(:,2) == AA * x(:,1) + [x(4,1)*cos(x(3,1))*self.system.dt;x(4,1)*sin(x(3,1))*self.system.dt;0; 0] + CC * u(:,1)];
cost = cost + (x(:,1)-[3;0.01;0;0])'*self.params.Q*(x(:,1)-[3;0.01;0;0]) + (u(:,1)-[0;0;1;1])'*self.params.R*(u(:,1)-[0;0;1;1]); % self.params.S*(w(:,1)-1)^2;
for i = 2:1:N
constraints = [constraints;
self.system.xl <= x(:,i) <= self.system.xu;
self.system.ul <= u(:,i) <= self.system.uu;
x(:,i+1) == AA * x(:,i) + [x(4,i)*cos(x(3,i))*self.system.dt;x(4,i)*sin(x(3,i))*self.system.dt;0; 0] + CC * u(:,i)];
cost = cost + (x(:,i)-[3;0.01;0;0])'*self.params.Q*(x(:,i)-[3;0.01;0;0]) + (u(:,i)-[0;0;1;1])'*self.params.R*(u(:,i)-[0;0;1;1]);% self.params.S*(w(:,i)-1)^2;
end
% add CBF constraints
for i = 1:N-1
pos = self.obs.pos1;
r = self.obs.r1 ;
b = (x([1:2],i)-pos)'*((x([1:2],i)-pos))-r^2;
b_next = (x([1:2],i+1)-pos)'*((x([1:2],i+1)-pos))-r^2;
b_next_next = (x([1:2],i+2)-pos)'*((x([1:2],i+2)-pos))-r^2;
b1 = (b_next - b)/self.system.dt + self.params.gamma1/self.system.dt * (b);
b1_next = (b_next_next - b_next)/self.system.dt + self.params.gamma1/self.system.dt * (b_next);
constraints = [constraints; b1_next >=u(4,i)*(1-self.params.gamma2)*b1;b_next >= u(3,i)*(1-self.params.gamma1)*b];%Highest order mcbf=2
end
% add terminal cost
cost = cost + (x(:,N+1)-[3;0.01;0;0])'*self.params.P*(x(:,N+1)-[3;0.01;0;0]);
ops = sdpsettings('solver','ipopt','verbose',0);
% solve optimization
diagnostics = optimize(constraints, cost, ops);
if diagnostics.problem == 0
feas = true;
xopt = value(x);
uopt = value(u);
Jopt = value(cost);
else
feas = false;
xopt = [];
uopt = [];
Jopt = [];
end
pos = self.obs.pos1;
r = self.obs.r1 ;
self.distlog = [self.distlog, (r^2-(xk(1:2)-pos)'*(xk(1:2)-pos))];
self.xopenloop{size(self.xopenloop,2)+1} = xopt;
self.uopenloop{size(self.uopenloop,2)+1} = uopt;
self.solvertime = [self.solvertime, diagnostics.solvertime];
fprintf('solver time: %f\n', diagnostics.solvertime);
end
end
end
================================================
FILE: matlab/acc2023/benchmark/gamma1-0p6gamma2-0p6/test_each_horizon/tabledata.m
================================================
nmpcdata=zeros(3,6);%Store data in matrix for nmpc and impc, first row: mean of computing time; second row: standard deviation of computing time; third row: infeasible rate
impcdata=zeros(3,6);
for ii = 1:6
i=ii*4;
filenm1 = ['timecom' num2str(i) '.mat'];
filenm2 = ['feasibility' num2str(i) '.mat'];
load(filenm1);
load(filenm2);
distnmpc = fitdist(nmpcplot1','Normal');
distimpc = fitdist(impcplot1','Normal');
mu1=distnmpc.mu;%Get mean of sample of computing time for NMPC-DCBF
mu2=distimpc.mu;%Get mean of sample of computing time for iMPC-DCBF
sigma1=distnmpc.sigma;%Get variance of sample of computing time for NMPC-DCBF
sigma2=distimpc.sigma;%Get variance of sample of computing time for iMPC-DCBF
nmpcdata(1,ii)=mu1;
impcdata(1,ii)=mu2;
nmpcdata(2,ii)=sigma1;
impcdata(2,ii)=sigma2;
nmpcdata(3,ii)=nmpcinf;
impcdata(3,ii)=impcinf;
end
================================================
FILE: matlab/acc2023/benchmark/gamma1-0p6gamma2-0p6/test_each_horizon/test_N12.m
================================================
close all
clear
%------------------------------------------------------------------------------------------
nb = 1000;%Total number of initial states
nmpcd=zeros(1,nb);%Store computing time for one time-step for NMPC-DCBF
impcd=zeros(1,nb);%Store computing time for one time-step for iMPC-DCBF
nmpcinf=zeros(1,1);%Store infeasible rate for one time-step for NMPC-DCBF
impcinf=zeros(1,1);%Store infeasible rate for one time-step for iMPC-DCBF
i=12;%i is number of horozon
ts = 1;%Total time steps
load(gamma1_0p6gamma2_0p6\InitialStateData');%Load the generated 1000 initial states
[a, b, c, d]=compare(i,ts,nb,InitialMat);%Involves NMPC-DCBF and iMPC-DCBF
nmpcd(1,:)=a;
impcd(1,:)=b;
nmpcinf(1,1)=c;
impcinf(1,1)=d;
nmpcplot1=nmpcd(1,:);
nmpcplot1(nmpcplot1==-1)=[];%Eliminate infeasible one time-step trajectories
impcplot1=impcd(1,:);
impcplot1(impcplot1==-1)=[];%Eliminate infeasible one time-step trajectories
save('timecom12','nmpcplot1','impcplot1');
save('feasibility12','nmpcinf','impcinf');
distnmpc = fitdist(nmpcplot1','Normal');
distimpc = fitdist(impcplot1','Normal');
mu1=distnmpc.mu;%Get mean of sample of computing time for NMPC-DCBF
mu2=distimpc.mu;%Get mean of sample of computing time for iMPC-DCBF
sigma1=distnmpc.sigma;%Get variance of sample of computing time for NMPC-DCBF
sigma2=distimpc.sigma;%Get variance of sample of computing time for iMPC-DCBF
%Initialize atmosphere parameters
function [tnmpc, timpc, ratiotnmpc, ratiotimpc]=compare(N11,ttsim,samplen,InitialMat)
tnmpc=[];
timpc=[];
for i=1:samplen
N1=N11;
tsim=ttsim;
xini1=InitialMat(1,i);
yini1=InitialMat(2,i);
thetaini1=InitialMat(3,i);
vini1=InitialMat(4,i);
x01=[xini1;yini1;thetaini1;vini1];
x02=[xini1 yini1 thetaini1 vini1];
t1 = nmpcdcbf(x01, N1, tsim);
t2 = impcdcbf(x02, N1, tsim);
tindex=[N11 i];
disp(tindex);
tnmpc=[tnmpc t1];%Computing time for NMPC-DCBF
timpc=[timpc t2];%Computing time for iMPC-DCBF
end
nnmpc1 = length(tnmpc);
nimpc1 = length(timpc);
tnmpcs = tnmpc;
timpcs = timpc;
tnmpcs(tnmpcs==-1)=[];
timpcs(timpcs==-1)=[];
nnmpc2 = length(tnmpcs);
nimpc2 = length(timpcs);
ratiotnmpc = (nnmpc1-nnmpc2)/nnmpc1;%Infeasible rate for NMPC-DCBF
ratiotimpc = (nimpc1-nimpc2)/nimpc1;%Infeasible rate for iMPC-DCBF
end
function t1 = nmpcdcbf(x00, N1, ttsim)
%% General Flags
run_nmpc_dcbf_one = true;
run_nmpc_dcbf_multiple = true;
%% Setup and Parameters
x0 = x00;
dt = 0.1;
time_total = ttsim *dt;
P = zeros(4,4);%Weight matrix P
P(1,1) = 10; P(2,2) = 10; P(3,3) = 10;P(4,4) = 10;
Q = zeros(4,4);%Weight matrix Q
Q(1,1) = 10; Q(2,2) = 10; Q(3,3) = 10;Q(4,4) = 10;
R = zeros(4,4);%Weight matrix R
R(1,1) = 1; R(2,2) = 1;R(3,3) = 1000;R(4,4) = 1000;
%Variables range as below
xmin = [-10; -10; -10;-10];
xmax = [10; 10; 10;10];
umin = [-7; -5;-inf;-inf];
umax = [7; 5;inf;inf];
%% Discrete-time unicycle model
system.dt = dt;
system.A = [1 0 0 0;
0 1 0 0;
0 0 1 0;
0 0 0 1];
system.B = [x0(4,1)*cos(x0(3,1))*dt;
x0(4,1)*sin(x0(3,1))*dt;
0;
0];
system.C =[0 0 0 0;
0 0 0 0;
1*dt 0 0 0;
0 1*dt 0 0];
system.xl = xmin;
system.xu = xmax;
system.ul = umin;
system.uu = umax;
%% NMPC-DCBF parameters
params_nmpc_dcbf.Q = Q;
params_nmpc_dcbf.R = R;
params_nmpc_dcbf.P = P;
params_nmpc_dcbf.gamma1 = 0.6;
params_nmpc_dcbf.gamma2 = 0.6;
%% Obstacle
obs.pos1 = [0; 0];
obs.r1 = 1;
%% Simulate NMPC-DCBF with other N
params_nmpc_dcbf.N = N1;
if run_nmpc_dcbf_one
fprintf('Run NMPC-DCBF-12\n');
controller_nmpc_dcbf_multiple12 = NMPCDCBF2(x0, system, params_nmpc_dcbf);%Highest order mcbf=2
controller_nmpc_dcbf_multiple12.obs = obs;
controller_nmpc_dcbf_multiple12.sim(time_total);
end
t1=controller_nmpc_dcbf_multiple12.tt;
end
function t2=impcdcbf(x00, N1, ttsim)
t2=0;
xo = 0;
yo = 0;
r = 1;
N = N1;%Number of horizon
K1 = 1000;%Maximum iteration times, jmax
K = 1000;%Maximum iteration times, jmax
dt = 0.1;
gamma1 = 6;
gamma2 = 6;
nsim = ttsim;
tic;
% Constraints
umin = [-7; -5; -inf; -inf;];
umax = [7; 5; Inf; Inf];
xmin = [-10; -10; -10; -10];
xmax = [ 10; 10; 10; 10];
% Objective function
Q = diag([10 10 10 10]);
QN = Q;
R = 1 * eye(4);
R(3,3) = 1000;
R(4,4) = 1000;
% Initial and reference states
x0 = x00;
xr = [3; 0.01; 0; 0];
ur = [0; 0; 1; 1];
% Dynamic system initialization
BB = [0 0 0 0;0 0 0 0;dt 0 0 0;0 dt 0 0];
% Convex MPC frame
[nx, nu] = size(BB);
% Initialize states set (x00,x01,,,x0N）
x_0 = [];
x0 = x0';
u_0 = zeros(nu, N);
u0 = zeros(nu, 1);% we may need to change this for a better warm-up start
impc = zeros(nx, nsim + 1);
impc(:, 1) = x0;
x0new = x0;
for i=1 : (N+1)
x_0 = [x_0 x0new];
x0new = [dt*x0new(4)*cos(x0new(3))+x0
gitextract_hxxrxdvy/
├── .gitignore
├── LICENSE
├── README.md
├── bibtex/
│ ├── acc2021_nmpc_dcbf.md
│ ├── acc2023_impc_dhocbf.md
│ ├── cdc2022_nmpc_dcbf_feasibility.md
│ ├── icra2022_nmpc_dcbf_polytope.md
│ └── rss2022_nmpc_dcbf_legged_robots.md
└── matlab/
├── acc2021/
│ ├── DCLFDCBF.m
│ ├── MPCCBF.m
│ ├── MPCDC.m
│ ├── README.md
│ ├── figures/
│ │ └── .gitignore
│ ├── test.m
│ ├── testBenchmark.m
│ ├── testGamma.m
│ └── testHorizon.m
├── acc2023/
│ ├── README.md
│ ├── benchmark/
│ │ ├── InitialState.m
│ │ ├── InitialStateData.mat
│ │ ├── gamma1-0p6/
│ │ │ ├── InitialStateData.mat
│ │ │ ├── NMPCDCBF1.m
│ │ │ ├── feasibility12.mat
│ │ │ ├── feasibility16.mat
│ │ │ ├── feasibility20.mat
│ │ │ ├── feasibility24.mat
│ │ │ ├── feasibility4.mat
│ │ │ ├── feasibility8.mat
│ │ │ ├── test_comprehensive.m
│ │ │ ├── test_each_horizon/
│ │ │ │ ├── InitialStateData.mat
│ │ │ │ ├── NMPCDCBF1.m
│ │ │ │ ├── tabledata.m
│ │ │ │ ├── test_N12.m
│ │ │ │ ├── test_N16.m
│ │ │ │ ├── test_N20.m
│ │ │ │ ├── test_N24.m
│ │ │ │ ├── test_N4.m
│ │ │ │ └── test_N8.m
│ │ │ ├── timecom12.mat
│ │ │ ├── timecom16.mat
│ │ │ ├── timecom20.mat
│ │ │ ├── timecom24.mat
│ │ │ ├── timecom4.mat
│ │ │ └── timecom8.mat
│ │ ├── gamma1-0p6gamma2-0p6/
│ │ │ ├── InitialStateData.mat
│ │ │ ├── NMPCDCBF2.m
│ │ │ ├── feasibility12.mat
│ │ │ ├── feasibility16.mat
│ │ │ ├── feasibility20.mat
│ │ │ ├── feasibility24.mat
│ │ │ ├── feasibility4.mat
│ │ │ ├── feasibility8.mat
│ │ │ ├── test_comprehensive.m
│ │ │ ├── test_each_horizon/
│ │ │ │ ├── InitialStateData.mat
│ │ │ │ ├── NMPCDCBF2.m
│ │ │ │ ├── tabledata.m
│ │ │ │ ├── test_N12.m
│ │ │ │ ├── test_N16.m
│ │ │ │ ├── test_N20.m
│ │ │ │ ├── test_N24.m
│ │ │ │ ├── test_N4.m
│ │ │ │ └── test_N8.m
│ │ │ ├── timecom12.mat
│ │ │ ├── timecom16.mat
│ │ │ ├── timecom20.mat
│ │ │ ├── timecom24.mat
│ │ │ ├── timecom4.mat
│ │ │ └── timecom8.mat
│ │ ├── gamma1_0p4/
│ │ │ ├── InitialStateData.mat
│ │ │ ├── NMPCDCBF1.m
│ │ │ ├── feasibility12.mat
│ │ │ ├── feasibility16.mat
│ │ │ ├── feasibility20.mat
│ │ │ ├── feasibility24.mat
│ │ │ ├── feasibility4.mat
│ │ │ ├── feasibility8.mat
│ │ │ ├── test_comprehensive.m
│ │ │ ├── test_each_horizon/
│ │ │ │ ├── InitialStateData.mat
│ │ │ │ ├── NMPCDCBF1.m
│ │ │ │ ├── tabledata.m
│ │ │ │ ├── test_N12.m
│ │ │ │ ├── test_N16.m
│ │ │ │ ├── test_N20.m
│ │ │ │ ├── test_N24.m
│ │ │ │ ├── test_N4.m
│ │ │ │ └── test_N8.m
│ │ │ ├── timecom12.mat
│ │ │ ├── timecom16.mat
│ │ │ ├── timecom20.mat
│ │ │ ├── timecom24.mat
│ │ │ ├── timecom4.mat
│ │ │ └── timecom8.mat
│ │ └── gamma1_0p4gamma2_0p4/
│ │ ├── InitialStateData.mat
│ │ ├── NMPCDCBF2.m
│ │ ├── feasibility12.mat
│ │ ├── feasibility16.mat
│ │ ├── feasibility20.mat
│ │ ├── feasibility24.mat
│ │ ├── feasibility4.mat
│ │ ├── feasibility8.mat
│ │ ├── test_comprehensive.m
│ │ ├── test_each_horizon/
│ │ │ ├── InitialStateData.mat
│ │ │ ├── NMPCDCBF2.m
│ │ │ ├── tabledata.m
│ │ │ ├── test_N12.m
│ │ │ ├── test_N16.m
│ │ │ ├── test_N20.m
│ │ │ ├── test_N24.m
│ │ │ ├── test_N4.m
│ │ │ └── test_N8.m
│ │ ├── timecom12.mat
│ │ ├── timecom16.mat
│ │ ├── timecom20.mat
│ │ ├── timecom24.mat
│ │ ├── timecom4.mat
│ │ └── timecom8.mat
│ └── closedloop_performance/
│ ├── Closedloop_Trajectories_Hyperparameters.m
│ ├── FigureGenerate.m
│ ├── Iterative_Convergence.m
│ ├── Maximum_Iterations.m
│ ├── NMPCDCBF1.m
│ ├── NMPCDCBF2.m
│ ├── figures/
│ │ ├── J-converge-1.eps
│ │ ├── J-converge-2.eps
│ │ ├── J-converge-3.eps
│ │ ├── J-converge-4.eps
│ │ ├── closedloop-snapshots1.eps
│ │ ├── closedloop-snapshots2.eps
│ │ ├── closedloop-snapshots3.eps
│ │ ├── openloop-snapshots.eps
│ │ ├── state-converge-1.eps
│ │ ├── state-converge-2.eps
│ │ ├── state-converge-3.eps
│ │ └── state-converge-4.eps
│ ├── impc_N16_gamma4_4.mat
│ ├── impc_N16_gamma6_6.mat
│ ├── impc_N24_Gamma4.mat
│ ├── impc_N24_Gamma6.mat
│ ├── impc_N24_gamma4_4.mat
│ ├── impc_N24_gamma6_6.mat
│ ├── iteration_theta.mat
│ ├── iteration_v.mat
│ ├── iteration_x.mat
│ ├── iteration_y.mat
│ ├── jtconv_Gamma4_4.mat
│ ├── jtconv_Gamma4_6.mat
│ ├── jtconv_Gamma6_4.mat
│ ├── jtconv_Gamma6_6.mat
│ ├── nmpc_N16_gamma4_4.mat
│ ├── nmpc_N16_gamma6_6.mat
│ ├── nmpc_N24_gamma4.mat
│ ├── nmpc_N24_gamma4_4.mat
│ ├── nmpc_N24_gamma6.mat
│ ├── nmpc_N24_gamma6_6.mat
│ └── trajectory.mat
└── cdc2021/
├── .gitignore
├── CBFDT.m
├── ParamDCLFDCBF.m
├── ParamMPCDCBF.m
├── ParamMPCGCBF.m
├── ParamNMPCDCBF.m
├── ParamNMPCDCLFDCBF.m
├── README.md
├── data/
│ ├── feasibility-dclfdcbf1.mat
│ ├── feasibility-dclfdcbf2.mat
│ ├── feasibility-dclfdcbf3.mat
│ ├── feasibility-dclfdcbf4.mat
│ ├── feasibility-mpcdcbf1.mat
│ ├── feasibility-mpcdcbf2.mat
│ ├── feasibility-mpcdcbf3.mat
│ ├── feasibility-mpcdcbf4.mat
│ ├── feasibility-mpcgcbf1.mat
│ ├── feasibility-mpcgcbf2.mat
│ ├── feasibility-mpcgcbf3.mat
│ ├── feasibility-mpcgcbf4.mat
│ ├── safety-mpcdcbf.mat
│ ├── safety-mpcgcbf.mat
│ └── safety-nmpccbf.mat
├── test.m
├── testComplexity.m
├── testFeasibilityDCLFDCBF.m
├── testFeasibilityMPCDCBF.m
├── testFeasibilityMPCGCBF.m
└── testSafety.m
Condensed preview — 184 files, each showing path, character count, and a content snippet. Download the .json file or copy for the full structured content (1,449K chars).
[
{
"path": ".gitignore",
"chars": 476,
"preview": "## MATLAB files to ignore ##\n\n# Windows default autosave extension\n*.asv\n\n# OSX / *nix default autosave extension\n*.m~\n\n"
},
{
"path": "LICENSE",
"chars": 1065,
"preview": "MIT License\n\nCopyright (c) 2021 Jun Zeng\n\nPermission is hereby granted, free of charge, to any person obtaining a copy\no"
},
{
"path": "README.md",
"chars": 2936,
"preview": "**Status**: The implementation code for corresponding papers will be merged here and new papers will be added in an inve"
},
{
"path": "bibtex/acc2021_nmpc_dcbf.md",
"chars": 311,
"preview": "```\n@inproceedings{zeng2021mpccbf,\n title={Safety-critical model predictive control with discrete-time control barrier "
},
{
"path": "bibtex/acc2023_impc_dhocbf.md",
"chars": 313,
"preview": "```\n@inproceedings{liu2023iterative,\n title={Iterative Convex Optimization for Model Predictive Control with Discrete-T"
},
{
"path": "bibtex/cdc2022_nmpc_dcbf_feasibility.md",
"chars": 362,
"preview": "```\n@inproceedings{zeng2021mpccbf-feasibility,\n title={Enhancing feasibility and safety of nonlinear model predictive c"
},
{
"path": "bibtex/icra2022_nmpc_dcbf_polytope.md",
"chars": 384,
"preview": "```\n@inproceedings{thirugnanam2021safetycritical,\n author={Thirugnanam, Akshay and Zeng, Jun and Sreenath, Koushil},\n "
},
{
"path": "bibtex/rss2022_nmpc_dcbf_legged_robots.md",
"chars": 424,
"preview": "```\n@inproceedings{li2022bridging, \n author = {Li, Zhongyu and Zeng, Jun and Thirugnanam, Akshay and Sreenath, Koushi"
},
{
"path": "matlab/acc2021/DCLFDCBF.m",
"chars": 3913,
"preview": "classdef DCLFDCBF < handle\n properties\n system\n params\n x0\n x_curr\n time_curr = 0."
},
{
"path": "matlab/acc2021/MPCCBF.m",
"chars": 5395,
"preview": "classdef MPCCBF < handle\n % MPC with distance constraints\n properties\n system\n params\n x0\n "
},
{
"path": "matlab/acc2021/MPCDC.m",
"chars": 5282,
"preview": "classdef MPCDC < handle\n % MPC with distance constraints\n properties\n system\n params\n x0\n "
},
{
"path": "matlab/acc2021/README.md",
"chars": 1153,
"preview": "## MPC-CBF-ACC2021\nWe propose a control framework which unifies the model predictive control and control barrier functio"
},
{
"path": "matlab/acc2021/figures/.gitignore",
"chars": 6,
"preview": "*.eps\n"
},
{
"path": "matlab/acc2021/test.m",
"chars": 7692,
"preview": "close all\nclear\n\n%% General Flags\nrun_dclf_dcbf = true;\ndisplay_dclf_dcbf = true;\nrun_mpc_cbf_one = true;\ndisplay_mpc_cb"
},
{
"path": "matlab/acc2021/testBenchmark.m",
"chars": 1497,
"preview": "close all\nclear\n\n%% Setup and Parameters\nx0 = [-5; -5; 0; 0];\ntime_total = 20.0;\ndt = 0.2;\nP = 100*eye(4);\nQ = 10*eye(4)"
},
{
"path": "matlab/acc2021/testGamma.m",
"chars": 2616,
"preview": "close all\nclear\n\n%% Setup and Parameters\nx0 = [-5; -5; 0; 0];\ntime_total = 20.0;\ndt = 0.2;\nP = 100*eye(4);\nQ = 10*eye(4)"
},
{
"path": "matlab/acc2021/testHorizon.m",
"chars": 8391,
"preview": "close all\nclear\n\n%% Setup and Parameters\nx0 = [-5; -5; 0; 0];\ntime_total = 20.0;\ndt = 0.2;\nP = 100*eye(4);\nQ = 10*eye(4)"
},
{
"path": "matlab/acc2023/README.md",
"chars": 5397,
"preview": "# Iterative-MPC-DHOCBF\nMatlab code for the paper *\"Iterative Convex Optimization for Model Predictive Control with Discr"
},
{
"path": "matlab/acc2023/benchmark/InitialState.m",
"chars": 543,
"preview": "samplen=1000;%Total number of initial states\r\nInitialMat=zeros(4,samplen);%Store initialized x, y, theta, v into each ro"
},
{
"path": "matlab/acc2023/benchmark/gamma1-0p6/NMPCDCBF1.m",
"chars": 5035,
"preview": "classdef NMPCDCBF1 < handle\r\n % MPC with distance constraints\r\n properties\r\n system\r\n params\r\n "
},
{
"path": "matlab/acc2023/benchmark/gamma1-0p6/test_comprehensive.m",
"chars": 15234,
"preview": "close all\r\nclear\r\n\r\n%------------------------------------------------------------------------------------------\r\nnmpcdat"
},
{
"path": "matlab/acc2023/benchmark/gamma1-0p6/test_each_horizon/NMPCDCBF1.m",
"chars": 5035,
"preview": "classdef NMPCDCBF1 < handle\r\n % MPC with distance constraints\r\n properties\r\n system\r\n params\r\n "
},
{
"path": "matlab/acc2023/benchmark/gamma1-0p6/test_each_horizon/tabledata.m",
"chars": 862,
"preview": "nmpcdata=zeros(3,6);%Store data in matrix for nmpc and impc, first row: mean of computing time; second row: standard dev"
},
{
"path": "matlab/acc2023/benchmark/gamma1-0p6/test_each_horizon/test_N12.m",
"chars": 14812,
"preview": "close all\r\nclear\r\n\r\n%------------------------------------------------------------------------------------------\r\nnb = 10"
},
{
"path": "matlab/acc2023/benchmark/gamma1-0p6/test_each_horizon/test_N16.m",
"chars": 14812,
"preview": "close all\r\nclear\r\n\r\n%------------------------------------------------------------------------------------------\r\nnb = 10"
},
{
"path": "matlab/acc2023/benchmark/gamma1-0p6/test_each_horizon/test_N20.m",
"chars": 14810,
"preview": "close all\r\nclear\r\n\r\n%------------------------------------------------------------------------------------------\r\nnb = 10"
},
{
"path": "matlab/acc2023/benchmark/gamma1-0p6/test_each_horizon/test_N24.m",
"chars": 14812,
"preview": "close all\r\nclear\r\n\r\n%------------------------------------------------------------------------------------------\r\nnb = 10"
},
{
"path": "matlab/acc2023/benchmark/gamma1-0p6/test_each_horizon/test_N4.m",
"chars": 14804,
"preview": "close all\r\nclear\r\n\r\n%------------------------------------------------------------------------------------------\r\nnb = 10"
},
{
"path": "matlab/acc2023/benchmark/gamma1-0p6/test_each_horizon/test_N8.m",
"chars": 14804,
"preview": "close all\r\nclear\r\n\r\n%------------------------------------------------------------------------------------------\r\nnb = 10"
},
{
"path": "matlab/acc2023/benchmark/gamma1-0p6gamma2-0p6/NMPCDCBF2.m",
"chars": 5405,
"preview": "classdef NMPCDCBF2 < handle\r\n % MPC with distance constraints\r\n properties\r\n system\r\n params\r\n "
},
{
"path": "matlab/acc2023/benchmark/gamma1-0p6gamma2-0p6/test_comprehensive.m",
"chars": 15246,
"preview": "close all\r\nclear\r\n\r\n%------------------------------------------------------------------------------------------\r\nnmpcdat"
},
{
"path": "matlab/acc2023/benchmark/gamma1-0p6gamma2-0p6/test_each_horizon/NMPCDCBF2.m",
"chars": 5405,
"preview": "classdef NMPCDCBF2 < handle\r\n % MPC with distance constraints\r\n properties\r\n system\r\n params\r\n "
},
{
"path": "matlab/acc2023/benchmark/gamma1-0p6gamma2-0p6/test_each_horizon/tabledata.m",
"chars": 862,
"preview": "nmpcdata=zeros(3,6);%Store data in matrix for nmpc and impc, first row: mean of computing time; second row: standard dev"
},
{
"path": "matlab/acc2023/benchmark/gamma1-0p6gamma2-0p6/test_each_horizon/test_N12.m",
"chars": 14820,
"preview": "close all\r\nclear\r\n\r\n%------------------------------------------------------------------------------------------\r\nnb = 10"
},
{
"path": "matlab/acc2023/benchmark/gamma1-0p6gamma2-0p6/test_each_horizon/test_N16.m",
"chars": 14822,
"preview": "close all\r\nclear\r\n\r\n%------------------------------------------------------------------------------------------\r\nnb = 10"
},
{
"path": "matlab/acc2023/benchmark/gamma1-0p6gamma2-0p6/test_each_horizon/test_N20.m",
"chars": 14822,
"preview": "close all\r\nclear\r\n\r\n%------------------------------------------------------------------------------------------\r\nnb = 10"
},
{
"path": "matlab/acc2023/benchmark/gamma1-0p6gamma2-0p6/test_each_horizon/test_N24.m",
"chars": 14822,
"preview": "close all\r\nclear\r\n\r\n%------------------------------------------------------------------------------------------\r\nnb = 10"
},
{
"path": "matlab/acc2023/benchmark/gamma1-0p6gamma2-0p6/test_each_horizon/test_N4.m",
"chars": 14814,
"preview": "close all\r\nclear\r\n\r\n%------------------------------------------------------------------------------------------\r\nnb = 10"
},
{
"path": "matlab/acc2023/benchmark/gamma1-0p6gamma2-0p6/test_each_horizon/test_N8.m",
"chars": 14814,
"preview": "close all\r\nclear\r\n\r\n%------------------------------------------------------------------------------------------\r\nnb = 10"
},
{
"path": "matlab/acc2023/benchmark/gamma1_0p4/NMPCDCBF1.m",
"chars": 5035,
"preview": "classdef NMPCDCBF1 < handle\r\n % MPC with distance constraints\r\n properties\r\n system\r\n params\r\n "
},
{
"path": "matlab/acc2023/benchmark/gamma1_0p4/test_comprehensive.m",
"chars": 15234,
"preview": "close all\r\nclear\r\n\r\n%------------------------------------------------------------------------------------------\r\nnmpcdat"
},
{
"path": "matlab/acc2023/benchmark/gamma1_0p4/test_each_horizon/NMPCDCBF1.m",
"chars": 5035,
"preview": "classdef NMPCDCBF1 < handle\r\n % MPC with distance constraints\r\n properties\r\n system\r\n params\r\n "
},
{
"path": "matlab/acc2023/benchmark/gamma1_0p4/test_each_horizon/tabledata.m",
"chars": 863,
"preview": "nmpcdata=zeros(3,6);%Store data in matrix for nmpc and impc, first row: mean of computing time; second row: standard dev"
},
{
"path": "matlab/acc2023/benchmark/gamma1_0p4/test_each_horizon/test_N12.m",
"chars": 14810,
"preview": "close all\r\nclear\r\n\r\n%------------------------------------------------------------------------------------------\r\nnb = 10"
},
{
"path": "matlab/acc2023/benchmark/gamma1_0p4/test_each_horizon/test_N16.m",
"chars": 14810,
"preview": "close all\r\nclear\r\n\r\n%------------------------------------------------------------------------------------------\r\nnb = 10"
},
{
"path": "matlab/acc2023/benchmark/gamma1_0p4/test_each_horizon/test_N20.m",
"chars": 14810,
"preview": "close all\r\nclear\r\n\r\n%------------------------------------------------------------------------------------------\r\nnb = 10"
},
{
"path": "matlab/acc2023/benchmark/gamma1_0p4/test_each_horizon/test_N24.m",
"chars": 14810,
"preview": "close all\r\nclear\r\n\r\n%------------------------------------------------------------------------------------------\r\nnb = 10"
},
{
"path": "matlab/acc2023/benchmark/gamma1_0p4/test_each_horizon/test_N4.m",
"chars": 14802,
"preview": "close all\r\nclear\r\n\r\n%------------------------------------------------------------------------------------------\r\nnb = 10"
},
{
"path": "matlab/acc2023/benchmark/gamma1_0p4/test_each_horizon/test_N8.m",
"chars": 14802,
"preview": "close all\r\nclear\r\n\r\n%------------------------------------------------------------------------------------------\r\nnb = 10"
},
{
"path": "matlab/acc2023/benchmark/gamma1_0p4gamma2_0p4/NMPCDCBF2.m",
"chars": 5405,
"preview": "classdef NMPCDCBF2 < handle\r\n % MPC with distance constraints\r\n properties\r\n system\r\n params\r\n "
},
{
"path": "matlab/acc2023/benchmark/gamma1_0p4gamma2_0p4/test_comprehensive.m",
"chars": 15244,
"preview": "close all\r\nclear\r\n\r\n%------------------------------------------------------------------------------------------\r\nnmpcdat"
},
{
"path": "matlab/acc2023/benchmark/gamma1_0p4gamma2_0p4/test_each_horizon/NMPCDCBF2.m",
"chars": 5405,
"preview": "classdef NMPCDCBF2 < handle\r\n % MPC with distance constraints\r\n properties\r\n system\r\n params\r\n "
},
{
"path": "matlab/acc2023/benchmark/gamma1_0p4gamma2_0p4/test_each_horizon/tabledata.m",
"chars": 862,
"preview": "nmpcdata=zeros(3,6);%Store data in matrix for nmpc and impc, first row: mean of computing time; second row: standard dev"
},
{
"path": "matlab/acc2023/benchmark/gamma1_0p4gamma2_0p4/test_each_horizon/test_N12.m",
"chars": 14820,
"preview": "close all\r\nclear\r\n\r\n%------------------------------------------------------------------------------------------\r\nnb = 10"
},
{
"path": "matlab/acc2023/benchmark/gamma1_0p4gamma2_0p4/test_each_horizon/test_N16.m",
"chars": 14820,
"preview": "close all\r\nclear\r\n\r\n%------------------------------------------------------------------------------------------\r\nnb = 10"
},
{
"path": "matlab/acc2023/benchmark/gamma1_0p4gamma2_0p4/test_each_horizon/test_N20.m",
"chars": 14820,
"preview": "close all\r\nclear\r\n\r\n%------------------------------------------------------------------------------------------\r\nnb = 10"
},
{
"path": "matlab/acc2023/benchmark/gamma1_0p4gamma2_0p4/test_each_horizon/test_N24.m",
"chars": 14820,
"preview": "close all\r\nclear\r\n\r\n%------------------------------------------------------------------------------------------\r\nnb = 10"
},
{
"path": "matlab/acc2023/benchmark/gamma1_0p4gamma2_0p4/test_each_horizon/test_N4.m",
"chars": 14810,
"preview": "close all\r\nclear\r\n\r\n%------------------------------------------------------------------------------------------\r\nnb = 10"
},
{
"path": "matlab/acc2023/benchmark/gamma1_0p4gamma2_0p4/test_each_horizon/test_N8.m",
"chars": 14812,
"preview": "close all\r\nclear\r\n\r\n%------------------------------------------------------------------------------------------\r\nnb = 10"
},
{
"path": "matlab/acc2023/closedloop_performance/Closedloop_Trajectories_Hyperparameters.m",
"chars": 23062,
"preview": "close all\r\nclear\r\n\r\n%------------------------------------------NMPC-DCBF---------------------------------------------\r\n\r"
},
{
"path": "matlab/acc2023/closedloop_performance/FigureGenerate.m",
"chars": 14301,
"preview": "close all\r\nclear\r\n\r\n%--------------------------------------------------------------------------------------\r\nnsim=100;%T"
},
{
"path": "matlab/acc2023/closedloop_performance/Iterative_Convergence.m",
"chars": 11575,
"preview": "close all\r\nclear\r\n\r\n%--------------------------------------iMPC-DCBF------------------------------------------------\r\nxo"
},
{
"path": "matlab/acc2023/closedloop_performance/Maximum_Iterations.m",
"chars": 11746,
"preview": "close all\r\nclear\r\n\r\n%--------------------------------------iMPC-DCBF------------------------------------------------\r\nkk"
},
{
"path": "matlab/acc2023/closedloop_performance/NMPCDCBF1.m",
"chars": 5035,
"preview": "classdef NMPCDCBF1 < handle\r\n % MPC with distance constraints\r\n properties\r\n system\r\n params\r\n "
},
{
"path": "matlab/acc2023/closedloop_performance/NMPCDCBF2.m",
"chars": 5405,
"preview": "classdef NMPCDCBF2 < handle\r\n % MPC with distance constraints\r\n properties\r\n system\r\n params\r\n "
},
{
"path": "matlab/acc2023/closedloop_performance/figures/J-converge-1.eps",
"chars": 37423,
"preview": "%!PS-Adobe-3.0 EPSF-3.0\r\n%%Creator: (MATLAB, The Mathworks, Inc. Version 9.10.0.1649659 \\(R2021a\\) Update 1. Operating S"
},
{
"path": "matlab/acc2023/closedloop_performance/figures/J-converge-2.eps",
"chars": 37345,
"preview": "%!PS-Adobe-3.0 EPSF-3.0\r\n%%Creator: (MATLAB, The Mathworks, Inc. Version 9.10.0.1649659 \\(R2021a\\) Update 1. Operating S"
},
{
"path": "matlab/acc2023/closedloop_performance/figures/J-converge-3.eps",
"chars": 37350,
"preview": "%!PS-Adobe-3.0 EPSF-3.0\r\n%%Creator: (MATLAB, The Mathworks, Inc. Version 9.10.0.1649659 \\(R2021a\\) Update 1. Operating S"
},
{
"path": "matlab/acc2023/closedloop_performance/figures/J-converge-4.eps",
"chars": 37350,
"preview": "%!PS-Adobe-3.0 EPSF-3.0\r\n%%Creator: (MATLAB, The Mathworks, Inc. Version 9.10.0.1649659 \\(R2021a\\) Update 1. Operating S"
},
{
"path": "matlab/acc2023/closedloop_performance/figures/closedloop-snapshots1.eps",
"chars": 70390,
"preview": "%!PS-Adobe-3.0 EPSF-3.0\r\n%%Creator: (MATLAB, The Mathworks, Inc. Version 9.10.0.1649659 \\(R2021a\\) Update 1. Operating S"
},
{
"path": "matlab/acc2023/closedloop_performance/figures/closedloop-snapshots2.eps",
"chars": 67317,
"preview": "%!PS-Adobe-3.0 EPSF-3.0\r\n%%Creator: (MATLAB, The Mathworks, Inc. Version 9.10.0.1649659 \\(R2021a\\) Update 1. Operating S"
},
{
"path": "matlab/acc2023/closedloop_performance/figures/closedloop-snapshots3.eps",
"chars": 54758,
"preview": "%!PS-Adobe-3.0 EPSF-3.0\r\n%%Creator: (MATLAB, The Mathworks, Inc. Version 9.10.0.1649659 \\(R2021a\\) Update 1. Operating S"
},
{
"path": "matlab/acc2023/closedloop_performance/figures/openloop-snapshots.eps",
"chars": 97602,
"preview": "%!PS-Adobe-3.0 EPSF-3.0\r\n%%Creator: (MATLAB, The Mathworks, Inc. Version 9.10.0.1649659 \\(R2021a\\) Update 1. Operating S"
},
{
"path": "matlab/acc2023/closedloop_performance/figures/state-converge-1.eps",
"chars": 72313,
"preview": "%!PS-Adobe-3.0 EPSF-3.0\r\n%%Creator: (MATLAB, The Mathworks, Inc. Version 9.10.0.1649659 \\(R2021a\\) Update 1. Operating S"
},
{
"path": "matlab/acc2023/closedloop_performance/figures/state-converge-2.eps",
"chars": 71625,
"preview": "%!PS-Adobe-3.0 EPSF-3.0\r\n%%Creator: (MATLAB, The Mathworks, Inc. Version 9.10.0.1649659 \\(R2021a\\) Update 1. Operating S"
},
{
"path": "matlab/acc2023/closedloop_performance/figures/state-converge-3.eps",
"chars": 71966,
"preview": "%!PS-Adobe-3.0 EPSF-3.0\r\n%%Creator: (MATLAB, The Mathworks, Inc. Version 9.10.0.1649659 \\(R2021a\\) Update 1. Operating S"
},
{
"path": "matlab/acc2023/closedloop_performance/figures/state-converge-4.eps",
"chars": 73336,
"preview": "%!PS-Adobe-3.0 EPSF-3.0\r\n%%Creator: (MATLAB, The Mathworks, Inc. Version 9.10.0.1649659 \\(R2021a\\) Update 1. Operating S"
},
{
"path": "matlab/cdc2021/.gitignore",
"chars": 6,
"preview": "*.eps\n"
},
{
"path": "matlab/cdc2021/CBFDT.m",
"chars": 11712,
"preview": "classdef CBFDT < handle\n properties\n systemParam\n optType\n optParam\n xcurr\n timecu"
},
{
"path": "matlab/cdc2021/ParamDCLFDCBF.m",
"chars": 428,
"preview": "classdef ParamDCLFDCBF < handle\n properties\n alpha\n gamma\n P % weight for Lyapunov function\n "
},
{
"path": "matlab/cdc2021/ParamMPCDCBF.m",
"chars": 436,
"preview": "classdef ParamMPCDCBF < handle\n properties\n horizon\n gamma\n P % weight for terminal cost (CLF)\n "
},
{
"path": "matlab/cdc2021/ParamMPCGCBF.m",
"chars": 436,
"preview": "classdef ParamMPCGCBF < handle\n properties\n horizon\n gamma\n P % weight for terminal cost (CLF)\n "
},
{
"path": "matlab/cdc2021/ParamNMPCDCBF.m",
"chars": 592,
"preview": "classdef ParamNMPCDCBF < handle\n properties\n horizon\n horizonCBF\n gamma\n P % weight for t"
},
{
"path": "matlab/cdc2021/ParamNMPCDCLFDCBF.m",
"chars": 789,
"preview": "classdef ParamNMPCDCLFDCBF < handle\n properties\n horizon\n horizonCLF\n horizonCBF\n alpha\n "
},
{
"path": "matlab/cdc2021/README.md",
"chars": 1464,
"preview": "## NMPC-DCLF-DCBF-CDC2021\nIn this paper, we discuss the feasibility, safety and complexity about existing optimal contro"
},
{
"path": "matlab/cdc2021/test.m",
"chars": 1118,
"preview": "close all\nclear\n\n% system setup and initial condition\ntimestep = 0.1;\nsystem_param.A = [[1 ,timestep, 0]; [0, 1, timeste"
},
{
"path": "matlab/cdc2021/testComplexity.m",
"chars": 4478,
"preview": "clc\nclose all\nclear\n% This file tests computational time for various approaches\n\n%% System setup\n\ntimestep = 0.1;\nsystem"
},
{
"path": "matlab/cdc2021/testFeasibilityDCLFDCBF.m",
"chars": 4239,
"preview": "clc\nclose all\nclear\n% This file tests feasibility between DCLF-DCBF and NMPC-DCLF-DCBF.\n\n%% System setup\n\ntimestep = 0.1"
},
{
"path": "matlab/cdc2021/testFeasibilityMPCDCBF.m",
"chars": 3696,
"preview": "clc\nclose all\nclear\n% This file tests feasibility between MPC-DCBF and NMPC-DCBF.\n\n%% System setup\n\ntimestep = 0.1;\nsyst"
},
{
"path": "matlab/cdc2021/testFeasibilityMPCGCBF.m",
"chars": 3722,
"preview": "clc\nclose all\nclear\n% This file tests feasibility between MPC-GCBF and NMPC-DCBF.\n\n%% System setup\n\ntimestep = 0.1;\nsyst"
},
{
"path": "matlab/cdc2021/testSafety.m",
"chars": 4170,
"preview": "clc\nclose all\nclear\n% This file tests safety performance for various approaches\n\n%% Setup and simulations\ntimestep = 0.1"
}
]
// ... and 93 more files (download for full content)
About this extraction
This page contains the full source code of the HybridRobotics/NMPC-DCLF-DCBF GitHub repository, extracted and formatted as plain text for AI agents and large language models (LLMs). The extraction includes 184 files (1.3 MB), approximately 621.8k tokens. Use this with OpenClaw, Claude, ChatGPT, Cursor, Windsurf, or any other AI tool that accepts text input. You can copy the full output to your clipboard or download it as a .txt file.
Extracted by GitExtract — free GitHub repo to text converter for AI. Built by Nikandr Surkov.