osqp
osqp copied to clipboard
osqp
The same code runs successfully on X86, but reports a segmentation fault on ARM
-
I compiled two 0.6.0 OSQP libraries based on the same code and placed them in my project file, one is the X86 version and the other is the ARM version.
-
I will call the corresponding . so library according to different system environments.
-
On my x86 PC, I obtained the correct optimization results by calling the OSQP library, but after deploying it on ARM, the OSQP library crashed.
-
The following is the error message:
ERROR in osqp_setup: Problem data validation. /pnc_server_node: ERROR in osqp_warm_start_x: Solver workspace not initialized. /pnc_server_node: ERROR in osqp_solve: Solver workspace not initialized. /pnc_server_node: [SignalHandling::handleSignal]: Stack trace (most recent call last) in thread 2845:#23 Object "", at 0xa531f123, in #22 Object "", at 0xa6cb93b9, in #21 Object "", at 0xa6cb9f67, in #20 Object "", ~ at 0xa6cbb749, in #19 Object "", at 0xa6cbbf93, in #18 Object "", at 0xa6cacded, in #17 Object "", at 0xa6cb0a97, in #16 Object "", at 0xa6cb0d3d, in #15 Object "", at 0xa6cb3397, in #14 Object ~ "", at 0xa6cb4a11, in #13 Object "", at 0xa6cb582b, in #12 Object "", at 0xa6ca9253, in #11 Object "", at 0x847106f9, in #10 Object "", at 0xa5916693, in #9 Object "", at 0xa59161cd, in #8 O ~ bject "", at 0xa591633f, in #7 Object "", at 0xa5913639, in #6 Object "", at 0xa5914bd5, in #5 Object "", at 0xa61ea53f, in #4 Object "", at 0xa6fae5d9, in #3 Object "", at 0xa6fae3bb, in #2 ~ Object "", at 0xa6fabf2d, in #1 Object "", at 0xa6fabea7, in #0 Object "", at 0xa6faea6f, in /pnc_server_node: Segmentation fault (Address not mapped to object [0x68]) /pnc_server_node: /pnc_server_node died from signal 11
- My code is as follows:
std::vector<std::pair<double, double>> raw_point2d;
std::vector<double> bounds;
std::vector<std::pair<double, double>> ptr_smoothed_point2d;
ROS_WARN("Input path size is: %d", contour_segments[i][j].poses.size());
for (int k = 0; k < contour_segments[i][j].poses.size(); k++) {
raw_point2d.push_back(
std::make_pair(contour_segments[i][j].poses[k].pose.position.x,
contour_segments[i][j].poses[k].pose.position.y));
bounds.push_back(0.06);
}
fem_contour_smoother_->initializeConfig();
fem_contour_smoother_->doSmooth(raw_point2d, bounds,
&ptr_smoothed_point2d);
``` c++
void initializeConfig(){
smoother_config_.weight_fem_pos_deviation_ = 1.0e7;
smoother_config_.weight_path_length_ = 1.0 ;
smoother_config_.weight_ref_deviation_ = 1.0 ;
smoother_config_.max_iter_ = 4000;
smoother_config_.time_limit_ = 0;
smoother_config_.verbose_ = false ;
smoother_config_.scaled_termination_ = true;
smoother_config_.warm_start_ = true;
smoother_config_.curvature_ = 0.2;
smoother_config_.curvature_slack_ = 1e5;
smoother_config_.sub_max_iter_ = 100;
smoother_config_.sqp_ftol_ = 1e-2;
smoother_config_.sqp_pen_max_iter_ = 100;
smoother_config_.sqp_ctol_ = 1e-2;
};
``` c++
bool FemPosDeviationSmoother::doSmooth(
const std::vector<std::pair<double, double>>& raw_point2d,
const std::vector<double>& bounds,
std::vector<std::pair<double, double>>* ptr_smoothed_point2d) {
// initializeConfig();
// box contraints on pos are used in fem pos smoother, thus shrink the
// bounds by 1.0 / sqrt(2.0)
std::vector<double> box_bounds = bounds;
const double box_ratio = 1.0 / std::sqrt(2.0);
for (auto& bound : box_bounds) {
bound *= box_ratio;
}
std::vector<double> opt_x;
std::vector<double> opt_y;
// arm x86
bool status = Solve(raw_point2d, box_bounds, &opt_x, &opt_y);
if (!status) {
return false;
}
if (opt_x.size() < 2 || opt_y.size() < 2) {
return false;
}
size_t point_size = opt_x.size();
for (size_t i = 0; i < point_size; ++i) {
ptr_smoothed_point2d->emplace_back(opt_x[i], opt_y[i]);
}
return true;
}
``` c++
bool FemPosDeviationSmoother::Solve(
const std::vector<std::pair<double, double>>& raw_point2d,
const std::vector<double>& bounds, std::vector<double>* opt_x,
std::vector<double>* opt_y) {
return SqpWithOsqp(raw_point2d, bounds, opt_x, opt_y);
}
bool FemPosDeviationSmoother::SqpWithOsqp(
const std::vector<std::pair<double, double>>& raw_point2d,
const std::vector<double>& bounds, std::vector<double>* opt_x,
std::vector<double>* opt_y) {
if (opt_x == nullptr || opt_y == nullptr) {
return false;
}
fem_pos_dev_ns::FemPosDeviationSqpOsqpInterface solver;
solver.set_weight_fem_pos_deviation(smoother_config_.weight_fem_pos_deviation_);
solver.set_weight_path_length(smoother_config_.weight_path_length_);
solver.set_weight_ref_deviation(smoother_config_.weight_ref_deviation_);
solver.set_weight_curvature_constraint_slack_var(
smoother_config_.curvature_slack_);
solver.set_curvature_constraint( smoother_config_.curvature_);
solver.set_sqp_sub_max_iter(smoother_config_.max_iter_);
solver.set_sqp_ftol(smoother_config_.sqp_ftol_);
solver.set_sqp_pen_max_iter(smoother_config_.sqp_pen_max_iter_);
solver.set_sqp_ctol(smoother_config_.sqp_ctol_);
solver.set_max_iter(smoother_config_.max_iter_);
solver.set_time_limit(smoother_config_.time_limit_);
solver.set_verbose(smoother_config_.verbose_);
solver.set_scaled_termination(smoother_config_.scaled_termination_);
solver.set_warm_start(smoother_config_.warm_start_);
solver.set_ref_points(raw_point2d);
solver.set_bounds_around_refs(bounds);
if (!solver.Solve()) {
return false;
}
std::vector<std::pair<double, double>> opt_xy = solver.opt_xy();
// TODO(Jinyun): unify output data container
opt_x->resize(opt_xy.size());
opt_y->resize(opt_xy.size());
for (size_t i = 0; i < opt_xy.size(); ++i) {
(*opt_x)[i] = opt_xy[i].first;
(*opt_y)[i] = opt_xy[i].second;
}
return true;
}
``` c++
bool FemPosDeviationSqpOsqpInterface::Solve() {
// Sanity Check
if (ref_points_.empty()) {
//AERROR << "reference points empty, solver early terminates";
return false;
}
if (ref_points_.size() != bounds_around_refs_.size()) {
//AERROR << "ref_points and bounds size not equal, solver early terminates";
return false;
}
if (ref_points_.size() < 3) {
//AERROR << "ref_points size smaller than 3, solver early terminates";
return false;
}
if (ref_points_.size() > std::numeric_limits<int>::max()) {
//AERROR << "ref_points size too large, solver early terminates";
return false;
}
// Calculate optimization states definitions
num_of_points_ = static_cast<int>(ref_points_.size());
num_of_pos_variables_ = num_of_points_ * 2;
num_of_slack_variables_ = num_of_points_ - 2;
num_of_variables_ = num_of_pos_variables_ + num_of_slack_variables_;
num_of_variable_constraints_ = num_of_variables_;
num_of_curvature_constraints_ = num_of_points_ - 2;
num_of_constraints_ =
num_of_variable_constraints_ + num_of_curvature_constraints_;
// Set primal warm start
std::vector<c_float> primal_warm_start;
SetPrimalWarmStart(ref_points_, &primal_warm_start);
// Calculate kernel
std::vector<c_float> P_data;
std::vector<c_int> P_indices;
std::vector<c_int> P_indptr;
CalculateKernel(&P_data, &P_indices, &P_indptr);
// Calculate offset
std::vector<c_float> q;
CalculateOffset(&q);
// Calculate affine constraints
std::vector<c_float> A_data;
std::vector<c_int> A_indices;
std::vector<c_int> A_indptr;
std::vector<c_float> lower_bounds;
std::vector<c_float> upper_bounds;
CalculateAffineConstraint(ref_points_, &A_data, &A_indices, &A_indptr,
&lower_bounds, &upper_bounds);
// Load matrices and vectors into OSQPData
OSQPData* data = reinterpret_cast<OSQPData*>(c_malloc(sizeof(OSQPData)));
data->n = num_of_variables_;
ROS_WARN("data->n = %d", num_of_variables_);
data->m = num_of_constraints_;
ROS_WARN("data->m = %d", num_of_constraints_);
data->P = csc_matrix(data->n, data->n, P_data.size(), P_data.data(),
P_indices.data(), P_indptr.data());
ROS_WARN("data->P = %d , %d", data->P->m, data->P->n);
data->q = q.data();
ROS_WARN("data->q = %d", q.size());
data->A = csc_matrix(data->m, data->n, A_data.size(), A_data.data(),
A_indices.data(), A_indptr.data());
ROS_WARN("data->A = %d , %d", data->A->m, data->A->n);
data->l = lower_bounds.data();
data->u = upper_bounds.data();
// Define osqp solver settings
OSQPSettings* settings =
reinterpret_cast<OSQPSettings*>(c_malloc(sizeof(OSQPSettings)));
osqp_set_default_settings(settings);
settings->max_iter = max_iter_;
settings->time_limit = time_limit_;
settings->verbose = verbose_;
settings->scaled_termination = scaled_termination_;
settings->warm_start = warm_start_;
settings->polish = true;
settings->eps_abs = 1e-5;
settings->eps_rel = 1e-5;
settings->eps_prim_inf = 1e-5;
settings->eps_dual_inf = 1e-5;
// Define osqp workspace
OSQPWorkspace* work = nullptr;
osqp_setup(&work, data, settings);
//work = osqp_setup(data, settings);
// Initial solution
bool initial_solve_res = OptimizeWithOsqp(primal_warm_start, &work);
if (!initial_solve_res) {
//AERROR << "initial iteration solving fails";
osqp_cleanup(work);
c_free(data->A);
c_free(data->P);
c_free(data);
c_free(settings);
return false;
}
// Sequential solution
int pen_itr = 0;
double ctol = 0.0;
double original_slack_penalty = weight_curvature_constraint_slack_var_;
double last_fvalue = work->info->obj_val;
while (pen_itr < sqp_pen_max_iter_) {
int sub_itr = 1;
bool fconverged = false;
while (sub_itr < sqp_sub_max_iter_) {
SetPrimalWarmStart(opt_xy_, &primal_warm_start);
CalculateOffset(&q);
CalculateAffineConstraint(opt_xy_, &A_data, &A_indices, &A_indptr,
&lower_bounds, &upper_bounds);
osqp_update_lin_cost(work, q.data());
osqp_update_A(work, A_data.data(), OSQP_NULL, A_data.size());
osqp_update_bounds(work, lower_bounds.data(), upper_bounds.data());
bool iterative_solve_res = OptimizeWithOsqp(primal_warm_start, &work);
if (!iterative_solve_res) {
//AERROR << "iteration at " << sub_itr
//<< ", solving fails with max sub iter " << sqp_sub_max_iter_;
weight_curvature_constraint_slack_var_ = original_slack_penalty;
osqp_cleanup(work);
c_free(data->A);
c_free(data->P);
c_free(data);
c_free(settings);
return false;
}
double cur_fvalue = work->info->obj_val;
double ftol = std::abs((last_fvalue - cur_fvalue) / last_fvalue);
if (ftol < sqp_ftol_) {
//ADBUG << "merit function value converges at sub itr num " << sub_itr;
//ADBUG << "merit function value converges to " << cur_fvalue
//<< ", with ftol " << ftol << ", under max_ftol " << sqp_ftol_;
fconverged = true;
break;
}
last_fvalue = cur_fvalue;
++sub_itr;
}
if (!fconverged) {
//AERROR << "Max number of iteration reached";
weight_curvature_constraint_slack_var_ = original_slack_penalty;
osqp_cleanup(work);
c_free(data->A);
c_free(data->P);
c_free(data);
c_free(settings);
return false;
}
ctol = CalculateConstraintViolation(opt_xy_);
//ADBUG << "ctol is " << ctol << ", at pen itr " << pen_itr;
if (ctol < sqp_ctol_) {
//ADBUG << "constraint satisfied at pen itr num " << pen_itr;
//ADBUG << "constraint voilation value drops to " << ctol
//<< ", under max_ctol " << sqp_ctol_;
weight_curvature_constraint_slack_var_ = original_slack_penalty;
osqp_cleanup(work);
c_free(data->A);
c_free(data->P);
c_free(data);
//ADBUG << "constraint not satisfied with total itr num " << pen_itr;
//ADBUG << "constraint voilation value drops to " << ctol
//<< ", higher than max_ctol " << sqp_ctol_;
weight_curvature_constraint_slack_var_ = original_slack_penalty;
osqp_cleanup(work);
c_free(data->A);
c_free(data->P);
c_free(data);
c_free(settings);
return true;
}c_free(settings);
return true;
}
weight_curvature_constraint_slack_var_ *= 10;
++pen_itr;
}
//ADBUG << "constraint not satisfied with total itr num " << pen_itr;
//ADBUG << "constraint voilation value drops to " << ctol
//<< ", higher than max_ctol " << sqp_ctol_;
weight_curvature_constraint_slack_var_ = original_slack_penalty;
osqp_cleanup(work);
c_free(data->A);
c_free(data->P);
c_free(data);
c_free(settings);
return true;
}
- My print shows：
Input path size is : 50
data->n = 148
data->m = 196
data->p = -1833899872 , 0
data->q = 148
data->A = -1833843736, 0
I hope to get everyone's help to solve this problem. thanks.
