Skip to content

Commit c97345b

Browse files
MC00614MC00614
committed
Multidimensional Input
1 parent 82c1bef commit c97345b

File tree

4 files changed

+76
-47
lines changed

4 files changed

+76
-47
lines changed

‎model/wmrobot_map.h

Lines changed: 3 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -35,10 +35,11 @@ WMRobotMap::WMRobotMap() {
3535
};
3636

3737
h = [&](Eigen::Ref<Eigen::MatrixXd> U) -> void {
38-
U.row(0) = U.row(0).cwiseMax(velocity).cwiseMin(velocity);
38+
// U.row(0) = U.row(0).cwiseMax(velocity).cwiseMin(velocity);
39+
U.row(0) = U.row(0).cwiseMax(0.0).cwiseMin(1.0);
3940
// U.row(0) = U.row(0).cwiseMax(0.0).cwiseMin(1.0);
4041
// U.row(1) = U.row(1).cwiseMax(-0.5).cwiseMin(0.5);
41-
U.row(1) = U.row(1).cwiseMax(-M_PI_4).cwiseMin(M_PI_4);
42+
U.row(1) = U.row(1).cwiseMax(-M_PI_2).cwiseMin(M_PI_2);
4243
return;
4344
};
4445
}

‎mppi/bi_mppi.h

Lines changed: 46 additions & 32 deletions
Original file line numberDiff line numberDiff line change
@@ -34,7 +34,8 @@ class BiMPPI {
3434
void guideMPPI();
3535
void partitioningControl();
3636

37-
void dbscan(std::vector<std::vector<int>> &clusters, const Eigen::VectorXd &Di, const Eigen::VectorXd &costs, const int &N, const int &T);
37+
void dbscan(std::vector<std::vector<int>> &clusters, const Eigen::MatrixXd &Di, const Eigen::VectorXd &costs, const int &N, const int &T);
38+
// void dbscan(std::vector<std::vector<int>> &clusters, const Eigen::VectorXd &Di, const Eigen::VectorXd &costs, const int &N, const int &T);
3839
void calculateU(Eigen::MatrixXd &U, const std::vector<std::vector<int>> &clusters, const Eigen::VectorXd &costs, const Eigen::MatrixXd &Ui, const int &T);
3940
void show();
4041
void showTraj();
@@ -187,16 +188,19 @@ void BiMPPI::solve() {
187188
// elapsed_1 = finish - start;
188189
// std::cout<<"Fir = "<<elapsed_1.count()<<std::endl;
189190

191+
std::cout<<"a"<<std::endl;
190192
start = std::chrono::high_resolution_clock::now();
191193
backwardRollout();
192194
forwardRollout();
193195
finish = std::chrono::high_resolution_clock::now();
194196
elapsed_1 = finish - start;
195197
// std::cout<<"Sec = "<<elapsed_1.count()<<std::endl;
196198

199+
std::cout<<"b"<<std::endl;
197200
// 2. Select Connection
198201
start = std::chrono::high_resolution_clock::now();
199202
selectConnection();
203+
std::cout<<"c"<<std::endl;
200204
concatenate();
201205
finish = std::chrono::high_resolution_clock::now();
202206
elapsed_2 = finish - start;
@@ -206,6 +210,7 @@ void BiMPPI::solve() {
206210
guideMPPI();
207211
finish = std::chrono::high_resolution_clock::now();
208212
elapsed_3 = finish - start;
213+
std::cout<<"d"<<std::endl;
209214

210215
// 4. Partitioning Control
211216
start = std::chrono::high_resolution_clock::now();
@@ -220,7 +225,8 @@ void BiMPPI::solve() {
220225

221226
void BiMPPI::backwardRollout() {
222227
Eigen::MatrixXd Ui = U_b0.replicate(Nb, 1);
223-
Eigen::VectorXd Di(Nb);
228+
Eigen::MatrixXd Di(dim_u, Nb);
229+
// Eigen::VectorXd Di(Nf);
224230
Eigen::VectorXd costs(Nb);
225231
bool all_feasible = true;
226232
#pragma omp parallel for
@@ -242,15 +248,15 @@ void BiMPPI::backwardRollout() {
242248
cost += p(Xi.col(j), x_init);
243249
}
244250
cost += p(Xi.col(0), x_init);
245-
for (int j = 0; j <= Tb; ++j) {
251+
for (int j = Tb; j >= 0; --j) {
246252
if (collision_checker->getCollisionGrid(Xi.col(j))) {
247-
cost = 1e8;
248253
all_feasible = false;
254+
cost = 1e8;
249255
break;
250256
}
251257
}
252258
costs(i) = cost;
253-
Di(i) = Xi.row(dim_x - 1).mean();
259+
Di.col(i) = (Ui.middleRows(i * dim_u, dim_u) - U_b0).rowwise().mean();
254260
}
255261

256262
if (!all_feasible) {dbscan(clusters_b, Di, costs, Nb, Tb);}
@@ -275,7 +281,8 @@ void BiMPPI::backwardRollout() {
275281

276282
void BiMPPI::forwardRollout() {
277283
Eigen::MatrixXd Ui = U_f0.replicate(Nf, 1);
278-
Eigen::VectorXd Di(Nf);
284+
Eigen::MatrixXd Di(dim_u, Nf);
285+
// Eigen::VectorXd Di(Nf);
279286
Eigen::VectorXd costs(Nf);
280287
bool all_feasible = true;
281288
#pragma omp parallel for
@@ -296,11 +303,12 @@ void BiMPPI::forwardRollout() {
296303
if (collision_checker->getCollisionGrid(Xi.col(j))) {
297304
all_feasible = false;
298305
cost = 1e8;
306+
// std::cout<<"F = "<<cost<<std::endl;
299307
break;
300308
}
301309
}
302310
costs(i) = cost;
303-
Di(i) = Xi.row(dim_x - 1).mean();
311+
Di.col(i) = (Ui.middleRows(i * dim_u, dim_u) - U_f0).rowwise().mean();
304312
}
305313

306314
if (!all_feasible) {dbscan(clusters_f, Di, costs, Nf, Tf);}
@@ -324,7 +332,7 @@ void BiMPPI::selectConnection() {
324332
double min_norm = std::numeric_limits<double>::max();
325333
int cb, df, db;
326334
for (int cb_ = 0; cb_ < clusters_b.size(); ++cb_) {
327-
for (int df_ = 1; df_ < Tf; ++df_) {
335+
for (int df_ = 1; df_ < Tf-1; ++df_) {
328336
for (int db_ = 0; db_ < Tb-1; ++db_) {
329337
double norm = (Xf.block(cf * dim_x, df_, dim_x, 1) - Xb.block(cb_ * dim_x, db_, dim_x, 1)).norm();
330338
if (norm < min_norm) {
@@ -336,6 +344,7 @@ void BiMPPI::selectConnection() {
336344
}
337345
}
338346
}
347+
// std::cout << "F: " << clusters_b.size() << ", B: " << clusters_b.size() << ", cf: " << cf << ", cb: " << cb << ", df: " << df << ", db: " << db << std::endl;
339348
joints.push_back({cf, cb, df, db});
340349
}
341350
}
@@ -349,6 +358,7 @@ void BiMPPI::concatenate() {
349358
int cb = joint[1];
350359
int df = joint[2];
351360
int db = joint[3];
361+
// std::cout << "F: " << clusters_b.size() << ", B: " << clusters_b.size() << ", cf: " << cf << ", cb: " << cb << ", df: " << df << ", db: " << db << std::endl;
352362

353363
Eigen::MatrixXd U(dim_u, df + (Tb - db) + 1);
354364
Eigen::MatrixXd X(dim_x, df + (Tb - db) + 2);
@@ -451,10 +461,12 @@ void BiMPPI::partitioningControl() {
451461
int n = std::ceil(psi * (T-1));
452462

453463
U_f0 = Uo.leftCols(std::min(n,T-1));
464+
// U_b0 = Eigen::MatrixXd::Zero(dim_u, std::min(n,T-1));
454465
U_b0 = Uo.rightCols(std::min(n,T-1));
455466
}
456467

457-
void BiMPPI::dbscan(std::vector<std::vector<int>> &clusters, const Eigen::VectorXd &Di, const Eigen::VectorXd &costs, const int &N, const int &T) {
468+
void BiMPPI::dbscan(std::vector<std::vector<int>> &clusters, const Eigen::MatrixXd &Di, const Eigen::VectorXd &costs, const int &N, const int &T) {
469+
// void BiMPPI::dbscan(std::vector<std::vector<int>> &clusters, const Eigen::VectorXd &Di, const Eigen::VectorXd &costs, const int &N, const int &T) {
458470
clusters.clear();
459471
std::vector<bool> core_points(N, false);
460472
std::map<int, std::vector<int>> core_tree;
@@ -464,7 +476,9 @@ void BiMPPI::dbscan(std::vector<std::vector<int>> &clusters, const Eigen::Vector
464476
if (costs(i) > 1E7) {continue;}
465477
for (int j = i + 1; j < N; ++j) {
466478
if (costs(j) > 1E7) {continue;}
467-
if (deviation_mu * std::abs(Di(i) - Di(j)) < epsilon) {
479+
// std::cout<<(Di.col(i) - Di.col(j)).norm()<<std::endl;
480+
if (deviation_mu * (Di.col(i) - Di.col(j)).norm() < epsilon) {
481+
// if (deviation_mu * std::abs(Di(i) - Di(j)) < epsilon) {
468482
#pragma omp critical
469483
{
470484
core_tree[i].push_back(j);
@@ -525,7 +539,7 @@ void BiMPPI::calculateU(Eigen::MatrixXd &U, const std::vector<std::vector<int>>
525539
}
526540
}
527541

528-
void BiMPPI::show() {
542+
void BiMPPI:: show() {
529543
namespace plt = matplotlibcpp;
530544
// plt::subplot(1,2,1);
531545

@@ -584,28 +598,28 @@ void BiMPPI::show() {
584598
// }
585599
// }
586600

587-
// for (int index = 0; index < clusters_f.size(); ++index) {
588-
// std::vector<std::vector<double>> X_MPPI(dim_x, std::vector<double>(Tf));
589-
// for (int i = 0; i < dim_x; ++i) {
590-
// for (int j = 0; j < Tf + 1; ++j) {
591-
// X_MPPI[i][j] = Xf(index * dim_x + i, j);
592-
// }
593-
// }
594-
// std::string color = "C" + std::to_string(index%10);
595-
// plt::plot(X_MPPI[0], X_MPPI[1], {{"color", color}, {"linewidth", "10.0"}});
596-
// }
597-
598-
// for (int index = 0; index < clusters_b.size(); ++index) {
599-
// std::vector<std::vector<double>> X_MPPI(dim_x, std::vector<double>(Tb));
600-
// for (int i = 0; i < dim_x; ++i) {
601-
// for (int j = 0; j < Tb + 1; ++j) {
602-
// X_MPPI[i][j] = Xb(index * dim_x + i, j);
603-
// }
604-
// }
605-
// std::string color = "C" + std::to_string(index%10);
606-
// plt::plot(X_MPPI[0], X_MPPI[1], {{"color", color}, {"linewidth", "10.0"}});
607-
// }
601+
for (int index = 0; index < clusters_f.size(); ++index) {
602+
std::vector<std::vector<double>> F_BRANCH(dim_x, std::vector<double>(Tf+1));
603+
for (int i = 0; i < dim_x; ++i) {
604+
for (int j = 0; j < Tf + 1; ++j) {
605+
F_BRANCH[i][j] = Xf(index * dim_x + i, j);
606+
}
607+
}
608+
std::string color = "C" + std::to_string(index%10);
609+
plt::plot(F_BRANCH[0], F_BRANCH[1], {{"color", color}, {"linewidth", "10.0"}});
610+
}
608611

612+
for (int index = 0; index < clusters_b.size(); ++index) {
613+
std::vector<std::vector<double>> B_BRANCH(dim_x, std::vector<double>(Tb+1));
614+
for (int i = 0; i < dim_x; ++i) {
615+
for (int j = 0; j < Tb + 1; ++j) {
616+
B_BRANCH[i][j] = Xb(index * dim_x + i, j);
617+
}
618+
}
619+
std::string color = "C" + std::to_string(index%10);
620+
plt::plot(B_BRANCH[0], B_BRANCH[1], {{"color", color}, {"linewidth", "10.0"}});
621+
}
622+
609623
// // plt::xlim(0, 3);
610624
// // plt::ylim(0, 5);
611625
// // plt::grid(true);

‎src/bi_mppi.cpp

Lines changed: 7 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -24,20 +24,20 @@ int main() {
2424
param.Nr = 3000;
2525
param.gamma_u = 10.0;
2626
Eigen::VectorXd sigma_u(model.dim_u);
27-
sigma_u << 0.0, 0.3;
27+
sigma_u << 0.5, 0.8;
2828
param.sigma_u = sigma_u.asDiagonal();
2929
param.deviation_mu = 1.0;
3030
param.cost_mu = 1.0;
31-
param.minpts = 5;
31+
param.minpts = 3;
3232
param.epsilon = 0.01;
33-
param.psi = 0.8;
33+
param.psi = 0.5;
3434

3535
int maxiter = 500;
3636

3737
// for (int map = 299; map >= 0 ; --map) {
3838
// for (int map = 276; map >= 0; --map) {
39-
for (int s = 2; s < 3; ++s) {
40-
// for (int s = 1; s < 2; ++s) {
39+
// for (int s = 2; s < 3; ++s) {
40+
for (int s = 1; s < 2; ++s) {
4141
switch (s)
4242
{
4343
case 0:
@@ -52,7 +52,8 @@ int main() {
5252
default:
5353
break;
5454
}
55-
for (int map = 299; map >= 0; --map) {
55+
for (int map = 298; map >= 0; --map) {
56+
// for (int map = 299; map >= 0; --map) {
5657
// for (int map = 0; map < 300; ++map) {
5758
CollisionChecker collision_checker = CollisionChecker();
5859
collision_checker.loadMap("../BARN_dataset/txt_files/output_"+std::to_string(map)+".txt", 0.1);

‎src/global.cpp

Lines changed: 20 additions & 7 deletions
Original file line numberDiff line numberDiff line change
@@ -13,8 +13,8 @@ int main() {
1313

1414
SolverParam param;
1515
param.dt = 0.1;
16-
param.Tf = 100;
17-
param.Tb = 100;
16+
param.Tf = 50;
17+
param.Tb = 50;
1818
param.x_init.resize(model.dim_x);
1919
param.x_init << 2.5, 0.0, M_PI_2;
2020
param.x_target.resize(model.dim_x);
@@ -24,13 +24,13 @@ int main() {
2424
param.Nr = 3000;
2525
param.gamma_u = 10.0;
2626
Eigen::VectorXd sigma_u(model.dim_u);
27-
sigma_u << 0.0, 0.5;
27+
sigma_u << 0.5, 1.0;
2828
param.sigma_u = sigma_u.asDiagonal();
2929
param.deviation_mu = 1.0;
3030
param.cost_mu = 1.0;
3131
param.minpts = 5;
32-
param.epsilon = 0.01;
33-
param.psi = 1.0;
32+
param.epsilon = 0.005;
33+
param.psi = 0.6;
3434

3535
int maxiter = 500;
3636

@@ -55,7 +55,9 @@ int main() {
5555
collision_checker.loadMap("../BARN_dataset/txt_files/output_"+std::to_string(map)+".txt", 0.1);
5656
Solver solver(model);
5757
solver.U_f0 = Eigen::MatrixXd::Zero(model.dim_u, param.Tf);
58+
solver.U_f0.row(0).array() += 0.5;
5859
solver.U_b0 = Eigen::MatrixXd::Zero(model.dim_u, param.Tb);
60+
solver.U_b0.row(0).array() += 0.5;
5961
solver.init(param);
6062
solver.setCollisionChecker(&collision_checker);
6163

@@ -64,8 +66,13 @@ int main() {
6466
double total_elapsed = 0.0;
6567
double f_err = 0.0;
6668
for (i = 0; i < maxiter; ++i) {
69+
std::cout<<"1"<<std::endl;
6770
solver.solve();
71+
std::cout<<"2"<<std::endl;
72+
solver.move();
73+
std::cout<<"3"<<std::endl;
6874
total_elapsed += solver.elapsed;
75+
// solver.U_b0.row(0).array() += 0.5;
6976

7077
bool is_collision = false;
7178
for (int j = 0; j <solver.Xo.cols(); ++j) {
@@ -75,14 +82,20 @@ int main() {
7582
}
7683
}
7784
if (!is_collision) {
78-
f_err = (solver.Xo.rightCols(1) - param.x_target).norm();
85+
f_err = (solver.x_init - param.x_target).norm();
86+
// f_err = (solver.Xo.rightCols(1) - param.x_target).norm();
7987
// if (solver.Xo.rightCols(1)(1) > 4) {
80-
if ((solver.Xo.rightCols(1) - param.x_target).norm() < 0.2) {
88+
if ((solver.x_init - param.x_target).norm() < 0.2) {
89+
// if ((solver.Xo.rightCols(1) - param.x_target).norm() < 0.2) {
8190
is_success = true;
8291
break;
8392
}
8493
}
94+
std::cout<<"4"<<std::endl;
95+
solver.show();
96+
std::cout<<"5"<<std::endl;
8597
}
98+
std::cout<<"6"<<std::endl;
8699
std::cout<<s<<'\t'<<map<<'\t'<<is_success<<'\t'<<i<<'\t'<<solver.Xo.cols()<<'\t'<<f_err<<'\t'<<total_elapsed<<std::endl;
87100
solver.show();
88101
}

0 commit comments

Comments
 (0)