solve error?

[longjianquan]

from numpy import array from qpsolvers import solve_qp

H=array([[1.,-1.],[-1.,2.]]) f=array([[-2.],[-6.]]).reshape((2,)) L=array([[-1.,2.],[1.,1.]]) k=array([[2.],[2.]]).reshape((2,))

x = solve_qp(H, f, L,k) print("QP solution: x = {}".format(x))

problem solve in python QP solution: x = [0.66666667 1.33333333]

hpipm

`#include <stdlib.h> #include <stdio.h> #include #include #include <blasfeo_d_aux_ext_dep.h> #include #include <hpipm_d_ocp_qp_ipm.h> #include <hpipm_d_ocp_qp_dim.h> #include <hpipm_d_ocp_qp.h> #include <hpipm_d_ocp_qp_sol.h> #include <hpipm_d_ocp_qp_red.h> #include <hpipm_d_ocp_qp_utils.h> #include <hpipm_timing.h> #include <Eigen/Core>

int main() { double max_v =99999.0; // horizon lenght int N = 1; // number of input static int nnu[6] = {0, 0, 0, 0, 0, 0}; // number of states static int nnx[6] = {2, 0, 0, 0, 0, 0}; // number of input box constraints static int nnbu[6] = {0, 0, 0, 0, 0, 0}; // number of states box constraints static int nnbx[6] = {2, 0, 0, 0, 0, 0}; // number of general constraints static int nng[6] = {2, 0, 0, 0, 0, 0}; // number of softed constraints on state box constraints static int nnsbx[6] = {0, 0, 0, 0, 0, 0}; // number of softed constraints on input box constraints static int nnsbu[6] = {0, 0, 0, 0, 0, 0}; // number of softed constraints on general constraints static int nnsg[6] = {0, 0, 0, 0, 0, 0}; // number of input box constraints considered as equality static int nnbue[6] = {0, 0, 0, 0, 0, 0}; // number of states box constraints considered as equality static int nnbxe[6] = {0, 0, 0, 0, 0, 0}; // number of general constraints considered as equality static int nnge[6] = {0, 0, 0, 0, 0, 0};

// QP data

//
static double A[] = {0, 0, 0, 0};
//
static double B[] = {0, 0};
//
static double b[] = {0, 0};

//
static double Q[] = {1,-1,-1,2};
//
static double R[] = {0};
//
static double S[] = {0};
//
static double q[] = {-2,-6};
//
static double r[] = {0};

//
static double lbx0[] = {0.0,0.0};
//
static double ubx0[] = {max_v,max_v};
//
static int idxbx0[] = {0,1};

//
static double u_guess[] = {};
//
static double x_guess[] = {};
//
static double sl_guess[] = {};
//
static double su_guess[] = {};

// array of pointers

//
static double *AA[5] = {A, A, A, A, A};
//
static double *BB[5] = {B, B, B, B, B};
//
static double *bb[5] = {b, b, b, b, b};
//
static double *QQ[6] = {Q, Q, Q, Q, Q, Q};
//
static double *RR[6] = {R, R, R, R, R, R};
//
static double *SS[6] = {S, S, S, S, S, S};
//
static double *qq[6] = {q, q, q, q, q, q};
//
static double *rr[6] = {r, r, r, r, r, r};
//
static int *iidxbx[6] = {idxbx0, NULL, NULL, NULL, NULL, NULL};
//
static double *llbx[6] = {lbx0, NULL, NULL, NULL, NULL, NULL};
//
static double *uubx[6] = {ubx0, NULL, NULL, NULL, NULL, NULL};
//
static int *iidxbu[6] = {};
//
static double *llbu[6] = {};
//
static double *uubu[6] = {};
//
static double gh[4] = {-1,2,1,1};
static double gh1[] = {0};

static double *CC[6] = {gh, NULL, NULL, NULL, NULL, NULL};
//
static double *DD[6] = {gh1,gh1,gh1,gh1,gh1,gh1};
//
static double ghl[2] = {-max_v,-max_v};
static double ghl1[] = {-max_v,-max_v};
static double *llg[6] = {ghl, NULL, NULL, NULL, NULL, NULL};
//
static double ghu[2] = {2,2};
static double ghu1[] = {2,2};
static double *uug[6] = {ghu, NULL, NULL, NULL, NULL, NULL};
//
static double *ZZl[6] = {};
//
static double *ZZu[6] = {};
//
static double *zzl[6] = {};
//
static double *zzu[6] = {};
//
static int *iidxs[6] = {};
//
static double *llls[6] = {};
//
static double *llus[6] = {};
//
static int *iidxe[6] = {};

//
static double *uu_guess[6] = {};
//
static double *xx_guess[6] = {};
//
static double *ssl_guess[6] = {};
//
static double *ssu_guess[6] = {};



// export as global data

int *nu = nnu;
int *nx = nnx;
int *nbu = nnbu;
int *nbx = nnbx;
int *ng = nng;
int *nsbx = nnsbx;
int *nsbu = nnsbu;
int *nsg = nnsg;
int *nbue = nnbue;
int *nbxe = nnbxe;
int *nge = nnge;

double **hA = AA;
double **hB = BB;
double **hb = bb;
double **hQ = QQ;
double **hR = RR;
double **hS = SS;
double **hq = qq;
double **hr = rr;
int **hidxbx = iidxbx;
double **hlbx = llbx;
double **hubx = uubx;
int **hidxbu = iidxbu;
double **hlbu = llbu;
double **hubu = uubu;
double **hC = CC;
double **hD = DD;
double **hlg = llg;
double **hug = uug;
double **hZl = ZZl;
double **hZu = ZZu;
double **hzl = zzl;
double **hzu = zzu;
int **hidxs = iidxs;
double **hlls = llls;
double **hlus = llus;
int **hidxe = iidxe;

double **hu_guess = uu_guess;
double **hx_guess = xx_guess;
double **hsl_guess = ssl_guess;
double **hsu_guess = ssu_guess;


// arg
hpipm_mode mode = hpipm_mode::SPEED_ABS;
int iter_max = 30;
double alpha_min = 1e-8;
double mu0 = 1e4;
double tol_stat = 1e-4;
double tol_eq = 1e-5;
double tol_ineq = 1e-5;
double tol_comp = 1e-5;
double reg_prim = 1e-12;
int warm_start = 0;
int pred_corr = 1;
int ric_alg = 0;
int split_step = 1;
int ii, jj;

int hpipm_status;

int rep, nrep=10;

hpipm_timer timer;

/************************************************

• ocp qp dim ************************************************/ hpipm_size_t dim_size = d_ocp_qp_dim_memsize(N); void *dim_mem = malloc(dim_size);

  struct d_ocp_qp_dim dim; d_ocp_qp_dim_create(N, &dim, dim_mem);

  d_ocp_qp_dim_set_all(nx, nu, nbx, nbu, ng, nsbx, nsbu, nsg, &dim);

  // set number of inequality constr to be considered as equality constr // d_ocp_qp_dim_set_nbxe(0, nx[0], &dim); d_ocp_qp_dim_set_nbxe(0, nbxe[0], &dim);

// d_ocp_qp_dim_codegen("examples/c/data/test_d_ocp_data.c", "w", &dim);

/************************************************

• ocp qp dim red eq dof (reduce equation dof, i.e. x0 elimination) ************************************************/

  hpipm_size_t dim_size2 = d_ocp_qp_dim_memsize(N); void *dim_mem2 = malloc(dim_size2);

  struct d_ocp_qp_dim dim2; d_ocp_qp_dim_create(N, &dim2, dim_mem2);

  d_ocp_qp_dim_reduce_eq_dof(&dim, &dim2);

/************************************************

• ocp qp ************************************************/

  hpipm_size_t qp_size = d_ocp_qp_memsize(&dim); void *qp_mem = malloc(qp_size);

  struct d_ocp_qp qp; d_ocp_qp_create(&dim, &qp, qp_mem);

  d_ocp_qp_set_all(hA, hB, hb, hQ, hS, hR, hq, hr, hidxbx, hlbx, hubx, hidxbu, hlbu, hubu, hC, hD, hlg, hug, hZl, hZu, hzl, hzu, hidxs, hlls, hlus, &qp);

  // mark the inequality constr on x0 as an equality constr // int idxbxe0 = (int ) malloc(nx[0]sizeof(int)); // for(ii=0; ii<=nx[0]; ii++) // idxbxe0[ii] = ii; // d_ocp_qp_set_idxbxe(0, idxbxe0, &qp); d_ocp_qp_set_idxe(0, hidxe[0], &qp); /*********************************************

• ocp qp red eq dof ************************************************/

  hpipm_size_t qp_size2 = d_ocp_qp_memsize(&dim2); void *qp_mem2 = malloc(qp_size2);

  struct d_ocp_qp qp2; d_ocp_qp_create(&dim2, &qp2, qp_mem2);

  /************************************************

• ocp qp sol ************************************************/

  hpipm_size_t qp_sol_size = d_ocp_qp_sol_memsize(&dim); void *qp_sol_mem = malloc(qp_sol_size);

  struct d_ocp_qp_sol qp_sol; d_ocp_qp_sol_create(&dim, &qp_sol, qp_sol_mem);

  hpipm_size_t qp_sol_size2 = d_ocp_qp_sol_memsize(&dim2); void *qp_sol_mem2 = malloc(qp_sol_size2);

  struct d_ocp_qp_sol qp_sol2; d_ocp_qp_sol_create(&dim2, &qp_sol2, qp_sol_mem2);

  /************************************************

• red eq dof arg ************************************************/

  hpipm_size_t qp_red_arg_size = d_ocp_qp_reduce_eq_dof_arg_memsize(); void *qp_red_arg_mem = malloc(qp_red_arg_size);

  struct d_ocp_qp_reduce_eq_dof_arg qp_red_arg; d_ocp_qp_reduce_eq_dof_arg_create(&qp_red_arg, qp_red_arg_mem);

  d_ocp_qp_reduce_eq_dof_arg_set_default(&qp_red_arg); d_ocp_qp_reduce_eq_dof_arg_set_alias_unchanged(&qp_red_arg, 1); d_ocp_qp_reduce_eq_dof_arg_set_comp_dual_sol_eq(&qp_red_arg, 1); d_ocp_qp_reduce_eq_dof_arg_set_comp_dual_sol_ineq(&qp_red_arg, 1);

  /************************************************

• ipm arg ************************************************/

  hpipm_size_t ipm_arg_size = d_ocp_qp_ipm_arg_memsize(&dim2); void *ipm_arg_mem = malloc(ipm_arg_size);

  struct d_ocp_qp_ipm_arg arg; d_ocp_qp_ipm_arg_create(&dim2, &arg, ipm_arg_mem);

  d_ocp_qp_ipm_arg_set_default(hpipm_mode::SPEED, &arg);

  d_ocp_qp_ipm_arg_set_mu0(&mu0, &arg); d_ocp_qp_ipm_arg_set_iter_max(&iter_max, &arg); d_ocp_qp_ipm_arg_set_alpha_min(&alpha_min, &arg); d_ocp_qp_ipm_arg_set_mu0(&mu0, &arg); d_ocp_qp_ipm_arg_set_tol_stat(&tol_stat, &arg); d_ocp_qp_ipm_arg_set_tol_eq(&tol_eq, &arg); d_ocp_qp_ipm_arg_set_tol_ineq(&tol_ineq, &arg); d_ocp_qp_ipm_arg_set_tol_comp(&tol_comp, &arg); d_ocp_qp_ipm_arg_set_reg_prim(&reg_prim, &arg); d_ocp_qp_ipm_arg_set_warm_start(&warm_start, &arg); d_ocp_qp_ipm_arg_set_pred_corr(&pred_corr, &arg); d_ocp_qp_ipm_arg_set_ric_alg(&ric_alg, &arg); d_ocp_qp_ipm_arg_set_split_step(&split_step, &arg);

  // d_ocp_qp_ipm_arg_codegen("examples/c/data/test_d_ocp_data.c", "a", &dim, &arg);

  /************************************************

• red eq dof workspace ************************************************/

  hpipm_size_t qp_red_work_size = d_ocp_qp_reduce_eq_dof_ws_memsize(&dim); void *qp_red_work_mem = malloc(qp_red_work_size);

  struct d_ocp_qp_reduce_eq_dof_ws qp_red_work; d_ocp_qp_reduce_eq_dof_ws_create(&dim, &qp_red_work, qp_red_work_mem);

  /************************************************

• ipm workspace ************************************************/

  hpipm_size_t ipm_size = d_ocp_qp_ipm_ws_memsize(&dim2, &arg); void *ipm_mem = malloc(ipm_size);

  struct d_ocp_qp_ipm_ws workspace; d_ocp_qp_ipm_ws_create(&dim2, &arg, &workspace, ipm_mem);

  /************************************************

• reduce equation dof (i.e. x0 elimination) ************************************************/

  hpipm_tic(&timer);

  for(rep=0; rep<nrep; rep++) { d_ocp_qp_reduce_eq_dof(&qp, &qp2, &qp_red_arg, &qp_red_work); }

  double time_red_eq_dof = hpipm_toc(&timer) / nrep;

  /************************************************

• ipm solver ************************************************/

  hpipm_tic(&timer);

  for(rep=0; rep<nrep; rep++) { // call solver d_ocp_qp_ipm_solve(&qp2, &qp_sol2, &arg, &workspace); d_ocp_qp_ipm_get_status(&workspace, &hpipm_status); }

  double time_ipm = hpipm_toc(&timer) / nrep;

  /************************************************

• ocp qp restore equation dof ************************************************/

  hpipm_tic(&timer);

  for(rep=0; rep<nrep; rep++) { d_ocp_qp_restore_eq_dof(&qp, &qp_sol2, &qp_sol, &qp_red_arg, &qp_red_work); }

  double time_res_eq_dof = hpipm_toc(&timer) / nrep;

  /************************************************

• print solution info ************************************************/

  printf("\nHPIPM returned with flag %i.\n", hpipm_status); if(hpipm_status == 0) { printf("\n -> QP solved!\n"); } else if(hpipm_status==1) { printf("\n -> Solver failed! Maximum number of iterations reached\n"); } else if(hpipm_status==2) { printf("\n -> Solver failed! Minimum step lenght reached\n"); } else if(hpipm_status==3) { printf("\n -> Solver failed! NaN in computations\n"); } else { printf("\n -> Solver failed! Unknown return flag\n"); } printf("\nAverage solution time over %i runs: %e [s]\n", nrep, time_ipm); printf("\n\n");

  /************************************************

• extract and print solution ************************************************/

  // u

  int nu_max = nu[0]; for(ii=1; ii<=N; ii++) if(nu[ii]>nu_max) nu_max = nu[ii];

  // double u = malloc(nu_maxsizeof(double));

  // printf("\nu = \n"); double *u =(double )malloc(nu_maxsizeof(double)); for(ii=0; ii<=N; ii++) { d_ocp_qp_sol_get_u(ii, &qp_sol, u); d_print_mat(1, nu[ii], u, 1); }

  // // x

  int nx_max = nx[0]; for(ii=1; ii<=N; ii++) if(nx[ii]>nx_max) nx_max = nx[ii];

  double *x = (double )malloc(nx_maxsizeof(double));

  printf("\nx = \n"); for(ii=0; ii<=N; ii++) { d_ocp_qp_sol_get_x(ii, &qp_sol, x); d_print_mat(1, nx[ii], x, 1); }

  // pi

  double *pi = (double )malloc(nx_maxsizeof(double));

  // printf("\npi = \n"); // for(ii=0; ii<N; ii++) // { // d_ocp_qp_sol_get_pi(ii, &qp_sol, pi); // d_print_mat(1, nx[ii+1], pi, 1); // }

  // all solution components at once

  double **u1 = (double **)malloc((N+1)*sizeof(double *)); for(ii=0; ii<=N; ii++) d_zeros(u1+ii, nu[ii], 1); double **x1 = (double **)malloc((N+1)*sizeof(double *)); for(ii=0; ii<=N; ii++) d_zeros(x1+ii, nx[ii], 1); double **ls1 = (double **)malloc((N+1)*sizeof(double *)); for(ii=0; ii<=N; ii++) d_zeros(ls1+ii, nsbu[ii]+nsbx[ii]+nsg[ii], 1); double **us1 = (double **)malloc((N+1)*sizeof(double *)); for(ii=0; ii<=N; ii++) d_zeros(us1+ii, nsbu[ii]+nsbx[ii]+nsg[ii], 1); double **pi1 = (double **)malloc((N)*sizeof(double *)); for(ii=0; ii<N; ii++) d_zeros(pi1+ii, nx[ii+1], 1); double **lam_lb1 = (double **)malloc((N+1)*sizeof(double *)); for(ii=0; ii<=N; ii++) d_zeros(lam_lb1+ii, nbu[ii]+nbx[ii], 1); double **lam_ub1 = (double **)malloc((N+1)*sizeof(double *)); for(ii=0; ii<=N; ii++) d_zeros(lam_ub1+ii, nbu[ii]+nbx[ii], 1); double **lam_lg1 = (double **)malloc((N+1)*sizeof(double *)); for(ii=0; ii<=N; ii++) d_zeros(lam_lg1+ii, ng[ii], 1); double **lam_ug1 = (double **)malloc((N+1)*sizeof(double *)); for(ii=0; ii<=N; ii++) d_zeros(lam_ug1+ii, ng[ii], 1); double **lam_ls1 = (double **)malloc((N+1)*sizeof(double *)); for(ii=0; ii<=N; ii++) d_zeros(lam_ls1+ii, nsbu[ii]+nsbx[ii]+nsg[ii], 1); double **lam_us1 = (double **)malloc((N+1)*sizeof(double *)); for(ii=0; ii<=N; ii++) d_zeros(lam_us1+ii, nsbu[ii]+nsbx[ii]+nsg[ii], 1);

  d_ocp_qp_sol_get_all(&qp_sol, u1, x1, ls1, us1, pi1, lam_lb1, lam_ub1, lam_lg1, lam_ug1, lam_ls1, lam_us1);

  // d_ocp_qp_sol_print(&dim, &qp_sol);

  /************************************************

• print ipm statistics ************************************************/

  int iter; d_ocp_qp_ipm_get_iter(&workspace, &iter); double res_stat; d_ocp_qp_ipm_get_max_res_stat(&workspace, &res_stat); double res_eq; d_ocp_qp_ipm_get_max_res_eq(&workspace, &res_eq); double res_ineq; d_ocp_qp_ipm_get_max_res_ineq(&workspace, &res_ineq); double res_comp; d_ocp_qp_ipm_get_max_res_comp(&workspace, &res_comp); double *stat; d_ocp_qp_ipm_get_stat(&workspace, &stat); int stat_m; d_ocp_qp_ipm_get_stat_m(&workspace, &stat_m);

  printf("\nipm return = %d\n", hpipm_status); printf("\ntotal time = %e [s]\n\n", time_red_eq_dof+time_ipm+time_res_eq_dof);

  free(dim_mem); free(qp_mem); free(qp_sol_mem); free(ipm_arg_mem); free(ipm_mem);

  free(dim_mem2); free(qp_mem2); free(qp_red_arg_mem); free(qp_red_work_mem); free(qp_sol_mem2);

  free(u); free(x); free(pi);

  for(ii=0; ii<N; ii++) { d_free(u1[ii]); d_free(x1[ii]); d_free(ls1[ii]); d_free(us1[ii]); d_free(pi1[ii]); d_free(lam_lb1[ii]); d_free(lam_ub1[ii]); d_free(lam_lg1[ii]); d_free(lam_ug1[ii]); d_free(lam_ls1[ii]); d_free(lam_us1[ii]); } d_free(u1[ii]); d_free(x1[ii]); d_free(ls1[ii]); d_free(us1[ii]); d_free(lam_lb1[ii]); d_free(lam_ub1[ii]); d_free(lam_lg1[ii]); d_free(lam_ug1[ii]); d_free(lam_ls1[ii]); d_free(lam_us1[ii]);

  free(u1); free(x1); free(ls1); free(us1); free(lam_lb1); free(lam_ub1); free(lam_lg1); free(lam_ug1); free(lam_ls1); free(lam_us1);

  return 0;

}

`

result is x = 0.00069 1.99863

[giaf]

The issue is in the problem formulation and comes from the fact that HPIPM expects the matrices to be in column-major, so if you write static double gh[4] = {-1,2,1,1}; this is interpreted as the 2x2 matrix [ -1 1 | | 2 1 | that is the transposed of what you likely meant.

Then in general I would strongly suggest to use the dense QP formulation and solver in case of an unstructured QP, instead of using the OCP QP one that is tailored to quadratic programs with optimal control structure: this is both easier and more efficient.