Skip to content

Commit 27bd710

Browse files
Meow404Meow404
committed
fix: use of correct N
1 parent 964bf8b commit 27bd710

File tree

1 file changed

+27
-26
lines changed

1 file changed

+27
-26
lines changed

‎examples/sampling_c3/test/lcm_log_loader.cc‎

Lines changed: 27 additions & 26 deletions
Original file line numberDiff line numberDiff line change
@@ -153,6 +153,7 @@ int DoMain(int argc, char* argv[]) {
153153
SamplingC3Options sampling_c3_options =
154154
drake::yaml::LoadYamlFile<SamplingC3Options>(sampling_c3_options_path +
155155
".yaml");
156+
int N = sampling_c3_options.N;
156157
// NOTE: can temporarily hard code many more ADMM iterations or other
157158
// changes here, e.g.:
158159
// c3_options.admm_iter = 8;
@@ -244,23 +245,23 @@ int DoMain(int argc, char* argv[]) {
244245
Eigen::VectorXd x_lcs_desired = Eigen::VectorXd::Zero(19);
245246
Eigen::VectorXd x_lcs_final_desired = Eigen::VectorXd::Zero(19);
246247
Eigen::MatrixXd dyn_feas_curr_plan_obj_pos =
247-
Eigen::MatrixXd::Zero(3, c3_options.N + 1);
248+
Eigen::MatrixXd::Zero(3, N + 1);
248249
Eigen::MatrixXd dyn_feas_curr_plan_ee_pos =
249-
Eigen::MatrixXd::Zero(3, c3_options.N + 1);
250+
Eigen::MatrixXd::Zero(3, N + 1);
250251
Eigen::MatrixXd dyn_feas_curr_plan_obj_orientation =
251-
Eigen::MatrixXd::Zero(4, c3_options.N + 1);
252+
Eigen::MatrixXd::Zero(4, N + 1);
252253
Eigen::MatrixXd dyn_feas_best_plan_obj_pos =
253-
Eigen::MatrixXd::Zero(3, c3_options.N + 1);
254+
Eigen::MatrixXd::Zero(3, N + 1);
254255
Eigen::MatrixXd dyn_feas_best_plan_ee_pos =
255-
Eigen::MatrixXd::Zero(3, c3_options.N + 1);
256+
Eigen::MatrixXd::Zero(3, N + 1);
256257
Eigen::MatrixXd dyn_feas_best_plan_obj_orientation =
257-
Eigen::MatrixXd::Zero(4, c3_options.N + 1);
258+
Eigen::MatrixXd::Zero(4, N + 1);
258259

259-
Eigen::MatrixXd u_sol = Eigen::MatrixXd::Zero(3, c3_options.N);
260-
Eigen::MatrixXd x_sol = Eigen::MatrixXd::Zero(19, c3_options.N);
261-
Eigen::MatrixXd lambda_sol = Eigen::MatrixXd::Zero(16, c3_options.N);
262-
Eigen::MatrixXd w_sol = Eigen::MatrixXd::Zero(38, c3_options.N);
263-
Eigen::MatrixXd delta_sol = Eigen::MatrixXd::Zero(38, c3_options.N);
260+
Eigen::MatrixXd u_sol = Eigen::MatrixXd::Zero(3, N);
261+
Eigen::MatrixXd x_sol = Eigen::MatrixXd::Zero(19, N);
262+
Eigen::MatrixXd lambda_sol = Eigen::MatrixXd::Zero(16, N);
263+
Eigen::MatrixXd w_sol = Eigen::MatrixXd::Zero(38, N);
264+
Eigen::MatrixXd delta_sol = Eigen::MatrixXd::Zero(38, N);
264265

265266
// Collect the sample locations
266267
std::vector<Eigen::VectorXd> sample_locations_in_log;
@@ -324,13 +325,13 @@ int DoMain(int argc, char* argv[]) {
324325
<< " and event " << "timestamp "
325326
<< adjusted_utimestamp / 1e6 << std::endl;
326327
for (int i = 0; i < 4; i++) {
327-
for (int j = 0; j < c3_options.N + 1; j++) {
328+
for (int j = 0; j < N + 1; j++) {
328329
dyn_feas_curr_plan_obj_orientation(i, j) =
329330
message.saved_traj.trajectories[0].datapoints[i][j];
330331
}
331332
}
332333
for (int i = 0; i < 3; i++) {
333-
for (int j = 0; j < c3_options.N + 1; j++) {
334+
for (int j = 0; j < N + 1; j++) {
334335
dyn_feas_curr_plan_obj_pos(i, j) =
335336
message.saved_traj.trajectories[1].datapoints[i][j];
336337
}
@@ -349,7 +350,7 @@ int DoMain(int argc, char* argv[]) {
349350
<< " and event timestamp " << adjusted_utimestamp / 1e6
350351
<< std::endl;
351352
for (int i = 0; i < 3; i++) {
352-
for (int j = 0; j < c3_options.N + 1; j++) {
353+
for (int j = 0; j < N + 1; j++) {
353354
dyn_feas_curr_plan_ee_pos(i, j) =
354355
message.saved_traj.trajectories[0].datapoints[i][j];
355356
}
@@ -368,13 +369,13 @@ int DoMain(int argc, char* argv[]) {
368369
<< " and event timestamp " << adjusted_utimestamp / 1e6
369370
<< std::endl;
370371
for (int i = 0; i < 4; i++) {
371-
for (int j = 0; j < c3_options.N + 1; j++) {
372+
for (int j = 0; j < N + 1; j++) {
372373
dyn_feas_best_plan_obj_orientation(i, j) =
373374
message.saved_traj.trajectories[0].datapoints[i][j];
374375
}
375376
}
376377
for (int i = 0; i < 3; i++) {
377-
for (int j = 0; j < c3_options.N + 1; j++) {
378+
for (int j = 0; j < N + 1; j++) {
378379
dyn_feas_best_plan_obj_pos(i, j) =
379380
message.saved_traj.trajectories[1].datapoints[i][j];
380381
}
@@ -393,7 +394,7 @@ int DoMain(int argc, char* argv[]) {
393394
<< " and event timestamp " << adjusted_utimestamp / 1e6
394395
<< std::endl;
395396
for (int i = 0; i < 3; i++) {
396-
for (int j = 0; j < c3_options.N + 1; j++) {
397+
for (int j = 0; j < N + 1; j++) {
397398
dyn_feas_best_plan_ee_pos(i, j) =
398399
message.saved_traj.trajectories[0].datapoints[i][j];
399400
}
@@ -411,31 +412,31 @@ int DoMain(int argc, char* argv[]) {
411412
<< (message.utime) / 1e6 << " and event timestamp "
412413
<< adjusted_utimestamp / 1e6 << std::endl;
413414
for (int i = 0; i < 3; i++) {
414-
for (int j = 0; j < c3_options.N; j++) {
415+
for (int j = 0; j < N; j++) {
415416
u_sol(i, j) =
416417
static_cast<double>(message.c3_solution.u_sol[i][j]);
417418
}
418419
}
419420
for (int i = 0; i < 19; i++) {
420-
for (int j = 0; j < c3_options.N; j++) {
421+
for (int j = 0; j < N; j++) {
421422
x_sol(i, j) =
422423
static_cast<double>(message.c3_solution.x_sol[i][j]);
423424
}
424425
}
425426
for (int i = 0; i < 16; i++) {
426-
for (int j = 0; j < c3_options.N; j++) {
427+
for (int j = 0; j < N; j++) {
427428
lambda_sol(i, j) =
428429
static_cast<double>(message.c3_solution.lambda_sol[i][j]);
429430
}
430431
}
431432
for (int i = 0; i < 38; i++) {
432-
for (int j = 0; j < c3_options.N; j++) {
433+
for (int j = 0; j < N; j++) {
433434
w_sol(i, j) =
434435
static_cast<double>(message.c3_intermediates.w_sol[i][j]);
435436
}
436437
}
437438
for (int i = 0; i < 38; i++) {
438-
for (int j = 0; j < c3_options.N; j++) {
439+
for (int j = 0; j < N; j++) {
439440
delta_sol(i, j) =
440441
static_cast<double>(message.c3_intermediates.delta_sol[i][j]);
441442
}
@@ -506,12 +507,12 @@ int DoMain(int argc, char* argv[]) {
506507
(x_lcs_desired != Eigen::VectorXd::Zero(19)) &&
507508
(x_lcs_final_desired != Eigen::VectorXd::Zero(19)) &&
508509
(dyn_feas_curr_plan_ee_pos !=
509-
Eigen::MatrixXd::Zero(3, c3_options.N + 1)) &&
510+
Eigen::MatrixXd::Zero(3, N + 1)) &&
510511
(dyn_feas_curr_plan_obj_pos !=
511-
Eigen::MatrixXd::Zero(3, c3_options.N + 1)) &&
512+
Eigen::MatrixXd::Zero(3, N + 1)) &&
512513
(dyn_feas_curr_plan_obj_orientation !=
513-
Eigen::MatrixXd::Zero(4, c3_options.N + 1)) &&
514-
(u_sol != Eigen::MatrixXd::Zero(3, c3_options.N)) &&
514+
Eigen::MatrixXd::Zero(4, N + 1)) &&
515+
(u_sol != Eigen::MatrixXd::Zero(3, N)) &&
515516
(is_c3_mode_set) && (sample_locations_in_log.size() > 0) &&
516517
(sample_costs_in_log.size() > 0)) {
517518
break;

0 commit comments

Comments
 (0)