Skip to content

Commit 03cf719

Browse files
MC00614MC00614
committed
Test Prepare
1 parent c97345b commit 03cf719

File tree

8 files changed

+323
-247
lines changed

8 files changed

+323
-247
lines changed

‎CMakeLists.txt

Lines changed: 1 addition & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -19,8 +19,7 @@ set(COMMON_SOURCES
1919
src/bi_mppi.cpp
2020
src/mppi.cpp
2121
src/log_mppi.cpp
22-
src/global.cpp
23-
src/glob.cpp
22+
src/cluster_mppi.cpp
2423
)
2524

2625
foreach(SRC ${COMMON_SOURCES})

‎mppi/bi_mppi.h

Lines changed: 31 additions & 45 deletions
Original file line numberDiff line numberDiff line change
@@ -165,42 +165,27 @@ Eigen::MatrixXd BiMPPI::getNoise(const int &T) {
165165

166166
void BiMPPI::move() {
167167
x_init = x_init + (dt * f(x_init, u0));
168-
U_f0 = U_f0.rightCols(U_f0.cols() - 1);
168+
U_f0.leftCols(U_f0.cols() - 1) = U_f0.rightCols(U_f0.cols() - 1);
169+
// U_f0 = U_f0.rightCols(U_f0.cols() - 1);
169170
}
170171

171172
void BiMPPI::solve() {
172173
// omp setting for nested parallel
173174
omp_set_nested(1);
174175

175176
// 1. Clustered MPPI
176-
Tf = U_f0.cols();
177-
Tb = U_b0.cols();
178-
// start = std::chrono::high_resolution_clock::now();
179-
// #pragma omp parallel sections num_threads(12)
180-
// {
181-
// #pragma omp section
182-
// backwardRollout();
183-
184-
// #pragma omp section
185-
// forwardRollout();
186-
// }
187-
// finish = std::chrono::high_resolution_clock::now();
188-
// elapsed_1 = finish - start;
189-
// std::cout<<"Fir = "<<elapsed_1.count()<<std::endl;
190-
191-
std::cout<<"a"<<std::endl;
177+
// Tf = U_f0.cols();
178+
// Tb = U_b0.cols();
192179
start = std::chrono::high_resolution_clock::now();
193180
backwardRollout();
194181
forwardRollout();
195182
finish = std::chrono::high_resolution_clock::now();
196183
elapsed_1 = finish - start;
197184
// std::cout<<"Sec = "<<elapsed_1.count()<<std::endl;
198185

199-
std::cout<<"b"<<std::endl;
200186
// 2. Select Connection
201187
start = std::chrono::high_resolution_clock::now();
202188
selectConnection();
203-
std::cout<<"c"<<std::endl;
204189
concatenate();
205190
finish = std::chrono::high_resolution_clock::now();
206191
elapsed_2 = finish - start;
@@ -210,13 +195,9 @@ void BiMPPI::solve() {
210195
guideMPPI();
211196
finish = std::chrono::high_resolution_clock::now();
212197
elapsed_3 = finish - start;
213-
std::cout<<"d"<<std::endl;
214198

215199
// 4. Partitioning Control
216-
start = std::chrono::high_resolution_clock::now();
217200
partitioningControl();
218-
finish = std::chrono::high_resolution_clock::now();
219-
elapsed_4 = finish - start;
220201

221202
elapsed = elapsed_1.count() + elapsed_2.count() + elapsed_3.count();
222203

@@ -303,7 +284,6 @@ void BiMPPI::forwardRollout() {
303284
if (collision_checker->getCollisionGrid(Xi.col(j))) {
304285
all_feasible = false;
305286
cost = 1e8;
306-
// std::cout<<"F = "<<cost<<std::endl;
307287
break;
308288
}
309289
}
@@ -332,8 +312,8 @@ void BiMPPI::selectConnection() {
332312
double min_norm = std::numeric_limits<double>::max();
333313
int cb, df, db;
334314
for (int cb_ = 0; cb_ < clusters_b.size(); ++cb_) {
335-
for (int df_ = 1; df_ < Tf-1; ++df_) {
336-
for (int db_ = 0; db_ < Tb-1; ++db_) {
315+
for (int df_ = 0; df_ <= Tf; ++df_) {
316+
for (int db_ = 0; db_ <= Tb; ++db_) {
337317
double norm = (Xf.block(cf * dim_x, df_, dim_x, 1) - Xb.block(cb_ * dim_x, db_, dim_x, 1)).norm();
338318
if (norm < min_norm) {
339319
min_norm = norm;
@@ -344,7 +324,6 @@ void BiMPPI::selectConnection() {
344324
}
345325
}
346326
}
347-
// std::cout << "F: " << clusters_b.size() << ", B: " << clusters_b.size() << ", cf: " << cf << ", cb: " << cb << ", df: " << df << ", db: " << db << std::endl;
348327
joints.push_back({cf, cb, df, db});
349328
}
350329
}
@@ -358,13 +337,28 @@ void BiMPPI::concatenate() {
358337
int cb = joint[1];
359338
int df = joint[2];
360339
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;
362340

363-
Eigen::MatrixXd U(dim_u, df + (Tb - db) + 1);
364-
Eigen::MatrixXd X(dim_x, df + (Tb - db) + 2);
341+
Eigen::MatrixXd U(dim_u, std::max(Tf, df + (Tb - db)));
342+
Eigen::MatrixXd X(dim_x, std::max(Tf, df + (Tb - db)) + 1);
365343

366-
U << Uf.block(cf * dim_u, 0, dim_u, df+1), Ub.block(cb * dim_u, db, dim_u, Tb - db);
367-
X << Xf.block(cf * dim_x, 0, dim_x, df+1), Xb.block(cb * dim_x, db, dim_x, Tb - db + 1);
344+
if (df == 0) {
345+
X.leftCols(df+1) = Xf.block(cf * dim_x, 0, dim_x, df+1);
346+
}
347+
else {
348+
U.leftCols(df) = Uf.block(cf * dim_u, 0, dim_u, df);
349+
X.leftCols(df+1) = Xf.block(cf * dim_x, 0, dim_x, df+1);
350+
}
351+
352+
if (db != Tb) {
353+
U.middleCols(df+1, Tb - db - 1) = Ub.block(cb * dim_u, db + 1, dim_u, Tb - db - 1);
354+
X.middleCols(df+2, Tb - db - 1) = Xb.block(cb * dim_x, db + 1, dim_x, Tb - db - 1);
355+
}
356+
357+
// Fill if lenght is shorter than Tf
358+
if (df + (Tb - db) < Tf) {
359+
U.rightCols(Tf - (df + (Tb - db))).colwise() = Eigen::VectorXd::Zero(dim_u);
360+
X.rightCols(Tf - (df + (Tb - db))).colwise() = x_target;
361+
}
368362

369363
Uc.push_back(U);
370364
Xc.push_back(X);
@@ -394,12 +388,11 @@ void BiMPPI::guideMPPI() {
394388
Xi.col(0) = x_init;
395389
double cost = 0.0;
396390
for (int j = 0; j < Tr; ++j) {
397-
// cost += q(Xi.col(j), Ui.block(i * dim_u, j, dim_u, 1));
398391
cost += p(Xi.col(j), x_target);
399392
Xi.col(j+1) = Xi.col(j) + (dt * f(Xi.col(j), Ui.block(i * dim_u, j, dim_u, 1)));
400393
}
401-
// Guide Cost
402394
cost = p(Xi.col(Tr), x_target);
395+
// Guide Cost
403396
cost += (Xi - X_ref).colwise().norm().sum();
404397
for (int j = 0; j < Tr + 1; ++j) {
405398
if (collision_checker->getCollisionGrid(Xi.col(j))) {
@@ -426,7 +419,6 @@ void BiMPPI::guideMPPI() {
426419
double cost = 0.0;
427420
for (int j = 0; j < Tr; ++j) {
428421
cost += p(Xi.col(j), x_target);
429-
// cost += q(Xi.col(j), Ures.col(j));
430422
Xi.col(j+1) = Xi.col(j) + (dt * f(Xi.col(j), Ures.col(j)));
431423
}
432424
cost += p(Xi.col(Tr), x_target);
@@ -457,16 +449,11 @@ void BiMPPI::guideMPPI() {
457449
}
458450

459451
void BiMPPI::partitioningControl() {
460-
int T = Uo.cols();
461-
int n = std::ceil(psi * (T-1));
462-
463-
U_f0 = Uo.leftCols(std::min(n,T-1));
464-
// U_b0 = Eigen::MatrixXd::Zero(dim_u, std::min(n,T-1));
465-
U_b0 = Uo.rightCols(std::min(n,T-1));
452+
U_f0 = Uo.leftCols(Tf);
453+
U_b0 = Eigen::MatrixXd::Zero(dim_u, Tb);
466454
}
467455

468456
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) {
470457
clusters.clear();
471458
std::vector<bool> core_points(N, false);
472459
std::map<int, std::vector<int>> core_tree;
@@ -476,9 +463,7 @@ void BiMPPI::dbscan(std::vector<std::vector<int>> &clusters, const Eigen::Matrix
476463
if (costs(i) > 1E7) {continue;}
477464
for (int j = i + 1; j < N; ++j) {
478465
if (costs(j) > 1E7) {continue;}
479-
// std::cout<<(Di.col(i) - Di.col(j)).norm()<<std::endl;
480466
if (deviation_mu * (Di.col(i) - Di.col(j)).norm() < epsilon) {
481-
// if (deviation_mu * std::abs(Di(i) - Di(j)) < epsilon) {
482467
#pragma omp critical
483468
{
484469
core_tree[i].push_back(j);
@@ -536,10 +521,11 @@ void BiMPPI::calculateU(Eigen::MatrixXd &U, const std::vector<std::vector<int>>
536521
for (size_t i = 0; i < pts; ++i) {
537522
U.middleRows(index * dim_u, dim_u) += (weights[i] / total_weight) * Ui.middleRows(clusters[index][i] * dim_u, dim_u);
538523
}
524+
h(U.middleRows(index * dim_u, dim_u));
539525
}
540526
}
541527

542-
void BiMPPI:: show() {
528+
void BiMPPI::show() {
543529
namespace plt = matplotlibcpp;
544530
// plt::subplot(1,2,1);
545531

‎mppi/cluster_mppi.h

Lines changed: 196 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,196 @@
1+
#pragma once
2+
3+
#include "mppi.h"
4+
5+
#include <deque>
6+
#include <map>
7+
8+
class ClusterMPPI : public MPPI {
9+
public:
10+
template<typename ModelClass>
11+
ClusterMPPI(ModelClass model);
12+
~ClusterMPPI();
13+
14+
Eigen::MatrixXd U;
15+
Eigen::MatrixXd X;
16+
17+
void dbscan(std::vector<std::vector<int>> &clusters, const Eigen::MatrixXd &Di, const Eigen::VectorXd &costs, const int &N, const int &T);
18+
void calculateU(Eigen::MatrixXd &U, const std::vector<std::vector<int>> &clusters, const Eigen::VectorXd &costs, const Eigen::MatrixXd &Ui, const int &T);
19+
20+
21+
void solve() override {
22+
std::vector<int> full_cluster(N);
23+
std::iota(full_cluster.begin(), full_cluster.end(), 0);
24+
25+
start = std::chrono::high_resolution_clock::now();
26+
Eigen::MatrixXd Ui = U_0.replicate(N, 1);
27+
Eigen::MatrixXd Di(dim_u, N);
28+
Eigen::VectorXd costs(N);
29+
Eigen::VectorXd weights(N);
30+
bool all_feasible = true;
31+
#pragma omp parallel for
32+
for (int i = 0; i < N; ++i) {
33+
Eigen::MatrixXd Xi(dim_x, T+1);
34+
Eigen::MatrixXd noise = getNoise(T);
35+
Ui.middleRows(i * dim_u, dim_u) += noise;
36+
h(Ui.middleRows(i * dim_u, dim_u));
37+
38+
Xi.col(0) = x_init;
39+
double cost = 0.0;
40+
for (int j = 0; j < T; ++j) {
41+
cost += p(Xi.col(j), x_target);
42+
// cost += q(Xi.col(j), Ui.block(i * dim_u, j, dim_u, 1));
43+
Xi.col(j+1) = Xi.col(j) + (dt * f(Xi.col(j), Ui.block(i * dim_u, j, dim_u, 1)));
44+
}
45+
cost += p(Xi.col(T), x_target);
46+
for (int j = 1; j < T+1; ++j) {
47+
if (collision_checker->getCollisionGrid(Xi.col(j))) {
48+
cost = 1e8;
49+
all_feasible = false;
50+
break;
51+
}
52+
}
53+
costs(i) = cost;
54+
Di.col(i) = (Ui.middleRows(i * dim_u, dim_u) - U_0).rowwise().mean();
55+
}
56+
57+
std::vector<std::vector<int>> clusters;
58+
59+
if (!all_feasible) {dbscan(clusters, Di, costs, N, T);}
60+
else {clusters.clear();}
61+
62+
if (clusters.empty()) {clusters.push_back(full_cluster);}
63+
calculateU(U, clusters, costs, Ui, T);
64+
65+
double min_cost = std::numeric_limits<double>::max();
66+
int min_index = 0;
67+
68+
for (int i = 0; i < clusters.size(); ++i) {
69+
Eigen::MatrixXd Xi(dim_x, T+1);
70+
Xi.col(0) = x_init;
71+
double cost = 0.0;
72+
for (int j = 0; j < T; ++j) {
73+
cost += p(Xi.col(j), x_target);
74+
Xi.col(j+1) = Xi.col(j) + (dt * f(Xi.col(j), U.block(i * dim_u, j, dim_u, 1)));
75+
}
76+
cost += p(Xi.col(T), x_target);
77+
for (int j = 1; j < T+1; ++j) {
78+
if (collision_checker->getCollisionGrid(Xi.col(j))) {
79+
cost = 1e8;
80+
break;
81+
}
82+
}
83+
if (cost < min_cost) {
84+
min_cost = cost;
85+
min_index = i;
86+
}
87+
}
88+
89+
Uo = U.middleRows(min_index * dim_u, dim_u);
90+
91+
finish = std::chrono::high_resolution_clock::now();
92+
elapsed_1 = finish - start;
93+
elapsed = elapsed_1.count();
94+
95+
u0 = Uo.col(0);
96+
97+
Xo.col(0) = x_init;
98+
for (int j = 0; j < T; ++j) {
99+
Xo.col(j+1) = Xo.col(j) + (dt * f(Xo.col(j), Uo.col(j)));
100+
}
101+
102+
visual_traj.push_back(x_init);
103+
}
104+
105+
private:
106+
// Parameters
107+
double deviation_mu;
108+
double cost_mu;
109+
int minpts;
110+
double epsilon;
111+
double psi;
112+
};
113+
114+
template<typename ModelClass>
115+
ClusterMPPI::ClusterMPPI(ModelClass model) : MPPI(model) {
116+
deviation_mu = 1.0;
117+
cost_mu = 1.0;
118+
minpts = 5;
119+
epsilon = 0.01;
120+
psi = 0.6;
121+
}
122+
123+
ClusterMPPI::~ClusterMPPI() {
124+
}
125+
126+
void ClusterMPPI::dbscan(std::vector<std::vector<int>> &clusters, const Eigen::MatrixXd &Di, const Eigen::VectorXd &costs, const int &N, const int &T) {
127+
clusters.clear();
128+
std::vector<bool> core_points(N, false);
129+
std::map<int, std::vector<int>> core_tree;
130+
131+
#pragma omp parallel for
132+
for (int i = 0; i < N; ++i) {
133+
if (costs(i) > 1E7) {continue;}
134+
for (int j = i + 1; j < N; ++j) {
135+
if (costs(j) > 1E7) {continue;}
136+
if (deviation_mu * (Di.col(i) - Di.col(j)).norm() < epsilon) {
137+
#pragma omp critical
138+
{
139+
core_tree[i].push_back(j);
140+
core_tree[j].push_back(i);
141+
}
142+
}
143+
}
144+
}
145+
146+
for (int i = 0; i < N; ++i) {
147+
if (minpts < core_tree[i].size()) {
148+
core_points[i] = true;
149+
}
150+
}
151+
152+
std::vector<bool> visited(N, false);
153+
for (int i = 0; i < N; ++i) {
154+
if (!core_points[i]) {continue;}
155+
if (visited[i]) {continue;}
156+
std::deque<int> branch;
157+
std::vector<int> cluster;
158+
branch.push_back(i);
159+
cluster.push_back(i);
160+
visited[i] = true;
161+
while (!branch.empty()) {
162+
int now = branch.front();
163+
for (int next : core_tree[now]) {
164+
if (visited[next]) {continue;}
165+
visited[next] = true;
166+
cluster.push_back(next);
167+
if (core_points[next]) {branch.push_back(next);}
168+
}
169+
branch.pop_front();
170+
}
171+
clusters.push_back(cluster);
172+
}
173+
}
174+
175+
void ClusterMPPI::calculateU(Eigen::MatrixXd &U, const std::vector<std::vector<int>> &clusters, const Eigen::VectorXd &costs, const Eigen::MatrixXd &Ui, const int &T) {
176+
U = Eigen::MatrixXd::Zero(clusters.size() * dim_u, T);
177+
#pragma omp parallel for
178+
for (int index = 0; index < clusters.size(); ++index) {
179+
int pts = clusters[index].size();
180+
std::vector<double> weights(pts);
181+
double min_cost = std::numeric_limits<double>::max();
182+
for (int k : clusters[index]) {
183+
min_cost = std::min(min_cost, costs(k));
184+
}
185+
186+
for (size_t i = 0; i < pts; ++i) {
187+
weights[i] = std::exp(-gamma_u * (costs(clusters[index][i]) - min_cost));
188+
}
189+
double total_weight = std::accumulate(weights.begin(), weights.end(), 0.0);
190+
191+
for (size_t i = 0; i < pts; ++i) {
192+
U.middleRows(index * dim_u, dim_u) += (weights[i] / total_weight) * Ui.middleRows(clusters[index][i] * dim_u, dim_u);
193+
}
194+
h(U.middleRows(index * dim_u, dim_u));
195+
}
196+
}

0 commit comments

Comments
 (0)