mayataka
commented
2 years ago Hi @hzz1997,
Thank you for reporting the issue. I could reproduce the segmentation fault. Unfortunately, I could not get the reason and also don't have enough time to fix it now.
hello, I try to adjust the horizon in the example_mpc and example_ocp_qp, but I found that if the horizon length is 6, 10, 50 and some other numbers,it will be Segmentation fault. I am confused about it. here is the code
include "hpipm-cpp/hpipm-cpp.hpp"
include
include
include
include "Eigen/Core"
int main() { // setup QP const int N = 6; // 10 50 core dumped hpipm::OcpQpDim dim(N); dim.nx = std::vector(N+1, 12);
dim.nu = std::vector(N, 4);
dim.nbx = std::vector(N+1, 3);
dim.nbx[0] = 12;
dim.nbu = std::vector(N, 4);
dim.ng = std::vector(N+1, 0);
dim.nsbx = std::vector(N+1, 0);
dim.nsbu = std::vector(N, 0);
dim.nsg = std::vector(N+1, 0);
hpipm::OcpQp qp(dim); // initial state qp.x0 << 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0; // dynamics Eigen::MatrixXd A(12, 12), B(12, 4); A << 1., 0., 0., 0., 0., 0., 0.1, 0., 0., 0., 0., 0. , 0., 1., 0., 0., 0., 0., 0., 0.1, 0., 0., 0., 0. , 0., 0., 1., 0., 0., 0., 0., 0., 0.1, 0., 0., 0. , 0.0488, 0., 0., 1., 0., 0., 0.0016, 0., 0., 0.0992, 0., 0. , 0., -0.0488, 0., 0., 1., 0., 0., -0.0016, 0., 0., 0.0992, 0. , 0., 0., 0., 0., 0., 1., 0., 0., 0., 0., 0., 0.0992, 0., 0., 0., 0., 0., 0., 1., 0., 0., 0., 0., 0. , 0., 0., 0., 0., 0., 0., 0., 1., 0., 0., 0., 0. , 0., 0., 0., 0., 0., 0., 0., 0., 1., 0., 0., 0. , 0.9734, 0., 0., 0., 0., 0., 0.0488, 0., 0., 0.9846, 0., 0. , 0., -0.9734, 0., 0., 0., 0., 0., -0.0488, 0., 0., 0.9846, 0. , 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0., 0.9846; B << 0., -0.0726, 0., 0.0726, -0.0726, 0., 0.0726, 0. , -0.0152, 0.0152, -0.0152, 0.0152, -0., -0.0006, -0., 0.0006, 0.0006, 0., -0.0006, 0.0000, 0.0106, 0.0106, 0.0106, 0.0106, 0, -1.4512, 0., 1.4512, -1.4512, 0., 1.4512, 0. , -0.3049, 0.3049, -0.3049, 0.3049, -0., -0.0236, 0., 0.0236, 0.0236, 0., -0.0236, 0. , 0.2107, 0.2107, 0.2107, 0.2107; const Eigen::VectorXd b = Eigen::VectorXd::Zero(12); for (int i=0; i<N; ++i) { qp.A[i] = A; qp.B[i] = B; qp.b[i] = b; } // cost Eigen::MatrixXd Q(12, 12), S(4, 12), R(4, 4); Q.setZero(); Q.diagonal() << 0, 0, 10., 10., 10., 10., 0, 0, 0, 5., 5., 5.; S.setZero(); R.setZero(); R.diagonal() << 0.1, 0.1, 0.1, 0.1; Eigen::VectorXd x_ref(12); x_ref << 0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0, 0; const Eigen::VectorXd q = - Q * x_ref; const Eigen::VectorXd r = Eigen::VectorXd::Zero(4); for (int i=0; i<N; ++i) { qp.Q[i] = Q; qp.R[i] = R; qp.S[i] = S; qp.q[i] = q; qp.r[i] = r; } qp.Q[N] = Q; qp.q[N] = q; // constraints const bool use_mask_for_one_sided_constraints = true; for (int i=1; i<=N; ++i) { constexpr double soft_inf = 1.0e10; qp.idxbx[i] = {0, 1, 5}; qp.lbx[i] = (Eigen::VectorXd(3) << -M_PI/6.0, -M_PI/6.0, -1.0).finished(); qp.ubx[i] = (Eigen::VectorXd(3) << M_PI/6.0, M_PI/6.0, soft_inf).finished(); if (use_mask_for_one_sided_constraints) { qp.ubx_mask[i] = (Eigen::VectorXd(3) << 1.0, 1.0, 0.0).finished(); // this mask disables upper bound by ubx[2] } } for (int i=0; i<N; ++i) { constexpr double u0 = 10.5916; qp.idxbu[i] = {0, 1, 2, 3}; qp.lbu[i] = (Eigen::VectorXd(4) << 9.6-u0, 9.6-u0, 9.6-u0, 9.6-u0).finished(); qp.ubu[i] = (Eigen::VectorXd(4) << 13.0-u0, 13.0-u0, 13.0-u0, 13.0-u0).finished(); }
hpipm::OcpQpIpmSolverSettings ipm_solver_settings; ipm_solver_settings.iter_max = 30; ipm_solver_settings.alpha_min = 1e-8; ipm_solver_settings.mu0 = 1e2; ipm_solver_settings.tol_stat = 1e-04; ipm_solver_settings.tol_eq = 1e-04; ipm_solver_settings.tol_ineq = 1e-04; ipm_solver_settings.tol_comp = 1e-04; ipm_solver_settings.reg_prim = 1e-12; ipm_solver_settings.warm_start = 1; ipm_solver_settings.pred_corr = 1; ipm_solver_settings.ric_alg = 0; ipm_solver_settings.split_step = 1;
hpipm::OcpQpSolution solution(dim); hpipm::OcpQpIpmSolver solver(dim, ipm_solver_settings);
Eigen::VectorXd u0(4); Eigen::VectorXd x(12); x << 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0;
const int sim_steps = 50; for (int t=0; t<sim_steps; ++t) { std::cout << "t: " << t << ", x: " << x.transpose() << std::endl; qp.x0 = x; if (solver.solve(qp, solution) != hpipm::HpipmStatus::Success) return 1; u0 = solution.u[0]; x = A x + B u0 + b; }
std::cout << "t: " << sim_steps << ", x: " << x.transpose() << std::endl; return 0; }