solve error? #157
Comments
|
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 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. |
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment
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};
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(®_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
The text was updated successfully, but these errors were encountered: