apollo
apollo copied to clipboard
Why add a scale factor to the boundaries in piecewise_jerk_problem? Wouldn't this undermine the original physical constraints?
// set x, x', x'' bounds
for (int i = 0; i < num_of_variables; ++i) {
if (i < n) {
variables[i].emplace_back(constraint_index, 1.0);
lower_bounds->at(constraint_index) =
x_bounds_[i].first * scale_factor_[0];
upper_bounds->at(constraint_index) =
x_bounds_[i].second * scale_factor_[0];
} else if (i < 2 * n) {
variables[i].emplace_back(constraint_index, 1.0);
lower_bounds->at(constraint_index) =
dx_bounds_[i - n].first * scale_factor_[1];
upper_bounds->at(constraint_index) =
dx_bounds_[i - n].second * scale_factor_[1];
} else {
variables[i].emplace_back(constraint_index, 1.0);
lower_bounds->at(constraint_index) =
ddx_bounds_[i - 2 * n].first * scale_factor_[2];
upper_bounds->at(constraint_index) =
ddx_bounds_[i - 2 * n].second * scale_factor_[2];
}
++constraint_index;
ADEBUG<<"scale factor:"<<scale_factor_[0]<<" "<<scale_factor_[1]<<" "<<scale_factor_[2];
ADEBUG << "ddx_bounds: lower, upper";
for (int i = 0; i < n; ++i) {
ADEBUG << ddx_bounds_[i].first << " " << ddx_bounds_[i].second;
}
}