From 2b3a376998795bc209952780cca04437eb1266fb Mon Sep 17 00:00:00 2001 From: zanellia Date: Thu, 19 Jul 2018 11:18:34 +0200 Subject: [PATCH 01/16] adding getting_started.c --- examples/c/getting_started.c | 190 +++++++++++++++++++++++++++++++++++ 1 file changed, 190 insertions(+) create mode 100644 examples/c/getting_started.c diff --git a/examples/c/getting_started.c b/examples/c/getting_started.c new file mode 100644 index 00000000..bf41ac5c --- /dev/null +++ b/examples/c/getting_started.c @@ -0,0 +1,190 @@ + +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +#include "../../include/hpipm_d_ocp_qp_ipm.h" +#include "../../include/hpipm_d_ocp_qp_dim.h" +#include "../../include/hpipm_d_ocp_qp.h" +#include "../../include/hpipm_d_ocp_qp_sol.h" + +#define QP_HORIZON 5 + +int main() { + + double A[] = {1, 0, 1, 1}; + double B[] = {0, 1}; + double b[] = {0, 0}; + + double Q[] = {1, 0, 0, 1}; + double S[] = {0, 0}; + double R[] = {1}; + double q[] = {1, 1}; + double r[] = {0}; + + double x0[] = {1, 1}; + int idxb0[] = {1, 2}; + + int nx[] = {2, 2, 2, 2, 2, 2}; + int nu[] = {1, 1, 1, 1, 1, 0}; + int nb[] = {2, 0, 0, 0, 0, 0}; + int ng[] = {0, 0, 0, 0, 0, 0}; + int ns[] = {0, 0, 0, 0, 0, 0}; + int nsbx[] = {0, 0, 0, 0, 0, 0}; + int nsbu[] = {0, 0, 0, 0, 0, 0}; + int nbx[] = {2, 0, 0, 0, 0, 0}; + int nbu[] = {0, 0, 0, 0, 0, 0}; + + int N = QP_HORIZON; + + /************************************************ + * ocp qp dim + ************************************************/ + + int dim_size = d_memsize_ocp_qp_dim(N); + void *dim_mem = malloc(dim_size); + + struct d_ocp_qp_dim dim; + d_create_ocp_qp_dim(N, &dim, dim_mem); + d_cvt_int_to_ocp_qp_dim(N, nx, nu, nbx, nbu, ng, ns, nsbx, nsbu, &dim); + + double *hA[] = {A, A, A, A, A}; + double *hB[] = {B, B, B, B, B}; + double *hb[] = {b, b, b, b, b}; + double *hQ[] = {Q, Q, Q, Q, Q, Q}; + double *hS[] = {S, S, S, S, S, S}; + double *hR[] = {R, R, R, R, R}; + double *hq[] = {q, q, q, q, q, q}; + double *hr[] = {r, r, r, r, r, r}; + int *hidxb[] = {idxb0}; + double *hlb[] = {x0}; + double *hub[] = {x0}; + + /************************************************ + * ocp qp + ************************************************/ + + int qp_size = d_memsize_ocp_qp(&dim); + void *qp_mem = malloc(qp_size); + + struct d_ocp_qp qp; + d_create_ocp_qp(&dim, &qp, qp_mem); + + d_cvt_colmaj_to_ocp_qp(hA, hB, hb, hQ, hS, hR, hq, hr, hidxb, hlb, hub, + NULL, NULL, NULL, NULL, NULL, NULL, NULL, NULL, NULL, NULL, NULL, &qp); + + /************************************************ + * ocp qp sol + ************************************************/ + + int qp_sol_size = d_memsize_ocp_qp_sol(&dim); + void *qp_sol_mem = malloc(qp_sol_size); + + struct d_ocp_qp_sol qp_sol; + d_create_ocp_qp_sol(&dim, &qp_sol, qp_sol_mem); + + /************************************************ + * ipm arg + ************************************************/ + + int ipm_arg_size = d_memsize_ocp_qp_ipm_arg(&dim); + void *ipm_arg_mem = malloc(ipm_arg_size); + + struct d_ocp_qp_ipm_arg arg; + d_create_ocp_qp_ipm_arg(&dim, &arg, ipm_arg_mem); + d_set_default_ocp_qp_ipm_arg(1, &arg); + + int ipm_size = d_memsize_ocp_qp_ipm(&dim, &arg); + void *ipm_mem = malloc(ipm_size); + + struct d_ocp_qp_ipm_workspace workspace; + d_create_ocp_qp_ipm(&dim, &arg, &workspace, ipm_mem); + + int hpipm_return; // 0 normal; 1 max iter + + int rep, nrep=1000; + + struct timeval tv0, tv1; + + gettimeofday(&tv0, NULL); // start + + for(rep=0; rep QP solved! Solution:\n\n"); + printf("\nu\n"); + for(ii=0; ii<=N; ii++) + d_print_mat(1, nu[ii], u[ii], 1); + printf("\nx\n"); + for(ii=0; ii<=N; ii++) + d_print_mat(1, nx[ii], x[ii], 1); + } else { + printf("\n -> Solver failed!\n"); + } + + for(ii=0; ii Date: Thu, 19 Jul 2018 11:41:53 +0200 Subject: [PATCH 02/16] fix Makefile.rule --- Makefile.rule | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Makefile.rule b/Makefile.rule index 3aa13358..bcd0f42c 100644 --- a/Makefile.rule +++ b/Makefile.rule @@ -82,7 +82,7 @@ CFLAGS += -O2 -fPIC CFLAGS += #-g #-Wall -pedantic -Wfloat-equal #-pg # search directories -CFLAGS += -I$(BLASFEO_PATH)/include -I$(TOP)/include +CFLAGS += -I$(BLASFEO_PATH)/include -I$(TOP)/include -I/$(TOP)/../../include # definirions #ifeq ($(OS), LINUX) From 70c0ced68e356ba2581442b989d6a856e6ea222c Mon Sep 17 00:00:00 2001 From: zanellia Date: Thu, 19 Jul 2018 12:00:15 +0200 Subject: [PATCH 03/16] adding print level field to args, retabbing --- include/hpipm_d_dense_qp_ipm.h | 1 + include/hpipm_s_dense_qp_ipm.h | 121 +++++++++++++++++---------------- 2 files changed, 62 insertions(+), 60 deletions(-) diff --git a/include/hpipm_d_dense_qp_ipm.h b/include/hpipm_d_dense_qp_ipm.h index 688a1abc..0157b312 100644 --- a/include/hpipm_d_dense_qp_ipm.h +++ b/include/hpipm_d_dense_qp_ipm.h @@ -72,6 +72,7 @@ struct d_dense_qp_ipm_arg int abs_form; // absolute IPM formulation int comp_res_exit; // compute residuals on exit (only for abs_form==1) int memsize; + int print_level; }; diff --git a/include/hpipm_s_dense_qp_ipm.h b/include/hpipm_s_dense_qp_ipm.h index 1e6ca38c..3255dd9c 100644 --- a/include/hpipm_s_dense_qp_ipm.h +++ b/include/hpipm_s_dense_qp_ipm.h @@ -50,70 +50,71 @@ extern "C" { struct s_dense_qp_ipm_arg - { - float mu0; // initial value for duality measure - float alpha_min; // exit cond on step length - float res_g_max; // exit cond on inf norm of residuals - float res_b_max; // exit cond on inf norm of residuals - float res_d_max; // exit cond on inf norm of residuals - float res_m_max; // exit cond on inf norm of residuals - float reg_prim; // reg of primal hessian - float reg_dual; // reg of dual hessian - float lam_min; // min value in lam vector - float t_min; // min value in t vector - int iter_max; // exit cond in iter number - int stat_max; // iterations saved in stat - int pred_corr; // Mehrotra's predictor-corrector IPM algirthm - int cond_pred_corr; // conditional Mehrotra's predictor-corrector - int scale; // scale hessian - int itref_pred_max; // max number of iterative refinement steps for predictor step - int itref_corr_max; // max number of iterative refinement steps for corrector step - int warm_start; // 0 no warm start, 1 warm start primal sol - int lq_fact; // 0 syrk+potrf, 1 mix, 2 lq - int abs_form; // absolute IPM formulation - int comp_res_exit; // compute residuals on exit (only for abs_form==1) - int memsize; - }; + { + float mu0; // initial value for duality measure + float alpha_min; // exit cond on step length + float res_g_max; // exit cond on inf norm of residuals + float res_b_max; // exit cond on inf norm of residuals + float res_d_max; // exit cond on inf norm of residuals + float res_m_max; // exit cond on inf norm of residuals + float reg_prim; // reg of primal hessian + float reg_dual; // reg of dual hessian + float lam_min; // min value in lam vector + float t_min; // min value in t vector + int iter_max; // exit cond in iter number + int stat_max; // iterations saved in stat + int pred_corr; // Mehrotra's predictor-corrector IPM algirthm + int cond_pred_corr; // conditional Mehrotra's predictor-corrector + int scale; // scale hessian + int itref_pred_max; // max number of iterative refinement steps for predictor step + int itref_corr_max; // max number of iterative refinement steps for corrector step + int warm_start; // 0 no warm start, 1 warm start primal sol + int lq_fact; // 0 syrk+potrf, 1 mix, 2 lq + int abs_form; // absolute IPM formulation + int comp_res_exit; // compute residuals on exit (only for abs_form==1) + int memsize; + int print_level; + }; struct s_dense_qp_ipm_workspace - { - struct s_core_qp_ipm_workspace *core_workspace; - struct s_dense_qp_res_workspace *res_workspace; - struct s_dense_qp_sol *sol_step; - struct s_dense_qp_sol *sol_itref; - struct s_dense_qp *qp_step; - struct s_dense_qp *qp_itref; - struct s_dense_qp_res *res; - struct s_dense_qp_res *res_itref; - struct blasfeo_svec *Gamma; // - struct blasfeo_svec *gamma; // - struct blasfeo_svec *Zs_inv; // - struct blasfeo_smat *Lv; // - struct blasfeo_smat *AL; // - struct blasfeo_smat *Le; // - struct blasfeo_smat *Ctx; // - struct blasfeo_svec *lv; // - struct blasfeo_svec *sv; // scale for Lv - struct blasfeo_svec *se; // scale for Le - struct blasfeo_svec *tmp_nbg; // work space of size nb+ng - struct blasfeo_svec *tmp_ns; // work space of size ns - struct blasfeo_smat *lq0; - struct blasfeo_smat *lq1; - struct blasfeo_svec *tmp_m; - float *stat; // convergence statistics -// int *ipiv_v; -// int *ipiv_e; - void *lq_work0; - void *lq_work1; - float qp_res[4]; // infinity norm of residuals - int iter; // iteration number - int stat_max; // iterations saved in stat - int scale; - int use_hess_fact; - int memsize; // memory size (in bytes) of workspace - }; + { + struct s_core_qp_ipm_workspace *core_workspace; + struct s_dense_qp_res_workspace *res_workspace; + struct s_dense_qp_sol *sol_step; + struct s_dense_qp_sol *sol_itref; + struct s_dense_qp *qp_step; + struct s_dense_qp *qp_itref; + struct s_dense_qp_res *res; + struct s_dense_qp_res *res_itref; + struct blasfeo_svec *Gamma; // + struct blasfeo_svec *gamma; // + struct blasfeo_svec *Zs_inv; // + struct blasfeo_smat *Lv; // + struct blasfeo_smat *AL; // + struct blasfeo_smat *Le; // + struct blasfeo_smat *Ctx; // + struct blasfeo_svec *lv; // + struct blasfeo_svec *sv; // scale for Lv + struct blasfeo_svec *se; // scale for Le + struct blasfeo_svec *tmp_nbg; // work space of size nb+ng + struct blasfeo_svec *tmp_ns; // work space of size ns + struct blasfeo_smat *lq0; + struct blasfeo_smat *lq1; + struct blasfeo_svec *tmp_m; + float *stat; // convergence statistics +// int *ipiv_v; +// int *ipiv_e; + void *lq_work0; + void *lq_work1; + float qp_res[4]; // infinity norm of residuals + int iter; // iteration number + int stat_max; // iterations saved in stat + int scale; + int use_hess_fact; + int memsize; // memory size (in bytes) of workspace + }; From c652c2737cde82519ca74267754ab7212362eab5 Mon Sep 17 00:00:00 2001 From: zanellia Date: Thu, 19 Jul 2018 12:02:27 +0200 Subject: [PATCH 04/16] adding print level field to args for ocp_qp too, retabbing --- include/hpipm_d_ocp_qp_ipm.h | 109 ++++++++++++++++++----------------- include/hpipm_s_ocp_qp_ipm.h | 107 +++++++++++++++++----------------- 2 files changed, 109 insertions(+), 107 deletions(-) diff --git a/include/hpipm_d_ocp_qp_ipm.h b/include/hpipm_d_ocp_qp_ipm.h index 075c48fb..b7385658 100644 --- a/include/hpipm_d_ocp_qp_ipm.h +++ b/include/hpipm_d_ocp_qp_ipm.h @@ -48,63 +48,64 @@ extern "C" { struct d_ocp_qp_ipm_arg - { - double mu0; // initial value for duality measure - double alpha_min; // exit cond on step length - double res_g_max; // exit cond on inf norm of residuals - double res_b_max; // exit cond on inf norm of residuals - double res_d_max; // exit cond on inf norm of residuals - double res_m_max; // exit cond on inf norm of residuals - double reg_prim; // reg of primal hessian - double lam_min; // min value in lam vector - double t_min; // min value in t vector - int iter_max; // exit cond in iter number - int stat_max; // iterations saved in stat - int pred_corr; // use Mehrotra's predictor-corrector IPM algirthm - int cond_pred_corr; // conditional Mehrotra's predictor-corrector - int itref_pred_max; // max number of iterative refinement steps for predictor step - int itref_corr_max; // max number of iterative refinement steps for corrector step - int warm_start; // 0 no warm start, 1 warm start primal sol - int lq_fact; // 0 syrk+potrf, 1 mix, 2 lq - int abs_form; // absolute IPM formulation - int comp_dual_sol; // dual solution (only for abs_form==1) - int comp_res_exit; // compute residuals on exit (only for abs_form==1 and comp_dual_sol==1) - int memsize; - }; + { + double mu0; // initial value for duality measure + double alpha_min; // exit cond on step length + double res_g_max; // exit cond on inf norm of residuals + double res_b_max; // exit cond on inf norm of residuals + double res_d_max; // exit cond on inf norm of residuals + double res_m_max; // exit cond on inf norm of residuals + double reg_prim; // reg of primal hessian + double lam_min; // min value in lam vector + double t_min; // min value in t vector + int iter_max; // exit cond in iter number + int stat_max; // iterations saved in stat + int pred_corr; // use Mehrotra's predictor-corrector IPM algirthm + int cond_pred_corr; // conditional Mehrotra's predictor-corrector + int itref_pred_max; // max number of iterative refinement steps for predictor step + int itref_corr_max; // max number of iterative refinement steps for corrector step + int warm_start; // 0 no warm start, 1 warm start primal sol + int lq_fact; // 0 syrk+potrf, 1 mix, 2 lq + int abs_form; // absolute IPM formulation + int comp_dual_sol; // dual solution (only for abs_form==1) + int comp_res_exit; // compute residuals on exit (only for abs_form==1 and comp_dual_sol==1) + int memsize; + int print_level; + }; struct d_ocp_qp_ipm_workspace - { - struct d_core_qp_ipm_workspace *core_workspace; - struct d_ocp_qp_res_workspace *res_workspace; - struct d_ocp_qp_sol *sol_step; - struct d_ocp_qp_sol *sol_itref; - struct d_ocp_qp *qp_step; - struct d_ocp_qp *qp_itref; - struct d_ocp_qp_res *res_itref; - struct d_ocp_qp_res *res; - struct blasfeo_dvec *Gamma; // hessian update - struct blasfeo_dvec *gamma; // hessian update - struct blasfeo_dvec *tmp_nxM; // work space of size nxM - struct blasfeo_dvec *tmp_nbgM; // work space of size nbM+ngM - struct blasfeo_dvec *tmp_nsM; // work space of size nsM - struct blasfeo_dvec *Pb; // Pb - struct blasfeo_dvec *Zs_inv; - struct blasfeo_dmat *L; - struct blasfeo_dmat *Lh; - struct blasfeo_dmat *AL; - struct blasfeo_dmat *lq0; - struct blasfeo_dvec *tmp_m; - double *stat; // convergence statistics - int *use_hess_fact; - void *lq_work0; - double qp_res[4]; // infinity norm of residuals - int iter; // iteration number - int stat_max; // iterations saved in stat - int use_Pb; - int memsize; - }; + { + struct d_core_qp_ipm_workspace *core_workspace; + struct d_ocp_qp_res_workspace *res_workspace; + struct d_ocp_qp_sol *sol_step; + struct d_ocp_qp_sol *sol_itref; + struct d_ocp_qp *qp_step; + struct d_ocp_qp *qp_itref; + struct d_ocp_qp_res *res_itref; + struct d_ocp_qp_res *res; + struct blasfeo_dvec *Gamma; // hessian update + struct blasfeo_dvec *gamma; // hessian update + struct blasfeo_dvec *tmp_nxM; // work space of size nxM + struct blasfeo_dvec *tmp_nbgM; // work space of size nbM+ngM + struct blasfeo_dvec *tmp_nsM; // work space of size nsM + struct blasfeo_dvec *Pb; // Pb + struct blasfeo_dvec *Zs_inv; + struct blasfeo_dmat *L; + struct blasfeo_dmat *Lh; + struct blasfeo_dmat *AL; + struct blasfeo_dmat *lq0; + struct blasfeo_dvec *tmp_m; + double *stat; // convergence statistics + int *use_hess_fact; + void *lq_work0; + double qp_res[4]; // infinity norm of residuals + int iter; // iteration number + int stat_max; // iterations saved in stat + int use_Pb; + int memsize; + }; @@ -125,7 +126,7 @@ int d_solve_ocp_qp_ipm(struct d_ocp_qp *qp, struct d_ocp_qp_sol *qp_sol, struct #ifdef __cplusplus -} // #extern "C" +} // #extern "C" #endif diff --git a/include/hpipm_s_ocp_qp_ipm.h b/include/hpipm_s_ocp_qp_ipm.h index 7f4ac4fc..6232304e 100644 --- a/include/hpipm_s_ocp_qp_ipm.h +++ b/include/hpipm_s_ocp_qp_ipm.h @@ -50,63 +50,64 @@ extern "C" { struct s_ocp_qp_ipm_arg - { - float mu0; // initial value for duality measure - float alpha_min; // exit cond on step length - float res_g_max; // exit cond on inf norm of residuals - float res_b_max; // exit cond on inf norm of residuals - float res_d_max; // exit cond on inf norm of residuals - float res_m_max; // exit cond on inf norm of residuals - float reg_prim; // reg of primal hessian - float lam_min; // min value in lam vector - float t_min; // min value in t vector - int iter_max; // exit cond in iter number - int stat_max; // iterations saved in stat - int pred_corr; // use Mehrotra's predictor-corrector IPM algirthm - int cond_pred_corr; // conditional Mehrotra's predictor-corrector - int itref_pred_max; // max number of iterative refinement steps for predictor step - int itref_corr_max; // max number of iterative refinement steps for corrector step - int warm_start; // 0 no warm start, 1 warm start primal sol - int lq_fact; // 0 syrk+potrf, 1 mix, 2 lq - int abs_form; // absolute IPM formulation - int comp_dual_sol; // dual solution (only for abs_form==1) - int comp_res_exit; // compute residuals on exit (only for abs_form==1 and comp_dual_sol==1) - int memsize; - }; + { + float mu0; // initial value for duality measure + float alpha_min; // exit cond on step length + float res_g_max; // exit cond on inf norm of residuals + float res_b_max; // exit cond on inf norm of residuals + float res_d_max; // exit cond on inf norm of residuals + float res_m_max; // exit cond on inf norm of residuals + float reg_prim; // reg of primal hessian + float lam_min; // min value in lam vector + float t_min; // min value in t vector + int iter_max; // exit cond in iter number + int stat_max; // iterations saved in stat + int pred_corr; // use Mehrotra's predictor-corrector IPM algirthm + int cond_pred_corr; // conditional Mehrotra's predictor-corrector + int itref_pred_max; // max number of iterative refinement steps for predictor step + int itref_corr_max; // max number of iterative refinement steps for corrector step + int warm_start; // 0 no warm start, 1 warm start primal sol + int lq_fact; // 0 syrk+potrf, 1 mix, 2 lq + int abs_form; // absolute IPM formulation + int comp_dual_sol; // dual solution (only for abs_form==1) + int comp_res_exit; // compute residuals on exit (only for abs_form==1 and comp_dual_sol==1) + int memsize; + int print_level; + }; struct s_ocp_qp_ipm_workspace - { - struct s_core_qp_ipm_workspace *core_workspace; - struct s_ocp_qp_res_workspace *res_workspace; - struct s_ocp_qp_sol *sol_step; - struct s_ocp_qp_sol *sol_itref; - struct s_ocp_qp *qp_step; - struct s_ocp_qp *qp_itref; - struct s_ocp_qp_res *res; - struct s_ocp_qp_res *res_itref; - struct blasfeo_svec *Gamma; // hessian update - struct blasfeo_svec *gamma; // hessian update - struct blasfeo_svec *tmp_nxM; // work space of size nxM - struct blasfeo_svec *tmp_nbgM; // work space of size nbM+ngM - struct blasfeo_svec *tmp_nsM; // work space of size nsM - struct blasfeo_svec *Pb; // Pb - struct blasfeo_svec *Zs_inv; - struct blasfeo_smat *L; - struct blasfeo_smat *Lh; - struct blasfeo_smat *AL; - struct blasfeo_smat *lq0; - struct blasfeo_svec *tmp_m; - float *stat; // convergence statistics - int *use_hess_fact; - void *lq_work0; - float qp_res[4]; // infinity norm of residuals - int iter; // iteration number - int stat_max; // iterations saved in stat - int use_Pb; - int memsize; - }; + { + struct s_core_qp_ipm_workspace *core_workspace; + struct s_ocp_qp_res_workspace *res_workspace; + struct s_ocp_qp_sol *sol_step; + struct s_ocp_qp_sol *sol_itref; + struct s_ocp_qp *qp_step; + struct s_ocp_qp *qp_itref; + struct s_ocp_qp_res *res; + struct s_ocp_qp_res *res_itref; + struct blasfeo_svec *Gamma; // hessian update + struct blasfeo_svec *gamma; // hessian update + struct blasfeo_svec *tmp_nxM; // work space of size nxM + struct blasfeo_svec *tmp_nbgM; // work space of size nbM+ngM + struct blasfeo_svec *tmp_nsM; // work space of size nsM + struct blasfeo_svec *Pb; // Pb + struct blasfeo_svec *Zs_inv; + struct blasfeo_smat *L; + struct blasfeo_smat *Lh; + struct blasfeo_smat *AL; + struct blasfeo_smat *lq0; + struct blasfeo_svec *tmp_m; + float *stat; // convergence statistics + int *use_hess_fact; + void *lq_work0; + float qp_res[4]; // infinity norm of residuals + int iter; // iteration number + int stat_max; // iterations saved in stat + int use_Pb; + int memsize; + }; From 9325e9ab68af2c9dbb18f9fc8d03a29fd3abbbe4 Mon Sep 17 00:00:00 2001 From: zanellia Date: Thu, 19 Jul 2018 17:58:09 +0200 Subject: [PATCH 05/16] added basic printing --- dense_qp/x_dense_qp_ipm.c | 107 ++++++++++++++++++++++++----------- examples/c/getting_started.c | 8 ++- ocp_qp/x_ocp_qp_ipm.c | 88 ++++++++++++++++++++-------- 3 files changed, 143 insertions(+), 60 deletions(-) diff --git a/dense_qp/x_dense_qp_ipm.c b/dense_qp/x_dense_qp_ipm.c index 4bf5f20d..4577cfca 100644 --- a/dense_qp/x_dense_qp_ipm.c +++ b/dense_qp/x_dense_qp_ipm.c @@ -50,6 +50,10 @@ void CREATE_DENSE_QP_IPM_ARG(struct DENSE_QP_DIM *dim, struct DENSE_QP_IPM_ARG * void SET_DEFAULT_DENSE_QP_IPM_ARG(enum HPIPM_MODE mode, struct DENSE_QP_IPM_ARG *arg) { + // set common default arguments + arg->print_level = 0; + + // set mode-specific default arguments if(mode==SPEED_ABS) { arg->mu0 = 1e1; @@ -687,25 +691,39 @@ int SOLVE_DENSE_QP_IPM(struct DENSE_QP *qp, struct DENSE_QP_SOL *qp_sol, struct ws->iter = kk; - // max iteration number reached - if(kk == arg->iter_max) - return 1; - - // min step lenght - if(cws->alpha <= arg->alpha_min) - return 2; + // max iteration number reached + if(kk == arg->iter_max) { + if (arg->print_level > 0) + printf(" \n -> ERROR: maximum number of iterations reached, exiting.\n\n"); + return 1; + } + + // min step lenght + if(cws->alpha <= arg->alpha_min) { + if (arg->print_level > 0) + printf(" \n -> ERROR: minimum step size reached, exiting.\n\n"); + return 2; + } + + // NaN in the solution +#ifdef USE_C99_MATH + if(isnan(cws->mu)) { + if (arg->print_level > 0) + printf(" \n -> ERROR: NaN detected, exiting.\n\n"); + return 3; + } +#else + if(cws->mu != cws->mu) { + if (arg->print_level > 0) + printf(" \n -> ERROR: NaN detected, exiting.\n\n"); + return 3; + } +#endif - // NaN in the solution - #ifdef USE_C99_MATH - if(isnan(cws->mu)) - return 3; - #else - if(cws->mu != cws->mu) - return 3; - #endif - - // normal return - return 0; + // normal return + if (arg->print_level > 0) + printf(" \n -> SOLUTION FOUND. \n\n"); + return 0; } @@ -1044,6 +1062,15 @@ blasfeo_print_tran_dvec(cws->nc, ws->sol_step->t, 0); VECNRM_INF(cws->nc, &str_res_d, 0, &qp_res[2]); VECNRM_INF(cws->nc, &str_res_m, 0, &qp_res[3]); + if(arg->print_level > 0){ + if(kk%10 == 0){ + printf("=======================================================================================================\n"); + printf("it. # res_g res_b res_d res_m alpha mu \n"); + printf("=======================================================================================================\n"); + } + printf("%-10d %-15e %-15e %-15e %-15e %-15e %-15e\n", kk, qp_res[0], qp_res[1], qp_res[2], qp_res[3], cws->alpha, cws->mu); + } + #if 0 printf("%e %e %e\n", cws->alpha, cws->alpha_prim, cws->alpha_dual); #endif @@ -1064,25 +1091,39 @@ printf("iter %3d alpha %1.3e %1.3e sigma %1.3e ndp %3d %3d itref %d %d ws->iter = kk; - // max iteration number reached - if(kk == arg->iter_max) - return 1; - - // min step lenght - if(cws->alpha <= arg->alpha_min) - return 2; - - // NaN in the solution + // max iteration number reached + if(kk == arg->iter_max) { + if (arg->print_level > 0) + printf(" \n -> ERROR: maximum number of iterations reached, exiting.\n\n"); + return 1; + } + + // min step lenght + if(cws->alpha <= arg->alpha_min) { + if (arg->print_level > 0) + printf(" \n -> ERROR: minimum step size reached, exiting.\n\n"); + return 2; + } + + // NaN in the solution #ifdef USE_C99_MATH - if(isnan(cws->mu)) - return 3; + if(isnan(cws->mu)) { + if (arg->print_level > 0) + printf(" \n -> ERROR: NaN detected, exiting.\n\n"); + return 3; + } #else - if(cws->mu != cws->mu) - return 3; + if(cws->mu != cws->mu) { + if (arg->print_level > 0) + printf(" \n -> ERROR: NaN detected, exiting.\n\n"); + return 3; + } #endif - // normal return - return 0; + // normal return + if (arg->print_level > 0) + printf(" \n -> SOLUTION FOUND. \n\n"); + return 0; } diff --git a/examples/c/getting_started.c b/examples/c/getting_started.c index bf41ac5c..c26c9393 100644 --- a/examples/c/getting_started.c +++ b/examples/c/getting_started.c @@ -114,11 +114,14 @@ int main() { struct timeval tv0, tv1; + arg.print_level = 1; + gettimeofday(&tv0, NULL); // start for(rep=0; rep QP solved! Solution:\n\n"); + printf("\n -> QP solved! \n\n"); + printf("optimal solution:\n"); printf("\nu\n"); for(ii=0; ii<=N; ii++) d_print_mat(1, nu[ii], u[ii], 1); diff --git a/ocp_qp/x_ocp_qp_ipm.c b/ocp_qp/x_ocp_qp_ipm.c index 145159fe..e6dc488f 100644 --- a/ocp_qp/x_ocp_qp_ipm.c +++ b/ocp_qp/x_ocp_qp_ipm.c @@ -50,6 +50,10 @@ void CREATE_OCP_QP_IPM_ARG(struct OCP_QP_DIM *dim, struct OCP_QP_IPM_ARG *arg, v void SET_DEFAULT_OCP_QP_IPM_ARG(enum HPIPM_MODE mode, struct OCP_QP_IPM_ARG *arg) { + // set common default arguments + arg->print_level = 0; + + // set mode-specific default arguments if(mode==SPEED_ABS) { arg->mu0 = 1e1; @@ -908,25 +912,39 @@ int SOLVE_OCP_QP_IPM(struct OCP_QP *qp, struct OCP_QP_SOL *qp_sol, struct OCP_QP ws->iter = kk; - // max iteration number reached - if(kk == arg->iter_max) - return 1; - - // min step lenght - if(cws->alpha <= arg->alpha_min) - return 2; - - // NaN in the solution - #ifdef USE_C99_MATH - if(isnan(cws->mu)) - return 3; - #else - if(cws->mu != cws->mu) - return 3; - #endif + // max iteration number reached + if(kk == arg->iter_max) { + if (arg->print_level > 0) + printf(" \n -> ERROR: maximum number of iterations reached, exiting.\n\n"); + return 1; + } + + // min step lenght + if(cws->alpha <= arg->alpha_min) { + if (arg->print_level > 0) + printf(" \n -> ERROR: minimum step size reached, exiting.\n\n"); + return 2; + } + + // NaN in the solution +#ifdef USE_C99_MATH + if(isnan(cws->mu)) { + if (arg->print_level > 0) + printf(" \n -> ERROR: NaN detected, exiting.\n\n"); + return 3; + } +#else + if(cws->mu != cws->mu) { + if (arg->print_level > 0) + printf(" \n -> ERROR: NaN detected, exiting.\n\n"); + return 3; + } +#endif - // normal return - return 0; + // normal return + if (arg->print_level > 0) + printf(" \n -> SOLUTION FOUND. \n\n"); + return 0; } @@ -943,7 +961,7 @@ int SOLVE_OCP_QP_IPM(struct OCP_QP *qp, struct OCP_QP_SOL *qp_sol, struct OCP_QP VECNRM_INF(cws->nc, &str_res_d, 0, &qp_res[2]); VECNRM_INF(cws->nc, &str_res_m, 0, &qp_res[3]); -//printf("\niter %d\t%e\t%e\t%e\t%e\n", -1, qp_res[0], qp_res[1], qp_res[2], qp_res[3]); + // printf("\niter %d\t%e\t%e\t%e\t%e\n", -1, qp_res[0], qp_res[1], qp_res[2], qp_res[3]); REAL itref_qp_norm[4] = {0,0,0,0}; REAL itref_qp_norm0[4] = {0,0,0,0}; @@ -1354,8 +1372,14 @@ exit(1); VECNRM_INF(cws->nc, &str_res_d, 0, &qp_res[2]); VECNRM_INF(cws->nc, &str_res_m, 0, &qp_res[3]); -//printf("\niter %d\t%e\t%e\t%e\t%e\n", kk, qp_res[0], qp_res[1], qp_res[2], qp_res[3]); - + if(arg->print_level > 0){ + if(kk%10 == 0){ + printf("=======================================================================================================\n"); + printf("it. # res_g res_b res_d res_m alpha mu \n"); + printf("=======================================================================================================\n"); + } + printf("%-10d %-15e %-15e %-15e %-15e %-15e %-15e\n", kk, qp_res[0], qp_res[1], qp_res[2], qp_res[3], cws->alpha, cws->mu); + } } ws->iter = kk; @@ -1367,23 +1391,37 @@ exit(1); #endif // max iteration number reached - if(kk == arg->iter_max) + if(kk == arg->iter_max) { + if (arg->print_level > 0) + printf(" \n -> ERROR: maximum number of iterations reached, exiting.\n\n"); return 1; + } // min step lenght - if(cws->alpha <= arg->alpha_min) + if(cws->alpha <= arg->alpha_min) { + if (arg->print_level > 0) + printf(" \n -> ERROR: minimum step size reached, exiting.\n\n"); return 2; + } // NaN in the solution #ifdef USE_C99_MATH - if(isnan(cws->mu)) + if(isnan(cws->mu)) { + if (arg->print_level > 0) + printf(" \n -> ERROR: NaN detected, exiting.\n\n"); return 3; + } #else - if(cws->mu != cws->mu) + if(cws->mu != cws->mu) { + if (arg->print_level > 0) + printf(" \n -> ERROR: NaN detected, exiting.\n\n"); return 3; + } #endif // normal return + if (arg->print_level > 0) + printf(" \n -> SOLUTION FOUND. \n\n"); return 0; } From 879900e4e5c1750f3ffba8498b3b238c04400a28 Mon Sep 17 00:00:00 2001 From: zanellia Date: Mon, 30 Jul 2018 10:30:54 +0200 Subject: [PATCH 06/16] adding printing for absolute formulation --- ocp_qp/x_ocp_qp_ipm.c | 29 +++++++++++++++++++++-------- 1 file changed, 21 insertions(+), 8 deletions(-) diff --git a/ocp_qp/x_ocp_qp_ipm.c b/ocp_qp/x_ocp_qp_ipm.c index 23ad15fa..4c1a7447 100644 --- a/ocp_qp/x_ocp_qp_ipm.c +++ b/ocp_qp/x_ocp_qp_ipm.c @@ -51,7 +51,7 @@ void SET_DEFAULT_OCP_QP_IPM_ARG(enum HPIPM_MODE mode, struct OCP_QP_IPM_ARG *arg { // set common default arguments - arg->print_level = 0; + arg->print_level = 1; // set mode-specific default arguments if(mode==SPEED_ABS) @@ -908,6 +908,17 @@ int SOLVE_OCP_QP_IPM(struct OCP_QP *qp, struct OCP_QP_SOL *qp_sol, struct OCP_QP VECNRM_INF(cws->ne, &str_res_b, 0, &qp_res[1]); VECNRM_INF(cws->nc, &str_res_d, 0, &qp_res[2]); VECNRM_INF(cws->nc, &str_res_m, 0, &qp_res[3]); + + if(arg->print_level > 0) + { + if(kk%10 == 0) + { + printf("=======================================================================================================\n"); + printf("it. # res_g res_b res_d res_m alpha mu \n"); + printf("=======================================================================================================\n"); + } + printf("%-10d %-15e %-15e %-15e %-15e %-15e %-15e\n", kk, qp_res[0], qp_res[1], qp_res[2], qp_res[3], cws->alpha, cws->mu); + } } ws->iter = kk; @@ -1372,14 +1383,16 @@ exit(1); VECNRM_INF(cws->nc, &str_res_d, 0, &qp_res[2]); VECNRM_INF(cws->nc, &str_res_m, 0, &qp_res[3]); - if(arg->print_level > 0){ - if(kk%10 == 0){ - printf("=======================================================================================================\n"); - printf("it. # res_g res_b res_d res_m alpha mu \n"); - printf("=======================================================================================================\n"); + if(arg->print_level > 0) + { + if(kk%10 == 0) + { + printf("=======================================================================================================\n"); + printf("it. # res_g res_b res_d res_m alpha mu \n"); + printf("=======================================================================================================\n"); + } + printf("%-10d %-15e %-15e %-15e %-15e %-15e %-15e\n", kk, qp_res[0], qp_res[1], qp_res[2], qp_res[3], cws->alpha, cws->mu); } - printf("%-10d %-15e %-15e %-15e %-15e %-15e %-15e\n", kk, qp_res[0], qp_res[1], qp_res[2], qp_res[3], cws->alpha, cws->mu); - } } ws->iter = kk; From b3113e681d77212e8c6a003882f24b0112f13d1c Mon Sep 17 00:00:00 2001 From: zanellia Date: Mon, 30 Jul 2018 10:32:56 +0200 Subject: [PATCH 07/16] added printing for dense abs --- dense_qp/x_dense_qp_ipm.c | 10 ++++++++++ 1 file changed, 10 insertions(+) diff --git a/dense_qp/x_dense_qp_ipm.c b/dense_qp/x_dense_qp_ipm.c index 4577cfca..ce9c4e01 100644 --- a/dense_qp/x_dense_qp_ipm.c +++ b/dense_qp/x_dense_qp_ipm.c @@ -687,6 +687,16 @@ int SOLVE_DENSE_QP_IPM(struct DENSE_QP *qp, struct DENSE_QP_SOL *qp_sol, struct VECNRM_INF(cws->ne, &str_res_b, 0, &qp_res[1]); VECNRM_INF(cws->nc, &str_res_d, 0, &qp_res[2]); VECNRM_INF(cws->nc, &str_res_m, 0, &qp_res[3]); + if(arg->print_level > 0) + { + if(kk%10 == 0) + { + printf("=======================================================================================================\n"); + printf("it. # res_g res_b res_d res_m alpha mu \n"); + printf("=======================================================================================================\n"); + } + printf("%-10d %-15e %-15e %-15e %-15e %-15e %-15e\n", kk, qp_res[0], qp_res[1], qp_res[2], qp_res[3], cws->alpha, cws->mu); + } } ws->iter = kk; From a8a10af727fb3e6b3e2d91413b5397a1dd4c2a85 Mon Sep 17 00:00:00 2001 From: zanellia Date: Mon, 30 Jul 2018 10:40:45 +0200 Subject: [PATCH 08/16] retabbing x_ocp_qp_ipm.c --- ocp_qp/x_ocp_qp_ipm.c | 2246 ++++++++++++++++++++--------------------- 1 file changed, 1123 insertions(+), 1123 deletions(-) diff --git a/ocp_qp/x_ocp_qp_ipm.c b/ocp_qp/x_ocp_qp_ipm.c index 4c1a7447..a95da306 100644 --- a/ocp_qp/x_ocp_qp_ipm.c +++ b/ocp_qp/x_ocp_qp_ipm.c @@ -28,886 +28,886 @@ int MEMSIZE_OCP_QP_IPM_ARG(struct OCP_QP_DIM *dim) - { + { - return 0; + return 0; - } + } void CREATE_OCP_QP_IPM_ARG(struct OCP_QP_DIM *dim, struct OCP_QP_IPM_ARG *arg, void *mem) - { + { - arg->memsize = 0; + arg->memsize = 0; - return; + return; - } + } void SET_DEFAULT_OCP_QP_IPM_ARG(enum HPIPM_MODE mode, struct OCP_QP_IPM_ARG *arg) - { + { // set common default arguments arg->print_level = 1; // set mode-specific default arguments - if(mode==SPEED_ABS) - { - arg->mu0 = 1e1; - arg->alpha_min = 1e-12; - arg->res_g_max = 1e0; // not used - arg->res_b_max = 1e0; // not used - arg->res_d_max = 1e0; // not used - arg->res_m_max = 1e-8; - arg->iter_max = 15; - arg->stat_max = 15; - arg->pred_corr = 1; - arg->cond_pred_corr = 0; // not used - arg->itref_pred_max = 0; // not used - arg->itref_corr_max = 0; // not used - arg->reg_prim = 1e-15; - arg->lq_fact = 0; // not used - arg->lam_min = 1e-30; - arg->t_min = 1e-30; - arg->warm_start = 0; - arg->abs_form = 1; - arg->comp_dual_sol = 0; - arg->comp_res_exit = 0; - } - else if(mode==SPEED) - { - arg->mu0 = 1e1; - arg->alpha_min = 1e-12; - arg->res_g_max = 1e-6; - arg->res_b_max = 1e-8; - arg->res_d_max = 1e-8; - arg->res_m_max = 1e-8; - arg->iter_max = 15; - arg->stat_max = 15; - arg->pred_corr = 1; - arg->cond_pred_corr = 1; - arg->itref_pred_max = 0; - arg->itref_corr_max = 0; - arg->reg_prim = 1e-15; - arg->lq_fact = 0; - arg->lam_min = 1e-30; - arg->t_min = 1e-30; - arg->warm_start = 0; - arg->abs_form = 0; - arg->comp_dual_sol = 1; - arg->comp_res_exit = 1; - } - else if(mode==BALANCE) - { - arg->mu0 = 1e1; - arg->alpha_min = 1e-12; - arg->res_g_max = 1e-6; - arg->res_b_max = 1e-8; - arg->res_d_max = 1e-8; - arg->res_m_max = 1e-8; - arg->iter_max = 30; - arg->stat_max = 30; - arg->pred_corr = 1; - arg->cond_pred_corr = 1; - arg->itref_pred_max = 0; - arg->itref_corr_max = 2; - arg->reg_prim = 1e-15; - arg->lq_fact = 1; - arg->lam_min = 1e-30; - arg->t_min = 1e-30; - arg->warm_start = 0; - arg->abs_form = 0; - arg->comp_dual_sol = 1; - arg->comp_res_exit = 1; - } - else if(mode==ROBUST) - { - arg->mu0 = 1e2; - arg->alpha_min = 1e-12; - arg->res_g_max = 1e-6; - arg->res_b_max = 1e-8; - arg->res_d_max = 1e-8; - arg->res_m_max = 1e-8; - arg->iter_max = 100; - arg->stat_max = 100; - arg->pred_corr = 1; - arg->cond_pred_corr = 1; - arg->itref_pred_max = 0; - arg->itref_corr_max = 4; - arg->reg_prim = 1e-15; - arg->lq_fact = 2; - arg->lam_min = 1e-30; - arg->t_min = 1e-30; - arg->warm_start = 0; - arg->abs_form = 0; - arg->comp_dual_sol = 1; - arg->comp_res_exit = 1; - } - else - { - printf("\nwrong set default mode\n"); - exit(1); - } - - return; - - } + if(mode==SPEED_ABS) + { + arg->mu0 = 1e1; + arg->alpha_min = 1e-12; + arg->res_g_max = 1e0; // not used + arg->res_b_max = 1e0; // not used + arg->res_d_max = 1e0; // not used + arg->res_m_max = 1e-8; + arg->iter_max = 15; + arg->stat_max = 15; + arg->pred_corr = 1; + arg->cond_pred_corr = 0; // not used + arg->itref_pred_max = 0; // not used + arg->itref_corr_max = 0; // not used + arg->reg_prim = 1e-15; + arg->lq_fact = 0; // not used + arg->lam_min = 1e-30; + arg->t_min = 1e-30; + arg->warm_start = 0; + arg->abs_form = 1; + arg->comp_dual_sol = 0; + arg->comp_res_exit = 0; + } + else if(mode==SPEED) + { + arg->mu0 = 1e1; + arg->alpha_min = 1e-12; + arg->res_g_max = 1e-6; + arg->res_b_max = 1e-8; + arg->res_d_max = 1e-8; + arg->res_m_max = 1e-8; + arg->iter_max = 15; + arg->stat_max = 15; + arg->pred_corr = 1; + arg->cond_pred_corr = 1; + arg->itref_pred_max = 0; + arg->itref_corr_max = 0; + arg->reg_prim = 1e-15; + arg->lq_fact = 0; + arg->lam_min = 1e-30; + arg->t_min = 1e-30; + arg->warm_start = 0; + arg->abs_form = 0; + arg->comp_dual_sol = 1; + arg->comp_res_exit = 1; + } + else if(mode==BALANCE) + { + arg->mu0 = 1e1; + arg->alpha_min = 1e-12; + arg->res_g_max = 1e-6; + arg->res_b_max = 1e-8; + arg->res_d_max = 1e-8; + arg->res_m_max = 1e-8; + arg->iter_max = 30; + arg->stat_max = 30; + arg->pred_corr = 1; + arg->cond_pred_corr = 1; + arg->itref_pred_max = 0; + arg->itref_corr_max = 2; + arg->reg_prim = 1e-15; + arg->lq_fact = 1; + arg->lam_min = 1e-30; + arg->t_min = 1e-30; + arg->warm_start = 0; + arg->abs_form = 0; + arg->comp_dual_sol = 1; + arg->comp_res_exit = 1; + } + else if(mode==ROBUST) + { + arg->mu0 = 1e2; + arg->alpha_min = 1e-12; + arg->res_g_max = 1e-6; + arg->res_b_max = 1e-8; + arg->res_d_max = 1e-8; + arg->res_m_max = 1e-8; + arg->iter_max = 100; + arg->stat_max = 100; + arg->pred_corr = 1; + arg->cond_pred_corr = 1; + arg->itref_pred_max = 0; + arg->itref_corr_max = 4; + arg->reg_prim = 1e-15; + arg->lq_fact = 2; + arg->lam_min = 1e-30; + arg->t_min = 1e-30; + arg->warm_start = 0; + arg->abs_form = 0; + arg->comp_dual_sol = 1; + arg->comp_res_exit = 1; + } + else + { + printf("\nwrong set default mode\n"); + exit(1); + } + + return; + + } int MEMSIZE_OCP_QP_IPM(struct OCP_QP_DIM *dim, struct OCP_QP_IPM_ARG *arg) - { - - // loop index - int ii; - - // extract ocp qp size - int N = dim->N; - int *nx = dim->nx; - int *nu = dim->nu; - int *nb = dim->nb; - int *ng = dim->ng; - int *ns = dim->ns; - - // compute core qp size and max size - int nvt = 0; - int net = 0; - int nct = 0; - int nxM = 0; - int nuM = 0; - int nbM = 0; - int ngM = 0; - int nsM = 0; - for(ii=0; iinxM ? nx[ii] : nxM; - nuM = nu[ii]>nuM ? nu[ii] : nuM; - nbM = nb[ii]>nbM ? nb[ii] : nbM; - ngM = ng[ii]>ngM ? ng[ii] : ngM; - nsM = ns[ii]>nsM ? ns[ii] : nsM; - } - nvt += nx[ii]+nu[ii]+2*ns[ii]; - nct += 2*nb[ii]+2*ng[ii]+2*ns[ii]; - nxM = nx[ii]>nxM ? nx[ii] : nxM; - nuM = nu[ii]>nuM ? nu[ii] : nuM; - nbM = nb[ii]>nbM ? nb[ii] : nbM; - ngM = ng[ii]>ngM ? ng[ii] : ngM; - nsM = ns[ii]>nsM ? ns[ii] : nsM; - - int size = 0; - - size += 1*sizeof(struct CORE_QP_IPM_WORKSPACE); - size += 1*MEMSIZE_CORE_QP_IPM(nvt, net, nct); - - size += 1*sizeof(struct OCP_QP_RES_WORKSPACE); // res_workspace - - size += 2*sizeof(struct OCP_QP); // qp_step qp_itref - - size += 2*sizeof(struct OCP_QP_SOL); // sol_step sol_itref - size += 1*MEMSIZE_OCP_QP_SOL(dim); // sol_itref - - size += 2*sizeof(struct OCP_QP_RES); // res res_itref - size += 1*MEMSIZE_OCP_QP_RES(dim); // res_itref - - size += 9*(N+1)*sizeof(struct STRVEC); // res_g res_d res_m Gamma gamma Zs_inv sol_step(v,lam,t) - size += 3*N*sizeof(struct STRVEC); // res_b Pb sol_step(pi) - size += 10*sizeof(struct STRVEC); // tmp_nxM (4+2)*tmp_nbgM (1+1)*tmp_nsM tmp_m - - size += 1*(N+1)*sizeof(struct STRMAT); // L - if(arg->lq_fact>0) - size += 1*(N+1)*sizeof(struct STRMAT); // Lh - size += 2*sizeof(struct STRMAT); // AL - if(arg->lq_fact>0) - size += 1*sizeof(struct STRMAT); // lq0 - - size += 1*SIZE_STRVEC(nxM); // tmp_nxM - size += 4*SIZE_STRVEC(nbM+ngM); // tmp_nbgM - size += 1*SIZE_STRVEC(nsM); // tmp_nsM - for(ii=0; iilq_fact>0) - for(ii=0; ii<=N; ii++) size += 2*SIZE_STRMAT(nu[ii]+nx[ii]+1, nu[ii]+nx[ii]); // Lh - size += 2*SIZE_STRMAT(nuM+nxM+1, nxM+ngM); // AL - if(arg->lq_fact>0) - size += 1*SIZE_STRMAT(nuM+nxM, 2*nuM+3*nxM+ngM); // lq0 - size += 1*SIZE_STRVEC(nct); // tmp_m - - if(arg->lq_fact>0) - size += 1*GELQF_WORKSIZE(nuM+nxM, 2*nuM+3*nxM+ngM); // lq_work0 - - size += 5*arg->stat_max*sizeof(REAL); - - size += (N+1)*sizeof(int); // use_hess_fact - - size = (size+63)/64*64; // make multiple of typical cache line size - size += 1*64; // align once to typical cache line size - - return size; - - } + { + + // loop index + int ii; + + // extract ocp qp size + int N = dim->N; + int *nx = dim->nx; + int *nu = dim->nu; + int *nb = dim->nb; + int *ng = dim->ng; + int *ns = dim->ns; + + // compute core qp size and max size + int nvt = 0; + int net = 0; + int nct = 0; + int nxM = 0; + int nuM = 0; + int nbM = 0; + int ngM = 0; + int nsM = 0; + for(ii=0; iinxM ? nx[ii] : nxM; + nuM = nu[ii]>nuM ? nu[ii] : nuM; + nbM = nb[ii]>nbM ? nb[ii] : nbM; + ngM = ng[ii]>ngM ? ng[ii] : ngM; + nsM = ns[ii]>nsM ? ns[ii] : nsM; + } + nvt += nx[ii]+nu[ii]+2*ns[ii]; + nct += 2*nb[ii]+2*ng[ii]+2*ns[ii]; + nxM = nx[ii]>nxM ? nx[ii] : nxM; + nuM = nu[ii]>nuM ? nu[ii] : nuM; + nbM = nb[ii]>nbM ? nb[ii] : nbM; + ngM = ng[ii]>ngM ? ng[ii] : ngM; + nsM = ns[ii]>nsM ? ns[ii] : nsM; + + int size = 0; + + size += 1*sizeof(struct CORE_QP_IPM_WORKSPACE); + size += 1*MEMSIZE_CORE_QP_IPM(nvt, net, nct); + + size += 1*sizeof(struct OCP_QP_RES_WORKSPACE); // res_workspace + + size += 2*sizeof(struct OCP_QP); // qp_step qp_itref + + size += 2*sizeof(struct OCP_QP_SOL); // sol_step sol_itref + size += 1*MEMSIZE_OCP_QP_SOL(dim); // sol_itref + + size += 2*sizeof(struct OCP_QP_RES); // res res_itref + size += 1*MEMSIZE_OCP_QP_RES(dim); // res_itref + + size += 9*(N+1)*sizeof(struct STRVEC); // res_g res_d res_m Gamma gamma Zs_inv sol_step(v,lam,t) + size += 3*N*sizeof(struct STRVEC); // res_b Pb sol_step(pi) + size += 10*sizeof(struct STRVEC); // tmp_nxM (4+2)*tmp_nbgM (1+1)*tmp_nsM tmp_m + + size += 1*(N+1)*sizeof(struct STRMAT); // L + if(arg->lq_fact>0) + size += 1*(N+1)*sizeof(struct STRMAT); // Lh + size += 2*sizeof(struct STRMAT); // AL + if(arg->lq_fact>0) + size += 1*sizeof(struct STRMAT); // lq0 + + size += 1*SIZE_STRVEC(nxM); // tmp_nxM + size += 4*SIZE_STRVEC(nbM+ngM); // tmp_nbgM + size += 1*SIZE_STRVEC(nsM); // tmp_nsM + for(ii=0; iilq_fact>0) + for(ii=0; ii<=N; ii++) size += 2*SIZE_STRMAT(nu[ii]+nx[ii]+1, nu[ii]+nx[ii]); // Lh + size += 2*SIZE_STRMAT(nuM+nxM+1, nxM+ngM); // AL + if(arg->lq_fact>0) + size += 1*SIZE_STRMAT(nuM+nxM, 2*nuM+3*nxM+ngM); // lq0 + size += 1*SIZE_STRVEC(nct); // tmp_m + + if(arg->lq_fact>0) + size += 1*GELQF_WORKSIZE(nuM+nxM, 2*nuM+3*nxM+ngM); // lq_work0 + + size += 5*arg->stat_max*sizeof(REAL); + + size += (N+1)*sizeof(int); // use_hess_fact + + size = (size+63)/64*64; // make multiple of typical cache line size + size += 1*64; // align once to typical cache line size + + return size; + + } void CREATE_OCP_QP_IPM(struct OCP_QP_DIM *dim, struct OCP_QP_IPM_ARG *arg, struct OCP_QP_IPM_WORKSPACE *workspace, void *mem) - { - - // loop index - int ii; - - // extract ocp qp size - int N = dim->N; - int *nx = dim->nx; - int *nu = dim->nu; - int *nb = dim->nb; - int *ng = dim->ng; - int *ns = dim->ns; - - - // compute core qp size and max size - int nvt = 0; - int net = 0; - int nct = 0; - int nxM = 0; - int nuM = 0; - int nbM = 0; - int ngM = 0; - int nsM = 0; - for(ii=0; iinxM ? nx[ii] : nxM; - nuM = nu[ii]>nuM ? nu[ii] : nuM; - nbM = nb[ii]>nbM ? nb[ii] : nbM; - ngM = ng[ii]>ngM ? ng[ii] : ngM; - nsM = ns[ii]>nsM ? ns[ii] : nsM; - } - nvt += nx[ii]+nu[ii]+2*ns[ii]; - nct += 2*nb[ii]+2*ng[ii]+2*ns[ii]; - nxM = nx[ii]>nxM ? nx[ii] : nxM; - nuM = nu[ii]>nuM ? nu[ii] : nuM; - nbM = nb[ii]>nbM ? nb[ii] : nbM; - ngM = ng[ii]>ngM ? ng[ii] : ngM; - nsM = ns[ii]>nsM ? ns[ii] : nsM; - - - // core struct - struct CORE_QP_IPM_WORKSPACE *sr_ptr = mem; - - // core workspace - workspace->core_workspace = sr_ptr; - sr_ptr += 1; - struct CORE_QP_IPM_WORKSPACE *cws = workspace->core_workspace; - - - // res struct - struct OCP_QP_RES *res_ptr = (struct OCP_QP_RES *) sr_ptr; - workspace->res = res_ptr; - res_ptr += 1; - workspace->res_itref = res_ptr; - res_ptr += 1; - - - // res workspace struct - struct OCP_QP_RES_WORKSPACE *res_ws_ptr = (struct OCP_QP_RES_WORKSPACE *) res_ptr; - workspace->res_workspace = res_ws_ptr; - res_ws_ptr += 1; - - - // qp sol struct - struct OCP_QP_SOL *qp_sol_ptr = (struct OCP_QP_SOL *) res_ws_ptr; - - workspace->sol_step = qp_sol_ptr; - qp_sol_ptr += 1; - workspace->sol_itref = qp_sol_ptr; - qp_sol_ptr += 1; - - - // qp struct - struct OCP_QP *qp_ptr = (struct OCP_QP *) qp_sol_ptr; - - workspace->qp_step = qp_ptr; - qp_ptr += 1; - workspace->qp_itref = qp_ptr; - qp_ptr += 1; - - - // matrix struct - struct STRMAT *sm_ptr = (struct STRMAT *) qp_ptr; - - workspace->L = sm_ptr; - sm_ptr += N+1; - if(arg->lq_fact>0) - { - workspace->Lh = sm_ptr; - sm_ptr += N+1; - } - workspace->AL = sm_ptr; - sm_ptr += 2; - if(arg->lq_fact>0) - { - workspace->lq0 = sm_ptr; - sm_ptr += 1; - } - - - // vector struct - struct STRVEC *sv_ptr = (struct STRVEC *) sm_ptr; - - workspace->sol_step->ux = sv_ptr; - sv_ptr += N+1; - workspace->sol_step->pi = sv_ptr; - sv_ptr += N; - workspace->sol_step->lam = sv_ptr; - sv_ptr += N+1; - workspace->sol_step->t = sv_ptr; - sv_ptr += N+1; - workspace->res->res_g = sv_ptr; - sv_ptr += N+1; - workspace->res->res_b = sv_ptr; - sv_ptr += N; - workspace->res->res_d = sv_ptr; - sv_ptr += N+1; - workspace->res->res_m = sv_ptr; - sv_ptr += N+1; - workspace->Gamma = sv_ptr; - sv_ptr += N+1; - workspace->gamma = sv_ptr; - sv_ptr += N+1; - workspace->Pb = sv_ptr; - sv_ptr += N; - workspace->Zs_inv = sv_ptr; - sv_ptr += N+1; - workspace->tmp_nxM = sv_ptr; - sv_ptr += 1; - workspace->tmp_nbgM = sv_ptr; - sv_ptr += 4; - workspace->res_workspace->tmp_nbgM = sv_ptr; - sv_ptr += 2; - workspace->tmp_nsM = sv_ptr; - sv_ptr += 1; - workspace->res_workspace->tmp_nsM = sv_ptr; - sv_ptr += 1; - workspace->tmp_m = sv_ptr; - sv_ptr += 1; - - - // double/float stuff - REAL *d_ptr = (REAL *) sv_ptr; - - workspace->stat = d_ptr; - d_ptr += 5*arg->stat_max; - - // int stuff - int *i_ptr = (int *) d_ptr; - - workspace->use_hess_fact = i_ptr; - i_ptr += N+1; - - - // align to typicl cache line size - size_t s_ptr = (size_t) i_ptr; - s_ptr = (s_ptr+63)/64*64; - - - // void stuf - char *c_ptr = (char *) s_ptr; - - CREATE_OCP_QP_SOL(dim, workspace->sol_itref, c_ptr); - c_ptr += workspace->sol_itref->memsize; - - CREATE_OCP_QP_RES(dim, workspace->res_itref, c_ptr); - c_ptr += workspace->res_itref->memsize; - - for(ii=0; ii<=N; ii++) - { - CREATE_STRMAT(nu[ii]+nx[ii]+1, nu[ii]+nx[ii], workspace->L+ii, c_ptr); - c_ptr += (workspace->L+ii)->memsize; - } - - if(arg->lq_fact>0) - { - for(ii=0; ii<=N; ii++) - { - CREATE_STRMAT(nu[ii]+nx[ii]+1, nu[ii]+nx[ii], workspace->Lh+ii, c_ptr); - c_ptr += (workspace->Lh+ii)->memsize; - } - } - - CREATE_STRMAT(nuM+nxM+1, nxM+ngM, workspace->AL+0, c_ptr); - c_ptr += (workspace->AL+0)->memsize; - - CREATE_STRMAT(nuM+nxM+1, nxM+ngM, workspace->AL+1, c_ptr); - c_ptr += (workspace->AL+1)->memsize; - - if(arg->lq_fact>0) - { - CREATE_STRMAT(nuM+nxM, 2*nuM+3*nxM+ngM, workspace->lq0, c_ptr); - c_ptr += (workspace->lq0)->memsize; - } - - for(ii=0; iiPb+ii, c_ptr); - c_ptr += (workspace->Pb+ii)->memsize; - } - - for(ii=0; iiZs_inv+ii, c_ptr); - c_ptr += (workspace->Zs_inv+ii)->memsize; - } - - CREATE_STRVEC(nxM, workspace->tmp_nxM, c_ptr); - c_ptr += workspace->tmp_nxM->memsize; - - CREATE_STRVEC(nbM+ngM, workspace->tmp_nbgM+0, c_ptr); - CREATE_STRVEC(nbM+ngM, workspace->res_workspace->tmp_nbgM+0, c_ptr); - c_ptr += (workspace->tmp_nbgM+0)->memsize; - - CREATE_STRVEC(nbM+ngM, workspace->tmp_nbgM+1, c_ptr); - CREATE_STRVEC(nbM+ngM, workspace->res_workspace->tmp_nbgM+1, c_ptr); - c_ptr += (workspace->tmp_nbgM+1)->memsize; - - CREATE_STRVEC(nbM+ngM, workspace->tmp_nbgM+2, c_ptr); - c_ptr += (workspace->tmp_nbgM+2)->memsize; - - CREATE_STRVEC(nbM+ngM, workspace->tmp_nbgM+3, c_ptr); - c_ptr += (workspace->tmp_nbgM+3)->memsize; - - CREATE_STRVEC(nsM, workspace->tmp_nsM+0, c_ptr); - CREATE_STRVEC(nsM, workspace->res_workspace->tmp_nsM+0, c_ptr); - c_ptr += (workspace->tmp_nsM+0)->memsize; - - CREATE_STRVEC(nct, workspace->tmp_m, c_ptr); - c_ptr += SIZE_STRVEC(nct); - - CREATE_CORE_QP_IPM(nvt, net, nct, cws, c_ptr); - c_ptr += workspace->core_workspace->memsize; - - if(arg->lq_fact>0) - { - workspace->lq_work0 = c_ptr; - c_ptr += GELQF_WORKSIZE(nuM+nxM, 2*nuM+3*nxM+ngM); - } - - - // alias members of workspace and core_workspace - // - c_ptr = (char *) cws->dv; - for(ii=0; ii<=N; ii++) - { - CREATE_STRVEC(nu[ii]+nx[ii]+2*ns[ii], workspace->sol_step->ux+ii, c_ptr); - c_ptr += (nu[ii]+nx[ii])*sizeof(REAL); - c_ptr += ns[ii]*sizeof(REAL); - c_ptr += ns[ii]*sizeof(REAL); - } - // - c_ptr = (char *) cws->dpi; - for(ii=0; iisol_step->pi+ii, c_ptr); - c_ptr += (nx[ii+1])*sizeof(REAL); - } - // - c_ptr = (char *) cws->dlam; - for(ii=0; ii<=N; ii++) - { - CREATE_STRVEC(2*nb[ii]+2*ng[ii]+2*ns[ii], workspace->sol_step->lam+ii, c_ptr); - c_ptr += nb[ii]*sizeof(REAL); - c_ptr += ng[ii]*sizeof(REAL); - c_ptr += nb[ii]*sizeof(REAL); - c_ptr += ng[ii]*sizeof(REAL); - c_ptr += ns[ii]*sizeof(REAL); - c_ptr += ns[ii]*sizeof(REAL); - } - // - c_ptr = (char *) cws->dt; - for(ii=0; ii<=N; ii++) - { - CREATE_STRVEC(2*nb[ii]+2*ng[ii]+2*ns[ii], workspace->sol_step->t+ii, c_ptr); - c_ptr += nb[ii]*sizeof(REAL); - c_ptr += ng[ii]*sizeof(REAL); - c_ptr += nb[ii]*sizeof(REAL); - c_ptr += ng[ii]*sizeof(REAL); - c_ptr += ns[ii]*sizeof(REAL); - c_ptr += ns[ii]*sizeof(REAL); - } - // - c_ptr = (char *) cws->res_g; - for(ii=0; ii<=N; ii++) - { - CREATE_STRVEC(nu[ii]+nx[ii]+2*ns[ii], workspace->res->res_g+ii, c_ptr); - c_ptr += (nu[ii]+nx[ii])*sizeof(REAL); - c_ptr += ns[ii]*sizeof(REAL); - c_ptr += ns[ii]*sizeof(REAL); - } - // - c_ptr = (char *) cws->res_b; - for(ii=0; iires->res_b+ii, c_ptr); - c_ptr += (nx[ii+1])*sizeof(REAL); - } - // - c_ptr = (char *) cws->res_d; - for(ii=0; ii<=N; ii++) - { - CREATE_STRVEC(2*nb[ii]+2*ng[ii]+2*ns[ii], workspace->res->res_d+ii, c_ptr); - c_ptr += nb[ii]*sizeof(REAL); - c_ptr += ng[ii]*sizeof(REAL); - c_ptr += nb[ii]*sizeof(REAL); - c_ptr += ng[ii]*sizeof(REAL); - c_ptr += ns[ii]*sizeof(REAL); - c_ptr += ns[ii]*sizeof(REAL); - } - // - c_ptr = (char *) cws->res_m; - for(ii=0; ii<=N; ii++) - { - CREATE_STRVEC(2*nb[ii]+2*ng[ii]+2*ns[ii], workspace->res->res_m+ii, c_ptr); - c_ptr += nb[ii]*sizeof(REAL); - c_ptr += ng[ii]*sizeof(REAL); - c_ptr += nb[ii]*sizeof(REAL); - c_ptr += ng[ii]*sizeof(REAL); - c_ptr += ns[ii]*sizeof(REAL); - c_ptr += ns[ii]*sizeof(REAL); - } - // - c_ptr = (char *) cws->Gamma; - for(ii=0; ii<=N; ii++) - { - CREATE_STRVEC(2*nb[ii]+2*ng[ii]+2*ns[ii], workspace->Gamma+ii, c_ptr); - c_ptr += nb[ii]*sizeof(REAL); - c_ptr += ng[ii]*sizeof(REAL); - c_ptr += nb[ii]*sizeof(REAL); - c_ptr += ng[ii]*sizeof(REAL); - c_ptr += ns[ii]*sizeof(REAL); - c_ptr += ns[ii]*sizeof(REAL); - } - // - c_ptr = (char *) cws->gamma; - for(ii=0; ii<=N; ii++) - { - CREATE_STRVEC(2*nb[ii]+2*ng[ii]+2*ns[ii], workspace->gamma+ii, c_ptr); - c_ptr += nb[ii]*sizeof(REAL); - c_ptr += ng[ii]*sizeof(REAL); - c_ptr += nb[ii]*sizeof(REAL); - c_ptr += ng[ii]*sizeof(REAL); - c_ptr += ns[ii]*sizeof(REAL); - c_ptr += ns[ii]*sizeof(REAL); - } - - - workspace->res->dim = dim; - - workspace->stat_max = arg->stat_max; - - for(ii=0; ii<=N; ii++) - workspace->use_hess_fact[ii] = 0; - - workspace->use_Pb = 0; - - workspace->memsize = MEMSIZE_OCP_QP_IPM(dim, arg); + { + + // loop index + int ii; + + // extract ocp qp size + int N = dim->N; + int *nx = dim->nx; + int *nu = dim->nu; + int *nb = dim->nb; + int *ng = dim->ng; + int *ns = dim->ns; + + + // compute core qp size and max size + int nvt = 0; + int net = 0; + int nct = 0; + int nxM = 0; + int nuM = 0; + int nbM = 0; + int ngM = 0; + int nsM = 0; + for(ii=0; iinxM ? nx[ii] : nxM; + nuM = nu[ii]>nuM ? nu[ii] : nuM; + nbM = nb[ii]>nbM ? nb[ii] : nbM; + ngM = ng[ii]>ngM ? ng[ii] : ngM; + nsM = ns[ii]>nsM ? ns[ii] : nsM; + } + nvt += nx[ii]+nu[ii]+2*ns[ii]; + nct += 2*nb[ii]+2*ng[ii]+2*ns[ii]; + nxM = nx[ii]>nxM ? nx[ii] : nxM; + nuM = nu[ii]>nuM ? nu[ii] : nuM; + nbM = nb[ii]>nbM ? nb[ii] : nbM; + ngM = ng[ii]>ngM ? ng[ii] : ngM; + nsM = ns[ii]>nsM ? ns[ii] : nsM; + + + // core struct + struct CORE_QP_IPM_WORKSPACE *sr_ptr = mem; + + // core workspace + workspace->core_workspace = sr_ptr; + sr_ptr += 1; + struct CORE_QP_IPM_WORKSPACE *cws = workspace->core_workspace; + + + // res struct + struct OCP_QP_RES *res_ptr = (struct OCP_QP_RES *) sr_ptr; + workspace->res = res_ptr; + res_ptr += 1; + workspace->res_itref = res_ptr; + res_ptr += 1; + + + // res workspace struct + struct OCP_QP_RES_WORKSPACE *res_ws_ptr = (struct OCP_QP_RES_WORKSPACE *) res_ptr; + workspace->res_workspace = res_ws_ptr; + res_ws_ptr += 1; + + + // qp sol struct + struct OCP_QP_SOL *qp_sol_ptr = (struct OCP_QP_SOL *) res_ws_ptr; + + workspace->sol_step = qp_sol_ptr; + qp_sol_ptr += 1; + workspace->sol_itref = qp_sol_ptr; + qp_sol_ptr += 1; + + + // qp struct + struct OCP_QP *qp_ptr = (struct OCP_QP *) qp_sol_ptr; + + workspace->qp_step = qp_ptr; + qp_ptr += 1; + workspace->qp_itref = qp_ptr; + qp_ptr += 1; + + + // matrix struct + struct STRMAT *sm_ptr = (struct STRMAT *) qp_ptr; + + workspace->L = sm_ptr; + sm_ptr += N+1; + if(arg->lq_fact>0) + { + workspace->Lh = sm_ptr; + sm_ptr += N+1; + } + workspace->AL = sm_ptr; + sm_ptr += 2; + if(arg->lq_fact>0) + { + workspace->lq0 = sm_ptr; + sm_ptr += 1; + } + + + // vector struct + struct STRVEC *sv_ptr = (struct STRVEC *) sm_ptr; + + workspace->sol_step->ux = sv_ptr; + sv_ptr += N+1; + workspace->sol_step->pi = sv_ptr; + sv_ptr += N; + workspace->sol_step->lam = sv_ptr; + sv_ptr += N+1; + workspace->sol_step->t = sv_ptr; + sv_ptr += N+1; + workspace->res->res_g = sv_ptr; + sv_ptr += N+1; + workspace->res->res_b = sv_ptr; + sv_ptr += N; + workspace->res->res_d = sv_ptr; + sv_ptr += N+1; + workspace->res->res_m = sv_ptr; + sv_ptr += N+1; + workspace->Gamma = sv_ptr; + sv_ptr += N+1; + workspace->gamma = sv_ptr; + sv_ptr += N+1; + workspace->Pb = sv_ptr; + sv_ptr += N; + workspace->Zs_inv = sv_ptr; + sv_ptr += N+1; + workspace->tmp_nxM = sv_ptr; + sv_ptr += 1; + workspace->tmp_nbgM = sv_ptr; + sv_ptr += 4; + workspace->res_workspace->tmp_nbgM = sv_ptr; + sv_ptr += 2; + workspace->tmp_nsM = sv_ptr; + sv_ptr += 1; + workspace->res_workspace->tmp_nsM = sv_ptr; + sv_ptr += 1; + workspace->tmp_m = sv_ptr; + sv_ptr += 1; + + + // double/float stuff + REAL *d_ptr = (REAL *) sv_ptr; + + workspace->stat = d_ptr; + d_ptr += 5*arg->stat_max; + + // int stuff + int *i_ptr = (int *) d_ptr; + + workspace->use_hess_fact = i_ptr; + i_ptr += N+1; + + + // align to typicl cache line size + size_t s_ptr = (size_t) i_ptr; + s_ptr = (s_ptr+63)/64*64; + + + // void stuf + char *c_ptr = (char *) s_ptr; + + CREATE_OCP_QP_SOL(dim, workspace->sol_itref, c_ptr); + c_ptr += workspace->sol_itref->memsize; + + CREATE_OCP_QP_RES(dim, workspace->res_itref, c_ptr); + c_ptr += workspace->res_itref->memsize; + + for(ii=0; ii<=N; ii++) + { + CREATE_STRMAT(nu[ii]+nx[ii]+1, nu[ii]+nx[ii], workspace->L+ii, c_ptr); + c_ptr += (workspace->L+ii)->memsize; + } + + if(arg->lq_fact>0) + { + for(ii=0; ii<=N; ii++) + { + CREATE_STRMAT(nu[ii]+nx[ii]+1, nu[ii]+nx[ii], workspace->Lh+ii, c_ptr); + c_ptr += (workspace->Lh+ii)->memsize; + } + } + + CREATE_STRMAT(nuM+nxM+1, nxM+ngM, workspace->AL+0, c_ptr); + c_ptr += (workspace->AL+0)->memsize; + + CREATE_STRMAT(nuM+nxM+1, nxM+ngM, workspace->AL+1, c_ptr); + c_ptr += (workspace->AL+1)->memsize; + + if(arg->lq_fact>0) + { + CREATE_STRMAT(nuM+nxM, 2*nuM+3*nxM+ngM, workspace->lq0, c_ptr); + c_ptr += (workspace->lq0)->memsize; + } + + for(ii=0; iiPb+ii, c_ptr); + c_ptr += (workspace->Pb+ii)->memsize; + } + + for(ii=0; iiZs_inv+ii, c_ptr); + c_ptr += (workspace->Zs_inv+ii)->memsize; + } + + CREATE_STRVEC(nxM, workspace->tmp_nxM, c_ptr); + c_ptr += workspace->tmp_nxM->memsize; + + CREATE_STRVEC(nbM+ngM, workspace->tmp_nbgM+0, c_ptr); + CREATE_STRVEC(nbM+ngM, workspace->res_workspace->tmp_nbgM+0, c_ptr); + c_ptr += (workspace->tmp_nbgM+0)->memsize; + + CREATE_STRVEC(nbM+ngM, workspace->tmp_nbgM+1, c_ptr); + CREATE_STRVEC(nbM+ngM, workspace->res_workspace->tmp_nbgM+1, c_ptr); + c_ptr += (workspace->tmp_nbgM+1)->memsize; + + CREATE_STRVEC(nbM+ngM, workspace->tmp_nbgM+2, c_ptr); + c_ptr += (workspace->tmp_nbgM+2)->memsize; + + CREATE_STRVEC(nbM+ngM, workspace->tmp_nbgM+3, c_ptr); + c_ptr += (workspace->tmp_nbgM+3)->memsize; + + CREATE_STRVEC(nsM, workspace->tmp_nsM+0, c_ptr); + CREATE_STRVEC(nsM, workspace->res_workspace->tmp_nsM+0, c_ptr); + c_ptr += (workspace->tmp_nsM+0)->memsize; + + CREATE_STRVEC(nct, workspace->tmp_m, c_ptr); + c_ptr += SIZE_STRVEC(nct); + + CREATE_CORE_QP_IPM(nvt, net, nct, cws, c_ptr); + c_ptr += workspace->core_workspace->memsize; + + if(arg->lq_fact>0) + { + workspace->lq_work0 = c_ptr; + c_ptr += GELQF_WORKSIZE(nuM+nxM, 2*nuM+3*nxM+ngM); + } + + + // alias members of workspace and core_workspace + // + c_ptr = (char *) cws->dv; + for(ii=0; ii<=N; ii++) + { + CREATE_STRVEC(nu[ii]+nx[ii]+2*ns[ii], workspace->sol_step->ux+ii, c_ptr); + c_ptr += (nu[ii]+nx[ii])*sizeof(REAL); + c_ptr += ns[ii]*sizeof(REAL); + c_ptr += ns[ii]*sizeof(REAL); + } + // + c_ptr = (char *) cws->dpi; + for(ii=0; iisol_step->pi+ii, c_ptr); + c_ptr += (nx[ii+1])*sizeof(REAL); + } + // + c_ptr = (char *) cws->dlam; + for(ii=0; ii<=N; ii++) + { + CREATE_STRVEC(2*nb[ii]+2*ng[ii]+2*ns[ii], workspace->sol_step->lam+ii, c_ptr); + c_ptr += nb[ii]*sizeof(REAL); + c_ptr += ng[ii]*sizeof(REAL); + c_ptr += nb[ii]*sizeof(REAL); + c_ptr += ng[ii]*sizeof(REAL); + c_ptr += ns[ii]*sizeof(REAL); + c_ptr += ns[ii]*sizeof(REAL); + } + // + c_ptr = (char *) cws->dt; + for(ii=0; ii<=N; ii++) + { + CREATE_STRVEC(2*nb[ii]+2*ng[ii]+2*ns[ii], workspace->sol_step->t+ii, c_ptr); + c_ptr += nb[ii]*sizeof(REAL); + c_ptr += ng[ii]*sizeof(REAL); + c_ptr += nb[ii]*sizeof(REAL); + c_ptr += ng[ii]*sizeof(REAL); + c_ptr += ns[ii]*sizeof(REAL); + c_ptr += ns[ii]*sizeof(REAL); + } + // + c_ptr = (char *) cws->res_g; + for(ii=0; ii<=N; ii++) + { + CREATE_STRVEC(nu[ii]+nx[ii]+2*ns[ii], workspace->res->res_g+ii, c_ptr); + c_ptr += (nu[ii]+nx[ii])*sizeof(REAL); + c_ptr += ns[ii]*sizeof(REAL); + c_ptr += ns[ii]*sizeof(REAL); + } + // + c_ptr = (char *) cws->res_b; + for(ii=0; iires->res_b+ii, c_ptr); + c_ptr += (nx[ii+1])*sizeof(REAL); + } + // + c_ptr = (char *) cws->res_d; + for(ii=0; ii<=N; ii++) + { + CREATE_STRVEC(2*nb[ii]+2*ng[ii]+2*ns[ii], workspace->res->res_d+ii, c_ptr); + c_ptr += nb[ii]*sizeof(REAL); + c_ptr += ng[ii]*sizeof(REAL); + c_ptr += nb[ii]*sizeof(REAL); + c_ptr += ng[ii]*sizeof(REAL); + c_ptr += ns[ii]*sizeof(REAL); + c_ptr += ns[ii]*sizeof(REAL); + } + // + c_ptr = (char *) cws->res_m; + for(ii=0; ii<=N; ii++) + { + CREATE_STRVEC(2*nb[ii]+2*ng[ii]+2*ns[ii], workspace->res->res_m+ii, c_ptr); + c_ptr += nb[ii]*sizeof(REAL); + c_ptr += ng[ii]*sizeof(REAL); + c_ptr += nb[ii]*sizeof(REAL); + c_ptr += ng[ii]*sizeof(REAL); + c_ptr += ns[ii]*sizeof(REAL); + c_ptr += ns[ii]*sizeof(REAL); + } + // + c_ptr = (char *) cws->Gamma; + for(ii=0; ii<=N; ii++) + { + CREATE_STRVEC(2*nb[ii]+2*ng[ii]+2*ns[ii], workspace->Gamma+ii, c_ptr); + c_ptr += nb[ii]*sizeof(REAL); + c_ptr += ng[ii]*sizeof(REAL); + c_ptr += nb[ii]*sizeof(REAL); + c_ptr += ng[ii]*sizeof(REAL); + c_ptr += ns[ii]*sizeof(REAL); + c_ptr += ns[ii]*sizeof(REAL); + } + // + c_ptr = (char *) cws->gamma; + for(ii=0; ii<=N; ii++) + { + CREATE_STRVEC(2*nb[ii]+2*ng[ii]+2*ns[ii], workspace->gamma+ii, c_ptr); + c_ptr += nb[ii]*sizeof(REAL); + c_ptr += ng[ii]*sizeof(REAL); + c_ptr += nb[ii]*sizeof(REAL); + c_ptr += ng[ii]*sizeof(REAL); + c_ptr += ns[ii]*sizeof(REAL); + c_ptr += ns[ii]*sizeof(REAL); + } + + + workspace->res->dim = dim; + + workspace->stat_max = arg->stat_max; + + for(ii=0; ii<=N; ii++) + workspace->use_hess_fact[ii] = 0; + + workspace->use_Pb = 0; + + workspace->memsize = MEMSIZE_OCP_QP_IPM(dim, arg); #if defined(RUNTIME_CHECKS) - if(c_ptr > ((char *) mem) + workspace->memsize) - { - printf("\nCreate_ocp_qp_ipm: outside memory bounds!\n\n"); - exit(1); - } + if(c_ptr > ((char *) mem) + workspace->memsize) + { + printf("\nCreate_ocp_qp_ipm: outside memory bounds!\n\n"); + exit(1); + } #endif - return; + return; - } + } int SOLVE_OCP_QP_IPM(struct OCP_QP *qp, struct OCP_QP_SOL *qp_sol, struct OCP_QP_IPM_ARG *arg, struct OCP_QP_IPM_WORKSPACE *ws) - { + { #if 0 - int N = qp->dim->N; - int *nx = qp->dim->nx; - int *nu = qp->dim->nu; - int *nb = qp->dim->nb; - int *ng = qp->dim->ng; - int *ns = qp->dim->ns; + int N = qp->dim->N; + int *nx = qp->dim->nx; + int *nu = qp->dim->nu; + int *nb = qp->dim->nb; + int *ng = qp->dim->ng; + int *ns = qp->dim->ns; - int ii; + int ii; #if 1 - printf("\nnx\n"); - int_print_mat(1, N+1, nx, 1); - printf("\nnu\n"); - int_print_mat(1, N+1, nu, 1); - printf("\nnb\n"); - int_print_mat(1, N+1, nb, 1); - printf("\nng\n"); - int_print_mat(1, N+1, ng, 1); - printf("\nns\n"); - int_print_mat(1, N+1, ns, 1); + printf("\nnx\n"); + int_print_mat(1, N+1, nx, 1); + printf("\nnu\n"); + int_print_mat(1, N+1, nu, 1); + printf("\nnb\n"); + int_print_mat(1, N+1, nb, 1); + printf("\nng\n"); + int_print_mat(1, N+1, ng, 1); + printf("\nns\n"); + int_print_mat(1, N+1, ns, 1); #endif - printf("\nBAt\n"); - for(ii=0; iiBAbt+ii, 0, 0); - printf("\nb\n"); - for(ii=0; iib+ii, 0); - printf("\nRSQ\n"); - for(ii=0; ii<=N; ii++) - blasfeo_print_dmat(nu[ii]+nx[ii], nu[ii]+nx[ii], qp->RSQrq+ii, 0, 0); - printf("\nrq\n"); - for(ii=0; ii<=N; ii++) - blasfeo_print_tran_dvec(nu[ii]+nx[ii], qp->rqz+ii, 0); - printf("\nidxb\n"); - for(ii=0; ii<=N; ii++) - int_print_mat(1, nb[ii], qp->idxb[ii], 1); - printf("\nDCt\n"); - for(ii=0; ii<=N; ii++) - blasfeo_print_dmat(nu[ii]+nx[ii], ng[ii], qp->DCt+ii, 0, 0); - printf("\nd\n"); - for(ii=0; ii<=N; ii++) - blasfeo_print_tran_dvec(2*nb[ii]+2*ng[ii]+2*ns[ii], qp->d+ii, 0); + printf("\nBAt\n"); + for(ii=0; iiBAbt+ii, 0, 0); + printf("\nb\n"); + for(ii=0; iib+ii, 0); + printf("\nRSQ\n"); + for(ii=0; ii<=N; ii++) + blasfeo_print_dmat(nu[ii]+nx[ii], nu[ii]+nx[ii], qp->RSQrq+ii, 0, 0); + printf("\nrq\n"); + for(ii=0; ii<=N; ii++) + blasfeo_print_tran_dvec(nu[ii]+nx[ii], qp->rqz+ii, 0); + printf("\nidxb\n"); + for(ii=0; ii<=N; ii++) + int_print_mat(1, nb[ii], qp->idxb[ii], 1); + printf("\nDCt\n"); + for(ii=0; ii<=N; ii++) + blasfeo_print_dmat(nu[ii]+nx[ii], ng[ii], qp->DCt+ii, 0, 0); + printf("\nd\n"); + for(ii=0; ii<=N; ii++) + blasfeo_print_tran_dvec(2*nb[ii]+2*ng[ii]+2*ns[ii], qp->d+ii, 0); #if 0 - printf("\nlb\n"); - for(ii=0; ii<=N; ii++) - blasfeo_print_tran_dvec(nb[ii], qp->d+ii, 0); - printf("\nlg\n"); - for(ii=0; ii<=N; ii++) - blasfeo_print_tran_dvec(ng[ii], qp->d+ii, nb[ii]); - printf("\nub\n"); - for(ii=0; ii<=N; ii++) - blasfeo_print_tran_dvec(nb[ii], qp->d+ii, nb[ii]+ng[ii]); - printf("\nug\n"); - for(ii=0; ii<=N; ii++) - blasfeo_print_tran_dvec(ng[ii], qp->d+ii, 2*nb[ii]+ng[ii]); - printf("\nls\n"); - for(ii=0; ii<=N; ii++) - blasfeo_print_tran_dvec(ns[ii], qp->d+ii, 2*nb[ii]+2*ng[ii]); - printf("\nus\n"); - for(ii=0; ii<=N; ii++) - blasfeo_print_tran_dvec(ns[ii], qp->d+ii, 2*nb[ii]+2*ng[ii]+ns[ii]); - printf("\nidxb\n"); - for(ii=0; ii<=N; ii++) - int_print_mat(1, nb[ii], qp->idxb[ii], 1); - printf("\nidxs\n"); - for(ii=0; ii<=N; ii++) - int_print_mat(1, ns[ii], qp->idxs[ii], 1); - printf("\nZ\n"); - for(ii=0; ii<=N; ii++) - blasfeo_print_tran_dvec(2*ns[ii], qp->Z+ii, 0); - printf("\nrqz\n"); - for(ii=0; ii<=N; ii++) - blasfeo_print_tran_dvec(nu[ii]+nx[ii]+2*ns[ii], qp->rqz+ii, 0); + printf("\nlb\n"); + for(ii=0; ii<=N; ii++) + blasfeo_print_tran_dvec(nb[ii], qp->d+ii, 0); + printf("\nlg\n"); + for(ii=0; ii<=N; ii++) + blasfeo_print_tran_dvec(ng[ii], qp->d+ii, nb[ii]); + printf("\nub\n"); + for(ii=0; ii<=N; ii++) + blasfeo_print_tran_dvec(nb[ii], qp->d+ii, nb[ii]+ng[ii]); + printf("\nug\n"); + for(ii=0; ii<=N; ii++) + blasfeo_print_tran_dvec(ng[ii], qp->d+ii, 2*nb[ii]+ng[ii]); + printf("\nls\n"); + for(ii=0; ii<=N; ii++) + blasfeo_print_tran_dvec(ns[ii], qp->d+ii, 2*nb[ii]+2*ng[ii]); + printf("\nus\n"); + for(ii=0; ii<=N; ii++) + blasfeo_print_tran_dvec(ns[ii], qp->d+ii, 2*nb[ii]+2*ng[ii]+ns[ii]); + printf("\nidxb\n"); + for(ii=0; ii<=N; ii++) + int_print_mat(1, nb[ii], qp->idxb[ii], 1); + printf("\nidxs\n"); + for(ii=0; ii<=N; ii++) + int_print_mat(1, ns[ii], qp->idxs[ii], 1); + printf("\nZ\n"); + for(ii=0; ii<=N; ii++) + blasfeo_print_tran_dvec(2*ns[ii], qp->Z+ii, 0); + printf("\nrqz\n"); + for(ii=0; ii<=N; ii++) + blasfeo_print_tran_dvec(nu[ii]+nx[ii]+2*ns[ii], qp->rqz+ii, 0); #endif #endif - struct CORE_QP_IPM_WORKSPACE *cws = ws->core_workspace; - - // arg to core workspace - cws->lam_min = arg->lam_min; - cws->t_min = arg->t_min; - - // alias qp vectors into qp_sol - cws->v = qp_sol->ux->pa; - cws->pi = qp_sol->pi->pa; - cws->lam = qp_sol->lam->pa; - cws->t = qp_sol->t->pa; - - // alias members of qp_step - ws->qp_step->dim = qp->dim; - ws->qp_step->RSQrq = qp->RSQrq; - ws->qp_step->BAbt = qp->BAbt; - ws->qp_step->DCt = qp->DCt; - ws->qp_step->Z = qp->Z; - ws->qp_step->idxb = qp->idxb; - ws->qp_step->idxs = qp->idxs; - ws->qp_step->rqz = ws->res->res_g; - ws->qp_step->b = ws->res->res_b; - ws->qp_step->d = ws->res->res_d; - ws->qp_step->m = ws->res->res_m; - - // alias members of qp_step - ws->qp_itref->dim = qp->dim; - ws->qp_itref->RSQrq = qp->RSQrq; - ws->qp_itref->BAbt = qp->BAbt; - ws->qp_itref->DCt = qp->DCt; - ws->qp_itref->Z = qp->Z; - ws->qp_itref->idxb = qp->idxb; - ws->qp_itref->idxs = qp->idxs; - ws->qp_itref->rqz = ws->res_itref->res_g; - ws->qp_itref->b = ws->res_itref->res_b; - ws->qp_itref->d = ws->res_itref->res_d; - ws->qp_itref->m = ws->res_itref->res_m; - - // alias members of qp_itref - - // no constraints - if(cws->nc==0) - { - FACT_SOLVE_KKT_UNCONSTR_OCP_QP(qp, qp_sol, arg, ws); - COMPUTE_RES_OCP_QP(qp, qp_sol, ws->res, ws->res_workspace); - cws->mu = ws->res->res_mu; - ws->iter = 0; - return 0; - } - - // blasfeo alias for residuals - struct STRVEC str_res_g; - struct STRVEC str_res_b; - struct STRVEC str_res_d; - struct STRVEC str_res_m; - str_res_g.m = cws->nv; - str_res_b.m = cws->ne; - str_res_d.m = cws->nc; - str_res_m.m = cws->nc; - str_res_g.pa = cws->res_g; - str_res_b.pa = cws->res_b; - str_res_d.pa = cws->res_d; - str_res_m.pa = cws->res_m; - - REAL *qp_res = ws->qp_res; - qp_res[0] = 0; - qp_res[1] = 0; - qp_res[2] = 0; - qp_res[3] = 0; - - int N = qp->dim->N; - int *nx = qp->dim->nx; - int *nu = qp->dim->nu; - int *nb = qp->dim->nb; - int *ng = qp->dim->ng; - int *ns = qp->dim->ns; - - int kk, ii, itref0=0, itref1=0, iter_ref_step; - REAL tmp; - REAL mu_aff0, mu; - - // init solver - INIT_VAR_OCP_QP(qp, qp_sol, arg, ws); - - cws->alpha = 1.0; - - - - // absolute IPM formulation - - if(arg->abs_form) - { - - // alias members of qp_step - ws->qp_step->dim = qp->dim; - ws->qp_step->RSQrq = qp->RSQrq; - ws->qp_step->BAbt = qp->BAbt; - ws->qp_step->DCt = qp->DCt; - ws->qp_step->Z = qp->Z; - ws->qp_step->idxb = qp->idxb; - ws->qp_step->idxs = qp->idxs; - ws->qp_step->rqz = qp->rqz; - ws->qp_step->b = qp->b; - ws->qp_step->d = qp->d; - ws->qp_step->m = ws->tmp_m; - - // alias core workspace - cws->res_m = ws->qp_step->m->pa; - cws->res_m_bkp = ws->qp_step->m->pa; - - mu = VECMULDOT(cws->nc, qp_sol->lam, 0, qp_sol->t, 0, ws->tmp_m, 0); - mu /= cws->nc; - cws->mu = mu; - - // IPM loop (absolute formulation) - for(kk=0; \ - kkiter_max & \ - cws->alpha>arg->alpha_min & \ - mu>arg->res_m_max; kk++) - { - - VECSC(cws->nc, -1.0, ws->tmp_m, 0); - - // fact solve - FACT_SOLVE_KKT_STEP_OCP_QP(ws->qp_step, ws->sol_step, arg, ws); + struct CORE_QP_IPM_WORKSPACE *cws = ws->core_workspace; + + // arg to core workspace + cws->lam_min = arg->lam_min; + cws->t_min = arg->t_min; + + // alias qp vectors into qp_sol + cws->v = qp_sol->ux->pa; + cws->pi = qp_sol->pi->pa; + cws->lam = qp_sol->lam->pa; + cws->t = qp_sol->t->pa; + + // alias members of qp_step + ws->qp_step->dim = qp->dim; + ws->qp_step->RSQrq = qp->RSQrq; + ws->qp_step->BAbt = qp->BAbt; + ws->qp_step->DCt = qp->DCt; + ws->qp_step->Z = qp->Z; + ws->qp_step->idxb = qp->idxb; + ws->qp_step->idxs = qp->idxs; + ws->qp_step->rqz = ws->res->res_g; + ws->qp_step->b = ws->res->res_b; + ws->qp_step->d = ws->res->res_d; + ws->qp_step->m = ws->res->res_m; + + // alias members of qp_step + ws->qp_itref->dim = qp->dim; + ws->qp_itref->RSQrq = qp->RSQrq; + ws->qp_itref->BAbt = qp->BAbt; + ws->qp_itref->DCt = qp->DCt; + ws->qp_itref->Z = qp->Z; + ws->qp_itref->idxb = qp->idxb; + ws->qp_itref->idxs = qp->idxs; + ws->qp_itref->rqz = ws->res_itref->res_g; + ws->qp_itref->b = ws->res_itref->res_b; + ws->qp_itref->d = ws->res_itref->res_d; + ws->qp_itref->m = ws->res_itref->res_m; + + // alias members of qp_itref + + // no constraints + if(cws->nc==0) + { + FACT_SOLVE_KKT_UNCONSTR_OCP_QP(qp, qp_sol, arg, ws); + COMPUTE_RES_OCP_QP(qp, qp_sol, ws->res, ws->res_workspace); + cws->mu = ws->res->res_mu; + ws->iter = 0; + return 0; + } + + // blasfeo alias for residuals + struct STRVEC str_res_g; + struct STRVEC str_res_b; + struct STRVEC str_res_d; + struct STRVEC str_res_m; + str_res_g.m = cws->nv; + str_res_b.m = cws->ne; + str_res_d.m = cws->nc; + str_res_m.m = cws->nc; + str_res_g.pa = cws->res_g; + str_res_b.pa = cws->res_b; + str_res_d.pa = cws->res_d; + str_res_m.pa = cws->res_m; + + REAL *qp_res = ws->qp_res; + qp_res[0] = 0; + qp_res[1] = 0; + qp_res[2] = 0; + qp_res[3] = 0; + + int N = qp->dim->N; + int *nx = qp->dim->nx; + int *nu = qp->dim->nu; + int *nb = qp->dim->nb; + int *ng = qp->dim->ng; + int *ns = qp->dim->ns; + + int kk, ii, itref0=0, itref1=0, iter_ref_step; + REAL tmp; + REAL mu_aff0, mu; + + // init solver + INIT_VAR_OCP_QP(qp, qp_sol, arg, ws); + + cws->alpha = 1.0; + + + + // absolute IPM formulation + + if(arg->abs_form) + { + + // alias members of qp_step + ws->qp_step->dim = qp->dim; + ws->qp_step->RSQrq = qp->RSQrq; + ws->qp_step->BAbt = qp->BAbt; + ws->qp_step->DCt = qp->DCt; + ws->qp_step->Z = qp->Z; + ws->qp_step->idxb = qp->idxb; + ws->qp_step->idxs = qp->idxs; + ws->qp_step->rqz = qp->rqz; + ws->qp_step->b = qp->b; + ws->qp_step->d = qp->d; + ws->qp_step->m = ws->tmp_m; + + // alias core workspace + cws->res_m = ws->qp_step->m->pa; + cws->res_m_bkp = ws->qp_step->m->pa; + + mu = VECMULDOT(cws->nc, qp_sol->lam, 0, qp_sol->t, 0, ws->tmp_m, 0); + mu /= cws->nc; + cws->mu = mu; + + // IPM loop (absolute formulation) + for(kk=0; \ + kkiter_max & \ + cws->alpha>arg->alpha_min & \ + mu>arg->res_m_max; kk++) + { + + VECSC(cws->nc, -1.0, ws->tmp_m, 0); + + // fact solve + FACT_SOLVE_KKT_STEP_OCP_QP(ws->qp_step, ws->sol_step, arg, ws); //blasfeo_print_tran_dvec(cws->nv, ws->sol_step->ux, 0); - // compute step - AXPY(cws->nv, -1.0, qp_sol->ux, 0, ws->sol_step->ux, 0, ws->sol_step->ux, 0); - AXPY(cws->ne, -1.0, qp_sol->pi, 0, ws->sol_step->pi, 0, ws->sol_step->pi, 0); - AXPY(cws->nc, -1.0, qp_sol->lam, 0, ws->sol_step->lam, 0, ws->sol_step->lam, 0); - AXPY(cws->nc, -1.0, qp_sol->t, 0, ws->sol_step->t, 0, ws->sol_step->t, 0); - - // alpha - COMPUTE_ALPHA_QP(cws); - if(kkstat_max) - ws->stat[5*kk+0] = cws->alpha; - - // Mehrotra's predictor-corrector - if(arg->pred_corr==1) - { - // mu_aff - COMPUTE_MU_AFF_QP(cws); - if(kkstat_max) - ws->stat[5*kk+1] = cws->mu_aff; - - tmp = cws->mu_aff/cws->mu; - cws->sigma = tmp*tmp*tmp; - if(kkstat_max) - ws->stat[5*kk+2] = cws->sigma; - - COMPUTE_CENTERING_CORRECTION_QP(cws); - - // fact and solve kkt - ws->use_Pb = 1; - SOLVE_KKT_STEP_OCP_QP(ws->qp_step, ws->sol_step, arg, ws); - - // compute step - AXPY(cws->nv, -1.0, qp_sol->ux, 0, ws->sol_step->ux, 0, ws->sol_step->ux, 0); - AXPY(cws->ne, -1.0, qp_sol->pi, 0, ws->sol_step->pi, 0, ws->sol_step->pi, 0); - AXPY(cws->nc, -1.0, qp_sol->lam, 0, ws->sol_step->lam, 0, ws->sol_step->lam, 0); - AXPY(cws->nc, -1.0, qp_sol->t, 0, ws->sol_step->t, 0, ws->sol_step->t, 0); - - // alpha - COMPUTE_ALPHA_QP(cws); - if(kkstat_max) - ws->stat[5*kk+3] = cws->alpha; - - } - - // - UPDATE_VAR_QP(cws); - - // compute mu - mu = VECMULDOT(cws->nc, qp_sol->lam, 0, qp_sol->t, 0, ws->tmp_m, 0); - mu /= cws->nc; - cws->mu = mu; - if(kkstat_max) - ws->stat[5*kk+4] = mu; - - // exit(1); - - } - - if(arg->comp_res_exit & arg->comp_dual_sol) - { - // compute residuals - COMPUTE_RES_OCP_QP(qp, qp_sol, ws->res, ws->res_workspace); - - // compute infinity norm of residuals - VECNRM_INF(cws->nv, &str_res_g, 0, &qp_res[0]); - VECNRM_INF(cws->ne, &str_res_b, 0, &qp_res[1]); - VECNRM_INF(cws->nc, &str_res_d, 0, &qp_res[2]); - VECNRM_INF(cws->nc, &str_res_m, 0, &qp_res[3]); + // compute step + AXPY(cws->nv, -1.0, qp_sol->ux, 0, ws->sol_step->ux, 0, ws->sol_step->ux, 0); + AXPY(cws->ne, -1.0, qp_sol->pi, 0, ws->sol_step->pi, 0, ws->sol_step->pi, 0); + AXPY(cws->nc, -1.0, qp_sol->lam, 0, ws->sol_step->lam, 0, ws->sol_step->lam, 0); + AXPY(cws->nc, -1.0, qp_sol->t, 0, ws->sol_step->t, 0, ws->sol_step->t, 0); + + // alpha + COMPUTE_ALPHA_QP(cws); + if(kkstat_max) + ws->stat[5*kk+0] = cws->alpha; + + // Mehrotra's predictor-corrector + if(arg->pred_corr==1) + { + // mu_aff + COMPUTE_MU_AFF_QP(cws); + if(kkstat_max) + ws->stat[5*kk+1] = cws->mu_aff; + + tmp = cws->mu_aff/cws->mu; + cws->sigma = tmp*tmp*tmp; + if(kkstat_max) + ws->stat[5*kk+2] = cws->sigma; + + COMPUTE_CENTERING_CORRECTION_QP(cws); + + // fact and solve kkt + ws->use_Pb = 1; + SOLVE_KKT_STEP_OCP_QP(ws->qp_step, ws->sol_step, arg, ws); + + // compute step + AXPY(cws->nv, -1.0, qp_sol->ux, 0, ws->sol_step->ux, 0, ws->sol_step->ux, 0); + AXPY(cws->ne, -1.0, qp_sol->pi, 0, ws->sol_step->pi, 0, ws->sol_step->pi, 0); + AXPY(cws->nc, -1.0, qp_sol->lam, 0, ws->sol_step->lam, 0, ws->sol_step->lam, 0); + AXPY(cws->nc, -1.0, qp_sol->t, 0, ws->sol_step->t, 0, ws->sol_step->t, 0); + + // alpha + COMPUTE_ALPHA_QP(cws); + if(kkstat_max) + ws->stat[5*kk+3] = cws->alpha; + + } + + // + UPDATE_VAR_QP(cws); + + // compute mu + mu = VECMULDOT(cws->nc, qp_sol->lam, 0, qp_sol->t, 0, ws->tmp_m, 0); + mu /= cws->nc; + cws->mu = mu; + if(kkstat_max) + ws->stat[5*kk+4] = mu; + + // exit(1); + + } + + if(arg->comp_res_exit & arg->comp_dual_sol) + { + // compute residuals + COMPUTE_RES_OCP_QP(qp, qp_sol, ws->res, ws->res_workspace); + + // compute infinity norm of residuals + VECNRM_INF(cws->nv, &str_res_g, 0, &qp_res[0]); + VECNRM_INF(cws->ne, &str_res_b, 0, &qp_res[1]); + VECNRM_INF(cws->nc, &str_res_d, 0, &qp_res[2]); + VECNRM_INF(cws->nc, &str_res_m, 0, &qp_res[3]); if(arg->print_level > 0) { @@ -919,9 +919,9 @@ int SOLVE_OCP_QP_IPM(struct OCP_QP *qp, struct OCP_QP_SOL *qp_sol, struct OCP_QP } printf("%-10d %-15e %-15e %-15e %-15e %-15e %-15e\n", kk, qp_res[0], qp_res[1], qp_res[2], qp_res[3], cws->alpha, cws->mu); } - } + } - ws->iter = kk; + ws->iter = kk; // max iteration number reached if(kk == arg->iter_max) { @@ -957,38 +957,38 @@ int SOLVE_OCP_QP_IPM(struct OCP_QP *qp, struct OCP_QP_SOL *qp_sol, struct OCP_QP printf(" \n -> SOLUTION FOUND. \n\n"); return 0; - } + } - // compute residuals - COMPUTE_RES_OCP_QP(qp, qp_sol, ws->res, ws->res_workspace); - BACKUP_RES_M(cws); - cws->mu = ws->res->res_mu; + // compute residuals + COMPUTE_RES_OCP_QP(qp, qp_sol, ws->res, ws->res_workspace); + BACKUP_RES_M(cws); + cws->mu = ws->res->res_mu; - // compute infinity norm of residuals - VECNRM_INF(cws->nv, &str_res_g, 0, &qp_res[0]); - VECNRM_INF(cws->ne, &str_res_b, 0, &qp_res[1]); - VECNRM_INF(cws->nc, &str_res_d, 0, &qp_res[2]); - VECNRM_INF(cws->nc, &str_res_m, 0, &qp_res[3]); + // compute infinity norm of residuals + VECNRM_INF(cws->nv, &str_res_g, 0, &qp_res[0]); + VECNRM_INF(cws->ne, &str_res_b, 0, &qp_res[1]); + VECNRM_INF(cws->nc, &str_res_d, 0, &qp_res[2]); + VECNRM_INF(cws->nc, &str_res_m, 0, &qp_res[3]); // printf("\niter %d\t%e\t%e\t%e\t%e\n", -1, qp_res[0], qp_res[1], qp_res[2], qp_res[3]); - REAL itref_qp_norm[4] = {0,0,0,0}; - REAL itref_qp_norm0[4] = {0,0,0,0}; - int ndp0, ndp1; + REAL itref_qp_norm[4] = {0,0,0,0}; + REAL itref_qp_norm0[4] = {0,0,0,0}; + int ndp0, ndp1; - int force_lq = 0; + int force_lq = 0; #if 0 - // IPM loop (absolute formulation) - for(kk=0; kkiter_max; kk++) - { + // IPM loop (absolute formulation) + for(kk=0; kkiter_max; kk++) + { - // fact solve - FACT_SOLVE_KKT_STEP_OCP_QP(qp, ws->sol_step, arg, ws); + // fact solve + FACT_SOLVE_KKT_STEP_OCP_QP(qp, ws->sol_step, arg, ws); #if 0 blasfeo_print_tran_dvec(cws->nv, ws->sol_step->ux, 0); @@ -997,11 +997,11 @@ blasfeo_print_tran_dvec(cws->nc, ws->sol_step->lam, 0); blasfeo_print_tran_dvec(cws->nc, ws->sol_step->t, 0); #endif - // compute step - AXPY(cws->nv, -1.0, qp_sol->ux, 0, ws->sol_step->ux, 0, ws->sol_step->ux, 0); - AXPY(cws->ne, -1.0, qp_sol->pi, 0, ws->sol_step->pi, 0, ws->sol_step->pi, 0); - AXPY(cws->nc, -1.0, qp_sol->lam, 0, ws->sol_step->lam, 0, ws->sol_step->lam, 0); - AXPY(cws->nc, -1.0, qp_sol->t, 0, ws->sol_step->t, 0, ws->sol_step->t, 0); + // compute step + AXPY(cws->nv, -1.0, qp_sol->ux, 0, ws->sol_step->ux, 0, ws->sol_step->ux, 0); + AXPY(cws->ne, -1.0, qp_sol->pi, 0, ws->sol_step->pi, 0, ws->sol_step->pi, 0); + AXPY(cws->nc, -1.0, qp_sol->lam, 0, ws->sol_step->lam, 0, ws->sol_step->lam, 0); + AXPY(cws->nc, -1.0, qp_sol->t, 0, ws->sol_step->t, 0, ws->sol_step->t, 0); #if 0 blasfeo_print_tran_dvec(cws->nv, ws->sol_step->ux, 0); @@ -1010,82 +1010,82 @@ blasfeo_print_tran_dvec(cws->nc, ws->sol_step->lam, 0); blasfeo_print_tran_dvec(cws->nc, ws->sol_step->t, 0); #endif - // alpha - COMPUTE_ALPHA_QP(cws); - if(kkstat_max) - ws->stat[5*kk+0] = cws->alpha; + // alpha + COMPUTE_ALPHA_QP(cws); + if(kkstat_max) + ws->stat[5*kk+0] = cws->alpha; - // - UPDATE_VAR_QP(cws); + // + UPDATE_VAR_QP(cws); - // compute residuals - COMPUTE_RES_OCP_QP(qp, qp_sol, ws->res, ws->res_workspace); - BACKUP_RES_M(cws); - cws->mu = ws->res->res_mu; - if(kkstat_max) - ws->stat[5*kk+4] = ws->res->res_mu; + // compute residuals + COMPUTE_RES_OCP_QP(qp, qp_sol, ws->res, ws->res_workspace); + BACKUP_RES_M(cws); + cws->mu = ws->res->res_mu; + if(kkstat_max) + ws->stat[5*kk+4] = ws->res->res_mu; - // compute infinity norm of residuals - VECNRM_INF(cws->nv, &str_res_g, 0, &qp_res[0]); - VECNRM_INF(cws->ne, &str_res_b, 0, &qp_res[1]); - VECNRM_INF(cws->nc, &str_res_d, 0, &qp_res[2]); - VECNRM_INF(cws->nc, &str_res_m, 0, &qp_res[3]); + // compute infinity norm of residuals + VECNRM_INF(cws->nv, &str_res_g, 0, &qp_res[0]); + VECNRM_INF(cws->ne, &str_res_b, 0, &qp_res[1]); + VECNRM_INF(cws->nc, &str_res_d, 0, &qp_res[2]); + VECNRM_INF(cws->nc, &str_res_m, 0, &qp_res[3]); -// exit(1); +// exit(1); - } + } - ws->iter = kk; + ws->iter = kk; - return 0; + return 0; #endif - // IPM loop - for(kk=0; kkiter_max & \ - cws->alpha>arg->alpha_min & \ - (qp_res[0]>arg->res_g_max | \ - qp_res[1]>arg->res_b_max | \ - qp_res[2]>arg->res_d_max | \ - qp_res[3]>arg->res_m_max); kk++) - { + // IPM loop + for(kk=0; kkiter_max & \ + cws->alpha>arg->alpha_min & \ + (qp_res[0]>arg->res_g_max | \ + qp_res[1]>arg->res_b_max | \ + qp_res[2]>arg->res_d_max | \ + qp_res[3]>arg->res_m_max); kk++) + { - // fact and solve kkt - if(arg->lq_fact==0) - { + // fact and solve kkt + if(arg->lq_fact==0) + { - // syrk+cholesky - FACT_SOLVE_KKT_STEP_OCP_QP(ws->qp_step, ws->sol_step, arg, ws); + // syrk+cholesky + FACT_SOLVE_KKT_STEP_OCP_QP(ws->qp_step, ws->sol_step, arg, ws); - } - else if(arg->lq_fact==1 & force_lq==0) - { + } + else if(arg->lq_fact==1 & force_lq==0) + { - // syrk+chol, switch to lq when needed - FACT_SOLVE_KKT_STEP_OCP_QP(ws->qp_step, ws->sol_step, arg, ws); + // syrk+chol, switch to lq when needed + FACT_SOLVE_KKT_STEP_OCP_QP(ws->qp_step, ws->sol_step, arg, ws); - // compute res of linear system - COMPUTE_LIN_RES_OCP_QP(ws->qp_step, qp_sol, ws->sol_step, ws->res_itref, ws->res_workspace); - VECNRM_INF(cws->nv, ws->res_itref->res_g, 0, &itref_qp_norm[0]); - VECNRM_INF(cws->ne, ws->res_itref->res_b, 0, &itref_qp_norm[1]); - VECNRM_INF(cws->nc, ws->res_itref->res_d, 0, &itref_qp_norm[2]); - VECNRM_INF(cws->nc, ws->res_itref->res_m, 0, &itref_qp_norm[3]); + // compute res of linear system + COMPUTE_LIN_RES_OCP_QP(ws->qp_step, qp_sol, ws->sol_step, ws->res_itref, ws->res_workspace); + VECNRM_INF(cws->nv, ws->res_itref->res_g, 0, &itref_qp_norm[0]); + VECNRM_INF(cws->ne, ws->res_itref->res_b, 0, &itref_qp_norm[1]); + VECNRM_INF(cws->nc, ws->res_itref->res_d, 0, &itref_qp_norm[2]); + VECNRM_INF(cws->nc, ws->res_itref->res_m, 0, &itref_qp_norm[3]); //printf("\n%e\t%e\t%e\t%e\n", itref_qp_norm[0], itref_qp_norm[1], itref_qp_norm[2], itref_qp_norm[3]); - // inaccurate factorization: switch to lq - if( + // inaccurate factorization: switch to lq + if( #ifdef USE_C99_MATH - ( itref_qp_norm[0]==0.0 & isnan(BLASFEO_DVECEL(ws->res_itref->res_g+0, 0)) ) | + ( itref_qp_norm[0]==0.0 & isnan(BLASFEO_DVECEL(ws->res_itref->res_g+0, 0)) ) | #else - ( itref_qp_norm[0]==0.0 & BLASFEO_DVECEL(ws->res_itref->res_g+0, 0)!=BLASFEO_DVECEL(ws->res_itref->res_g+0, 0) ) | + ( itref_qp_norm[0]==0.0 & BLASFEO_DVECEL(ws->res_itref->res_g+0, 0)!=BLASFEO_DVECEL(ws->res_itref->res_g+0, 0) ) | #endif - itref_qp_norm[0]>1e-5 | - itref_qp_norm[1]>1e-5 | - itref_qp_norm[2]>1e-5 | - itref_qp_norm[3]>1e-5 ) - { + itref_qp_norm[0]>1e-5 | + itref_qp_norm[1]>1e-5 | + itref_qp_norm[2]>1e-5 | + itref_qp_norm[3]>1e-5 ) + { #if 0 blasfeo_print_tran_dvec(cws->nv, ws->sol_step->v, 0); @@ -1094,11 +1094,11 @@ blasfeo_print_tran_dvec(cws->nc, ws->sol_step->lam, 0); blasfeo_print_tran_dvec(cws->nc, ws->sol_step->t, 0); #endif - // refactorize using lq - FACT_LQ_SOLVE_KKT_STEP_OCP_QP(ws->qp_step, ws->sol_step, arg, ws); + // refactorize using lq + FACT_LQ_SOLVE_KKT_STEP_OCP_QP(ws->qp_step, ws->sol_step, arg, ws); - // switch to lq - force_lq = 1; + // switch to lq + force_lq = 1; #if 0 blasfeo_print_tran_dvec(cws->nv, ws->sol_step->v, 0); @@ -1107,61 +1107,61 @@ blasfeo_print_tran_dvec(cws->nc, ws->sol_step->lam, 0); blasfeo_print_tran_dvec(cws->nc, ws->sol_step->t, 0); #endif - } - - } - else // arg->lq_fact==2 - { - - FACT_LQ_SOLVE_KKT_STEP_OCP_QP(ws->qp_step, ws->sol_step, arg, ws); - - } - - // iterative refinement on prediction step - for(itref0=0; itref0itref_pred_max; itref0++) - { - - COMPUTE_LIN_RES_OCP_QP(ws->qp_step, qp_sol, ws->sol_step, ws->res_itref, ws->res_workspace); - - VECNRM_INF(cws->nv, ws->res_itref->res_g, 0, &itref_qp_norm[0]); - VECNRM_INF(cws->ne, ws->res_itref->res_b, 0, &itref_qp_norm[1]); - VECNRM_INF(cws->nc, ws->res_itref->res_d, 0, &itref_qp_norm[2]); - VECNRM_INF(cws->nc, ws->res_itref->res_m, 0, &itref_qp_norm[3]); - - if(itref0==0) - { - itref_qp_norm0[0] = itref_qp_norm[0]; - itref_qp_norm0[1] = itref_qp_norm[1]; - itref_qp_norm0[2] = itref_qp_norm[2]; - itref_qp_norm0[3] = itref_qp_norm[3]; - } - - if( \ - (itref_qp_norm[0]<1e0*arg->res_g_max | itref_qp_norm[0]<1e-3*qp_res[0]) & \ - (itref_qp_norm[1]<1e0*arg->res_b_max | itref_qp_norm[1]<1e-3*qp_res[1]) & \ - (itref_qp_norm[2]<1e0*arg->res_d_max | itref_qp_norm[2]<1e-3*qp_res[2]) & \ - (itref_qp_norm[3]<1e0*arg->res_m_max | itref_qp_norm[3]<1e-3*qp_res[3]) ) -// (itref_qp_norm[0]<=arg->res_g_max) & \ - (itref_qp_norm[1]<=arg->res_b_max) & \ - (itref_qp_norm[2]<=arg->res_d_max) & \ - (itref_qp_norm[3]<=arg->res_m_max) ) - { - break; - } - - ws->use_Pb = 0; - SOLVE_KKT_STEP_OCP_QP(ws->qp_itref, ws->sol_itref, arg, ws); - - for(ii=0; ii<=N; ii++) - AXPY(nu[ii]+nx[ii]+2*ns[ii], 1.0, ws->sol_itref->ux+ii, 0, ws->sol_step->ux+ii, 0, ws->sol_step->ux+ii, 0); - for(ii=0; iisol_itref->pi+ii, 0, ws->sol_step->pi+ii, 0, ws->sol_step->pi+ii, 0); - for(ii=0; ii<=N; ii++) - AXPY(2*nb[ii]+2*ng[ii]+2*ns[ii], 1.0, ws->sol_itref->lam+ii, 0, ws->sol_step->lam+ii, 0, ws->sol_step->lam+ii, 0); - for(ii=0; ii<=N; ii++) - AXPY(2*nb[ii]+2*ng[ii]+2*ns[ii], 1.0, ws->sol_itref->t+ii, 0, ws->sol_step->t+ii, 0, ws->sol_step->t+ii, 0); - - } + } + + } + else // arg->lq_fact==2 + { + + FACT_LQ_SOLVE_KKT_STEP_OCP_QP(ws->qp_step, ws->sol_step, arg, ws); + + } + + // iterative refinement on prediction step + for(itref0=0; itref0itref_pred_max; itref0++) + { + + COMPUTE_LIN_RES_OCP_QP(ws->qp_step, qp_sol, ws->sol_step, ws->res_itref, ws->res_workspace); + + VECNRM_INF(cws->nv, ws->res_itref->res_g, 0, &itref_qp_norm[0]); + VECNRM_INF(cws->ne, ws->res_itref->res_b, 0, &itref_qp_norm[1]); + VECNRM_INF(cws->nc, ws->res_itref->res_d, 0, &itref_qp_norm[2]); + VECNRM_INF(cws->nc, ws->res_itref->res_m, 0, &itref_qp_norm[3]); + + if(itref0==0) + { + itref_qp_norm0[0] = itref_qp_norm[0]; + itref_qp_norm0[1] = itref_qp_norm[1]; + itref_qp_norm0[2] = itref_qp_norm[2]; + itref_qp_norm0[3] = itref_qp_norm[3]; + } + + if( \ + (itref_qp_norm[0]<1e0*arg->res_g_max | itref_qp_norm[0]<1e-3*qp_res[0]) & \ + (itref_qp_norm[1]<1e0*arg->res_b_max | itref_qp_norm[1]<1e-3*qp_res[1]) & \ + (itref_qp_norm[2]<1e0*arg->res_d_max | itref_qp_norm[2]<1e-3*qp_res[2]) & \ + (itref_qp_norm[3]<1e0*arg->res_m_max | itref_qp_norm[3]<1e-3*qp_res[3]) ) +// (itref_qp_norm[0]<=arg->res_g_max) & \ + (itref_qp_norm[1]<=arg->res_b_max) & \ + (itref_qp_norm[2]<=arg->res_d_max) & \ + (itref_qp_norm[3]<=arg->res_m_max) ) + { + break; + } + + ws->use_Pb = 0; + SOLVE_KKT_STEP_OCP_QP(ws->qp_itref, ws->sol_itref, arg, ws); + + for(ii=0; ii<=N; ii++) + AXPY(nu[ii]+nx[ii]+2*ns[ii], 1.0, ws->sol_itref->ux+ii, 0, ws->sol_step->ux+ii, 0, ws->sol_step->ux+ii, 0); + for(ii=0; iisol_itref->pi+ii, 0, ws->sol_step->pi+ii, 0, ws->sol_step->pi+ii, 0); + for(ii=0; ii<=N; ii++) + AXPY(2*nb[ii]+2*ng[ii]+2*ns[ii], 1.0, ws->sol_itref->lam+ii, 0, ws->sol_step->lam+ii, 0, ws->sol_step->lam+ii, 0); + for(ii=0; ii<=N; ii++) + AXPY(2*nb[ii]+2*ng[ii]+2*ns[ii], 1.0, ws->sol_itref->t+ii, 0, ws->sol_step->t+ii, 0, ws->sol_step->t+ii, 0); + + } #if 0 int N = qp->dim->N; @@ -1176,212 +1176,212 @@ int ii; //exit(1); //for(ii=0; ii<=N; ii++) -// blasfeo_print_dmat(nu[ii]+nx[ii], nu[ii]+nx[ii], ws->L+ii, 0, 0); +// blasfeo_print_dmat(nu[ii]+nx[ii], nu[ii]+nx[ii], ws->L+ii, 0, 0); //exit(1); printf("\nux\n"); for(ii=0; ii<=N; ii++) - blasfeo_print_tran_dvec(nu[ii]+nx[ii]+2*ns[ii], ws->dux+ii, 0); + blasfeo_print_tran_dvec(nu[ii]+nx[ii]+2*ns[ii], ws->dux+ii, 0); printf("\npi\n"); for(ii=0; iidpi+ii, 0); + blasfeo_print_tran_dvec(nx[ii+1], ws->dpi+ii, 0); //printf("\nlam\n"); //for(ii=0; ii<=N; ii++) -// blasfeo_print_tran_dvec(2*nb[ii]+2*ng[ii]+2*ns[ii], ws->dlam+ii, 0); +// blasfeo_print_tran_dvec(2*nb[ii]+2*ng[ii]+2*ns[ii], ws->dlam+ii, 0); printf("\nt\n"); for(ii=0; ii<=N; ii++) - blasfeo_print_tran_dvec(2*nb[ii]+2*ng[ii]+2*ns[ii], ws->dt+ii, 0); + blasfeo_print_tran_dvec(2*nb[ii]+2*ng[ii]+2*ns[ii], ws->dt+ii, 0); SOLVE_KKT_STEP_OCP_QP(qp, ws); printf("\nux\n"); for(ii=0; ii<=N; ii++) - blasfeo_print_tran_dvec(nu[ii]+nx[ii]+2*ns[ii], ws->dux+ii, 0); + blasfeo_print_tran_dvec(nu[ii]+nx[ii]+2*ns[ii], ws->dux+ii, 0); printf("\npi\n"); for(ii=0; iidpi+ii, 0); + blasfeo_print_tran_dvec(nx[ii+1], ws->dpi+ii, 0); //printf("\nlam\n"); //for(ii=0; ii<=N; ii++) -// blasfeo_print_tran_dvec(2*nb[ii]+2*ng[ii]+2*ns[ii], ws->dlam+ii, 0); +// blasfeo_print_tran_dvec(2*nb[ii]+2*ng[ii]+2*ns[ii], ws->dlam+ii, 0); printf("\nt\n"); for(ii=0; ii<=N; ii++) - blasfeo_print_tran_dvec(2*nb[ii]+2*ng[ii]+2*ns[ii], ws->dt+ii, 0); + blasfeo_print_tran_dvec(2*nb[ii]+2*ng[ii]+2*ns[ii], ws->dt+ii, 0); exit(1); #endif - // alpha - COMPUTE_ALPHA_QP(cws); - if(kkstat_max) - ws->stat[5*kk+0] = cws->alpha; + // alpha + COMPUTE_ALPHA_QP(cws); + if(kkstat_max) + ws->stat[5*kk+0] = cws->alpha; - // Mehrotra's predictor-corrector - if(arg->pred_corr==1) - { - // mu_aff - COMPUTE_MU_AFF_QP(cws); - if(kkstat_max) - ws->stat[5*kk+1] = cws->mu_aff; + // Mehrotra's predictor-corrector + if(arg->pred_corr==1) + { + // mu_aff + COMPUTE_MU_AFF_QP(cws); + if(kkstat_max) + ws->stat[5*kk+1] = cws->mu_aff; - tmp = cws->mu_aff/cws->mu; - cws->sigma = tmp*tmp*tmp; - if(kkstat_max) - ws->stat[5*kk+2] = cws->sigma; + tmp = cws->mu_aff/cws->mu; + cws->sigma = tmp*tmp*tmp; + if(kkstat_max) + ws->stat[5*kk+2] = cws->sigma; - COMPUTE_CENTERING_CORRECTION_QP(cws); + COMPUTE_CENTERING_CORRECTION_QP(cws); - // fact and solve kkt - ws->use_Pb = 1; - SOLVE_KKT_STEP_OCP_QP(ws->qp_step, ws->sol_step, arg, ws); + // fact and solve kkt + ws->use_Pb = 1; + SOLVE_KKT_STEP_OCP_QP(ws->qp_step, ws->sol_step, arg, ws); - // alpha - COMPUTE_ALPHA_QP(cws); - if(kkstat_max) - ws->stat[5*kk+3] = cws->alpha; + // alpha + COMPUTE_ALPHA_QP(cws); + if(kkstat_max) + ws->stat[5*kk+3] = cws->alpha; - // conditional Mehrotra's predictor-corrector - if(arg->cond_pred_corr==1) - { + // conditional Mehrotra's predictor-corrector + if(arg->cond_pred_corr==1) + { - // save mu_aff (from prediction step) - mu_aff0 = cws->mu_aff; + // save mu_aff (from prediction step) + mu_aff0 = cws->mu_aff; - // compute mu for predictor-corrector-centering - COMPUTE_MU_AFF_QP(cws); + // compute mu for predictor-corrector-centering + COMPUTE_MU_AFF_QP(cws); -// if(cws->mu_aff > 2.0*cws->mu) - if(cws->mu_aff > 2.0*mu_aff0) - { +// if(cws->mu_aff > 2.0*cws->mu) + if(cws->mu_aff > 2.0*mu_aff0) + { - // centering direction - COMPUTE_CENTERING_QP(cws); + // centering direction + COMPUTE_CENTERING_QP(cws); - // solve kkt - ws->use_Pb = 1; - SOLVE_KKT_STEP_OCP_QP(ws->qp_step, ws->sol_step, arg, ws); + // solve kkt + ws->use_Pb = 1; + SOLVE_KKT_STEP_OCP_QP(ws->qp_step, ws->sol_step, arg, ws); - // alpha - COMPUTE_ALPHA_QP(cws); - if(kkstat_max) - ws->stat[5*kk+3] = cws->alpha; + // alpha + COMPUTE_ALPHA_QP(cws); + if(kkstat_max) + ws->stat[5*kk+3] = cws->alpha; - } + } - } + } - iter_ref_step = 0; - for(itref1=0; itref1itref_corr_max; itref1++) - { + iter_ref_step = 0; + for(itref1=0; itref1itref_corr_max; itref1++) + { - COMPUTE_LIN_RES_OCP_QP(ws->qp_step, qp_sol, ws->sol_step, ws->res_itref, ws->res_workspace); + COMPUTE_LIN_RES_OCP_QP(ws->qp_step, qp_sol, ws->sol_step, ws->res_itref, ws->res_workspace); //for(ii=0; ii<=N; ii++) -// blasfeo_dvecse(nu[ii]+nx[ii], 0.0, ws->res_itref->res_g+ii, 0); +// blasfeo_dvecse(nu[ii]+nx[ii], 0.0, ws->res_itref->res_g+ii, 0); //for(ii=0; iires_itref->res_b+ii, 0); +// blasfeo_dvecse(nx[ii+1], 0.0, ws->res_itref->res_b+ii, 0); //for(ii=0; ii<=N; ii++) -// blasfeo_dvecse(2*nb[ii]+2*ng[ii], 0.0, ws->res_itref->res_d+ii, 0); +// blasfeo_dvecse(2*nb[ii]+2*ng[ii], 0.0, ws->res_itref->res_d+ii, 0); //for(ii=0; ii<=N; ii++) -// blasfeo_dvecse(2*nb[ii]+2*ng[ii], 0.0, ws->res_itref->res_m+ii, 0); - VECNRM_INF(cws->nv, ws->res_itref->res_g, 0, &itref_qp_norm[0]); - VECNRM_INF(cws->ne, ws->res_itref->res_b, 0, &itref_qp_norm[1]); - VECNRM_INF(cws->nc, ws->res_itref->res_d, 0, &itref_qp_norm[2]); - VECNRM_INF(cws->nc, ws->res_itref->res_m, 0, &itref_qp_norm[3]); - - if(itref1==0) - { - itref_qp_norm0[0] = itref_qp_norm[0]; - itref_qp_norm0[1] = itref_qp_norm[1]; - itref_qp_norm0[2] = itref_qp_norm[2]; - itref_qp_norm0[3] = itref_qp_norm[3]; - } +// blasfeo_dvecse(2*nb[ii]+2*ng[ii], 0.0, ws->res_itref->res_m+ii, 0); + VECNRM_INF(cws->nv, ws->res_itref->res_g, 0, &itref_qp_norm[0]); + VECNRM_INF(cws->ne, ws->res_itref->res_b, 0, &itref_qp_norm[1]); + VECNRM_INF(cws->nc, ws->res_itref->res_d, 0, &itref_qp_norm[2]); + VECNRM_INF(cws->nc, ws->res_itref->res_m, 0, &itref_qp_norm[3]); + + if(itref1==0) + { + itref_qp_norm0[0] = itref_qp_norm[0]; + itref_qp_norm0[1] = itref_qp_norm[1]; + itref_qp_norm0[2] = itref_qp_norm[2]; + itref_qp_norm0[3] = itref_qp_norm[3]; + } //printf("\nitref1 %d\t%e\t%e\t%e\t%e\n", itref1, itref_qp_norm[0], itref_qp_norm[1], itref_qp_norm[2], itref_qp_norm[3]); - if( \ - (itref_qp_norm[0]<1e0*arg->res_g_max | itref_qp_norm[0]<1e-3*qp_res[0]) & \ - (itref_qp_norm[1]<1e0*arg->res_b_max | itref_qp_norm[1]<1e-3*qp_res[1]) & \ - (itref_qp_norm[2]<1e0*arg->res_d_max | itref_qp_norm[2]<1e-3*qp_res[2]) & \ - (itref_qp_norm[3]<1e0*arg->res_m_max | itref_qp_norm[3]<1e-3*qp_res[3]) ) -// (itref_qp_norm[0]<=arg->res_g_max) & \ - (itref_qp_norm[1]<=arg->res_b_max) & \ - (itref_qp_norm[2]<=arg->res_d_max) & \ - (itref_qp_norm[3]<=arg->res_m_max) ) - { - break; - } + if( \ + (itref_qp_norm[0]<1e0*arg->res_g_max | itref_qp_norm[0]<1e-3*qp_res[0]) & \ + (itref_qp_norm[1]<1e0*arg->res_b_max | itref_qp_norm[1]<1e-3*qp_res[1]) & \ + (itref_qp_norm[2]<1e0*arg->res_d_max | itref_qp_norm[2]<1e-3*qp_res[2]) & \ + (itref_qp_norm[3]<1e0*arg->res_m_max | itref_qp_norm[3]<1e-3*qp_res[3]) ) +// (itref_qp_norm[0]<=arg->res_g_max) & \ + (itref_qp_norm[1]<=arg->res_b_max) & \ + (itref_qp_norm[2]<=arg->res_d_max) & \ + (itref_qp_norm[3]<=arg->res_m_max) ) + { + break; + } //printf("\nres_g\n"); //ii = 0; //blasfeo_print_exp_tran_dvec(nu[ii]+nx[ii], ws->res_itref->res_g+ii, 0); //for(ii=0; ii<=N; ii++) -// blasfeo_print_exp_tran_dvec(nu[ii]+nx[ii], ws->res_itref->res_g+ii, 0); +// blasfeo_print_exp_tran_dvec(nu[ii]+nx[ii], ws->res_itref->res_g+ii, 0); //printf("\nres_b\n"); //for(ii=0; iires_itref->res_b+ii, 0); +// blasfeo_print_exp_tran_dvec(nx[ii+1], ws->res_itref->res_b+ii, 0); //printf("\nres_d\n"); //for(ii=0; ii<=N; ii++) -// blasfeo_print_exp_tran_dvec(2*nb[ii]+2*ng[ii], ws->res_itref->res_d+ii, 0); +// blasfeo_print_exp_tran_dvec(2*nb[ii]+2*ng[ii], ws->res_itref->res_d+ii, 0); //printf("\nres_m\n"); //for(ii=0; ii<=N; ii++) -// blasfeo_print_exp_tran_dvec(2*nb[ii]+2*ng[ii], ws->res_itref->res_m+ii, 0); +// blasfeo_print_exp_tran_dvec(2*nb[ii]+2*ng[ii], ws->res_itref->res_m+ii, 0); - ws->use_Pb = 0; - SOLVE_KKT_STEP_OCP_QP(ws->qp_itref, ws->sol_itref, arg, ws); -// FACT_SOLVE_LQ_KKT_STEP_OCP_QP(ws->qp_itref, ws->sol_itref, arg, ws); - iter_ref_step = 1; + ws->use_Pb = 0; + SOLVE_KKT_STEP_OCP_QP(ws->qp_itref, ws->sol_itref, arg, ws); +// FACT_SOLVE_LQ_KKT_STEP_OCP_QP(ws->qp_itref, ws->sol_itref, arg, ws); + iter_ref_step = 1; //printf("\nux_corr\n"); //ii = 0; //blasfeo_print_exp_tran_dvec(nu[ii]+nx[ii], ws->sol_itref->ux+ii, 0); //for(ii=0; ii<=N; ii++) -// blasfeo_print_exp_tran_dvec(nu[ii]+nx[ii], ws->sol_itref->ux+ii, 0); +// blasfeo_print_exp_tran_dvec(nu[ii]+nx[ii], ws->sol_itref->ux+ii, 0); //printf("\npi_corr\n"); //for(ii=0; iisol_itref->pi+ii, 0); +// blasfeo_print_exp_tran_dvec(nx[ii+1], ws->sol_itref->pi+ii, 0); //printf("\nlam_corr\n"); //for(ii=0; ii<=N; ii++) -// blasfeo_print_exp_tran_dvec(2*nb[ii]+2*ng[ii], ws->sol_itref->lam+ii, 0); +// blasfeo_print_exp_tran_dvec(2*nb[ii]+2*ng[ii], ws->sol_itref->lam+ii, 0); //printf("\nt_corr\n"); //for(ii=0; ii<=N; ii++) -// blasfeo_print_exp_tran_dvec(2*nb[ii]+2*ng[ii], ws->sol_itref->t+ii, 0); - - for(ii=0; ii<=N; ii++) - AXPY(nu[ii]+nx[ii]+2*ns[ii], 1.0, ws->sol_itref->ux+ii, 0, ws->sol_step->ux+ii, 0, ws->sol_step->ux+ii, 0); - for(ii=0; iisol_itref->pi+ii, 0, ws->sol_step->pi+ii, 0, ws->sol_step->pi+ii, 0); - for(ii=0; ii<=N; ii++) - AXPY(2*nb[ii]+2*ng[ii]+2*ns[ii], 1.0, ws->sol_itref->lam+ii, 0, ws->sol_step->lam+ii, 0, ws->sol_step->lam+ii, 0); - for(ii=0; ii<=N; ii++) - AXPY(2*nb[ii]+2*ng[ii]+2*ns[ii], 1.0, ws->sol_itref->t+ii, 0, ws->sol_step->t+ii, 0, ws->sol_step->t+ii, 0); - - } - - if(iter_ref_step) - { - // alpha - COMPUTE_ALPHA_QP(cws); - if(kkstat_max) - ws->stat[5*kk+3] = cws->alpha; - } - - } - - // - UPDATE_VAR_QP(cws); - - // compute residuals - COMPUTE_RES_OCP_QP(qp, qp_sol, ws->res, ws->res_workspace); - BACKUP_RES_M(cws); - cws->mu = ws->res->res_mu; - if(kkstat_max) - ws->stat[5*kk+4] = ws->res->res_mu; - - // compute infinity norm of residuals - VECNRM_INF(cws->nv, &str_res_g, 0, &qp_res[0]); - VECNRM_INF(cws->ne, &str_res_b, 0, &qp_res[1]); - VECNRM_INF(cws->nc, &str_res_d, 0, &qp_res[2]); - VECNRM_INF(cws->nc, &str_res_m, 0, &qp_res[3]); +// blasfeo_print_exp_tran_dvec(2*nb[ii]+2*ng[ii], ws->sol_itref->t+ii, 0); + + for(ii=0; ii<=N; ii++) + AXPY(nu[ii]+nx[ii]+2*ns[ii], 1.0, ws->sol_itref->ux+ii, 0, ws->sol_step->ux+ii, 0, ws->sol_step->ux+ii, 0); + for(ii=0; iisol_itref->pi+ii, 0, ws->sol_step->pi+ii, 0, ws->sol_step->pi+ii, 0); + for(ii=0; ii<=N; ii++) + AXPY(2*nb[ii]+2*ng[ii]+2*ns[ii], 1.0, ws->sol_itref->lam+ii, 0, ws->sol_step->lam+ii, 0, ws->sol_step->lam+ii, 0); + for(ii=0; ii<=N; ii++) + AXPY(2*nb[ii]+2*ng[ii]+2*ns[ii], 1.0, ws->sol_itref->t+ii, 0, ws->sol_step->t+ii, 0, ws->sol_step->t+ii, 0); + + } + + if(iter_ref_step) + { + // alpha + COMPUTE_ALPHA_QP(cws); + if(kkstat_max) + ws->stat[5*kk+3] = cws->alpha; + } + + } + + // + UPDATE_VAR_QP(cws); + + // compute residuals + COMPUTE_RES_OCP_QP(qp, qp_sol, ws->res, ws->res_workspace); + BACKUP_RES_M(cws); + cws->mu = ws->res->res_mu; + if(kkstat_max) + ws->stat[5*kk+4] = ws->res->res_mu; + + // compute infinity norm of residuals + VECNRM_INF(cws->nv, &str_res_g, 0, &qp_res[0]); + VECNRM_INF(cws->ne, &str_res_b, 0, &qp_res[1]); + VECNRM_INF(cws->nc, &str_res_d, 0, &qp_res[2]); + VECNRM_INF(cws->nc, &str_res_m, 0, &qp_res[3]); if(arg->print_level > 0) { @@ -1393,51 +1393,51 @@ exit(1); } printf("%-10d %-15e %-15e %-15e %-15e %-15e %-15e\n", kk, qp_res[0], qp_res[1], qp_res[2], qp_res[3], cws->alpha, cws->mu); } - } + } - ws->iter = kk; + ws->iter = kk; #if 0 - printf("\nux\n"); - for(ii=0; ii<=N; ii++) - blasfeo_print_tran_dvec(nu[ii]+nx[ii], qp_sol->ux+ii, 0); + printf("\nux\n"); + for(ii=0; ii<=N; ii++) + blasfeo_print_tran_dvec(nu[ii]+nx[ii], qp_sol->ux+ii, 0); #endif - // max iteration number reached - if(kk == arg->iter_max) { + // max iteration number reached + if(kk == arg->iter_max) { if (arg->print_level > 0) printf(" \n -> ERROR: maximum number of iterations reached, exiting.\n\n"); - return 1; + return 1; } - // min step lenght - if(cws->alpha <= arg->alpha_min) { + // min step lenght + if(cws->alpha <= arg->alpha_min) { if (arg->print_level > 0) printf(" \n -> ERROR: minimum step size reached, exiting.\n\n"); - return 2; + return 2; } - // NaN in the solution + // NaN in the solution #ifdef USE_C99_MATH - if(isnan(cws->mu)) { + if(isnan(cws->mu)) { if (arg->print_level > 0) printf(" \n -> ERROR: NaN detected, exiting.\n\n"); - return 3; + return 3; } #else - if(cws->mu != cws->mu) { + if(cws->mu != cws->mu) { if (arg->print_level > 0) printf(" \n -> ERROR: NaN detected, exiting.\n\n"); - return 3; + return 3; } #endif - // normal return + // normal return if (arg->print_level > 0) printf(" \n -> SOLUTION FOUND. \n\n"); - return 0; + return 0; - } + } // interface functions int SIZEOF_OCP_QP_IPM_ARG() From 37671fe7880f70ddde06999138aafd0c4e86f949 Mon Sep 17 00:00:00 2001 From: zanellia Date: Mon, 30 Jul 2018 10:41:21 +0200 Subject: [PATCH 09/16] retabbing x_dense_qp_ipm.c --- dense_qp/x_dense_qp_ipm.c | 1734 ++++++++++++++++++------------------- 1 file changed, 867 insertions(+), 867 deletions(-) diff --git a/dense_qp/x_dense_qp_ipm.c b/dense_qp/x_dense_qp_ipm.c index ce9c4e01..4d3a292d 100644 --- a/dense_qp/x_dense_qp_ipm.c +++ b/dense_qp/x_dense_qp_ipm.c @@ -28,665 +28,665 @@ int MEMSIZE_DENSE_QP_IPM_ARG(struct DENSE_QP_DIM *dim) - { + { - return 0; + return 0; - } + } void CREATE_DENSE_QP_IPM_ARG(struct DENSE_QP_DIM *dim, struct DENSE_QP_IPM_ARG *arg, void *mem) - { + { - arg->memsize = 0; + arg->memsize = 0; - return; + return; - } + } void SET_DEFAULT_DENSE_QP_IPM_ARG(enum HPIPM_MODE mode, struct DENSE_QP_IPM_ARG *arg) - { + { // set common default arguments arg->print_level = 0; // set mode-specific default arguments - if(mode==SPEED_ABS) - { - arg->mu0 = 1e1; - arg->alpha_min = 1e-12; - arg->res_g_max = 1e0; // not used - arg->res_b_max = 1e0; // not used - arg->res_d_max = 1e0; // not used - arg->res_m_max = 1e-8; - arg->iter_max = 15; - arg->stat_max = 15; - arg->pred_corr = 1; - arg->cond_pred_corr = 0; // not used - arg->itref_pred_max = 0; // not used - arg->itref_corr_max = 0; // not used - arg->reg_prim = 1e-15; - arg->reg_dual = 1e-15; - arg->lq_fact = 0; // not used - arg->scale = 0; - arg->lam_min = 1e-30; - arg->t_min = 1e-30; - arg->warm_start = 0; - arg->abs_form = 1; - arg->comp_res_exit = 0; - } - else if(mode==SPEED) - { - arg->mu0 = 1e1; - arg->alpha_min = 1e-12; - arg->res_g_max = 1e-6; - arg->res_b_max = 1e-8; - arg->res_d_max = 1e-8; - arg->res_m_max = 1e-8; - arg->iter_max = 15; - arg->stat_max = 15; - arg->pred_corr = 1; - arg->cond_pred_corr = 1; - arg->itref_pred_max = 0; - arg->itref_corr_max = 0; - arg->reg_prim = 1e-15; - arg->reg_dual = 1e-15; - arg->lq_fact = 0; - arg->scale = 0; - arg->lam_min = 1e-30; - arg->t_min = 1e-30; - arg->warm_start = 0; - arg->abs_form = 0; - arg->comp_res_exit = 1; - } - else if(mode==BALANCE) - { - arg->mu0 = 1e1; - arg->alpha_min = 1e-12; - arg->res_g_max = 1e-6; - arg->res_b_max = 1e-8; - arg->res_d_max = 1e-8; - arg->res_m_max = 1e-8; - arg->iter_max = 30; - arg->stat_max = 30; - arg->pred_corr = 1; - arg->cond_pred_corr = 1; - arg->itref_pred_max = 0; - arg->itref_corr_max = 2; - arg->reg_prim = 1e-15; - arg->reg_dual = 1e-15; - arg->lq_fact = 1; - arg->scale = 1; - arg->lam_min = 1e-30; - arg->t_min = 1e-30; - arg->warm_start = 0; - arg->abs_form = 0; - arg->comp_res_exit = 1; - } - else if(mode==ROBUST) - { - arg->mu0 = 1e2; - arg->alpha_min = 1e-12; - arg->res_g_max = 1e-6; - arg->res_b_max = 1e-8; - arg->res_d_max = 1e-8; - arg->res_m_max = 1e-8; - arg->iter_max = 100; - arg->stat_max = 100; - arg->pred_corr = 1; - arg->cond_pred_corr = 1; - arg->itref_pred_max = 0; - arg->itref_corr_max = 4; - arg->reg_prim = 1e-15; - arg->reg_dual = 1e-15; - arg->lq_fact = 2; - arg->scale = 0; - arg->lam_min = 1e-30; - arg->t_min = 1e-30; - arg->warm_start = 0; - arg->abs_form = 0; - arg->comp_res_exit = 1; - } - else - { - printf("\nwrong set default mode\n"); - exit(1); - } - - return; - - } + if(mode==SPEED_ABS) + { + arg->mu0 = 1e1; + arg->alpha_min = 1e-12; + arg->res_g_max = 1e0; // not used + arg->res_b_max = 1e0; // not used + arg->res_d_max = 1e0; // not used + arg->res_m_max = 1e-8; + arg->iter_max = 15; + arg->stat_max = 15; + arg->pred_corr = 1; + arg->cond_pred_corr = 0; // not used + arg->itref_pred_max = 0; // not used + arg->itref_corr_max = 0; // not used + arg->reg_prim = 1e-15; + arg->reg_dual = 1e-15; + arg->lq_fact = 0; // not used + arg->scale = 0; + arg->lam_min = 1e-30; + arg->t_min = 1e-30; + arg->warm_start = 0; + arg->abs_form = 1; + arg->comp_res_exit = 0; + } + else if(mode==SPEED) + { + arg->mu0 = 1e1; + arg->alpha_min = 1e-12; + arg->res_g_max = 1e-6; + arg->res_b_max = 1e-8; + arg->res_d_max = 1e-8; + arg->res_m_max = 1e-8; + arg->iter_max = 15; + arg->stat_max = 15; + arg->pred_corr = 1; + arg->cond_pred_corr = 1; + arg->itref_pred_max = 0; + arg->itref_corr_max = 0; + arg->reg_prim = 1e-15; + arg->reg_dual = 1e-15; + arg->lq_fact = 0; + arg->scale = 0; + arg->lam_min = 1e-30; + arg->t_min = 1e-30; + arg->warm_start = 0; + arg->abs_form = 0; + arg->comp_res_exit = 1; + } + else if(mode==BALANCE) + { + arg->mu0 = 1e1; + arg->alpha_min = 1e-12; + arg->res_g_max = 1e-6; + arg->res_b_max = 1e-8; + arg->res_d_max = 1e-8; + arg->res_m_max = 1e-8; + arg->iter_max = 30; + arg->stat_max = 30; + arg->pred_corr = 1; + arg->cond_pred_corr = 1; + arg->itref_pred_max = 0; + arg->itref_corr_max = 2; + arg->reg_prim = 1e-15; + arg->reg_dual = 1e-15; + arg->lq_fact = 1; + arg->scale = 1; + arg->lam_min = 1e-30; + arg->t_min = 1e-30; + arg->warm_start = 0; + arg->abs_form = 0; + arg->comp_res_exit = 1; + } + else if(mode==ROBUST) + { + arg->mu0 = 1e2; + arg->alpha_min = 1e-12; + arg->res_g_max = 1e-6; + arg->res_b_max = 1e-8; + arg->res_d_max = 1e-8; + arg->res_m_max = 1e-8; + arg->iter_max = 100; + arg->stat_max = 100; + arg->pred_corr = 1; + arg->cond_pred_corr = 1; + arg->itref_pred_max = 0; + arg->itref_corr_max = 4; + arg->reg_prim = 1e-15; + arg->reg_dual = 1e-15; + arg->lq_fact = 2; + arg->scale = 0; + arg->lam_min = 1e-30; + arg->t_min = 1e-30; + arg->warm_start = 0; + arg->abs_form = 0; + arg->comp_res_exit = 1; + } + else + { + printf("\nwrong set default mode\n"); + exit(1); + } + + return; + + } int MEMSIZE_DENSE_QP_IPM(struct DENSE_QP_DIM *dim, struct DENSE_QP_IPM_ARG *arg) - { + { + + int nv = dim->nv; + int ne = dim->ne; + int nb = dim->nb; + int ng = dim->ng; + int ns = dim->ns; + + int size = 0; + + size += 1*sizeof(struct CORE_QP_IPM_WORKSPACE); + size += 1*MEMSIZE_CORE_QP_IPM(nv+2*ns, ne, 2*nb+2*ng+2*ns); + + size += 1*sizeof(struct DENSE_QP_RES_WORKSPACE); // res_workspace + + size += 2*sizeof(struct DENSE_QP); // qp_step qp_itref + + size += 2*sizeof(struct DENSE_QP_SOL); // sol_step sol_itref + size += 1*MEMSIZE_DENSE_QP_SOL(dim); // sol_itref + + size += 2*sizeof(struct DENSE_QP_RES); // res res_itref + size += 1*MEMSIZE_DENSE_QP_RES(dim); // res_itref + + size += 23*sizeof(struct STRVEC); // sol_step(v,pi,lam,t) res_g res_b res_d res_m lv (4+2)*tmp_nbg (1+1)*tmp_ns Gamma gamma Zs_inv sv se tmp_m + size += 5*sizeof(struct STRMAT); // 2*Lv AL Le Ctx + if(arg->lq_fact>0) + size += 2*sizeof(struct STRMAT); // lq0 lq1 + + size += 4*SIZE_STRVEC(nb+ng); // 4*tmp_nbg + size += 1*SIZE_STRVEC(ns); // tmp_ns + size += 2*SIZE_STRVEC(nv); // lv sv + size += 1*SIZE_STRVEC(ne); // se + size += 1*SIZE_STRVEC(2*ns); // Zs_inv + size += 2*SIZE_STRMAT(nv+1, nv); // Lv + size += 1*SIZE_STRMAT(ne, nv); // AL + size += 1*SIZE_STRMAT(ne, ne); // Le + size += 1*SIZE_STRMAT(nv+1, ng); // Ctx + if(arg->lq_fact>0) + { + size += 1*SIZE_STRMAT(ne, ne+nv); // lq0 + size += 1*SIZE_STRMAT(nv, nv+nv+ng); // lq1 + } + size += 1*SIZE_STRVEC(2*nb+2*ng+2*ns); // tmp_m - int nv = dim->nv; - int ne = dim->ne; - int nb = dim->nb; - int ng = dim->ng; - int ns = dim->ns; +// size += nv*sizeof(int); // ipiv_v // TODO remove !!!!! +// size += ne*sizeof(int); // ipiv_e // TODO remove !!!!! - int size = 0; + if(arg->lq_fact>0) + { + size += 1*GELQF_WORKSIZE(ne, nv); // lq_work0 + size += 1*GELQF_WORKSIZE(nv, nv+nv+ng); // lq_work1 + } - size += 1*sizeof(struct CORE_QP_IPM_WORKSPACE); - size += 1*MEMSIZE_CORE_QP_IPM(nv+2*ns, ne, 2*nb+2*ng+2*ns); + size += 5*arg->stat_max*sizeof(REAL); - size += 1*sizeof(struct DENSE_QP_RES_WORKSPACE); // res_workspace + size = (size+63)/64*64; // make multiple of typical cache line size + size += 1*64; // align once to typical cache line size - size += 2*sizeof(struct DENSE_QP); // qp_step qp_itref + return size; + + } - size += 2*sizeof(struct DENSE_QP_SOL); // sol_step sol_itref - size += 1*MEMSIZE_DENSE_QP_SOL(dim); // sol_itref - size += 2*sizeof(struct DENSE_QP_RES); // res res_itref - size += 1*MEMSIZE_DENSE_QP_RES(dim); // res_itref - size += 23*sizeof(struct STRVEC); // sol_step(v,pi,lam,t) res_g res_b res_d res_m lv (4+2)*tmp_nbg (1+1)*tmp_ns Gamma gamma Zs_inv sv se tmp_m - size += 5*sizeof(struct STRMAT); // 2*Lv AL Le Ctx - if(arg->lq_fact>0) - size += 2*sizeof(struct STRMAT); // lq0 lq1 +void CREATE_DENSE_QP_IPM(struct DENSE_QP_DIM *dim, struct DENSE_QP_IPM_ARG *arg, struct DENSE_QP_IPM_WORKSPACE *workspace, void *mem) + { - size += 4*SIZE_STRVEC(nb+ng); // 4*tmp_nbg - size += 1*SIZE_STRVEC(ns); // tmp_ns - size += 2*SIZE_STRVEC(nv); // lv sv - size += 1*SIZE_STRVEC(ne); // se - size += 1*SIZE_STRVEC(2*ns); // Zs_inv - size += 2*SIZE_STRMAT(nv+1, nv); // Lv - size += 1*SIZE_STRMAT(ne, nv); // AL - size += 1*SIZE_STRMAT(ne, ne); // Le - size += 1*SIZE_STRMAT(nv+1, ng); // Ctx - if(arg->lq_fact>0) - { - size += 1*SIZE_STRMAT(ne, ne+nv); // lq0 - size += 1*SIZE_STRMAT(nv, nv+nv+ng); // lq1 - } - size += 1*SIZE_STRVEC(2*nb+2*ng+2*ns); // tmp_m + int nv = dim->nv; + int ne = dim->ne; + int nb = dim->nb; + int ng = dim->ng; + int ns = dim->ns; -// size += nv*sizeof(int); // ipiv_v // TODO remove !!!!! -// size += ne*sizeof(int); // ipiv_e // TODO remove !!!!! - if(arg->lq_fact>0) - { - size += 1*GELQF_WORKSIZE(ne, nv); // lq_work0 - size += 1*GELQF_WORKSIZE(nv, nv+nv+ng); // lq_work1 - } + // core struct + struct CORE_QP_IPM_WORKSPACE *sr_ptr = mem; - size += 5*arg->stat_max*sizeof(REAL); + // core workspace + workspace->core_workspace = sr_ptr; + sr_ptr += 1; + struct CORE_QP_IPM_WORKSPACE *cws = workspace->core_workspace; - size = (size+63)/64*64; // make multiple of typical cache line size - size += 1*64; // align once to typical cache line size - return size; + // res struct + struct DENSE_QP_RES *res_ptr = (struct DENSE_QP_RES *) sr_ptr; - } + workspace->res = res_ptr; + res_ptr += 1; + workspace->res_itref = res_ptr; + res_ptr += 1; + // res workspace struct + struct DENSE_QP_RES_WORKSPACE *res_ws_ptr = (struct DENSE_QP_RES_WORKSPACE *) res_ptr; -void CREATE_DENSE_QP_IPM(struct DENSE_QP_DIM *dim, struct DENSE_QP_IPM_ARG *arg, struct DENSE_QP_IPM_WORKSPACE *workspace, void *mem) - { - - int nv = dim->nv; - int ne = dim->ne; - int nb = dim->nb; - int ng = dim->ng; - int ns = dim->ns; - - - // core struct - struct CORE_QP_IPM_WORKSPACE *sr_ptr = mem; - - // core workspace - workspace->core_workspace = sr_ptr; - sr_ptr += 1; - struct CORE_QP_IPM_WORKSPACE *cws = workspace->core_workspace; - - - // res struct - struct DENSE_QP_RES *res_ptr = (struct DENSE_QP_RES *) sr_ptr; - - workspace->res = res_ptr; - res_ptr += 1; - workspace->res_itref = res_ptr; - res_ptr += 1; - - - // res workspace struct - struct DENSE_QP_RES_WORKSPACE *res_ws_ptr = (struct DENSE_QP_RES_WORKSPACE *) res_ptr; - - workspace->res_workspace = res_ws_ptr; - res_ws_ptr += 1; - - - // qp sol struct - struct DENSE_QP_SOL *qp_sol_ptr = (struct DENSE_QP_SOL *) res_ws_ptr; - - workspace->sol_step = qp_sol_ptr; - qp_sol_ptr += 1; - workspace->sol_itref = qp_sol_ptr; - qp_sol_ptr += 1; - - - // qp struct - struct DENSE_QP *qp_ptr = (struct DENSE_QP *) qp_sol_ptr; - - workspace->qp_step = qp_ptr; - qp_ptr += 1; - workspace->qp_itref = qp_ptr; - qp_ptr += 1; + workspace->res_workspace = res_ws_ptr; + res_ws_ptr += 1; - // matrix struct - struct STRMAT *sm_ptr = (struct STRMAT *) qp_ptr; - - workspace->Lv = sm_ptr; - sm_ptr += 2; - workspace->AL = sm_ptr; - sm_ptr += 1; - workspace->Le = sm_ptr; - sm_ptr += 1; - workspace->Ctx = sm_ptr; - sm_ptr += 1; - if(arg->lq_fact>0) - { - workspace->lq0 = sm_ptr; - sm_ptr += 1; - workspace->lq1 = sm_ptr; - sm_ptr += 1; - } + // qp sol struct + struct DENSE_QP_SOL *qp_sol_ptr = (struct DENSE_QP_SOL *) res_ws_ptr; + workspace->sol_step = qp_sol_ptr; + qp_sol_ptr += 1; + workspace->sol_itref = qp_sol_ptr; + qp_sol_ptr += 1; - // vector struct - struct STRVEC *sv_ptr = (struct STRVEC *) sm_ptr; - workspace->sol_step->v = sv_ptr; - sv_ptr += 1; - workspace->sol_step->pi = sv_ptr; - sv_ptr += 1; - workspace->sol_step->lam = sv_ptr; - sv_ptr += 1; - workspace->sol_step->t = sv_ptr; - sv_ptr += 1; - workspace->res->res_g = sv_ptr; - sv_ptr += 1; - workspace->res->res_b = sv_ptr; - sv_ptr += 1; - workspace->res->res_d = sv_ptr; - sv_ptr += 1; - workspace->res->res_m = sv_ptr; - sv_ptr += 1; - workspace->Gamma = sv_ptr; - sv_ptr += 1; - workspace->gamma = sv_ptr; - sv_ptr += 1; - workspace->Zs_inv = sv_ptr; - sv_ptr += 1; - workspace->lv = sv_ptr; - sv_ptr += 1; - workspace->sv = sv_ptr; - sv_ptr += 1; - workspace->se = sv_ptr; - sv_ptr += 1; - workspace->tmp_nbg = sv_ptr; - sv_ptr += 4; - workspace->res_workspace->tmp_nbg = sv_ptr; - sv_ptr += 2; - workspace->tmp_ns = sv_ptr; - sv_ptr += 1; - workspace->res_workspace->tmp_ns = sv_ptr; - sv_ptr += 1; - workspace->tmp_m = sv_ptr; - sv_ptr += 1; + // qp struct + struct DENSE_QP *qp_ptr = (struct DENSE_QP *) qp_sol_ptr; + workspace->qp_step = qp_ptr; + qp_ptr += 1; + workspace->qp_itref = qp_ptr; + qp_ptr += 1; - // double/float stuff - REAL *d_ptr = (REAL *) sv_ptr; - - workspace->stat = d_ptr; - d_ptr += 5*arg->stat_max; - workspace->stat_max = arg->stat_max; + // matrix struct + struct STRMAT *sm_ptr = (struct STRMAT *) qp_ptr; + workspace->Lv = sm_ptr; + sm_ptr += 2; + workspace->AL = sm_ptr; + sm_ptr += 1; + workspace->Le = sm_ptr; + sm_ptr += 1; + workspace->Ctx = sm_ptr; + sm_ptr += 1; + if(arg->lq_fact>0) + { + workspace->lq0 = sm_ptr; + sm_ptr += 1; + workspace->lq1 = sm_ptr; + sm_ptr += 1; + } - // int suff - int *i_ptr = (int *) d_ptr; - -// workspace->ipiv_v = i_ptr; -// i_ptr += nv; - -// workspace->ipiv_e = i_ptr; -// i_ptr += ne; - - - // align to typicl cache line size - size_t s_ptr = (size_t) i_ptr; - s_ptr = (s_ptr+63)/64*64; - - - // void stuf - char *c_ptr = (char *) s_ptr; - - CREATE_DENSE_QP_SOL(dim, workspace->sol_itref, c_ptr); - c_ptr += workspace->sol_itref->memsize; - - CREATE_DENSE_QP_RES(dim, workspace->res_itref, c_ptr); - c_ptr += workspace->res_itref->memsize; - - CREATE_STRMAT(nv+1, nv, workspace->Lv, c_ptr); - c_ptr += workspace->Lv->memsize; - - CREATE_STRMAT(nv+1, nv, workspace->Lv+1, c_ptr); - c_ptr += workspace->Lv[1].memsize; - - CREATE_STRMAT(ne, nv, workspace->AL, c_ptr); - c_ptr += workspace->AL->memsize; - - CREATE_STRMAT(ne, ne, workspace->Le, c_ptr); - c_ptr += workspace->Le->memsize; - - CREATE_STRMAT(nv+1, ng, workspace->Ctx, c_ptr); - c_ptr += workspace->Ctx->memsize; - - if(arg->lq_fact>0) - { - CREATE_STRMAT(ne, ne+nv, workspace->lq0, c_ptr); - c_ptr += workspace->lq0->memsize; - - CREATE_STRMAT(nv, nv+nv+ng, workspace->lq1, c_ptr); - c_ptr += workspace->lq1->memsize; - } - - CREATE_STRVEC(nv, workspace->lv, c_ptr); - c_ptr += workspace->lv->memsize; - - CREATE_STRVEC(nv, workspace->sv, c_ptr); - c_ptr += workspace->sv->memsize; - - CREATE_STRVEC(ne, workspace->se, c_ptr); - c_ptr += workspace->se->memsize; - - CREATE_STRVEC(2*ns, workspace->Zs_inv, c_ptr); - c_ptr += workspace->Zs_inv->memsize; - - CREATE_STRVEC(nb+ng, workspace->tmp_nbg+0, c_ptr); - CREATE_STRVEC(nb+ng, workspace->res_workspace->tmp_nbg+0, c_ptr); - c_ptr += (workspace->tmp_nbg+0)->memsize; - CREATE_STRVEC(nb+ng, workspace->tmp_nbg+1, c_ptr); - CREATE_STRVEC(nb+ng, workspace->res_workspace->tmp_nbg+1, c_ptr); - c_ptr += (workspace->tmp_nbg+1)->memsize; + // vector struct + struct STRVEC *sv_ptr = (struct STRVEC *) sm_ptr; + + workspace->sol_step->v = sv_ptr; + sv_ptr += 1; + workspace->sol_step->pi = sv_ptr; + sv_ptr += 1; + workspace->sol_step->lam = sv_ptr; + sv_ptr += 1; + workspace->sol_step->t = sv_ptr; + sv_ptr += 1; + workspace->res->res_g = sv_ptr; + sv_ptr += 1; + workspace->res->res_b = sv_ptr; + sv_ptr += 1; + workspace->res->res_d = sv_ptr; + sv_ptr += 1; + workspace->res->res_m = sv_ptr; + sv_ptr += 1; + workspace->Gamma = sv_ptr; + sv_ptr += 1; + workspace->gamma = sv_ptr; + sv_ptr += 1; + workspace->Zs_inv = sv_ptr; + sv_ptr += 1; + workspace->lv = sv_ptr; + sv_ptr += 1; + workspace->sv = sv_ptr; + sv_ptr += 1; + workspace->se = sv_ptr; + sv_ptr += 1; + workspace->tmp_nbg = sv_ptr; + sv_ptr += 4; + workspace->res_workspace->tmp_nbg = sv_ptr; + sv_ptr += 2; + workspace->tmp_ns = sv_ptr; + sv_ptr += 1; + workspace->res_workspace->tmp_ns = sv_ptr; + sv_ptr += 1; + workspace->tmp_m = sv_ptr; + sv_ptr += 1; + + + // double/float stuff + REAL *d_ptr = (REAL *) sv_ptr; + + workspace->stat = d_ptr; + d_ptr += 5*arg->stat_max; + + workspace->stat_max = arg->stat_max; + + + // int suff + int *i_ptr = (int *) d_ptr; + +// workspace->ipiv_v = i_ptr; +// i_ptr += nv; + +// workspace->ipiv_e = i_ptr; +// i_ptr += ne; + + + // align to typicl cache line size + size_t s_ptr = (size_t) i_ptr; + s_ptr = (s_ptr+63)/64*64; + + + // void stuf + char *c_ptr = (char *) s_ptr; + + CREATE_DENSE_QP_SOL(dim, workspace->sol_itref, c_ptr); + c_ptr += workspace->sol_itref->memsize; + + CREATE_DENSE_QP_RES(dim, workspace->res_itref, c_ptr); + c_ptr += workspace->res_itref->memsize; + + CREATE_STRMAT(nv+1, nv, workspace->Lv, c_ptr); + c_ptr += workspace->Lv->memsize; + + CREATE_STRMAT(nv+1, nv, workspace->Lv+1, c_ptr); + c_ptr += workspace->Lv[1].memsize; + + CREATE_STRMAT(ne, nv, workspace->AL, c_ptr); + c_ptr += workspace->AL->memsize; + + CREATE_STRMAT(ne, ne, workspace->Le, c_ptr); + c_ptr += workspace->Le->memsize; + + CREATE_STRMAT(nv+1, ng, workspace->Ctx, c_ptr); + c_ptr += workspace->Ctx->memsize; + + if(arg->lq_fact>0) + { + CREATE_STRMAT(ne, ne+nv, workspace->lq0, c_ptr); + c_ptr += workspace->lq0->memsize; + + CREATE_STRMAT(nv, nv+nv+ng, workspace->lq1, c_ptr); + c_ptr += workspace->lq1->memsize; + } - CREATE_STRVEC(nb+ng, workspace->tmp_nbg+2, c_ptr); - c_ptr += (workspace->tmp_nbg+2)->memsize; + CREATE_STRVEC(nv, workspace->lv, c_ptr); + c_ptr += workspace->lv->memsize; - CREATE_STRVEC(nb+ng, workspace->tmp_nbg+3, c_ptr); - c_ptr += (workspace->tmp_nbg+3)->memsize; + CREATE_STRVEC(nv, workspace->sv, c_ptr); + c_ptr += workspace->sv->memsize; - CREATE_STRVEC(ns, workspace->tmp_ns+0, c_ptr); - CREATE_STRVEC(ns, workspace->res_workspace->tmp_ns+0, c_ptr); - c_ptr += (workspace->tmp_ns+0)->memsize; + CREATE_STRVEC(ne, workspace->se, c_ptr); + c_ptr += workspace->se->memsize; - CREATE_STRVEC(2*nb+2*ng+2*ns, workspace->tmp_m, c_ptr); - c_ptr += (workspace->tmp_m)->memsize; + CREATE_STRVEC(2*ns, workspace->Zs_inv, c_ptr); + c_ptr += workspace->Zs_inv->memsize; - CREATE_CORE_QP_IPM(nv+2*ns, ne, 2*nb+2*ng+2*ns, cws, c_ptr); - c_ptr += workspace->core_workspace->memsize; + CREATE_STRVEC(nb+ng, workspace->tmp_nbg+0, c_ptr); + CREATE_STRVEC(nb+ng, workspace->res_workspace->tmp_nbg+0, c_ptr); + c_ptr += (workspace->tmp_nbg+0)->memsize; - if(arg->lq_fact>0) - { - workspace->lq_work0 = c_ptr; - c_ptr += GELQF_WORKSIZE(ne, nv); + CREATE_STRVEC(nb+ng, workspace->tmp_nbg+1, c_ptr); + CREATE_STRVEC(nb+ng, workspace->res_workspace->tmp_nbg+1, c_ptr); + c_ptr += (workspace->tmp_nbg+1)->memsize; - workspace->lq_work1 = c_ptr; - c_ptr += GELQF_WORKSIZE(nv, nv+nv+ng); - } + CREATE_STRVEC(nb+ng, workspace->tmp_nbg+2, c_ptr); + c_ptr += (workspace->tmp_nbg+2)->memsize; + CREATE_STRVEC(nb+ng, workspace->tmp_nbg+3, c_ptr); + c_ptr += (workspace->tmp_nbg+3)->memsize; - // alias members of workspace and core_workspace - // - CREATE_STRVEC(nv+2*ns, workspace->sol_step->v, cws->dv); - // - CREATE_STRVEC(ne, workspace->sol_step->pi, cws->dpi); - // - CREATE_STRVEC(2*nb+2*ng+2*ns, workspace->sol_step->lam, cws->dlam); - // - CREATE_STRVEC(2*nb+2*ng+2*ns, workspace->sol_step->t, cws->dt); - // - CREATE_STRVEC(nv+2*ns, workspace->res->res_g, cws->res_g); - // - CREATE_STRVEC(ne, workspace->res->res_b, cws->res_b); - // - CREATE_STRVEC(2*nb+2*ng+2*ns, workspace->res->res_d, cws->res_d); - // - CREATE_STRVEC(2*nb+2*ng+2*ns, workspace->res->res_m, cws->res_m); - // - CREATE_STRVEC(2*nb+2*ng+2*ns, workspace->Gamma, cws->Gamma); - // - CREATE_STRVEC(2*nb+2*ng+2*ns, workspace->gamma, cws->gamma); + CREATE_STRVEC(ns, workspace->tmp_ns+0, c_ptr); + CREATE_STRVEC(ns, workspace->res_workspace->tmp_ns+0, c_ptr); + c_ptr += (workspace->tmp_ns+0)->memsize; - // - workspace->sol_step->dim = dim; + CREATE_STRVEC(2*nb+2*ng+2*ns, workspace->tmp_m, c_ptr); + c_ptr += (workspace->tmp_m)->memsize; - // - workspace->use_hess_fact = 0; + CREATE_CORE_QP_IPM(nv+2*ns, ne, 2*nb+2*ng+2*ns, cws, c_ptr); + c_ptr += workspace->core_workspace->memsize; - // - workspace->memsize = MEMSIZE_DENSE_QP_IPM(dim, arg); + if(arg->lq_fact>0) + { + workspace->lq_work0 = c_ptr; + c_ptr += GELQF_WORKSIZE(ne, nv); + + workspace->lq_work1 = c_ptr; + c_ptr += GELQF_WORKSIZE(nv, nv+nv+ng); + } + + + // alias members of workspace and core_workspace + // + CREATE_STRVEC(nv+2*ns, workspace->sol_step->v, cws->dv); + // + CREATE_STRVEC(ne, workspace->sol_step->pi, cws->dpi); + // + CREATE_STRVEC(2*nb+2*ng+2*ns, workspace->sol_step->lam, cws->dlam); + // + CREATE_STRVEC(2*nb+2*ng+2*ns, workspace->sol_step->t, cws->dt); + // + CREATE_STRVEC(nv+2*ns, workspace->res->res_g, cws->res_g); + // + CREATE_STRVEC(ne, workspace->res->res_b, cws->res_b); + // + CREATE_STRVEC(2*nb+2*ng+2*ns, workspace->res->res_d, cws->res_d); + // + CREATE_STRVEC(2*nb+2*ng+2*ns, workspace->res->res_m, cws->res_m); + // + CREATE_STRVEC(2*nb+2*ng+2*ns, workspace->Gamma, cws->Gamma); + // + CREATE_STRVEC(2*nb+2*ng+2*ns, workspace->gamma, cws->gamma); + + // + workspace->sol_step->dim = dim; + + // + workspace->use_hess_fact = 0; + + // + workspace->memsize = MEMSIZE_DENSE_QP_IPM(dim, arg); #if defined(RUNTIME_CHECKS) - if(c_ptr > ((char *) mem) + workspace->memsize) - { - printf("\nCreate_dense_qp_ipm: outside memory bounds!\n\n"); - exit(1); - } + if(c_ptr > ((char *) mem) + workspace->memsize) + { + printf("\nCreate_dense_qp_ipm: outside memory bounds!\n\n"); + exit(1); + } #endif - return; + return; - } + } int SOLVE_DENSE_QP_IPM(struct DENSE_QP *qp, struct DENSE_QP_SOL *qp_sol, struct DENSE_QP_IPM_ARG *arg, struct DENSE_QP_IPM_WORKSPACE *ws) - { - - struct CORE_QP_IPM_WORKSPACE *cws = ws->core_workspace; - - // arg to core workspace - cws->lam_min = arg->lam_min; - cws->t_min = arg->t_min; - - // alias qp vectors into qp_sol - cws->v = qp_sol->v->pa; - cws->pi = qp_sol->pi->pa; - cws->lam = qp_sol->lam->pa; - cws->t = qp_sol->t->pa; - - // alias members of qp_step - ws->qp_step->dim = qp->dim; - ws->qp_step->Hv = qp->Hv; - ws->qp_step->A = qp->A; - ws->qp_step->Ct = qp->Ct; - ws->qp_step->Z = qp->Z; - ws->qp_step->idxb = qp->idxb; - ws->qp_step->idxs = qp->idxs; - ws->qp_step->gz = ws->res->res_g; - ws->qp_step->b = ws->res->res_b; - ws->qp_step->d = ws->res->res_d; - ws->qp_step->m = ws->res->res_m; - - // alias members of qp_itref - ws->qp_itref->dim = qp->dim; - ws->qp_itref->Hv = qp->Hv; - ws->qp_itref->A = qp->A; - ws->qp_itref->Ct = qp->Ct; - ws->qp_itref->Z = qp->Z; - ws->qp_itref->idxb = qp->idxb; - ws->qp_itref->idxs = qp->idxs; - ws->qp_itref->gz = ws->res_itref->res_g; - ws->qp_itref->b = ws->res_itref->res_b; - ws->qp_itref->d = ws->res_itref->res_d; - ws->qp_itref->m = ws->res_itref->res_m; - - // no constraints - if(cws->nc==0) - { - FACT_SOLVE_KKT_UNCONSTR_DENSE_QP(qp, qp_sol, arg, ws); - COMPUTE_RES_DENSE_QP(qp, qp_sol, ws->res, ws->res_workspace); - cws->mu = ws->res->res_mu; - ws->iter = 0; - return 0; - } - - // blasfeo alias for residuals - struct STRVEC str_res_g; - struct STRVEC str_res_b; - struct STRVEC str_res_d; - struct STRVEC str_res_m; - str_res_g.m = cws->nv; - str_res_b.m = cws->ne; - str_res_d.m = cws->nc; - str_res_m.m = cws->nc; - str_res_g.pa = cws->res_g; - str_res_b.pa = cws->res_b; - str_res_d.pa = cws->res_d; - str_res_m.pa = cws->res_m; - - REAL *qp_res = ws->qp_res; - qp_res[0] = 0; - qp_res[1] = 0; - qp_res[2] = 0; - qp_res[3] = 0; - - // dims - int nv = qp->dim->nv; - int ne = qp->dim->ne; - int nb = qp->dim->nb; - int ng = qp->dim->ng; - int ns = qp->dim->ns; - - int kk, ii, itref0=0, itref1=0; - REAL tmp; - REAL mu_aff0, mu; - int iter_ref_step; - - // init solver - INIT_VAR_DENSE_QP(qp, qp_sol, arg, ws); - - cws->alpha = 1.0; - - - - // absolute IPM formulation - - if(arg->abs_form) - { - - // alias members of qp_step - ws->qp_step->dim = qp->dim; - ws->qp_step->Hv = qp->Hv; - ws->qp_step->A = qp->A; - ws->qp_step->Ct = qp->Ct; - ws->qp_step->Z = qp->Z; - ws->qp_step->idxb = qp->idxb; - ws->qp_step->idxs = qp->idxs; - ws->qp_step->gz = qp->gz; - ws->qp_step->b = qp->b; - ws->qp_step->d = qp->d; - ws->qp_step->m = ws->tmp_m; - - // alias core workspace - cws->res_m = ws->qp_step->m->pa; - cws->res_m_bkp = ws->qp_step->m->pa; - - mu = VECMULDOT(cws->nc, qp_sol->lam, 0, qp_sol->t, 0, ws->tmp_m, 0); - mu /= cws->nc; - cws->mu = mu; - - // IPM loop (absolute formulation) - for(kk=0; \ - kkiter_max & \ - cws->alpha>arg->alpha_min & \ - mu>arg->res_m_max; kk++) - { - - VECSC(cws->nc, -1.0, ws->tmp_m, 0); - - // fact solve - FACT_SOLVE_KKT_STEP_DENSE_QP(ws->qp_step, ws->sol_step, arg, ws); - - // compute step - AXPY(cws->nv, -1.0, qp_sol->v, 0, ws->sol_step->v, 0, ws->sol_step->v, 0); - AXPY(cws->ne, -1.0, qp_sol->pi, 0, ws->sol_step->pi, 0, ws->sol_step->pi, 0); - AXPY(cws->nc, -1.0, qp_sol->lam, 0, ws->sol_step->lam, 0, ws->sol_step->lam, 0); - AXPY(cws->nc, -1.0, qp_sol->t, 0, ws->sol_step->t, 0, ws->sol_step->t, 0); - - // alpha - COMPUTE_ALPHA_QP(cws); - if(kkstat_max) - ws->stat[5*kk+0] = cws->alpha; - - // Mehrotra's predictor-corrector - if(arg->pred_corr==1) - { - // mu_aff - COMPUTE_MU_AFF_QP(cws); - if(kkstat_max) - ws->stat[5*kk+1] = cws->mu_aff; - - tmp = cws->mu_aff/cws->mu; - cws->sigma = tmp*tmp*tmp; - if(kkstat_max) - ws->stat[5*kk+2] = cws->sigma; - - COMPUTE_CENTERING_CORRECTION_QP(cws); - - // fact and solve kkt - SOLVE_KKT_STEP_DENSE_QP(ws->qp_step, ws->sol_step, arg, ws); - - // compute step - AXPY(cws->nv, -1.0, qp_sol->v, 0, ws->sol_step->v, 0, ws->sol_step->v, 0); - AXPY(cws->ne, -1.0, qp_sol->pi, 0, ws->sol_step->pi, 0, ws->sol_step->pi, 0); - AXPY(cws->nc, -1.0, qp_sol->lam, 0, ws->sol_step->lam, 0, ws->sol_step->lam, 0); - AXPY(cws->nc, -1.0, qp_sol->t, 0, ws->sol_step->t, 0, ws->sol_step->t, 0); - - // alpha - COMPUTE_ALPHA_QP(cws); - if(kkstat_max) - ws->stat[5*kk+3] = cws->alpha; - - } - - // - UPDATE_VAR_QP(cws); - - // compute mu - mu = VECMULDOT(cws->nc, qp_sol->lam, 0, qp_sol->t, 0, ws->tmp_m, 0); - mu /= cws->nc; - cws->mu = mu; - if(kkstat_max) - ws->stat[5*kk+4] = mu; - - // exit(1); - - } - - if(arg->comp_res_exit) - { - // compute residuals - COMPUTE_RES_DENSE_QP(qp, qp_sol, ws->res, ws->res_workspace); - - // compute infinity norm of residuals - VECNRM_INF(cws->nv, &str_res_g, 0, &qp_res[0]); - VECNRM_INF(cws->ne, &str_res_b, 0, &qp_res[1]); - VECNRM_INF(cws->nc, &str_res_d, 0, &qp_res[2]); - VECNRM_INF(cws->nc, &str_res_m, 0, &qp_res[3]); + { + + struct CORE_QP_IPM_WORKSPACE *cws = ws->core_workspace; + + // arg to core workspace + cws->lam_min = arg->lam_min; + cws->t_min = arg->t_min; + + // alias qp vectors into qp_sol + cws->v = qp_sol->v->pa; + cws->pi = qp_sol->pi->pa; + cws->lam = qp_sol->lam->pa; + cws->t = qp_sol->t->pa; + + // alias members of qp_step + ws->qp_step->dim = qp->dim; + ws->qp_step->Hv = qp->Hv; + ws->qp_step->A = qp->A; + ws->qp_step->Ct = qp->Ct; + ws->qp_step->Z = qp->Z; + ws->qp_step->idxb = qp->idxb; + ws->qp_step->idxs = qp->idxs; + ws->qp_step->gz = ws->res->res_g; + ws->qp_step->b = ws->res->res_b; + ws->qp_step->d = ws->res->res_d; + ws->qp_step->m = ws->res->res_m; + + // alias members of qp_itref + ws->qp_itref->dim = qp->dim; + ws->qp_itref->Hv = qp->Hv; + ws->qp_itref->A = qp->A; + ws->qp_itref->Ct = qp->Ct; + ws->qp_itref->Z = qp->Z; + ws->qp_itref->idxb = qp->idxb; + ws->qp_itref->idxs = qp->idxs; + ws->qp_itref->gz = ws->res_itref->res_g; + ws->qp_itref->b = ws->res_itref->res_b; + ws->qp_itref->d = ws->res_itref->res_d; + ws->qp_itref->m = ws->res_itref->res_m; + + // no constraints + if(cws->nc==0) + { + FACT_SOLVE_KKT_UNCONSTR_DENSE_QP(qp, qp_sol, arg, ws); + COMPUTE_RES_DENSE_QP(qp, qp_sol, ws->res, ws->res_workspace); + cws->mu = ws->res->res_mu; + ws->iter = 0; + return 0; + } + + // blasfeo alias for residuals + struct STRVEC str_res_g; + struct STRVEC str_res_b; + struct STRVEC str_res_d; + struct STRVEC str_res_m; + str_res_g.m = cws->nv; + str_res_b.m = cws->ne; + str_res_d.m = cws->nc; + str_res_m.m = cws->nc; + str_res_g.pa = cws->res_g; + str_res_b.pa = cws->res_b; + str_res_d.pa = cws->res_d; + str_res_m.pa = cws->res_m; + + REAL *qp_res = ws->qp_res; + qp_res[0] = 0; + qp_res[1] = 0; + qp_res[2] = 0; + qp_res[3] = 0; + + // dims + int nv = qp->dim->nv; + int ne = qp->dim->ne; + int nb = qp->dim->nb; + int ng = qp->dim->ng; + int ns = qp->dim->ns; + + int kk, ii, itref0=0, itref1=0; + REAL tmp; + REAL mu_aff0, mu; + int iter_ref_step; + + // init solver + INIT_VAR_DENSE_QP(qp, qp_sol, arg, ws); + + cws->alpha = 1.0; + + + + // absolute IPM formulation + + if(arg->abs_form) + { + + // alias members of qp_step + ws->qp_step->dim = qp->dim; + ws->qp_step->Hv = qp->Hv; + ws->qp_step->A = qp->A; + ws->qp_step->Ct = qp->Ct; + ws->qp_step->Z = qp->Z; + ws->qp_step->idxb = qp->idxb; + ws->qp_step->idxs = qp->idxs; + ws->qp_step->gz = qp->gz; + ws->qp_step->b = qp->b; + ws->qp_step->d = qp->d; + ws->qp_step->m = ws->tmp_m; + + // alias core workspace + cws->res_m = ws->qp_step->m->pa; + cws->res_m_bkp = ws->qp_step->m->pa; + + mu = VECMULDOT(cws->nc, qp_sol->lam, 0, qp_sol->t, 0, ws->tmp_m, 0); + mu /= cws->nc; + cws->mu = mu; + + // IPM loop (absolute formulation) + for(kk=0; \ + kkiter_max & \ + cws->alpha>arg->alpha_min & \ + mu>arg->res_m_max; kk++) + { + + VECSC(cws->nc, -1.0, ws->tmp_m, 0); + + // fact solve + FACT_SOLVE_KKT_STEP_DENSE_QP(ws->qp_step, ws->sol_step, arg, ws); + + // compute step + AXPY(cws->nv, -1.0, qp_sol->v, 0, ws->sol_step->v, 0, ws->sol_step->v, 0); + AXPY(cws->ne, -1.0, qp_sol->pi, 0, ws->sol_step->pi, 0, ws->sol_step->pi, 0); + AXPY(cws->nc, -1.0, qp_sol->lam, 0, ws->sol_step->lam, 0, ws->sol_step->lam, 0); + AXPY(cws->nc, -1.0, qp_sol->t, 0, ws->sol_step->t, 0, ws->sol_step->t, 0); + + // alpha + COMPUTE_ALPHA_QP(cws); + if(kkstat_max) + ws->stat[5*kk+0] = cws->alpha; + + // Mehrotra's predictor-corrector + if(arg->pred_corr==1) + { + // mu_aff + COMPUTE_MU_AFF_QP(cws); + if(kkstat_max) + ws->stat[5*kk+1] = cws->mu_aff; + + tmp = cws->mu_aff/cws->mu; + cws->sigma = tmp*tmp*tmp; + if(kkstat_max) + ws->stat[5*kk+2] = cws->sigma; + + COMPUTE_CENTERING_CORRECTION_QP(cws); + + // fact and solve kkt + SOLVE_KKT_STEP_DENSE_QP(ws->qp_step, ws->sol_step, arg, ws); + + // compute step + AXPY(cws->nv, -1.0, qp_sol->v, 0, ws->sol_step->v, 0, ws->sol_step->v, 0); + AXPY(cws->ne, -1.0, qp_sol->pi, 0, ws->sol_step->pi, 0, ws->sol_step->pi, 0); + AXPY(cws->nc, -1.0, qp_sol->lam, 0, ws->sol_step->lam, 0, ws->sol_step->lam, 0); + AXPY(cws->nc, -1.0, qp_sol->t, 0, ws->sol_step->t, 0, ws->sol_step->t, 0); + + // alpha + COMPUTE_ALPHA_QP(cws); + if(kkstat_max) + ws->stat[5*kk+3] = cws->alpha; + + } + + // + UPDATE_VAR_QP(cws); + + // compute mu + mu = VECMULDOT(cws->nc, qp_sol->lam, 0, qp_sol->t, 0, ws->tmp_m, 0); + mu /= cws->nc; + cws->mu = mu; + if(kkstat_max) + ws->stat[5*kk+4] = mu; + + // exit(1); + + } + + if(arg->comp_res_exit) + { + // compute residuals + COMPUTE_RES_DENSE_QP(qp, qp_sol, ws->res, ws->res_workspace); + + // compute infinity norm of residuals + VECNRM_INF(cws->nv, &str_res_g, 0, &qp_res[0]); + VECNRM_INF(cws->ne, &str_res_b, 0, &qp_res[1]); + VECNRM_INF(cws->nc, &str_res_d, 0, &qp_res[2]); + VECNRM_INF(cws->nc, &str_res_m, 0, &qp_res[3]); if(arg->print_level > 0) { if(kk%10 == 0) @@ -697,9 +697,9 @@ int SOLVE_DENSE_QP_IPM(struct DENSE_QP *qp, struct DENSE_QP_SOL *qp_sol, struct } printf("%-10d %-15e %-15e %-15e %-15e %-15e %-15e\n", kk, qp_res[0], qp_res[1], qp_res[2], qp_res[3], cws->alpha, cws->mu); } - } + } - ws->iter = kk; + ws->iter = kk; // max iteration number reached if(kk == arg->iter_max) { @@ -735,85 +735,85 @@ int SOLVE_DENSE_QP_IPM(struct DENSE_QP *qp, struct DENSE_QP_SOL *qp_sol, struct printf(" \n -> SOLUTION FOUND. \n\n"); return 0; - } + } - // compute residuals - COMPUTE_RES_DENSE_QP(qp, qp_sol, ws->res, ws->res_workspace); - BACKUP_RES_M(cws); - cws->mu = ws->res->res_mu; + // compute residuals + COMPUTE_RES_DENSE_QP(qp, qp_sol, ws->res, ws->res_workspace); + BACKUP_RES_M(cws); + cws->mu = ws->res->res_mu; - // compute infinity norm of residuals - VECNRM_INF(cws->nv, &str_res_g, 0, &qp_res[0]); - VECNRM_INF(cws->ne, &str_res_b, 0, &qp_res[1]); - VECNRM_INF(cws->nc, &str_res_d, 0, &qp_res[2]); - VECNRM_INF(cws->nc, &str_res_m, 0, &qp_res[3]); + // compute infinity norm of residuals + VECNRM_INF(cws->nv, &str_res_g, 0, &qp_res[0]); + VECNRM_INF(cws->ne, &str_res_b, 0, &qp_res[1]); + VECNRM_INF(cws->nc, &str_res_d, 0, &qp_res[2]); + VECNRM_INF(cws->nc, &str_res_m, 0, &qp_res[3]); -// REAL sigma_min = 1e9; -// sigma_min = arg->res_g_maxres_g_max : sigma_min; -// sigma_min = arg->res_b_maxres_b_max : sigma_min; -// sigma_min = arg->res_d_maxres_d_max : sigma_min; -// sigma_min = arg->res_m_maxres_m_max : sigma_min; -// sigma_min *= 0.1; +// REAL sigma_min = 1e9; +// sigma_min = arg->res_g_maxres_g_max : sigma_min; +// sigma_min = arg->res_b_maxres_b_max : sigma_min; +// sigma_min = arg->res_d_maxres_d_max : sigma_min; +// sigma_min = arg->res_m_maxres_m_max : sigma_min; +// sigma_min *= 0.1; - REAL itref_qp_norm[4] = {0,0,0,0}; - REAL itref_qp_norm0[4] = {0,0,0,0}; - int ndp0, ndp1; + REAL itref_qp_norm[4] = {0,0,0,0}; + REAL itref_qp_norm0[4] = {0,0,0,0}; + int ndp0, ndp1; - int force_lq = 0; + int force_lq = 0; - // relative IPM formulation + // relative IPM formulation - // IPM loop - for(kk=0; \ - kkiter_max & \ - cws->alpha>arg->alpha_min & \ - (qp_res[0]>arg->res_g_max | \ - qp_res[1]>arg->res_b_max | \ - qp_res[2]>arg->res_d_max | \ - qp_res[3]>arg->res_m_max); kk++) - { + // IPM loop + for(kk=0; \ + kkiter_max & \ + cws->alpha>arg->alpha_min & \ + (qp_res[0]>arg->res_g_max | \ + qp_res[1]>arg->res_b_max | \ + qp_res[2]>arg->res_d_max | \ + qp_res[3]>arg->res_m_max); kk++) + { - ws->scale = arg->scale; + ws->scale = arg->scale; - // fact and solve kkt - if(arg->lq_fact==0) - { + // fact and solve kkt + if(arg->lq_fact==0) + { - // syrk+cholesky - FACT_SOLVE_KKT_STEP_DENSE_QP(ws->qp_step, ws->sol_step, arg, ws); + // syrk+cholesky + FACT_SOLVE_KKT_STEP_DENSE_QP(ws->qp_step, ws->sol_step, arg, ws); - } - else if(arg->lq_fact==1 & force_lq==0) - { + } + else if(arg->lq_fact==1 & force_lq==0) + { - // syrk+chol, switch to lq when needed - FACT_SOLVE_KKT_STEP_DENSE_QP(ws->qp_step, ws->sol_step, arg, ws); + // syrk+chol, switch to lq when needed + FACT_SOLVE_KKT_STEP_DENSE_QP(ws->qp_step, ws->sol_step, arg, ws); - // compute res of linear system - COMPUTE_LIN_RES_DENSE_QP(ws->qp_step, qp_sol, ws->sol_step, ws->res_itref, ws->res_workspace); - VECNRM_INF(cws->nv, ws->res_itref->res_g, 0, &itref_qp_norm[0]); - VECNRM_INF(cws->ne, ws->res_itref->res_b, 0, &itref_qp_norm[1]); - VECNRM_INF(cws->nc, ws->res_itref->res_d, 0, &itref_qp_norm[2]); - VECNRM_INF(cws->nc, ws->res_itref->res_m, 0, &itref_qp_norm[3]); + // compute res of linear system + COMPUTE_LIN_RES_DENSE_QP(ws->qp_step, qp_sol, ws->sol_step, ws->res_itref, ws->res_workspace); + VECNRM_INF(cws->nv, ws->res_itref->res_g, 0, &itref_qp_norm[0]); + VECNRM_INF(cws->ne, ws->res_itref->res_b, 0, &itref_qp_norm[1]); + VECNRM_INF(cws->nc, ws->res_itref->res_d, 0, &itref_qp_norm[2]); + VECNRM_INF(cws->nc, ws->res_itref->res_m, 0, &itref_qp_norm[3]); //printf("\n%e\t%e\t%e\t%e\n", itref_qp_norm[0], itref_qp_norm[1], itref_qp_norm[2], itref_qp_norm[3]); - // inaccurate factorization: switch to lq - if( + // inaccurate factorization: switch to lq + if( #ifdef USE_C99_MATH - ( itref_qp_norm[0]==0.0 & isnan(BLASFEO_DVECEL(ws->res_itref->res_g, 0)) ) | + ( itref_qp_norm[0]==0.0 & isnan(BLASFEO_DVECEL(ws->res_itref->res_g, 0)) ) | #else - ( itref_qp_norm[0]==0.0 & BLASFEO_DVECEL(ws->res_itref->res_g, 0)!=BLASFEO_DVECEL(ws->res_itref->res_g, 0) ) | + ( itref_qp_norm[0]==0.0 & BLASFEO_DVECEL(ws->res_itref->res_g, 0)!=BLASFEO_DVECEL(ws->res_itref->res_g, 0) ) | #endif - itref_qp_norm[0]>1e-5 | - itref_qp_norm[1]>1e-5 | - itref_qp_norm[2]>1e-5 | - itref_qp_norm[3]>1e-5 ) - { + itref_qp_norm[0]>1e-5 | + itref_qp_norm[1]>1e-5 | + itref_qp_norm[2]>1e-5 | + itref_qp_norm[3]>1e-5 ) + { #if 0 blasfeo_print_tran_dvec(cws->nv, ws->sol_step->v, 0); @@ -822,11 +822,11 @@ blasfeo_print_tran_dvec(cws->nc, ws->sol_step->lam, 0); blasfeo_print_tran_dvec(cws->nc, ws->sol_step->t, 0); #endif - // refactorize using lq - FACT_LQ_SOLVE_KKT_STEP_DENSE_QP(ws->qp_step, ws->sol_step, arg, ws); + // refactorize using lq + FACT_LQ_SOLVE_KKT_STEP_DENSE_QP(ws->qp_step, ws->sol_step, arg, ws); - // switch to lq - force_lq = 1; + // switch to lq + force_lq = 1; #if 0 blasfeo_print_tran_dvec(cws->nv, ws->sol_step->v, 0); @@ -835,242 +835,242 @@ blasfeo_print_tran_dvec(cws->nc, ws->sol_step->lam, 0); blasfeo_print_tran_dvec(cws->nc, ws->sol_step->t, 0); #endif - } + } - } - else // arg->lq_fact==2 - { + } + else // arg->lq_fact==2 + { - // lq - FACT_LQ_SOLVE_KKT_STEP_DENSE_QP(ws->qp_step, ws->sol_step, arg, ws); + // lq + FACT_LQ_SOLVE_KKT_STEP_DENSE_QP(ws->qp_step, ws->sol_step, arg, ws); - } + } #if 0 - COMPUTE_LIN_RES_DENSE_QP(ws->qp_step, qp_sol, ws->sol_step, ws->res_itref, ws->res_workspace); - VECNRM_INF(cws->nv, ws->res_itref->res_g, 0, &itref_qp_norm[0]); - VECNRM_INF(cws->ne, ws->res_itref->res_b, 0, &itref_qp_norm[1]); - VECNRM_INF(cws->nc, ws->res_itref->res_d, 0, &itref_qp_norm[2]); - VECNRM_INF(cws->nc, ws->res_itref->res_m, 0, &itref_qp_norm[3]); -// printf("%e\t%e\t%e\t%e\t%e\t%e\t%e\t%e\t\n", qp_res[0], qp_res[1], qp_res[2], qp_res[3], itref_qp_norm[0], itref_qp_norm[1], itref_qp_norm[2], itref_qp_norm[3]); - if(itref_qp_norm[0]==0.0 & BLASFEO_DVECEL(ws->res_itref->res_g, 0)!=BLASFEO_DVECEL(ws->res_itref->res_g, 0)) - printf("NaN!!!\n"); + COMPUTE_LIN_RES_DENSE_QP(ws->qp_step, qp_sol, ws->sol_step, ws->res_itref, ws->res_workspace); + VECNRM_INF(cws->nv, ws->res_itref->res_g, 0, &itref_qp_norm[0]); + VECNRM_INF(cws->ne, ws->res_itref->res_b, 0, &itref_qp_norm[1]); + VECNRM_INF(cws->nc, ws->res_itref->res_d, 0, &itref_qp_norm[2]); + VECNRM_INF(cws->nc, ws->res_itref->res_m, 0, &itref_qp_norm[3]); +// printf("%e\t%e\t%e\t%e\t%e\t%e\t%e\t%e\t\n", qp_res[0], qp_res[1], qp_res[2], qp_res[3], itref_qp_norm[0], itref_qp_norm[1], itref_qp_norm[2], itref_qp_norm[3]); + if(itref_qp_norm[0]==0.0 & BLASFEO_DVECEL(ws->res_itref->res_g, 0)!=BLASFEO_DVECEL(ws->res_itref->res_g, 0)) + printf("NaN!!!\n"); #endif - // iterative refinement on prediction step - for(itref0=0; itref0itref_pred_max; itref0++) - { - - COMPUTE_LIN_RES_DENSE_QP(ws->qp_step, qp_sol, ws->sol_step, ws->res_itref, ws->res_workspace); - - VECNRM_INF(cws->nv, ws->res_itref->res_g, 0, &itref_qp_norm[0]); - VECNRM_INF(cws->ne, ws->res_itref->res_b, 0, &itref_qp_norm[1]); - VECNRM_INF(cws->nc, ws->res_itref->res_d, 0, &itref_qp_norm[2]); - VECNRM_INF(cws->nc, ws->res_itref->res_m, 0, &itref_qp_norm[3]); - - if(itref0==0) - { - itref_qp_norm0[0] = itref_qp_norm[0]; - itref_qp_norm0[1] = itref_qp_norm[1]; - itref_qp_norm0[2] = itref_qp_norm[2]; - itref_qp_norm0[3] = itref_qp_norm[3]; - } - - if( \ - (itref_qp_norm[0]<1e0*arg->res_g_max | itref_qp_norm[0]<1e-3*qp_res[0]) & \ - (itref_qp_norm[1]<1e0*arg->res_b_max | itref_qp_norm[1]<1e-3*qp_res[1]) & \ - (itref_qp_norm[2]<1e0*arg->res_d_max | itref_qp_norm[2]<1e-3*qp_res[2]) & \ - (itref_qp_norm[3]<1e0*arg->res_m_max | itref_qp_norm[3]<1e-3*qp_res[3]) ) -// (itref_qp_norm[0]<=arg->res_g_max) & \ - (itref_qp_norm[1]<=arg->res_b_max) & \ - (itref_qp_norm[2]<=arg->res_d_max) & \ - (itref_qp_norm[3]<=arg->res_m_max) ) - { - break; - } - - SOLVE_KKT_STEP_DENSE_QP(ws->qp_itref, ws->sol_itref, arg, ws); - - AXPY(nv+2*ns, 1.0, ws->sol_itref->v, 0, ws->sol_step->v, 0, ws->sol_step->v, 0); - AXPY(ne, 1.0, ws->sol_itref->pi, 0, ws->sol_step->pi, 0, ws->sol_step->pi, 0); - AXPY(2*nb+2*ng+2*ns, 1.0, ws->sol_itref->lam, 0, ws->sol_step->lam, 0, ws->sol_step->lam, 0); - AXPY(2*nb+2*ng+2*ns, 1.0, ws->sol_itref->t, 0, ws->sol_step->t, 0, ws->sol_step->t, 0); - - } + // iterative refinement on prediction step + for(itref0=0; itref0itref_pred_max; itref0++) + { + + COMPUTE_LIN_RES_DENSE_QP(ws->qp_step, qp_sol, ws->sol_step, ws->res_itref, ws->res_workspace); + + VECNRM_INF(cws->nv, ws->res_itref->res_g, 0, &itref_qp_norm[0]); + VECNRM_INF(cws->ne, ws->res_itref->res_b, 0, &itref_qp_norm[1]); + VECNRM_INF(cws->nc, ws->res_itref->res_d, 0, &itref_qp_norm[2]); + VECNRM_INF(cws->nc, ws->res_itref->res_m, 0, &itref_qp_norm[3]); + + if(itref0==0) + { + itref_qp_norm0[0] = itref_qp_norm[0]; + itref_qp_norm0[1] = itref_qp_norm[1]; + itref_qp_norm0[2] = itref_qp_norm[2]; + itref_qp_norm0[3] = itref_qp_norm[3]; + } + + if( \ + (itref_qp_norm[0]<1e0*arg->res_g_max | itref_qp_norm[0]<1e-3*qp_res[0]) & \ + (itref_qp_norm[1]<1e0*arg->res_b_max | itref_qp_norm[1]<1e-3*qp_res[1]) & \ + (itref_qp_norm[2]<1e0*arg->res_d_max | itref_qp_norm[2]<1e-3*qp_res[2]) & \ + (itref_qp_norm[3]<1e0*arg->res_m_max | itref_qp_norm[3]<1e-3*qp_res[3]) ) +// (itref_qp_norm[0]<=arg->res_g_max) & \ + (itref_qp_norm[1]<=arg->res_b_max) & \ + (itref_qp_norm[2]<=arg->res_d_max) & \ + (itref_qp_norm[3]<=arg->res_m_max) ) + { + break; + } + + SOLVE_KKT_STEP_DENSE_QP(ws->qp_itref, ws->sol_itref, arg, ws); + + AXPY(nv+2*ns, 1.0, ws->sol_itref->v, 0, ws->sol_step->v, 0, ws->sol_step->v, 0); + AXPY(ne, 1.0, ws->sol_itref->pi, 0, ws->sol_step->pi, 0, ws->sol_step->pi, 0); + AXPY(2*nb+2*ng+2*ns, 1.0, ws->sol_itref->lam, 0, ws->sol_step->lam, 0, ws->sol_step->lam, 0); + AXPY(2*nb+2*ng+2*ns, 1.0, ws->sol_itref->t, 0, ws->sol_step->t, 0, ws->sol_step->t, 0); + + } #if 0 - COMPUTE_LIN_RES_DENSE_QP(ws->qp_step, qp_sol, ws->sol_step, ws->res_itref, ws->res_workspace); - VECNRM_INF(cws->nv, ws->res_itref->res_g, 0, &itref_qp_norm0[0]); - VECNRM_INF(cws->ne, ws->res_itref->res_b, 0, &itref_qp_norm0[1]); - VECNRM_INF(cws->nc, ws->res_itref->res_d, 0, &itref_qp_norm0[2]); - VECNRM_INF(cws->nc, ws->res_itref->res_m, 0, &itref_qp_norm0[3]); -// printf("\nkk = %d\n", kk); -// blasfeo_print_exp_tran_dvec(qp->dim->nv, ws->res_itref->res_g, 0); -// blasfeo_print_exp_tran_dvec(qp->dim->ne, ws->res_itref->res_b, 0); -// blasfeo_print_exp_tran_dvec(2*qp->dim->nb+2*qp->dim->ng, ws->res_itref->res_d, 0); + COMPUTE_LIN_RES_DENSE_QP(ws->qp_step, qp_sol, ws->sol_step, ws->res_itref, ws->res_workspace); + VECNRM_INF(cws->nv, ws->res_itref->res_g, 0, &itref_qp_norm0[0]); + VECNRM_INF(cws->ne, ws->res_itref->res_b, 0, &itref_qp_norm0[1]); + VECNRM_INF(cws->nc, ws->res_itref->res_d, 0, &itref_qp_norm0[2]); + VECNRM_INF(cws->nc, ws->res_itref->res_m, 0, &itref_qp_norm0[3]); +// printf("\nkk = %d\n", kk); +// blasfeo_print_exp_tran_dvec(qp->dim->nv, ws->res_itref->res_g, 0); +// blasfeo_print_exp_tran_dvec(qp->dim->ne, ws->res_itref->res_b, 0); +// blasfeo_print_exp_tran_dvec(2*qp->dim->nb+2*qp->dim->ng, ws->res_itref->res_d, 0); #endif #if 0 - COMPUTE_RES_DENSE_QP(ws->qp_step, ws->sol_step, ws->res_itref, ws->res_workspace); - VECNRM_INF(cws->nv, ws->res_itref->res_g, 0, &itref_qp_norm0[0]); - VECNRM_INF(cws->ne, ws->res_itref->res_b, 0, &itref_qp_norm0[1]); - VECNRM_INF(cws->nc, ws->res_itref->res_d, 0, &itref_qp_norm0[2]); - VECNRM_INF(cws->nc, ws->res_itref->res_m, 0, &itref_qp_norm0[3]); -// printf("\nkk = %d\n", kk); -// blasfeo_print_exp_tran_dvec(qp->dim->nv, ws->res_itref->res_g, 0); -// blasfeo_print_exp_tran_dvec(qp->dim->ne, ws->res_itref->res_b, 0); -// blasfeo_print_exp_tran_dvec(2*qp->dim->nb+2*qp->dim->ng, ws->res_itref->res_d, 0); + COMPUTE_RES_DENSE_QP(ws->qp_step, ws->sol_step, ws->res_itref, ws->res_workspace); + VECNRM_INF(cws->nv, ws->res_itref->res_g, 0, &itref_qp_norm0[0]); + VECNRM_INF(cws->ne, ws->res_itref->res_b, 0, &itref_qp_norm0[1]); + VECNRM_INF(cws->nc, ws->res_itref->res_d, 0, &itref_qp_norm0[2]); + VECNRM_INF(cws->nc, ws->res_itref->res_m, 0, &itref_qp_norm0[3]); +// printf("\nkk = %d\n", kk); +// blasfeo_print_exp_tran_dvec(qp->dim->nv, ws->res_itref->res_g, 0); +// blasfeo_print_exp_tran_dvec(qp->dim->ne, ws->res_itref->res_b, 0); +// blasfeo_print_exp_tran_dvec(2*qp->dim->nb+2*qp->dim->ng, ws->res_itref->res_d, 0); #endif #if 0 - ndp0 = 0; - for(ii=0; iidim->nv; ii++) - { - if(ws->Lv->dA[ii]<=0) - { - ndp0 = ii; - break; - } - } - ndp1 = 0; - for(ii=0; iidim->ne; ii++) - { - if(ws->Le->dA[ii]<=0) - { - ndp1 = ii; - break; - } - } + ndp0 = 0; + for(ii=0; iidim->nv; ii++) + { + if(ws->Lv->dA[ii]<=0) + { + ndp0 = ii; + break; + } + } + ndp1 = 0; + for(ii=0; iidim->ne; ii++) + { + if(ws->Le->dA[ii]<=0) + { + ndp1 = ii; + break; + } + } #endif - // alpha - COMPUTE_ALPHA_QP(cws); - if(kkstat_max) - ws->stat[5*kk+0] = cws->alpha; - - // Mehrotra's predictor-corrector - if(arg->pred_corr==1) - { - // mu_aff - COMPUTE_MU_AFF_QP(cws); - if(kkstat_max) - ws->stat[5*kk+1] = cws->mu_aff; - - // compute centering parameter - tmp = cws->mu_aff/cws->mu; - cws->sigma = tmp*tmp*tmp; -// cws->sigma = sigma_min>cws->sigma ? sigma_min : cws->sigma; - if(kkstat_max) - ws->stat[5*kk+2] = cws->sigma; - - COMPUTE_CENTERING_CORRECTION_QP(cws); - - // solve kkt - SOLVE_KKT_STEP_DENSE_QP(ws->qp_step, ws->sol_step, arg, ws); - - // alpha - COMPUTE_ALPHA_QP(cws); - if(kkstat_max) - ws->stat[5*kk+3] = cws->alpha; - - // conditional Mehrotra's predictor-corrector - if(arg->cond_pred_corr==1) - { - - // save mu_aff (from prediction sol_step) - mu_aff0 = cws->mu_aff; - - // compute mu for predictor-corrector-centering - COMPUTE_MU_AFF_QP(cws); - -// if(cws->mu_aff > 2.0*cws->mu) - if(cws->mu_aff > 2.0*mu_aff0) - { - - // centering direction - COMPUTE_CENTERING_QP(cws); - - // solve kkt - SOLVE_KKT_STEP_DENSE_QP(ws->qp_step, ws->sol_step, arg, ws); - - // alpha - COMPUTE_ALPHA_QP(cws); - if(kkstat_max) - ws->stat[5*kk+3] = cws->alpha; - - } - - } - - iter_ref_step = 0; - for(itref1=0; itref1itref_corr_max; itref1++) - { - - COMPUTE_LIN_RES_DENSE_QP(ws->qp_step, qp_sol, ws->sol_step, ws->res_itref, ws->res_workspace); - - VECNRM_INF(cws->nv, ws->res_itref->res_g, 0, &itref_qp_norm[0]); - VECNRM_INF(cws->ne, ws->res_itref->res_b, 0, &itref_qp_norm[1]); - VECNRM_INF(cws->nc, ws->res_itref->res_d, 0, &itref_qp_norm[2]); - VECNRM_INF(cws->nc, ws->res_itref->res_m, 0, &itref_qp_norm[3]); - - if(itref1==0) - { - itref_qp_norm0[0] = itref_qp_norm[0]; - itref_qp_norm0[1] = itref_qp_norm[1]; - itref_qp_norm0[2] = itref_qp_norm[2]; - itref_qp_norm0[3] = itref_qp_norm[3]; - } - - if( \ - (itref_qp_norm[0]<1e0*arg->res_g_max | itref_qp_norm[0]<1e-3*qp_res[0]) & \ - (itref_qp_norm[1]<1e0*arg->res_b_max | itref_qp_norm[1]<1e-3*qp_res[1]) & \ - (itref_qp_norm[2]<1e0*arg->res_d_max | itref_qp_norm[2]<1e-3*qp_res[2]) & \ - (itref_qp_norm[3]<1e0*arg->res_m_max | itref_qp_norm[3]<1e-3*qp_res[3]) ) -// (itref_qp_norm[0]<=arg->res_g_max) & \ - (itref_qp_norm[1]<=arg->res_b_max) & \ - (itref_qp_norm[2]<=arg->res_d_max) & \ - (itref_qp_norm[3]<=arg->res_m_max) ) - { - break; - } - - SOLVE_KKT_STEP_DENSE_QP(ws->qp_itref, ws->sol_itref, arg, ws); - iter_ref_step = 1; - - AXPY(nv+2*ns, 1.0, ws->sol_itref->v, 0, ws->sol_step->v, 0, ws->sol_step->v, 0); - AXPY(ne, 1.0, ws->sol_itref->pi, 0, ws->sol_step->pi, 0, ws->sol_step->pi, 0); - AXPY(2*nb+2*ng+2*ns, 1.0, ws->sol_itref->lam, 0, ws->sol_step->lam, 0, ws->sol_step->lam, 0); - AXPY(2*nb+2*ng+2*ns, 1.0, ws->sol_itref->t, 0, ws->sol_step->t, 0, ws->sol_step->t, 0); - - } - - if(iter_ref_step) - { - // alpha - COMPUTE_ALPHA_QP(cws); - if(kkstat_max) - ws->stat[5*kk+3] = cws->alpha; - } - - } - - // - UPDATE_VAR_QP(cws); - - // compute residuals - COMPUTE_RES_DENSE_QP(qp, qp_sol, ws->res, ws->res_workspace); - BACKUP_RES_M(cws); - cws->mu = ws->res->res_mu; - if(kkstat_max) - ws->stat[5*kk+4] = ws->res->res_mu; - - // compute infinity norm of residuals - VECNRM_INF(cws->nv, &str_res_g, 0, &qp_res[0]); - VECNRM_INF(cws->ne, &str_res_b, 0, &qp_res[1]); - VECNRM_INF(cws->nc, &str_res_d, 0, &qp_res[2]); - VECNRM_INF(cws->nc, &str_res_m, 0, &qp_res[3]); + // alpha + COMPUTE_ALPHA_QP(cws); + if(kkstat_max) + ws->stat[5*kk+0] = cws->alpha; + + // Mehrotra's predictor-corrector + if(arg->pred_corr==1) + { + // mu_aff + COMPUTE_MU_AFF_QP(cws); + if(kkstat_max) + ws->stat[5*kk+1] = cws->mu_aff; + + // compute centering parameter + tmp = cws->mu_aff/cws->mu; + cws->sigma = tmp*tmp*tmp; +// cws->sigma = sigma_min>cws->sigma ? sigma_min : cws->sigma; + if(kkstat_max) + ws->stat[5*kk+2] = cws->sigma; + + COMPUTE_CENTERING_CORRECTION_QP(cws); + + // solve kkt + SOLVE_KKT_STEP_DENSE_QP(ws->qp_step, ws->sol_step, arg, ws); + + // alpha + COMPUTE_ALPHA_QP(cws); + if(kkstat_max) + ws->stat[5*kk+3] = cws->alpha; + + // conditional Mehrotra's predictor-corrector + if(arg->cond_pred_corr==1) + { + + // save mu_aff (from prediction sol_step) + mu_aff0 = cws->mu_aff; + + // compute mu for predictor-corrector-centering + COMPUTE_MU_AFF_QP(cws); + +// if(cws->mu_aff > 2.0*cws->mu) + if(cws->mu_aff > 2.0*mu_aff0) + { + + // centering direction + COMPUTE_CENTERING_QP(cws); + + // solve kkt + SOLVE_KKT_STEP_DENSE_QP(ws->qp_step, ws->sol_step, arg, ws); + + // alpha + COMPUTE_ALPHA_QP(cws); + if(kkstat_max) + ws->stat[5*kk+3] = cws->alpha; + + } + + } + + iter_ref_step = 0; + for(itref1=0; itref1itref_corr_max; itref1++) + { + + COMPUTE_LIN_RES_DENSE_QP(ws->qp_step, qp_sol, ws->sol_step, ws->res_itref, ws->res_workspace); + + VECNRM_INF(cws->nv, ws->res_itref->res_g, 0, &itref_qp_norm[0]); + VECNRM_INF(cws->ne, ws->res_itref->res_b, 0, &itref_qp_norm[1]); + VECNRM_INF(cws->nc, ws->res_itref->res_d, 0, &itref_qp_norm[2]); + VECNRM_INF(cws->nc, ws->res_itref->res_m, 0, &itref_qp_norm[3]); + + if(itref1==0) + { + itref_qp_norm0[0] = itref_qp_norm[0]; + itref_qp_norm0[1] = itref_qp_norm[1]; + itref_qp_norm0[2] = itref_qp_norm[2]; + itref_qp_norm0[3] = itref_qp_norm[3]; + } + + if( \ + (itref_qp_norm[0]<1e0*arg->res_g_max | itref_qp_norm[0]<1e-3*qp_res[0]) & \ + (itref_qp_norm[1]<1e0*arg->res_b_max | itref_qp_norm[1]<1e-3*qp_res[1]) & \ + (itref_qp_norm[2]<1e0*arg->res_d_max | itref_qp_norm[2]<1e-3*qp_res[2]) & \ + (itref_qp_norm[3]<1e0*arg->res_m_max | itref_qp_norm[3]<1e-3*qp_res[3]) ) +// (itref_qp_norm[0]<=arg->res_g_max) & \ + (itref_qp_norm[1]<=arg->res_b_max) & \ + (itref_qp_norm[2]<=arg->res_d_max) & \ + (itref_qp_norm[3]<=arg->res_m_max) ) + { + break; + } + + SOLVE_KKT_STEP_DENSE_QP(ws->qp_itref, ws->sol_itref, arg, ws); + iter_ref_step = 1; + + AXPY(nv+2*ns, 1.0, ws->sol_itref->v, 0, ws->sol_step->v, 0, ws->sol_step->v, 0); + AXPY(ne, 1.0, ws->sol_itref->pi, 0, ws->sol_step->pi, 0, ws->sol_step->pi, 0); + AXPY(2*nb+2*ng+2*ns, 1.0, ws->sol_itref->lam, 0, ws->sol_step->lam, 0, ws->sol_step->lam, 0); + AXPY(2*nb+2*ng+2*ns, 1.0, ws->sol_itref->t, 0, ws->sol_step->t, 0, ws->sol_step->t, 0); + + } + + if(iter_ref_step) + { + // alpha + COMPUTE_ALPHA_QP(cws); + if(kkstat_max) + ws->stat[5*kk+3] = cws->alpha; + } + + } + + // + UPDATE_VAR_QP(cws); + + // compute residuals + COMPUTE_RES_DENSE_QP(qp, qp_sol, ws->res, ws->res_workspace); + BACKUP_RES_M(cws); + cws->mu = ws->res->res_mu; + if(kkstat_max) + ws->stat[5*kk+4] = ws->res->res_mu; + + // compute infinity norm of residuals + VECNRM_INF(cws->nv, &str_res_g, 0, &qp_res[0]); + VECNRM_INF(cws->ne, &str_res_b, 0, &qp_res[1]); + VECNRM_INF(cws->nc, &str_res_d, 0, &qp_res[2]); + VECNRM_INF(cws->nc, &str_res_m, 0, &qp_res[3]); if(arg->print_level > 0){ if(kk%10 == 0){ @@ -1097,9 +1097,9 @@ printf("%e %e %e %e\n", qp_res[0], qp_res[1], qp_res[2], qp_res[3]); printf("iter %3d alpha %1.3e %1.3e sigma %1.3e ndp %3d %3d itref %d %d res_kkt %1.3e %1.3e %1.3e %1.3e res_kkt %1.3e %1.3e %1.3e %1.3e res_qp %1.3e %1.3e %1.3e %1.3e\n", kk, cws->alpha_prim, cws->alpha_dual, cws->sigma, ndp0, ndp1, itref0, itref1, itref_qp_norm0[0], itref_qp_norm0[1], itref_qp_norm0[2], itref_qp_norm0[3], itref_qp_norm[0], itref_qp_norm[1], itref_qp_norm[2], itref_qp_norm[3], qp_res[0], qp_res[1], qp_res[2], qp_res[3]); #endif - } + } - ws->iter = kk; + ws->iter = kk; // max iteration number reached if(kk == arg->iter_max) { @@ -1135,6 +1135,6 @@ printf("iter %3d alpha %1.3e %1.3e sigma %1.3e ndp %3d %3d itref %d %d printf(" \n -> SOLUTION FOUND. \n\n"); return 0; - } + } From de0a1b632d00bdff30b408c904e9c68d211c5c1a Mon Sep 17 00:00:00 2001 From: zanellia Date: Mon, 30 Jul 2018 10:47:26 +0200 Subject: [PATCH 10/16] retabbing hpipm_d_dense_qp_ipm.h --- include/hpipm_d_dense_qp_ipm.h | 122 ++++++++++++++++----------------- 1 file changed, 61 insertions(+), 61 deletions(-) diff --git a/include/hpipm_d_dense_qp_ipm.h b/include/hpipm_d_dense_qp_ipm.h index 0157b312..73588447 100644 --- a/include/hpipm_d_dense_qp_ipm.h +++ b/include/hpipm_d_dense_qp_ipm.h @@ -49,71 +49,71 @@ extern "C" { struct d_dense_qp_ipm_arg - { - double mu0; // initial value for duality measure - double alpha_min; // exit cond on step length - double res_g_max; // exit cond on inf norm of residuals - double res_b_max; // exit cond on inf norm of residuals - double res_d_max; // exit cond on inf norm of residuals - double res_m_max; // exit cond on inf norm of residuals - double reg_prim; // reg of primal hessian - double reg_dual; // reg of dual hessian - double lam_min; // min value in lam vector - double t_min; // min value in t vector - int iter_max; // exit cond in iter number - int stat_max; // iterations saved in stat - int pred_corr; // Mehrotra's predictor-corrector IPM algirthm - int cond_pred_corr; // conditional Mehrotra's predictor-corrector - int scale; // scale hessian - int itref_pred_max; // max number of iterative refinement steps for predictor step - int itref_corr_max; // max number of iterative refinement steps for corrector step - int warm_start; // 0 no warm start, 1 warm start primal sol - int lq_fact; // 0 syrk+potrf, 1 mix, 2 lq - int abs_form; // absolute IPM formulation - int comp_res_exit; // compute residuals on exit (only for abs_form==1) - int memsize; - int print_level; - }; + { + double mu0; // initial value for duality measure + double alpha_min; // exit cond on step length + double res_g_max; // exit cond on inf norm of residuals + double res_b_max; // exit cond on inf norm of residuals + double res_d_max; // exit cond on inf norm of residuals + double res_m_max; // exit cond on inf norm of residuals + double reg_prim; // reg of primal hessian + double reg_dual; // reg of dual hessian + double lam_min; // min value in lam vector + double t_min; // min value in t vector + int iter_max; // exit cond in iter number + int stat_max; // iterations saved in stat + int pred_corr; // Mehrotra's predictor-corrector IPM algirthm + int cond_pred_corr; // conditional Mehrotra's predictor-corrector + int scale; // scale hessian + int itref_pred_max; // max number of iterative refinement steps for predictor step + int itref_corr_max; // max number of iterative refinement steps for corrector step + int warm_start; // 0 no warm start, 1 warm start primal sol + int lq_fact; // 0 syrk+potrf, 1 mix, 2 lq + int abs_form; // absolute IPM formulation + int comp_res_exit; // compute residuals on exit (only for abs_form==1) + int memsize; + int print_level; + }; struct d_dense_qp_ipm_workspace - { - struct d_core_qp_ipm_workspace *core_workspace; - struct d_dense_qp_res_workspace *res_workspace; - struct d_dense_qp_sol *sol_step; - struct d_dense_qp_sol *sol_itref; - struct d_dense_qp *qp_step; - struct d_dense_qp *qp_itref; - struct d_dense_qp_res *res_itref; - struct d_dense_qp_res *res; - struct blasfeo_dvec *Gamma; // - struct blasfeo_dvec *gamma; // - struct blasfeo_dvec *Zs_inv; // - struct blasfeo_dmat *Lv; // - struct blasfeo_dmat *AL; // - struct blasfeo_dmat *Le; // - struct blasfeo_dmat *Ctx; // - struct blasfeo_dvec *lv; // - struct blasfeo_dvec *sv; // scale for Lv - struct blasfeo_dvec *se; // scale for Le - struct blasfeo_dvec *tmp_nbg; // work space of size nb+ng - struct blasfeo_dvec *tmp_ns; // work space of size ns - struct blasfeo_dmat *lq0; - struct blasfeo_dmat *lq1; - struct blasfeo_dvec *tmp_m; - double *stat; // convergence statistics -// int *ipiv_v; -// int *ipiv_e; - void *lq_work0; - void *lq_work1; - double qp_res[4]; // infinity norm of residuals - int iter; // iteration number - int stat_max; // iterations saved in stat - int scale; - int use_hess_fact; - int memsize; // memory size (in bytes) of workspace - }; + { + struct d_core_qp_ipm_workspace *core_workspace; + struct d_dense_qp_res_workspace *res_workspace; + struct d_dense_qp_sol *sol_step; + struct d_dense_qp_sol *sol_itref; + struct d_dense_qp *qp_step; + struct d_dense_qp *qp_itref; + struct d_dense_qp_res *res_itref; + struct d_dense_qp_res *res; + struct blasfeo_dvec *Gamma; // + struct blasfeo_dvec *gamma; // + struct blasfeo_dvec *Zs_inv; // + struct blasfeo_dmat *Lv; // + struct blasfeo_dmat *AL; // + struct blasfeo_dmat *Le; // + struct blasfeo_dmat *Ctx; // + struct blasfeo_dvec *lv; // + struct blasfeo_dvec *sv; // scale for Lv + struct blasfeo_dvec *se; // scale for Le + struct blasfeo_dvec *tmp_nbg; // work space of size nb+ng + struct blasfeo_dvec *tmp_ns; // work space of size ns + struct blasfeo_dmat *lq0; + struct blasfeo_dmat *lq1; + struct blasfeo_dvec *tmp_m; + double *stat; // convergence statistics +// int *ipiv_v; +// int *ipiv_e; + void *lq_work0; + void *lq_work1; + double qp_res[4]; // infinity norm of residuals + int iter; // iteration number + int stat_max; // iterations saved in stat + int scale; + int use_hess_fact; + int memsize; // memory size (in bytes) of workspace + }; From c29c1c7fe07166aad556c1504774ed6c71ba206b Mon Sep 17 00:00:00 2001 From: zanellia Date: Mon, 30 Jul 2018 17:53:05 +0200 Subject: [PATCH 11/16] Revert "retabbing hpipm_d_dense_qp_ipm.h" This reverts commit de0a1b632d00bdff30b408c904e9c68d211c5c1a. --- include/hpipm_d_dense_qp_ipm.h | 122 ++++++++++++++++----------------- 1 file changed, 61 insertions(+), 61 deletions(-) diff --git a/include/hpipm_d_dense_qp_ipm.h b/include/hpipm_d_dense_qp_ipm.h index 73588447..0157b312 100644 --- a/include/hpipm_d_dense_qp_ipm.h +++ b/include/hpipm_d_dense_qp_ipm.h @@ -49,71 +49,71 @@ extern "C" { struct d_dense_qp_ipm_arg - { - double mu0; // initial value for duality measure - double alpha_min; // exit cond on step length - double res_g_max; // exit cond on inf norm of residuals - double res_b_max; // exit cond on inf norm of residuals - double res_d_max; // exit cond on inf norm of residuals - double res_m_max; // exit cond on inf norm of residuals - double reg_prim; // reg of primal hessian - double reg_dual; // reg of dual hessian - double lam_min; // min value in lam vector - double t_min; // min value in t vector - int iter_max; // exit cond in iter number - int stat_max; // iterations saved in stat - int pred_corr; // Mehrotra's predictor-corrector IPM algirthm - int cond_pred_corr; // conditional Mehrotra's predictor-corrector - int scale; // scale hessian - int itref_pred_max; // max number of iterative refinement steps for predictor step - int itref_corr_max; // max number of iterative refinement steps for corrector step - int warm_start; // 0 no warm start, 1 warm start primal sol - int lq_fact; // 0 syrk+potrf, 1 mix, 2 lq - int abs_form; // absolute IPM formulation - int comp_res_exit; // compute residuals on exit (only for abs_form==1) - int memsize; - int print_level; - }; + { + double mu0; // initial value for duality measure + double alpha_min; // exit cond on step length + double res_g_max; // exit cond on inf norm of residuals + double res_b_max; // exit cond on inf norm of residuals + double res_d_max; // exit cond on inf norm of residuals + double res_m_max; // exit cond on inf norm of residuals + double reg_prim; // reg of primal hessian + double reg_dual; // reg of dual hessian + double lam_min; // min value in lam vector + double t_min; // min value in t vector + int iter_max; // exit cond in iter number + int stat_max; // iterations saved in stat + int pred_corr; // Mehrotra's predictor-corrector IPM algirthm + int cond_pred_corr; // conditional Mehrotra's predictor-corrector + int scale; // scale hessian + int itref_pred_max; // max number of iterative refinement steps for predictor step + int itref_corr_max; // max number of iterative refinement steps for corrector step + int warm_start; // 0 no warm start, 1 warm start primal sol + int lq_fact; // 0 syrk+potrf, 1 mix, 2 lq + int abs_form; // absolute IPM formulation + int comp_res_exit; // compute residuals on exit (only for abs_form==1) + int memsize; + int print_level; + }; struct d_dense_qp_ipm_workspace - { - struct d_core_qp_ipm_workspace *core_workspace; - struct d_dense_qp_res_workspace *res_workspace; - struct d_dense_qp_sol *sol_step; - struct d_dense_qp_sol *sol_itref; - struct d_dense_qp *qp_step; - struct d_dense_qp *qp_itref; - struct d_dense_qp_res *res_itref; - struct d_dense_qp_res *res; - struct blasfeo_dvec *Gamma; // - struct blasfeo_dvec *gamma; // - struct blasfeo_dvec *Zs_inv; // - struct blasfeo_dmat *Lv; // - struct blasfeo_dmat *AL; // - struct blasfeo_dmat *Le; // - struct blasfeo_dmat *Ctx; // - struct blasfeo_dvec *lv; // - struct blasfeo_dvec *sv; // scale for Lv - struct blasfeo_dvec *se; // scale for Le - struct blasfeo_dvec *tmp_nbg; // work space of size nb+ng - struct blasfeo_dvec *tmp_ns; // work space of size ns - struct blasfeo_dmat *lq0; - struct blasfeo_dmat *lq1; - struct blasfeo_dvec *tmp_m; - double *stat; // convergence statistics -// int *ipiv_v; -// int *ipiv_e; - void *lq_work0; - void *lq_work1; - double qp_res[4]; // infinity norm of residuals - int iter; // iteration number - int stat_max; // iterations saved in stat - int scale; - int use_hess_fact; - int memsize; // memory size (in bytes) of workspace - }; + { + struct d_core_qp_ipm_workspace *core_workspace; + struct d_dense_qp_res_workspace *res_workspace; + struct d_dense_qp_sol *sol_step; + struct d_dense_qp_sol *sol_itref; + struct d_dense_qp *qp_step; + struct d_dense_qp *qp_itref; + struct d_dense_qp_res *res_itref; + struct d_dense_qp_res *res; + struct blasfeo_dvec *Gamma; // + struct blasfeo_dvec *gamma; // + struct blasfeo_dvec *Zs_inv; // + struct blasfeo_dmat *Lv; // + struct blasfeo_dmat *AL; // + struct blasfeo_dmat *Le; // + struct blasfeo_dmat *Ctx; // + struct blasfeo_dvec *lv; // + struct blasfeo_dvec *sv; // scale for Lv + struct blasfeo_dvec *se; // scale for Le + struct blasfeo_dvec *tmp_nbg; // work space of size nb+ng + struct blasfeo_dvec *tmp_ns; // work space of size ns + struct blasfeo_dmat *lq0; + struct blasfeo_dmat *lq1; + struct blasfeo_dvec *tmp_m; + double *stat; // convergence statistics +// int *ipiv_v; +// int *ipiv_e; + void *lq_work0; + void *lq_work1; + double qp_res[4]; // infinity norm of residuals + int iter; // iteration number + int stat_max; // iterations saved in stat + int scale; + int use_hess_fact; + int memsize; // memory size (in bytes) of workspace + }; From 85b2d753337217f747db89f0bc34919057c598c8 Mon Sep 17 00:00:00 2001 From: zanellia Date: Mon, 30 Jul 2018 17:53:24 +0200 Subject: [PATCH 12/16] Revert "retabbing x_dense_qp_ipm.c" This reverts commit 37671fe7880f70ddde06999138aafd0c4e86f949. --- dense_qp/x_dense_qp_ipm.c | 1734 ++++++++++++++++++------------------- 1 file changed, 867 insertions(+), 867 deletions(-) diff --git a/dense_qp/x_dense_qp_ipm.c b/dense_qp/x_dense_qp_ipm.c index 4d3a292d..ce9c4e01 100644 --- a/dense_qp/x_dense_qp_ipm.c +++ b/dense_qp/x_dense_qp_ipm.c @@ -28,665 +28,665 @@ int MEMSIZE_DENSE_QP_IPM_ARG(struct DENSE_QP_DIM *dim) - { + { - return 0; + return 0; - } + } void CREATE_DENSE_QP_IPM_ARG(struct DENSE_QP_DIM *dim, struct DENSE_QP_IPM_ARG *arg, void *mem) - { + { - arg->memsize = 0; + arg->memsize = 0; - return; + return; - } + } void SET_DEFAULT_DENSE_QP_IPM_ARG(enum HPIPM_MODE mode, struct DENSE_QP_IPM_ARG *arg) - { + { // set common default arguments arg->print_level = 0; // set mode-specific default arguments - if(mode==SPEED_ABS) - { - arg->mu0 = 1e1; - arg->alpha_min = 1e-12; - arg->res_g_max = 1e0; // not used - arg->res_b_max = 1e0; // not used - arg->res_d_max = 1e0; // not used - arg->res_m_max = 1e-8; - arg->iter_max = 15; - arg->stat_max = 15; - arg->pred_corr = 1; - arg->cond_pred_corr = 0; // not used - arg->itref_pred_max = 0; // not used - arg->itref_corr_max = 0; // not used - arg->reg_prim = 1e-15; - arg->reg_dual = 1e-15; - arg->lq_fact = 0; // not used - arg->scale = 0; - arg->lam_min = 1e-30; - arg->t_min = 1e-30; - arg->warm_start = 0; - arg->abs_form = 1; - arg->comp_res_exit = 0; - } - else if(mode==SPEED) - { - arg->mu0 = 1e1; - arg->alpha_min = 1e-12; - arg->res_g_max = 1e-6; - arg->res_b_max = 1e-8; - arg->res_d_max = 1e-8; - arg->res_m_max = 1e-8; - arg->iter_max = 15; - arg->stat_max = 15; - arg->pred_corr = 1; - arg->cond_pred_corr = 1; - arg->itref_pred_max = 0; - arg->itref_corr_max = 0; - arg->reg_prim = 1e-15; - arg->reg_dual = 1e-15; - arg->lq_fact = 0; - arg->scale = 0; - arg->lam_min = 1e-30; - arg->t_min = 1e-30; - arg->warm_start = 0; - arg->abs_form = 0; - arg->comp_res_exit = 1; - } - else if(mode==BALANCE) - { - arg->mu0 = 1e1; - arg->alpha_min = 1e-12; - arg->res_g_max = 1e-6; - arg->res_b_max = 1e-8; - arg->res_d_max = 1e-8; - arg->res_m_max = 1e-8; - arg->iter_max = 30; - arg->stat_max = 30; - arg->pred_corr = 1; - arg->cond_pred_corr = 1; - arg->itref_pred_max = 0; - arg->itref_corr_max = 2; - arg->reg_prim = 1e-15; - arg->reg_dual = 1e-15; - arg->lq_fact = 1; - arg->scale = 1; - arg->lam_min = 1e-30; - arg->t_min = 1e-30; - arg->warm_start = 0; - arg->abs_form = 0; - arg->comp_res_exit = 1; - } - else if(mode==ROBUST) - { - arg->mu0 = 1e2; - arg->alpha_min = 1e-12; - arg->res_g_max = 1e-6; - arg->res_b_max = 1e-8; - arg->res_d_max = 1e-8; - arg->res_m_max = 1e-8; - arg->iter_max = 100; - arg->stat_max = 100; - arg->pred_corr = 1; - arg->cond_pred_corr = 1; - arg->itref_pred_max = 0; - arg->itref_corr_max = 4; - arg->reg_prim = 1e-15; - arg->reg_dual = 1e-15; - arg->lq_fact = 2; - arg->scale = 0; - arg->lam_min = 1e-30; - arg->t_min = 1e-30; - arg->warm_start = 0; - arg->abs_form = 0; - arg->comp_res_exit = 1; - } - else - { - printf("\nwrong set default mode\n"); - exit(1); - } - - return; - - } + if(mode==SPEED_ABS) + { + arg->mu0 = 1e1; + arg->alpha_min = 1e-12; + arg->res_g_max = 1e0; // not used + arg->res_b_max = 1e0; // not used + arg->res_d_max = 1e0; // not used + arg->res_m_max = 1e-8; + arg->iter_max = 15; + arg->stat_max = 15; + arg->pred_corr = 1; + arg->cond_pred_corr = 0; // not used + arg->itref_pred_max = 0; // not used + arg->itref_corr_max = 0; // not used + arg->reg_prim = 1e-15; + arg->reg_dual = 1e-15; + arg->lq_fact = 0; // not used + arg->scale = 0; + arg->lam_min = 1e-30; + arg->t_min = 1e-30; + arg->warm_start = 0; + arg->abs_form = 1; + arg->comp_res_exit = 0; + } + else if(mode==SPEED) + { + arg->mu0 = 1e1; + arg->alpha_min = 1e-12; + arg->res_g_max = 1e-6; + arg->res_b_max = 1e-8; + arg->res_d_max = 1e-8; + arg->res_m_max = 1e-8; + arg->iter_max = 15; + arg->stat_max = 15; + arg->pred_corr = 1; + arg->cond_pred_corr = 1; + arg->itref_pred_max = 0; + arg->itref_corr_max = 0; + arg->reg_prim = 1e-15; + arg->reg_dual = 1e-15; + arg->lq_fact = 0; + arg->scale = 0; + arg->lam_min = 1e-30; + arg->t_min = 1e-30; + arg->warm_start = 0; + arg->abs_form = 0; + arg->comp_res_exit = 1; + } + else if(mode==BALANCE) + { + arg->mu0 = 1e1; + arg->alpha_min = 1e-12; + arg->res_g_max = 1e-6; + arg->res_b_max = 1e-8; + arg->res_d_max = 1e-8; + arg->res_m_max = 1e-8; + arg->iter_max = 30; + arg->stat_max = 30; + arg->pred_corr = 1; + arg->cond_pred_corr = 1; + arg->itref_pred_max = 0; + arg->itref_corr_max = 2; + arg->reg_prim = 1e-15; + arg->reg_dual = 1e-15; + arg->lq_fact = 1; + arg->scale = 1; + arg->lam_min = 1e-30; + arg->t_min = 1e-30; + arg->warm_start = 0; + arg->abs_form = 0; + arg->comp_res_exit = 1; + } + else if(mode==ROBUST) + { + arg->mu0 = 1e2; + arg->alpha_min = 1e-12; + arg->res_g_max = 1e-6; + arg->res_b_max = 1e-8; + arg->res_d_max = 1e-8; + arg->res_m_max = 1e-8; + arg->iter_max = 100; + arg->stat_max = 100; + arg->pred_corr = 1; + arg->cond_pred_corr = 1; + arg->itref_pred_max = 0; + arg->itref_corr_max = 4; + arg->reg_prim = 1e-15; + arg->reg_dual = 1e-15; + arg->lq_fact = 2; + arg->scale = 0; + arg->lam_min = 1e-30; + arg->t_min = 1e-30; + arg->warm_start = 0; + arg->abs_form = 0; + arg->comp_res_exit = 1; + } + else + { + printf("\nwrong set default mode\n"); + exit(1); + } + + return; + + } int MEMSIZE_DENSE_QP_IPM(struct DENSE_QP_DIM *dim, struct DENSE_QP_IPM_ARG *arg) - { - - int nv = dim->nv; - int ne = dim->ne; - int nb = dim->nb; - int ng = dim->ng; - int ns = dim->ns; - - int size = 0; - - size += 1*sizeof(struct CORE_QP_IPM_WORKSPACE); - size += 1*MEMSIZE_CORE_QP_IPM(nv+2*ns, ne, 2*nb+2*ng+2*ns); - - size += 1*sizeof(struct DENSE_QP_RES_WORKSPACE); // res_workspace - - size += 2*sizeof(struct DENSE_QP); // qp_step qp_itref - - size += 2*sizeof(struct DENSE_QP_SOL); // sol_step sol_itref - size += 1*MEMSIZE_DENSE_QP_SOL(dim); // sol_itref - - size += 2*sizeof(struct DENSE_QP_RES); // res res_itref - size += 1*MEMSIZE_DENSE_QP_RES(dim); // res_itref - - size += 23*sizeof(struct STRVEC); // sol_step(v,pi,lam,t) res_g res_b res_d res_m lv (4+2)*tmp_nbg (1+1)*tmp_ns Gamma gamma Zs_inv sv se tmp_m - size += 5*sizeof(struct STRMAT); // 2*Lv AL Le Ctx - if(arg->lq_fact>0) - size += 2*sizeof(struct STRMAT); // lq0 lq1 - - size += 4*SIZE_STRVEC(nb+ng); // 4*tmp_nbg - size += 1*SIZE_STRVEC(ns); // tmp_ns - size += 2*SIZE_STRVEC(nv); // lv sv - size += 1*SIZE_STRVEC(ne); // se - size += 1*SIZE_STRVEC(2*ns); // Zs_inv - size += 2*SIZE_STRMAT(nv+1, nv); // Lv - size += 1*SIZE_STRMAT(ne, nv); // AL - size += 1*SIZE_STRMAT(ne, ne); // Le - size += 1*SIZE_STRMAT(nv+1, ng); // Ctx - if(arg->lq_fact>0) - { - size += 1*SIZE_STRMAT(ne, ne+nv); // lq0 - size += 1*SIZE_STRMAT(nv, nv+nv+ng); // lq1 - } - size += 1*SIZE_STRVEC(2*nb+2*ng+2*ns); // tmp_m + { -// size += nv*sizeof(int); // ipiv_v // TODO remove !!!!! -// size += ne*sizeof(int); // ipiv_e // TODO remove !!!!! + int nv = dim->nv; + int ne = dim->ne; + int nb = dim->nb; + int ng = dim->ng; + int ns = dim->ns; - if(arg->lq_fact>0) - { - size += 1*GELQF_WORKSIZE(ne, nv); // lq_work0 - size += 1*GELQF_WORKSIZE(nv, nv+nv+ng); // lq_work1 - } + int size = 0; - size += 5*arg->stat_max*sizeof(REAL); + size += 1*sizeof(struct CORE_QP_IPM_WORKSPACE); + size += 1*MEMSIZE_CORE_QP_IPM(nv+2*ns, ne, 2*nb+2*ng+2*ns); - size = (size+63)/64*64; // make multiple of typical cache line size - size += 1*64; // align once to typical cache line size + size += 1*sizeof(struct DENSE_QP_RES_WORKSPACE); // res_workspace - return size; - - } + size += 2*sizeof(struct DENSE_QP); // qp_step qp_itref + size += 2*sizeof(struct DENSE_QP_SOL); // sol_step sol_itref + size += 1*MEMSIZE_DENSE_QP_SOL(dim); // sol_itref + size += 2*sizeof(struct DENSE_QP_RES); // res res_itref + size += 1*MEMSIZE_DENSE_QP_RES(dim); // res_itref -void CREATE_DENSE_QP_IPM(struct DENSE_QP_DIM *dim, struct DENSE_QP_IPM_ARG *arg, struct DENSE_QP_IPM_WORKSPACE *workspace, void *mem) - { + size += 23*sizeof(struct STRVEC); // sol_step(v,pi,lam,t) res_g res_b res_d res_m lv (4+2)*tmp_nbg (1+1)*tmp_ns Gamma gamma Zs_inv sv se tmp_m + size += 5*sizeof(struct STRMAT); // 2*Lv AL Le Ctx + if(arg->lq_fact>0) + size += 2*sizeof(struct STRMAT); // lq0 lq1 - int nv = dim->nv; - int ne = dim->ne; - int nb = dim->nb; - int ng = dim->ng; - int ns = dim->ns; + size += 4*SIZE_STRVEC(nb+ng); // 4*tmp_nbg + size += 1*SIZE_STRVEC(ns); // tmp_ns + size += 2*SIZE_STRVEC(nv); // lv sv + size += 1*SIZE_STRVEC(ne); // se + size += 1*SIZE_STRVEC(2*ns); // Zs_inv + size += 2*SIZE_STRMAT(nv+1, nv); // Lv + size += 1*SIZE_STRMAT(ne, nv); // AL + size += 1*SIZE_STRMAT(ne, ne); // Le + size += 1*SIZE_STRMAT(nv+1, ng); // Ctx + if(arg->lq_fact>0) + { + size += 1*SIZE_STRMAT(ne, ne+nv); // lq0 + size += 1*SIZE_STRMAT(nv, nv+nv+ng); // lq1 + } + size += 1*SIZE_STRVEC(2*nb+2*ng+2*ns); // tmp_m +// size += nv*sizeof(int); // ipiv_v // TODO remove !!!!! +// size += ne*sizeof(int); // ipiv_e // TODO remove !!!!! - // core struct - struct CORE_QP_IPM_WORKSPACE *sr_ptr = mem; + if(arg->lq_fact>0) + { + size += 1*GELQF_WORKSIZE(ne, nv); // lq_work0 + size += 1*GELQF_WORKSIZE(nv, nv+nv+ng); // lq_work1 + } - // core workspace - workspace->core_workspace = sr_ptr; - sr_ptr += 1; - struct CORE_QP_IPM_WORKSPACE *cws = workspace->core_workspace; + size += 5*arg->stat_max*sizeof(REAL); + size = (size+63)/64*64; // make multiple of typical cache line size + size += 1*64; // align once to typical cache line size - // res struct - struct DENSE_QP_RES *res_ptr = (struct DENSE_QP_RES *) sr_ptr; + return size; - workspace->res = res_ptr; - res_ptr += 1; - workspace->res_itref = res_ptr; - res_ptr += 1; + } - // res workspace struct - struct DENSE_QP_RES_WORKSPACE *res_ws_ptr = (struct DENSE_QP_RES_WORKSPACE *) res_ptr; - workspace->res_workspace = res_ws_ptr; - res_ws_ptr += 1; +void CREATE_DENSE_QP_IPM(struct DENSE_QP_DIM *dim, struct DENSE_QP_IPM_ARG *arg, struct DENSE_QP_IPM_WORKSPACE *workspace, void *mem) + { + + int nv = dim->nv; + int ne = dim->ne; + int nb = dim->nb; + int ng = dim->ng; + int ns = dim->ns; + + + // core struct + struct CORE_QP_IPM_WORKSPACE *sr_ptr = mem; + + // core workspace + workspace->core_workspace = sr_ptr; + sr_ptr += 1; + struct CORE_QP_IPM_WORKSPACE *cws = workspace->core_workspace; + + + // res struct + struct DENSE_QP_RES *res_ptr = (struct DENSE_QP_RES *) sr_ptr; + + workspace->res = res_ptr; + res_ptr += 1; + workspace->res_itref = res_ptr; + res_ptr += 1; + + + // res workspace struct + struct DENSE_QP_RES_WORKSPACE *res_ws_ptr = (struct DENSE_QP_RES_WORKSPACE *) res_ptr; + + workspace->res_workspace = res_ws_ptr; + res_ws_ptr += 1; + + + // qp sol struct + struct DENSE_QP_SOL *qp_sol_ptr = (struct DENSE_QP_SOL *) res_ws_ptr; + + workspace->sol_step = qp_sol_ptr; + qp_sol_ptr += 1; + workspace->sol_itref = qp_sol_ptr; + qp_sol_ptr += 1; + + + // qp struct + struct DENSE_QP *qp_ptr = (struct DENSE_QP *) qp_sol_ptr; + + workspace->qp_step = qp_ptr; + qp_ptr += 1; + workspace->qp_itref = qp_ptr; + qp_ptr += 1; - // qp sol struct - struct DENSE_QP_SOL *qp_sol_ptr = (struct DENSE_QP_SOL *) res_ws_ptr; + // matrix struct + struct STRMAT *sm_ptr = (struct STRMAT *) qp_ptr; + + workspace->Lv = sm_ptr; + sm_ptr += 2; + workspace->AL = sm_ptr; + sm_ptr += 1; + workspace->Le = sm_ptr; + sm_ptr += 1; + workspace->Ctx = sm_ptr; + sm_ptr += 1; + if(arg->lq_fact>0) + { + workspace->lq0 = sm_ptr; + sm_ptr += 1; + workspace->lq1 = sm_ptr; + sm_ptr += 1; + } - workspace->sol_step = qp_sol_ptr; - qp_sol_ptr += 1; - workspace->sol_itref = qp_sol_ptr; - qp_sol_ptr += 1; + // vector struct + struct STRVEC *sv_ptr = (struct STRVEC *) sm_ptr; - // qp struct - struct DENSE_QP *qp_ptr = (struct DENSE_QP *) qp_sol_ptr; + workspace->sol_step->v = sv_ptr; + sv_ptr += 1; + workspace->sol_step->pi = sv_ptr; + sv_ptr += 1; + workspace->sol_step->lam = sv_ptr; + sv_ptr += 1; + workspace->sol_step->t = sv_ptr; + sv_ptr += 1; + workspace->res->res_g = sv_ptr; + sv_ptr += 1; + workspace->res->res_b = sv_ptr; + sv_ptr += 1; + workspace->res->res_d = sv_ptr; + sv_ptr += 1; + workspace->res->res_m = sv_ptr; + sv_ptr += 1; + workspace->Gamma = sv_ptr; + sv_ptr += 1; + workspace->gamma = sv_ptr; + sv_ptr += 1; + workspace->Zs_inv = sv_ptr; + sv_ptr += 1; + workspace->lv = sv_ptr; + sv_ptr += 1; + workspace->sv = sv_ptr; + sv_ptr += 1; + workspace->se = sv_ptr; + sv_ptr += 1; + workspace->tmp_nbg = sv_ptr; + sv_ptr += 4; + workspace->res_workspace->tmp_nbg = sv_ptr; + sv_ptr += 2; + workspace->tmp_ns = sv_ptr; + sv_ptr += 1; + workspace->res_workspace->tmp_ns = sv_ptr; + sv_ptr += 1; + workspace->tmp_m = sv_ptr; + sv_ptr += 1; - workspace->qp_step = qp_ptr; - qp_ptr += 1; - workspace->qp_itref = qp_ptr; - qp_ptr += 1; + // double/float stuff + REAL *d_ptr = (REAL *) sv_ptr; + + workspace->stat = d_ptr; + d_ptr += 5*arg->stat_max; - // matrix struct - struct STRMAT *sm_ptr = (struct STRMAT *) qp_ptr; + workspace->stat_max = arg->stat_max; - workspace->Lv = sm_ptr; - sm_ptr += 2; - workspace->AL = sm_ptr; - sm_ptr += 1; - workspace->Le = sm_ptr; - sm_ptr += 1; - workspace->Ctx = sm_ptr; - sm_ptr += 1; - if(arg->lq_fact>0) - { - workspace->lq0 = sm_ptr; - sm_ptr += 1; - workspace->lq1 = sm_ptr; - sm_ptr += 1; - } + // int suff + int *i_ptr = (int *) d_ptr; + +// workspace->ipiv_v = i_ptr; +// i_ptr += nv; + +// workspace->ipiv_e = i_ptr; +// i_ptr += ne; + + + // align to typicl cache line size + size_t s_ptr = (size_t) i_ptr; + s_ptr = (s_ptr+63)/64*64; + + + // void stuf + char *c_ptr = (char *) s_ptr; + + CREATE_DENSE_QP_SOL(dim, workspace->sol_itref, c_ptr); + c_ptr += workspace->sol_itref->memsize; + + CREATE_DENSE_QP_RES(dim, workspace->res_itref, c_ptr); + c_ptr += workspace->res_itref->memsize; + + CREATE_STRMAT(nv+1, nv, workspace->Lv, c_ptr); + c_ptr += workspace->Lv->memsize; + + CREATE_STRMAT(nv+1, nv, workspace->Lv+1, c_ptr); + c_ptr += workspace->Lv[1].memsize; + + CREATE_STRMAT(ne, nv, workspace->AL, c_ptr); + c_ptr += workspace->AL->memsize; + + CREATE_STRMAT(ne, ne, workspace->Le, c_ptr); + c_ptr += workspace->Le->memsize; + + CREATE_STRMAT(nv+1, ng, workspace->Ctx, c_ptr); + c_ptr += workspace->Ctx->memsize; + + if(arg->lq_fact>0) + { + CREATE_STRMAT(ne, ne+nv, workspace->lq0, c_ptr); + c_ptr += workspace->lq0->memsize; + + CREATE_STRMAT(nv, nv+nv+ng, workspace->lq1, c_ptr); + c_ptr += workspace->lq1->memsize; + } + + CREATE_STRVEC(nv, workspace->lv, c_ptr); + c_ptr += workspace->lv->memsize; + + CREATE_STRVEC(nv, workspace->sv, c_ptr); + c_ptr += workspace->sv->memsize; + + CREATE_STRVEC(ne, workspace->se, c_ptr); + c_ptr += workspace->se->memsize; + + CREATE_STRVEC(2*ns, workspace->Zs_inv, c_ptr); + c_ptr += workspace->Zs_inv->memsize; + + CREATE_STRVEC(nb+ng, workspace->tmp_nbg+0, c_ptr); + CREATE_STRVEC(nb+ng, workspace->res_workspace->tmp_nbg+0, c_ptr); + c_ptr += (workspace->tmp_nbg+0)->memsize; - // vector struct - struct STRVEC *sv_ptr = (struct STRVEC *) sm_ptr; - - workspace->sol_step->v = sv_ptr; - sv_ptr += 1; - workspace->sol_step->pi = sv_ptr; - sv_ptr += 1; - workspace->sol_step->lam = sv_ptr; - sv_ptr += 1; - workspace->sol_step->t = sv_ptr; - sv_ptr += 1; - workspace->res->res_g = sv_ptr; - sv_ptr += 1; - workspace->res->res_b = sv_ptr; - sv_ptr += 1; - workspace->res->res_d = sv_ptr; - sv_ptr += 1; - workspace->res->res_m = sv_ptr; - sv_ptr += 1; - workspace->Gamma = sv_ptr; - sv_ptr += 1; - workspace->gamma = sv_ptr; - sv_ptr += 1; - workspace->Zs_inv = sv_ptr; - sv_ptr += 1; - workspace->lv = sv_ptr; - sv_ptr += 1; - workspace->sv = sv_ptr; - sv_ptr += 1; - workspace->se = sv_ptr; - sv_ptr += 1; - workspace->tmp_nbg = sv_ptr; - sv_ptr += 4; - workspace->res_workspace->tmp_nbg = sv_ptr; - sv_ptr += 2; - workspace->tmp_ns = sv_ptr; - sv_ptr += 1; - workspace->res_workspace->tmp_ns = sv_ptr; - sv_ptr += 1; - workspace->tmp_m = sv_ptr; - sv_ptr += 1; - - - // double/float stuff - REAL *d_ptr = (REAL *) sv_ptr; - - workspace->stat = d_ptr; - d_ptr += 5*arg->stat_max; - - workspace->stat_max = arg->stat_max; - - - // int suff - int *i_ptr = (int *) d_ptr; - -// workspace->ipiv_v = i_ptr; -// i_ptr += nv; - -// workspace->ipiv_e = i_ptr; -// i_ptr += ne; - - - // align to typicl cache line size - size_t s_ptr = (size_t) i_ptr; - s_ptr = (s_ptr+63)/64*64; - - - // void stuf - char *c_ptr = (char *) s_ptr; - - CREATE_DENSE_QP_SOL(dim, workspace->sol_itref, c_ptr); - c_ptr += workspace->sol_itref->memsize; - - CREATE_DENSE_QP_RES(dim, workspace->res_itref, c_ptr); - c_ptr += workspace->res_itref->memsize; - - CREATE_STRMAT(nv+1, nv, workspace->Lv, c_ptr); - c_ptr += workspace->Lv->memsize; - - CREATE_STRMAT(nv+1, nv, workspace->Lv+1, c_ptr); - c_ptr += workspace->Lv[1].memsize; - - CREATE_STRMAT(ne, nv, workspace->AL, c_ptr); - c_ptr += workspace->AL->memsize; - - CREATE_STRMAT(ne, ne, workspace->Le, c_ptr); - c_ptr += workspace->Le->memsize; - - CREATE_STRMAT(nv+1, ng, workspace->Ctx, c_ptr); - c_ptr += workspace->Ctx->memsize; - - if(arg->lq_fact>0) - { - CREATE_STRMAT(ne, ne+nv, workspace->lq0, c_ptr); - c_ptr += workspace->lq0->memsize; - - CREATE_STRMAT(nv, nv+nv+ng, workspace->lq1, c_ptr); - c_ptr += workspace->lq1->memsize; - } + CREATE_STRVEC(nb+ng, workspace->tmp_nbg+1, c_ptr); + CREATE_STRVEC(nb+ng, workspace->res_workspace->tmp_nbg+1, c_ptr); + c_ptr += (workspace->tmp_nbg+1)->memsize; - CREATE_STRVEC(nv, workspace->lv, c_ptr); - c_ptr += workspace->lv->memsize; + CREATE_STRVEC(nb+ng, workspace->tmp_nbg+2, c_ptr); + c_ptr += (workspace->tmp_nbg+2)->memsize; - CREATE_STRVEC(nv, workspace->sv, c_ptr); - c_ptr += workspace->sv->memsize; + CREATE_STRVEC(nb+ng, workspace->tmp_nbg+3, c_ptr); + c_ptr += (workspace->tmp_nbg+3)->memsize; - CREATE_STRVEC(ne, workspace->se, c_ptr); - c_ptr += workspace->se->memsize; + CREATE_STRVEC(ns, workspace->tmp_ns+0, c_ptr); + CREATE_STRVEC(ns, workspace->res_workspace->tmp_ns+0, c_ptr); + c_ptr += (workspace->tmp_ns+0)->memsize; - CREATE_STRVEC(2*ns, workspace->Zs_inv, c_ptr); - c_ptr += workspace->Zs_inv->memsize; + CREATE_STRVEC(2*nb+2*ng+2*ns, workspace->tmp_m, c_ptr); + c_ptr += (workspace->tmp_m)->memsize; - CREATE_STRVEC(nb+ng, workspace->tmp_nbg+0, c_ptr); - CREATE_STRVEC(nb+ng, workspace->res_workspace->tmp_nbg+0, c_ptr); - c_ptr += (workspace->tmp_nbg+0)->memsize; + CREATE_CORE_QP_IPM(nv+2*ns, ne, 2*nb+2*ng+2*ns, cws, c_ptr); + c_ptr += workspace->core_workspace->memsize; - CREATE_STRVEC(nb+ng, workspace->tmp_nbg+1, c_ptr); - CREATE_STRVEC(nb+ng, workspace->res_workspace->tmp_nbg+1, c_ptr); - c_ptr += (workspace->tmp_nbg+1)->memsize; + if(arg->lq_fact>0) + { + workspace->lq_work0 = c_ptr; + c_ptr += GELQF_WORKSIZE(ne, nv); - CREATE_STRVEC(nb+ng, workspace->tmp_nbg+2, c_ptr); - c_ptr += (workspace->tmp_nbg+2)->memsize; + workspace->lq_work1 = c_ptr; + c_ptr += GELQF_WORKSIZE(nv, nv+nv+ng); + } - CREATE_STRVEC(nb+ng, workspace->tmp_nbg+3, c_ptr); - c_ptr += (workspace->tmp_nbg+3)->memsize; - CREATE_STRVEC(ns, workspace->tmp_ns+0, c_ptr); - CREATE_STRVEC(ns, workspace->res_workspace->tmp_ns+0, c_ptr); - c_ptr += (workspace->tmp_ns+0)->memsize; + // alias members of workspace and core_workspace + // + CREATE_STRVEC(nv+2*ns, workspace->sol_step->v, cws->dv); + // + CREATE_STRVEC(ne, workspace->sol_step->pi, cws->dpi); + // + CREATE_STRVEC(2*nb+2*ng+2*ns, workspace->sol_step->lam, cws->dlam); + // + CREATE_STRVEC(2*nb+2*ng+2*ns, workspace->sol_step->t, cws->dt); + // + CREATE_STRVEC(nv+2*ns, workspace->res->res_g, cws->res_g); + // + CREATE_STRVEC(ne, workspace->res->res_b, cws->res_b); + // + CREATE_STRVEC(2*nb+2*ng+2*ns, workspace->res->res_d, cws->res_d); + // + CREATE_STRVEC(2*nb+2*ng+2*ns, workspace->res->res_m, cws->res_m); + // + CREATE_STRVEC(2*nb+2*ng+2*ns, workspace->Gamma, cws->Gamma); + // + CREATE_STRVEC(2*nb+2*ng+2*ns, workspace->gamma, cws->gamma); - CREATE_STRVEC(2*nb+2*ng+2*ns, workspace->tmp_m, c_ptr); - c_ptr += (workspace->tmp_m)->memsize; + // + workspace->sol_step->dim = dim; - CREATE_CORE_QP_IPM(nv+2*ns, ne, 2*nb+2*ng+2*ns, cws, c_ptr); - c_ptr += workspace->core_workspace->memsize; + // + workspace->use_hess_fact = 0; - if(arg->lq_fact>0) - { - workspace->lq_work0 = c_ptr; - c_ptr += GELQF_WORKSIZE(ne, nv); - - workspace->lq_work1 = c_ptr; - c_ptr += GELQF_WORKSIZE(nv, nv+nv+ng); - } - - - // alias members of workspace and core_workspace - // - CREATE_STRVEC(nv+2*ns, workspace->sol_step->v, cws->dv); - // - CREATE_STRVEC(ne, workspace->sol_step->pi, cws->dpi); - // - CREATE_STRVEC(2*nb+2*ng+2*ns, workspace->sol_step->lam, cws->dlam); - // - CREATE_STRVEC(2*nb+2*ng+2*ns, workspace->sol_step->t, cws->dt); - // - CREATE_STRVEC(nv+2*ns, workspace->res->res_g, cws->res_g); - // - CREATE_STRVEC(ne, workspace->res->res_b, cws->res_b); - // - CREATE_STRVEC(2*nb+2*ng+2*ns, workspace->res->res_d, cws->res_d); - // - CREATE_STRVEC(2*nb+2*ng+2*ns, workspace->res->res_m, cws->res_m); - // - CREATE_STRVEC(2*nb+2*ng+2*ns, workspace->Gamma, cws->Gamma); - // - CREATE_STRVEC(2*nb+2*ng+2*ns, workspace->gamma, cws->gamma); - - // - workspace->sol_step->dim = dim; - - // - workspace->use_hess_fact = 0; - - // - workspace->memsize = MEMSIZE_DENSE_QP_IPM(dim, arg); + // + workspace->memsize = MEMSIZE_DENSE_QP_IPM(dim, arg); #if defined(RUNTIME_CHECKS) - if(c_ptr > ((char *) mem) + workspace->memsize) - { - printf("\nCreate_dense_qp_ipm: outside memory bounds!\n\n"); - exit(1); - } + if(c_ptr > ((char *) mem) + workspace->memsize) + { + printf("\nCreate_dense_qp_ipm: outside memory bounds!\n\n"); + exit(1); + } #endif - return; + return; - } + } int SOLVE_DENSE_QP_IPM(struct DENSE_QP *qp, struct DENSE_QP_SOL *qp_sol, struct DENSE_QP_IPM_ARG *arg, struct DENSE_QP_IPM_WORKSPACE *ws) - { - - struct CORE_QP_IPM_WORKSPACE *cws = ws->core_workspace; - - // arg to core workspace - cws->lam_min = arg->lam_min; - cws->t_min = arg->t_min; - - // alias qp vectors into qp_sol - cws->v = qp_sol->v->pa; - cws->pi = qp_sol->pi->pa; - cws->lam = qp_sol->lam->pa; - cws->t = qp_sol->t->pa; - - // alias members of qp_step - ws->qp_step->dim = qp->dim; - ws->qp_step->Hv = qp->Hv; - ws->qp_step->A = qp->A; - ws->qp_step->Ct = qp->Ct; - ws->qp_step->Z = qp->Z; - ws->qp_step->idxb = qp->idxb; - ws->qp_step->idxs = qp->idxs; - ws->qp_step->gz = ws->res->res_g; - ws->qp_step->b = ws->res->res_b; - ws->qp_step->d = ws->res->res_d; - ws->qp_step->m = ws->res->res_m; - - // alias members of qp_itref - ws->qp_itref->dim = qp->dim; - ws->qp_itref->Hv = qp->Hv; - ws->qp_itref->A = qp->A; - ws->qp_itref->Ct = qp->Ct; - ws->qp_itref->Z = qp->Z; - ws->qp_itref->idxb = qp->idxb; - ws->qp_itref->idxs = qp->idxs; - ws->qp_itref->gz = ws->res_itref->res_g; - ws->qp_itref->b = ws->res_itref->res_b; - ws->qp_itref->d = ws->res_itref->res_d; - ws->qp_itref->m = ws->res_itref->res_m; - - // no constraints - if(cws->nc==0) - { - FACT_SOLVE_KKT_UNCONSTR_DENSE_QP(qp, qp_sol, arg, ws); - COMPUTE_RES_DENSE_QP(qp, qp_sol, ws->res, ws->res_workspace); - cws->mu = ws->res->res_mu; - ws->iter = 0; - return 0; - } - - // blasfeo alias for residuals - struct STRVEC str_res_g; - struct STRVEC str_res_b; - struct STRVEC str_res_d; - struct STRVEC str_res_m; - str_res_g.m = cws->nv; - str_res_b.m = cws->ne; - str_res_d.m = cws->nc; - str_res_m.m = cws->nc; - str_res_g.pa = cws->res_g; - str_res_b.pa = cws->res_b; - str_res_d.pa = cws->res_d; - str_res_m.pa = cws->res_m; - - REAL *qp_res = ws->qp_res; - qp_res[0] = 0; - qp_res[1] = 0; - qp_res[2] = 0; - qp_res[3] = 0; - - // dims - int nv = qp->dim->nv; - int ne = qp->dim->ne; - int nb = qp->dim->nb; - int ng = qp->dim->ng; - int ns = qp->dim->ns; - - int kk, ii, itref0=0, itref1=0; - REAL tmp; - REAL mu_aff0, mu; - int iter_ref_step; - - // init solver - INIT_VAR_DENSE_QP(qp, qp_sol, arg, ws); - - cws->alpha = 1.0; - - - - // absolute IPM formulation - - if(arg->abs_form) - { - - // alias members of qp_step - ws->qp_step->dim = qp->dim; - ws->qp_step->Hv = qp->Hv; - ws->qp_step->A = qp->A; - ws->qp_step->Ct = qp->Ct; - ws->qp_step->Z = qp->Z; - ws->qp_step->idxb = qp->idxb; - ws->qp_step->idxs = qp->idxs; - ws->qp_step->gz = qp->gz; - ws->qp_step->b = qp->b; - ws->qp_step->d = qp->d; - ws->qp_step->m = ws->tmp_m; - - // alias core workspace - cws->res_m = ws->qp_step->m->pa; - cws->res_m_bkp = ws->qp_step->m->pa; - - mu = VECMULDOT(cws->nc, qp_sol->lam, 0, qp_sol->t, 0, ws->tmp_m, 0); - mu /= cws->nc; - cws->mu = mu; - - // IPM loop (absolute formulation) - for(kk=0; \ - kkiter_max & \ - cws->alpha>arg->alpha_min & \ - mu>arg->res_m_max; kk++) - { - - VECSC(cws->nc, -1.0, ws->tmp_m, 0); - - // fact solve - FACT_SOLVE_KKT_STEP_DENSE_QP(ws->qp_step, ws->sol_step, arg, ws); - - // compute step - AXPY(cws->nv, -1.0, qp_sol->v, 0, ws->sol_step->v, 0, ws->sol_step->v, 0); - AXPY(cws->ne, -1.0, qp_sol->pi, 0, ws->sol_step->pi, 0, ws->sol_step->pi, 0); - AXPY(cws->nc, -1.0, qp_sol->lam, 0, ws->sol_step->lam, 0, ws->sol_step->lam, 0); - AXPY(cws->nc, -1.0, qp_sol->t, 0, ws->sol_step->t, 0, ws->sol_step->t, 0); - - // alpha - COMPUTE_ALPHA_QP(cws); - if(kkstat_max) - ws->stat[5*kk+0] = cws->alpha; - - // Mehrotra's predictor-corrector - if(arg->pred_corr==1) - { - // mu_aff - COMPUTE_MU_AFF_QP(cws); - if(kkstat_max) - ws->stat[5*kk+1] = cws->mu_aff; - - tmp = cws->mu_aff/cws->mu; - cws->sigma = tmp*tmp*tmp; - if(kkstat_max) - ws->stat[5*kk+2] = cws->sigma; - - COMPUTE_CENTERING_CORRECTION_QP(cws); - - // fact and solve kkt - SOLVE_KKT_STEP_DENSE_QP(ws->qp_step, ws->sol_step, arg, ws); - - // compute step - AXPY(cws->nv, -1.0, qp_sol->v, 0, ws->sol_step->v, 0, ws->sol_step->v, 0); - AXPY(cws->ne, -1.0, qp_sol->pi, 0, ws->sol_step->pi, 0, ws->sol_step->pi, 0); - AXPY(cws->nc, -1.0, qp_sol->lam, 0, ws->sol_step->lam, 0, ws->sol_step->lam, 0); - AXPY(cws->nc, -1.0, qp_sol->t, 0, ws->sol_step->t, 0, ws->sol_step->t, 0); - - // alpha - COMPUTE_ALPHA_QP(cws); - if(kkstat_max) - ws->stat[5*kk+3] = cws->alpha; - - } - - // - UPDATE_VAR_QP(cws); - - // compute mu - mu = VECMULDOT(cws->nc, qp_sol->lam, 0, qp_sol->t, 0, ws->tmp_m, 0); - mu /= cws->nc; - cws->mu = mu; - if(kkstat_max) - ws->stat[5*kk+4] = mu; - - // exit(1); - - } - - if(arg->comp_res_exit) - { - // compute residuals - COMPUTE_RES_DENSE_QP(qp, qp_sol, ws->res, ws->res_workspace); - - // compute infinity norm of residuals - VECNRM_INF(cws->nv, &str_res_g, 0, &qp_res[0]); - VECNRM_INF(cws->ne, &str_res_b, 0, &qp_res[1]); - VECNRM_INF(cws->nc, &str_res_d, 0, &qp_res[2]); - VECNRM_INF(cws->nc, &str_res_m, 0, &qp_res[3]); + { + + struct CORE_QP_IPM_WORKSPACE *cws = ws->core_workspace; + + // arg to core workspace + cws->lam_min = arg->lam_min; + cws->t_min = arg->t_min; + + // alias qp vectors into qp_sol + cws->v = qp_sol->v->pa; + cws->pi = qp_sol->pi->pa; + cws->lam = qp_sol->lam->pa; + cws->t = qp_sol->t->pa; + + // alias members of qp_step + ws->qp_step->dim = qp->dim; + ws->qp_step->Hv = qp->Hv; + ws->qp_step->A = qp->A; + ws->qp_step->Ct = qp->Ct; + ws->qp_step->Z = qp->Z; + ws->qp_step->idxb = qp->idxb; + ws->qp_step->idxs = qp->idxs; + ws->qp_step->gz = ws->res->res_g; + ws->qp_step->b = ws->res->res_b; + ws->qp_step->d = ws->res->res_d; + ws->qp_step->m = ws->res->res_m; + + // alias members of qp_itref + ws->qp_itref->dim = qp->dim; + ws->qp_itref->Hv = qp->Hv; + ws->qp_itref->A = qp->A; + ws->qp_itref->Ct = qp->Ct; + ws->qp_itref->Z = qp->Z; + ws->qp_itref->idxb = qp->idxb; + ws->qp_itref->idxs = qp->idxs; + ws->qp_itref->gz = ws->res_itref->res_g; + ws->qp_itref->b = ws->res_itref->res_b; + ws->qp_itref->d = ws->res_itref->res_d; + ws->qp_itref->m = ws->res_itref->res_m; + + // no constraints + if(cws->nc==0) + { + FACT_SOLVE_KKT_UNCONSTR_DENSE_QP(qp, qp_sol, arg, ws); + COMPUTE_RES_DENSE_QP(qp, qp_sol, ws->res, ws->res_workspace); + cws->mu = ws->res->res_mu; + ws->iter = 0; + return 0; + } + + // blasfeo alias for residuals + struct STRVEC str_res_g; + struct STRVEC str_res_b; + struct STRVEC str_res_d; + struct STRVEC str_res_m; + str_res_g.m = cws->nv; + str_res_b.m = cws->ne; + str_res_d.m = cws->nc; + str_res_m.m = cws->nc; + str_res_g.pa = cws->res_g; + str_res_b.pa = cws->res_b; + str_res_d.pa = cws->res_d; + str_res_m.pa = cws->res_m; + + REAL *qp_res = ws->qp_res; + qp_res[0] = 0; + qp_res[1] = 0; + qp_res[2] = 0; + qp_res[3] = 0; + + // dims + int nv = qp->dim->nv; + int ne = qp->dim->ne; + int nb = qp->dim->nb; + int ng = qp->dim->ng; + int ns = qp->dim->ns; + + int kk, ii, itref0=0, itref1=0; + REAL tmp; + REAL mu_aff0, mu; + int iter_ref_step; + + // init solver + INIT_VAR_DENSE_QP(qp, qp_sol, arg, ws); + + cws->alpha = 1.0; + + + + // absolute IPM formulation + + if(arg->abs_form) + { + + // alias members of qp_step + ws->qp_step->dim = qp->dim; + ws->qp_step->Hv = qp->Hv; + ws->qp_step->A = qp->A; + ws->qp_step->Ct = qp->Ct; + ws->qp_step->Z = qp->Z; + ws->qp_step->idxb = qp->idxb; + ws->qp_step->idxs = qp->idxs; + ws->qp_step->gz = qp->gz; + ws->qp_step->b = qp->b; + ws->qp_step->d = qp->d; + ws->qp_step->m = ws->tmp_m; + + // alias core workspace + cws->res_m = ws->qp_step->m->pa; + cws->res_m_bkp = ws->qp_step->m->pa; + + mu = VECMULDOT(cws->nc, qp_sol->lam, 0, qp_sol->t, 0, ws->tmp_m, 0); + mu /= cws->nc; + cws->mu = mu; + + // IPM loop (absolute formulation) + for(kk=0; \ + kkiter_max & \ + cws->alpha>arg->alpha_min & \ + mu>arg->res_m_max; kk++) + { + + VECSC(cws->nc, -1.0, ws->tmp_m, 0); + + // fact solve + FACT_SOLVE_KKT_STEP_DENSE_QP(ws->qp_step, ws->sol_step, arg, ws); + + // compute step + AXPY(cws->nv, -1.0, qp_sol->v, 0, ws->sol_step->v, 0, ws->sol_step->v, 0); + AXPY(cws->ne, -1.0, qp_sol->pi, 0, ws->sol_step->pi, 0, ws->sol_step->pi, 0); + AXPY(cws->nc, -1.0, qp_sol->lam, 0, ws->sol_step->lam, 0, ws->sol_step->lam, 0); + AXPY(cws->nc, -1.0, qp_sol->t, 0, ws->sol_step->t, 0, ws->sol_step->t, 0); + + // alpha + COMPUTE_ALPHA_QP(cws); + if(kkstat_max) + ws->stat[5*kk+0] = cws->alpha; + + // Mehrotra's predictor-corrector + if(arg->pred_corr==1) + { + // mu_aff + COMPUTE_MU_AFF_QP(cws); + if(kkstat_max) + ws->stat[5*kk+1] = cws->mu_aff; + + tmp = cws->mu_aff/cws->mu; + cws->sigma = tmp*tmp*tmp; + if(kkstat_max) + ws->stat[5*kk+2] = cws->sigma; + + COMPUTE_CENTERING_CORRECTION_QP(cws); + + // fact and solve kkt + SOLVE_KKT_STEP_DENSE_QP(ws->qp_step, ws->sol_step, arg, ws); + + // compute step + AXPY(cws->nv, -1.0, qp_sol->v, 0, ws->sol_step->v, 0, ws->sol_step->v, 0); + AXPY(cws->ne, -1.0, qp_sol->pi, 0, ws->sol_step->pi, 0, ws->sol_step->pi, 0); + AXPY(cws->nc, -1.0, qp_sol->lam, 0, ws->sol_step->lam, 0, ws->sol_step->lam, 0); + AXPY(cws->nc, -1.0, qp_sol->t, 0, ws->sol_step->t, 0, ws->sol_step->t, 0); + + // alpha + COMPUTE_ALPHA_QP(cws); + if(kkstat_max) + ws->stat[5*kk+3] = cws->alpha; + + } + + // + UPDATE_VAR_QP(cws); + + // compute mu + mu = VECMULDOT(cws->nc, qp_sol->lam, 0, qp_sol->t, 0, ws->tmp_m, 0); + mu /= cws->nc; + cws->mu = mu; + if(kkstat_max) + ws->stat[5*kk+4] = mu; + + // exit(1); + + } + + if(arg->comp_res_exit) + { + // compute residuals + COMPUTE_RES_DENSE_QP(qp, qp_sol, ws->res, ws->res_workspace); + + // compute infinity norm of residuals + VECNRM_INF(cws->nv, &str_res_g, 0, &qp_res[0]); + VECNRM_INF(cws->ne, &str_res_b, 0, &qp_res[1]); + VECNRM_INF(cws->nc, &str_res_d, 0, &qp_res[2]); + VECNRM_INF(cws->nc, &str_res_m, 0, &qp_res[3]); if(arg->print_level > 0) { if(kk%10 == 0) @@ -697,9 +697,9 @@ int SOLVE_DENSE_QP_IPM(struct DENSE_QP *qp, struct DENSE_QP_SOL *qp_sol, struct } printf("%-10d %-15e %-15e %-15e %-15e %-15e %-15e\n", kk, qp_res[0], qp_res[1], qp_res[2], qp_res[3], cws->alpha, cws->mu); } - } + } - ws->iter = kk; + ws->iter = kk; // max iteration number reached if(kk == arg->iter_max) { @@ -735,85 +735,85 @@ int SOLVE_DENSE_QP_IPM(struct DENSE_QP *qp, struct DENSE_QP_SOL *qp_sol, struct printf(" \n -> SOLUTION FOUND. \n\n"); return 0; - } + } - // compute residuals - COMPUTE_RES_DENSE_QP(qp, qp_sol, ws->res, ws->res_workspace); - BACKUP_RES_M(cws); - cws->mu = ws->res->res_mu; + // compute residuals + COMPUTE_RES_DENSE_QP(qp, qp_sol, ws->res, ws->res_workspace); + BACKUP_RES_M(cws); + cws->mu = ws->res->res_mu; - // compute infinity norm of residuals - VECNRM_INF(cws->nv, &str_res_g, 0, &qp_res[0]); - VECNRM_INF(cws->ne, &str_res_b, 0, &qp_res[1]); - VECNRM_INF(cws->nc, &str_res_d, 0, &qp_res[2]); - VECNRM_INF(cws->nc, &str_res_m, 0, &qp_res[3]); + // compute infinity norm of residuals + VECNRM_INF(cws->nv, &str_res_g, 0, &qp_res[0]); + VECNRM_INF(cws->ne, &str_res_b, 0, &qp_res[1]); + VECNRM_INF(cws->nc, &str_res_d, 0, &qp_res[2]); + VECNRM_INF(cws->nc, &str_res_m, 0, &qp_res[3]); -// REAL sigma_min = 1e9; -// sigma_min = arg->res_g_maxres_g_max : sigma_min; -// sigma_min = arg->res_b_maxres_b_max : sigma_min; -// sigma_min = arg->res_d_maxres_d_max : sigma_min; -// sigma_min = arg->res_m_maxres_m_max : sigma_min; -// sigma_min *= 0.1; +// REAL sigma_min = 1e9; +// sigma_min = arg->res_g_maxres_g_max : sigma_min; +// sigma_min = arg->res_b_maxres_b_max : sigma_min; +// sigma_min = arg->res_d_maxres_d_max : sigma_min; +// sigma_min = arg->res_m_maxres_m_max : sigma_min; +// sigma_min *= 0.1; - REAL itref_qp_norm[4] = {0,0,0,0}; - REAL itref_qp_norm0[4] = {0,0,0,0}; - int ndp0, ndp1; + REAL itref_qp_norm[4] = {0,0,0,0}; + REAL itref_qp_norm0[4] = {0,0,0,0}; + int ndp0, ndp1; - int force_lq = 0; + int force_lq = 0; - // relative IPM formulation + // relative IPM formulation - // IPM loop - for(kk=0; \ - kkiter_max & \ - cws->alpha>arg->alpha_min & \ - (qp_res[0]>arg->res_g_max | \ - qp_res[1]>arg->res_b_max | \ - qp_res[2]>arg->res_d_max | \ - qp_res[3]>arg->res_m_max); kk++) - { + // IPM loop + for(kk=0; \ + kkiter_max & \ + cws->alpha>arg->alpha_min & \ + (qp_res[0]>arg->res_g_max | \ + qp_res[1]>arg->res_b_max | \ + qp_res[2]>arg->res_d_max | \ + qp_res[3]>arg->res_m_max); kk++) + { - ws->scale = arg->scale; + ws->scale = arg->scale; - // fact and solve kkt - if(arg->lq_fact==0) - { + // fact and solve kkt + if(arg->lq_fact==0) + { - // syrk+cholesky - FACT_SOLVE_KKT_STEP_DENSE_QP(ws->qp_step, ws->sol_step, arg, ws); + // syrk+cholesky + FACT_SOLVE_KKT_STEP_DENSE_QP(ws->qp_step, ws->sol_step, arg, ws); - } - else if(arg->lq_fact==1 & force_lq==0) - { + } + else if(arg->lq_fact==1 & force_lq==0) + { - // syrk+chol, switch to lq when needed - FACT_SOLVE_KKT_STEP_DENSE_QP(ws->qp_step, ws->sol_step, arg, ws); + // syrk+chol, switch to lq when needed + FACT_SOLVE_KKT_STEP_DENSE_QP(ws->qp_step, ws->sol_step, arg, ws); - // compute res of linear system - COMPUTE_LIN_RES_DENSE_QP(ws->qp_step, qp_sol, ws->sol_step, ws->res_itref, ws->res_workspace); - VECNRM_INF(cws->nv, ws->res_itref->res_g, 0, &itref_qp_norm[0]); - VECNRM_INF(cws->ne, ws->res_itref->res_b, 0, &itref_qp_norm[1]); - VECNRM_INF(cws->nc, ws->res_itref->res_d, 0, &itref_qp_norm[2]); - VECNRM_INF(cws->nc, ws->res_itref->res_m, 0, &itref_qp_norm[3]); + // compute res of linear system + COMPUTE_LIN_RES_DENSE_QP(ws->qp_step, qp_sol, ws->sol_step, ws->res_itref, ws->res_workspace); + VECNRM_INF(cws->nv, ws->res_itref->res_g, 0, &itref_qp_norm[0]); + VECNRM_INF(cws->ne, ws->res_itref->res_b, 0, &itref_qp_norm[1]); + VECNRM_INF(cws->nc, ws->res_itref->res_d, 0, &itref_qp_norm[2]); + VECNRM_INF(cws->nc, ws->res_itref->res_m, 0, &itref_qp_norm[3]); //printf("\n%e\t%e\t%e\t%e\n", itref_qp_norm[0], itref_qp_norm[1], itref_qp_norm[2], itref_qp_norm[3]); - // inaccurate factorization: switch to lq - if( + // inaccurate factorization: switch to lq + if( #ifdef USE_C99_MATH - ( itref_qp_norm[0]==0.0 & isnan(BLASFEO_DVECEL(ws->res_itref->res_g, 0)) ) | + ( itref_qp_norm[0]==0.0 & isnan(BLASFEO_DVECEL(ws->res_itref->res_g, 0)) ) | #else - ( itref_qp_norm[0]==0.0 & BLASFEO_DVECEL(ws->res_itref->res_g, 0)!=BLASFEO_DVECEL(ws->res_itref->res_g, 0) ) | + ( itref_qp_norm[0]==0.0 & BLASFEO_DVECEL(ws->res_itref->res_g, 0)!=BLASFEO_DVECEL(ws->res_itref->res_g, 0) ) | #endif - itref_qp_norm[0]>1e-5 | - itref_qp_norm[1]>1e-5 | - itref_qp_norm[2]>1e-5 | - itref_qp_norm[3]>1e-5 ) - { + itref_qp_norm[0]>1e-5 | + itref_qp_norm[1]>1e-5 | + itref_qp_norm[2]>1e-5 | + itref_qp_norm[3]>1e-5 ) + { #if 0 blasfeo_print_tran_dvec(cws->nv, ws->sol_step->v, 0); @@ -822,11 +822,11 @@ blasfeo_print_tran_dvec(cws->nc, ws->sol_step->lam, 0); blasfeo_print_tran_dvec(cws->nc, ws->sol_step->t, 0); #endif - // refactorize using lq - FACT_LQ_SOLVE_KKT_STEP_DENSE_QP(ws->qp_step, ws->sol_step, arg, ws); + // refactorize using lq + FACT_LQ_SOLVE_KKT_STEP_DENSE_QP(ws->qp_step, ws->sol_step, arg, ws); - // switch to lq - force_lq = 1; + // switch to lq + force_lq = 1; #if 0 blasfeo_print_tran_dvec(cws->nv, ws->sol_step->v, 0); @@ -835,242 +835,242 @@ blasfeo_print_tran_dvec(cws->nc, ws->sol_step->lam, 0); blasfeo_print_tran_dvec(cws->nc, ws->sol_step->t, 0); #endif - } + } - } - else // arg->lq_fact==2 - { + } + else // arg->lq_fact==2 + { - // lq - FACT_LQ_SOLVE_KKT_STEP_DENSE_QP(ws->qp_step, ws->sol_step, arg, ws); + // lq + FACT_LQ_SOLVE_KKT_STEP_DENSE_QP(ws->qp_step, ws->sol_step, arg, ws); - } + } #if 0 - COMPUTE_LIN_RES_DENSE_QP(ws->qp_step, qp_sol, ws->sol_step, ws->res_itref, ws->res_workspace); - VECNRM_INF(cws->nv, ws->res_itref->res_g, 0, &itref_qp_norm[0]); - VECNRM_INF(cws->ne, ws->res_itref->res_b, 0, &itref_qp_norm[1]); - VECNRM_INF(cws->nc, ws->res_itref->res_d, 0, &itref_qp_norm[2]); - VECNRM_INF(cws->nc, ws->res_itref->res_m, 0, &itref_qp_norm[3]); -// printf("%e\t%e\t%e\t%e\t%e\t%e\t%e\t%e\t\n", qp_res[0], qp_res[1], qp_res[2], qp_res[3], itref_qp_norm[0], itref_qp_norm[1], itref_qp_norm[2], itref_qp_norm[3]); - if(itref_qp_norm[0]==0.0 & BLASFEO_DVECEL(ws->res_itref->res_g, 0)!=BLASFEO_DVECEL(ws->res_itref->res_g, 0)) - printf("NaN!!!\n"); + COMPUTE_LIN_RES_DENSE_QP(ws->qp_step, qp_sol, ws->sol_step, ws->res_itref, ws->res_workspace); + VECNRM_INF(cws->nv, ws->res_itref->res_g, 0, &itref_qp_norm[0]); + VECNRM_INF(cws->ne, ws->res_itref->res_b, 0, &itref_qp_norm[1]); + VECNRM_INF(cws->nc, ws->res_itref->res_d, 0, &itref_qp_norm[2]); + VECNRM_INF(cws->nc, ws->res_itref->res_m, 0, &itref_qp_norm[3]); +// printf("%e\t%e\t%e\t%e\t%e\t%e\t%e\t%e\t\n", qp_res[0], qp_res[1], qp_res[2], qp_res[3], itref_qp_norm[0], itref_qp_norm[1], itref_qp_norm[2], itref_qp_norm[3]); + if(itref_qp_norm[0]==0.0 & BLASFEO_DVECEL(ws->res_itref->res_g, 0)!=BLASFEO_DVECEL(ws->res_itref->res_g, 0)) + printf("NaN!!!\n"); #endif - // iterative refinement on prediction step - for(itref0=0; itref0itref_pred_max; itref0++) - { - - COMPUTE_LIN_RES_DENSE_QP(ws->qp_step, qp_sol, ws->sol_step, ws->res_itref, ws->res_workspace); - - VECNRM_INF(cws->nv, ws->res_itref->res_g, 0, &itref_qp_norm[0]); - VECNRM_INF(cws->ne, ws->res_itref->res_b, 0, &itref_qp_norm[1]); - VECNRM_INF(cws->nc, ws->res_itref->res_d, 0, &itref_qp_norm[2]); - VECNRM_INF(cws->nc, ws->res_itref->res_m, 0, &itref_qp_norm[3]); - - if(itref0==0) - { - itref_qp_norm0[0] = itref_qp_norm[0]; - itref_qp_norm0[1] = itref_qp_norm[1]; - itref_qp_norm0[2] = itref_qp_norm[2]; - itref_qp_norm0[3] = itref_qp_norm[3]; - } - - if( \ - (itref_qp_norm[0]<1e0*arg->res_g_max | itref_qp_norm[0]<1e-3*qp_res[0]) & \ - (itref_qp_norm[1]<1e0*arg->res_b_max | itref_qp_norm[1]<1e-3*qp_res[1]) & \ - (itref_qp_norm[2]<1e0*arg->res_d_max | itref_qp_norm[2]<1e-3*qp_res[2]) & \ - (itref_qp_norm[3]<1e0*arg->res_m_max | itref_qp_norm[3]<1e-3*qp_res[3]) ) -// (itref_qp_norm[0]<=arg->res_g_max) & \ - (itref_qp_norm[1]<=arg->res_b_max) & \ - (itref_qp_norm[2]<=arg->res_d_max) & \ - (itref_qp_norm[3]<=arg->res_m_max) ) - { - break; - } - - SOLVE_KKT_STEP_DENSE_QP(ws->qp_itref, ws->sol_itref, arg, ws); - - AXPY(nv+2*ns, 1.0, ws->sol_itref->v, 0, ws->sol_step->v, 0, ws->sol_step->v, 0); - AXPY(ne, 1.0, ws->sol_itref->pi, 0, ws->sol_step->pi, 0, ws->sol_step->pi, 0); - AXPY(2*nb+2*ng+2*ns, 1.0, ws->sol_itref->lam, 0, ws->sol_step->lam, 0, ws->sol_step->lam, 0); - AXPY(2*nb+2*ng+2*ns, 1.0, ws->sol_itref->t, 0, ws->sol_step->t, 0, ws->sol_step->t, 0); - - } + // iterative refinement on prediction step + for(itref0=0; itref0itref_pred_max; itref0++) + { + + COMPUTE_LIN_RES_DENSE_QP(ws->qp_step, qp_sol, ws->sol_step, ws->res_itref, ws->res_workspace); + + VECNRM_INF(cws->nv, ws->res_itref->res_g, 0, &itref_qp_norm[0]); + VECNRM_INF(cws->ne, ws->res_itref->res_b, 0, &itref_qp_norm[1]); + VECNRM_INF(cws->nc, ws->res_itref->res_d, 0, &itref_qp_norm[2]); + VECNRM_INF(cws->nc, ws->res_itref->res_m, 0, &itref_qp_norm[3]); + + if(itref0==0) + { + itref_qp_norm0[0] = itref_qp_norm[0]; + itref_qp_norm0[1] = itref_qp_norm[1]; + itref_qp_norm0[2] = itref_qp_norm[2]; + itref_qp_norm0[3] = itref_qp_norm[3]; + } + + if( \ + (itref_qp_norm[0]<1e0*arg->res_g_max | itref_qp_norm[0]<1e-3*qp_res[0]) & \ + (itref_qp_norm[1]<1e0*arg->res_b_max | itref_qp_norm[1]<1e-3*qp_res[1]) & \ + (itref_qp_norm[2]<1e0*arg->res_d_max | itref_qp_norm[2]<1e-3*qp_res[2]) & \ + (itref_qp_norm[3]<1e0*arg->res_m_max | itref_qp_norm[3]<1e-3*qp_res[3]) ) +// (itref_qp_norm[0]<=arg->res_g_max) & \ + (itref_qp_norm[1]<=arg->res_b_max) & \ + (itref_qp_norm[2]<=arg->res_d_max) & \ + (itref_qp_norm[3]<=arg->res_m_max) ) + { + break; + } + + SOLVE_KKT_STEP_DENSE_QP(ws->qp_itref, ws->sol_itref, arg, ws); + + AXPY(nv+2*ns, 1.0, ws->sol_itref->v, 0, ws->sol_step->v, 0, ws->sol_step->v, 0); + AXPY(ne, 1.0, ws->sol_itref->pi, 0, ws->sol_step->pi, 0, ws->sol_step->pi, 0); + AXPY(2*nb+2*ng+2*ns, 1.0, ws->sol_itref->lam, 0, ws->sol_step->lam, 0, ws->sol_step->lam, 0); + AXPY(2*nb+2*ng+2*ns, 1.0, ws->sol_itref->t, 0, ws->sol_step->t, 0, ws->sol_step->t, 0); + + } #if 0 - COMPUTE_LIN_RES_DENSE_QP(ws->qp_step, qp_sol, ws->sol_step, ws->res_itref, ws->res_workspace); - VECNRM_INF(cws->nv, ws->res_itref->res_g, 0, &itref_qp_norm0[0]); - VECNRM_INF(cws->ne, ws->res_itref->res_b, 0, &itref_qp_norm0[1]); - VECNRM_INF(cws->nc, ws->res_itref->res_d, 0, &itref_qp_norm0[2]); - VECNRM_INF(cws->nc, ws->res_itref->res_m, 0, &itref_qp_norm0[3]); -// printf("\nkk = %d\n", kk); -// blasfeo_print_exp_tran_dvec(qp->dim->nv, ws->res_itref->res_g, 0); -// blasfeo_print_exp_tran_dvec(qp->dim->ne, ws->res_itref->res_b, 0); -// blasfeo_print_exp_tran_dvec(2*qp->dim->nb+2*qp->dim->ng, ws->res_itref->res_d, 0); + COMPUTE_LIN_RES_DENSE_QP(ws->qp_step, qp_sol, ws->sol_step, ws->res_itref, ws->res_workspace); + VECNRM_INF(cws->nv, ws->res_itref->res_g, 0, &itref_qp_norm0[0]); + VECNRM_INF(cws->ne, ws->res_itref->res_b, 0, &itref_qp_norm0[1]); + VECNRM_INF(cws->nc, ws->res_itref->res_d, 0, &itref_qp_norm0[2]); + VECNRM_INF(cws->nc, ws->res_itref->res_m, 0, &itref_qp_norm0[3]); +// printf("\nkk = %d\n", kk); +// blasfeo_print_exp_tran_dvec(qp->dim->nv, ws->res_itref->res_g, 0); +// blasfeo_print_exp_tran_dvec(qp->dim->ne, ws->res_itref->res_b, 0); +// blasfeo_print_exp_tran_dvec(2*qp->dim->nb+2*qp->dim->ng, ws->res_itref->res_d, 0); #endif #if 0 - COMPUTE_RES_DENSE_QP(ws->qp_step, ws->sol_step, ws->res_itref, ws->res_workspace); - VECNRM_INF(cws->nv, ws->res_itref->res_g, 0, &itref_qp_norm0[0]); - VECNRM_INF(cws->ne, ws->res_itref->res_b, 0, &itref_qp_norm0[1]); - VECNRM_INF(cws->nc, ws->res_itref->res_d, 0, &itref_qp_norm0[2]); - VECNRM_INF(cws->nc, ws->res_itref->res_m, 0, &itref_qp_norm0[3]); -// printf("\nkk = %d\n", kk); -// blasfeo_print_exp_tran_dvec(qp->dim->nv, ws->res_itref->res_g, 0); -// blasfeo_print_exp_tran_dvec(qp->dim->ne, ws->res_itref->res_b, 0); -// blasfeo_print_exp_tran_dvec(2*qp->dim->nb+2*qp->dim->ng, ws->res_itref->res_d, 0); + COMPUTE_RES_DENSE_QP(ws->qp_step, ws->sol_step, ws->res_itref, ws->res_workspace); + VECNRM_INF(cws->nv, ws->res_itref->res_g, 0, &itref_qp_norm0[0]); + VECNRM_INF(cws->ne, ws->res_itref->res_b, 0, &itref_qp_norm0[1]); + VECNRM_INF(cws->nc, ws->res_itref->res_d, 0, &itref_qp_norm0[2]); + VECNRM_INF(cws->nc, ws->res_itref->res_m, 0, &itref_qp_norm0[3]); +// printf("\nkk = %d\n", kk); +// blasfeo_print_exp_tran_dvec(qp->dim->nv, ws->res_itref->res_g, 0); +// blasfeo_print_exp_tran_dvec(qp->dim->ne, ws->res_itref->res_b, 0); +// blasfeo_print_exp_tran_dvec(2*qp->dim->nb+2*qp->dim->ng, ws->res_itref->res_d, 0); #endif #if 0 - ndp0 = 0; - for(ii=0; iidim->nv; ii++) - { - if(ws->Lv->dA[ii]<=0) - { - ndp0 = ii; - break; - } - } - ndp1 = 0; - for(ii=0; iidim->ne; ii++) - { - if(ws->Le->dA[ii]<=0) - { - ndp1 = ii; - break; - } - } + ndp0 = 0; + for(ii=0; iidim->nv; ii++) + { + if(ws->Lv->dA[ii]<=0) + { + ndp0 = ii; + break; + } + } + ndp1 = 0; + for(ii=0; iidim->ne; ii++) + { + if(ws->Le->dA[ii]<=0) + { + ndp1 = ii; + break; + } + } #endif - // alpha - COMPUTE_ALPHA_QP(cws); - if(kkstat_max) - ws->stat[5*kk+0] = cws->alpha; - - // Mehrotra's predictor-corrector - if(arg->pred_corr==1) - { - // mu_aff - COMPUTE_MU_AFF_QP(cws); - if(kkstat_max) - ws->stat[5*kk+1] = cws->mu_aff; - - // compute centering parameter - tmp = cws->mu_aff/cws->mu; - cws->sigma = tmp*tmp*tmp; -// cws->sigma = sigma_min>cws->sigma ? sigma_min : cws->sigma; - if(kkstat_max) - ws->stat[5*kk+2] = cws->sigma; - - COMPUTE_CENTERING_CORRECTION_QP(cws); - - // solve kkt - SOLVE_KKT_STEP_DENSE_QP(ws->qp_step, ws->sol_step, arg, ws); - - // alpha - COMPUTE_ALPHA_QP(cws); - if(kkstat_max) - ws->stat[5*kk+3] = cws->alpha; - - // conditional Mehrotra's predictor-corrector - if(arg->cond_pred_corr==1) - { - - // save mu_aff (from prediction sol_step) - mu_aff0 = cws->mu_aff; - - // compute mu for predictor-corrector-centering - COMPUTE_MU_AFF_QP(cws); - -// if(cws->mu_aff > 2.0*cws->mu) - if(cws->mu_aff > 2.0*mu_aff0) - { - - // centering direction - COMPUTE_CENTERING_QP(cws); - - // solve kkt - SOLVE_KKT_STEP_DENSE_QP(ws->qp_step, ws->sol_step, arg, ws); - - // alpha - COMPUTE_ALPHA_QP(cws); - if(kkstat_max) - ws->stat[5*kk+3] = cws->alpha; - - } - - } - - iter_ref_step = 0; - for(itref1=0; itref1itref_corr_max; itref1++) - { - - COMPUTE_LIN_RES_DENSE_QP(ws->qp_step, qp_sol, ws->sol_step, ws->res_itref, ws->res_workspace); - - VECNRM_INF(cws->nv, ws->res_itref->res_g, 0, &itref_qp_norm[0]); - VECNRM_INF(cws->ne, ws->res_itref->res_b, 0, &itref_qp_norm[1]); - VECNRM_INF(cws->nc, ws->res_itref->res_d, 0, &itref_qp_norm[2]); - VECNRM_INF(cws->nc, ws->res_itref->res_m, 0, &itref_qp_norm[3]); - - if(itref1==0) - { - itref_qp_norm0[0] = itref_qp_norm[0]; - itref_qp_norm0[1] = itref_qp_norm[1]; - itref_qp_norm0[2] = itref_qp_norm[2]; - itref_qp_norm0[3] = itref_qp_norm[3]; - } - - if( \ - (itref_qp_norm[0]<1e0*arg->res_g_max | itref_qp_norm[0]<1e-3*qp_res[0]) & \ - (itref_qp_norm[1]<1e0*arg->res_b_max | itref_qp_norm[1]<1e-3*qp_res[1]) & \ - (itref_qp_norm[2]<1e0*arg->res_d_max | itref_qp_norm[2]<1e-3*qp_res[2]) & \ - (itref_qp_norm[3]<1e0*arg->res_m_max | itref_qp_norm[3]<1e-3*qp_res[3]) ) -// (itref_qp_norm[0]<=arg->res_g_max) & \ - (itref_qp_norm[1]<=arg->res_b_max) & \ - (itref_qp_norm[2]<=arg->res_d_max) & \ - (itref_qp_norm[3]<=arg->res_m_max) ) - { - break; - } - - SOLVE_KKT_STEP_DENSE_QP(ws->qp_itref, ws->sol_itref, arg, ws); - iter_ref_step = 1; - - AXPY(nv+2*ns, 1.0, ws->sol_itref->v, 0, ws->sol_step->v, 0, ws->sol_step->v, 0); - AXPY(ne, 1.0, ws->sol_itref->pi, 0, ws->sol_step->pi, 0, ws->sol_step->pi, 0); - AXPY(2*nb+2*ng+2*ns, 1.0, ws->sol_itref->lam, 0, ws->sol_step->lam, 0, ws->sol_step->lam, 0); - AXPY(2*nb+2*ng+2*ns, 1.0, ws->sol_itref->t, 0, ws->sol_step->t, 0, ws->sol_step->t, 0); - - } - - if(iter_ref_step) - { - // alpha - COMPUTE_ALPHA_QP(cws); - if(kkstat_max) - ws->stat[5*kk+3] = cws->alpha; - } - - } - - // - UPDATE_VAR_QP(cws); - - // compute residuals - COMPUTE_RES_DENSE_QP(qp, qp_sol, ws->res, ws->res_workspace); - BACKUP_RES_M(cws); - cws->mu = ws->res->res_mu; - if(kkstat_max) - ws->stat[5*kk+4] = ws->res->res_mu; - - // compute infinity norm of residuals - VECNRM_INF(cws->nv, &str_res_g, 0, &qp_res[0]); - VECNRM_INF(cws->ne, &str_res_b, 0, &qp_res[1]); - VECNRM_INF(cws->nc, &str_res_d, 0, &qp_res[2]); - VECNRM_INF(cws->nc, &str_res_m, 0, &qp_res[3]); + // alpha + COMPUTE_ALPHA_QP(cws); + if(kkstat_max) + ws->stat[5*kk+0] = cws->alpha; + + // Mehrotra's predictor-corrector + if(arg->pred_corr==1) + { + // mu_aff + COMPUTE_MU_AFF_QP(cws); + if(kkstat_max) + ws->stat[5*kk+1] = cws->mu_aff; + + // compute centering parameter + tmp = cws->mu_aff/cws->mu; + cws->sigma = tmp*tmp*tmp; +// cws->sigma = sigma_min>cws->sigma ? sigma_min : cws->sigma; + if(kkstat_max) + ws->stat[5*kk+2] = cws->sigma; + + COMPUTE_CENTERING_CORRECTION_QP(cws); + + // solve kkt + SOLVE_KKT_STEP_DENSE_QP(ws->qp_step, ws->sol_step, arg, ws); + + // alpha + COMPUTE_ALPHA_QP(cws); + if(kkstat_max) + ws->stat[5*kk+3] = cws->alpha; + + // conditional Mehrotra's predictor-corrector + if(arg->cond_pred_corr==1) + { + + // save mu_aff (from prediction sol_step) + mu_aff0 = cws->mu_aff; + + // compute mu for predictor-corrector-centering + COMPUTE_MU_AFF_QP(cws); + +// if(cws->mu_aff > 2.0*cws->mu) + if(cws->mu_aff > 2.0*mu_aff0) + { + + // centering direction + COMPUTE_CENTERING_QP(cws); + + // solve kkt + SOLVE_KKT_STEP_DENSE_QP(ws->qp_step, ws->sol_step, arg, ws); + + // alpha + COMPUTE_ALPHA_QP(cws); + if(kkstat_max) + ws->stat[5*kk+3] = cws->alpha; + + } + + } + + iter_ref_step = 0; + for(itref1=0; itref1itref_corr_max; itref1++) + { + + COMPUTE_LIN_RES_DENSE_QP(ws->qp_step, qp_sol, ws->sol_step, ws->res_itref, ws->res_workspace); + + VECNRM_INF(cws->nv, ws->res_itref->res_g, 0, &itref_qp_norm[0]); + VECNRM_INF(cws->ne, ws->res_itref->res_b, 0, &itref_qp_norm[1]); + VECNRM_INF(cws->nc, ws->res_itref->res_d, 0, &itref_qp_norm[2]); + VECNRM_INF(cws->nc, ws->res_itref->res_m, 0, &itref_qp_norm[3]); + + if(itref1==0) + { + itref_qp_norm0[0] = itref_qp_norm[0]; + itref_qp_norm0[1] = itref_qp_norm[1]; + itref_qp_norm0[2] = itref_qp_norm[2]; + itref_qp_norm0[3] = itref_qp_norm[3]; + } + + if( \ + (itref_qp_norm[0]<1e0*arg->res_g_max | itref_qp_norm[0]<1e-3*qp_res[0]) & \ + (itref_qp_norm[1]<1e0*arg->res_b_max | itref_qp_norm[1]<1e-3*qp_res[1]) & \ + (itref_qp_norm[2]<1e0*arg->res_d_max | itref_qp_norm[2]<1e-3*qp_res[2]) & \ + (itref_qp_norm[3]<1e0*arg->res_m_max | itref_qp_norm[3]<1e-3*qp_res[3]) ) +// (itref_qp_norm[0]<=arg->res_g_max) & \ + (itref_qp_norm[1]<=arg->res_b_max) & \ + (itref_qp_norm[2]<=arg->res_d_max) & \ + (itref_qp_norm[3]<=arg->res_m_max) ) + { + break; + } + + SOLVE_KKT_STEP_DENSE_QP(ws->qp_itref, ws->sol_itref, arg, ws); + iter_ref_step = 1; + + AXPY(nv+2*ns, 1.0, ws->sol_itref->v, 0, ws->sol_step->v, 0, ws->sol_step->v, 0); + AXPY(ne, 1.0, ws->sol_itref->pi, 0, ws->sol_step->pi, 0, ws->sol_step->pi, 0); + AXPY(2*nb+2*ng+2*ns, 1.0, ws->sol_itref->lam, 0, ws->sol_step->lam, 0, ws->sol_step->lam, 0); + AXPY(2*nb+2*ng+2*ns, 1.0, ws->sol_itref->t, 0, ws->sol_step->t, 0, ws->sol_step->t, 0); + + } + + if(iter_ref_step) + { + // alpha + COMPUTE_ALPHA_QP(cws); + if(kkstat_max) + ws->stat[5*kk+3] = cws->alpha; + } + + } + + // + UPDATE_VAR_QP(cws); + + // compute residuals + COMPUTE_RES_DENSE_QP(qp, qp_sol, ws->res, ws->res_workspace); + BACKUP_RES_M(cws); + cws->mu = ws->res->res_mu; + if(kkstat_max) + ws->stat[5*kk+4] = ws->res->res_mu; + + // compute infinity norm of residuals + VECNRM_INF(cws->nv, &str_res_g, 0, &qp_res[0]); + VECNRM_INF(cws->ne, &str_res_b, 0, &qp_res[1]); + VECNRM_INF(cws->nc, &str_res_d, 0, &qp_res[2]); + VECNRM_INF(cws->nc, &str_res_m, 0, &qp_res[3]); if(arg->print_level > 0){ if(kk%10 == 0){ @@ -1097,9 +1097,9 @@ printf("%e %e %e %e\n", qp_res[0], qp_res[1], qp_res[2], qp_res[3]); printf("iter %3d alpha %1.3e %1.3e sigma %1.3e ndp %3d %3d itref %d %d res_kkt %1.3e %1.3e %1.3e %1.3e res_kkt %1.3e %1.3e %1.3e %1.3e res_qp %1.3e %1.3e %1.3e %1.3e\n", kk, cws->alpha_prim, cws->alpha_dual, cws->sigma, ndp0, ndp1, itref0, itref1, itref_qp_norm0[0], itref_qp_norm0[1], itref_qp_norm0[2], itref_qp_norm0[3], itref_qp_norm[0], itref_qp_norm[1], itref_qp_norm[2], itref_qp_norm[3], qp_res[0], qp_res[1], qp_res[2], qp_res[3]); #endif - } + } - ws->iter = kk; + ws->iter = kk; // max iteration number reached if(kk == arg->iter_max) { @@ -1135,6 +1135,6 @@ printf("iter %3d alpha %1.3e %1.3e sigma %1.3e ndp %3d %3d itref %d %d printf(" \n -> SOLUTION FOUND. \n\n"); return 0; - } + } From 6af3b7411223ddbc8a8ec17f36c07e056208dd52 Mon Sep 17 00:00:00 2001 From: zanellia Date: Mon, 30 Jul 2018 20:23:19 +0200 Subject: [PATCH 13/16] Revert "adding print level field to args for ocp_qp too, retabbing" This reverts commit c652c2737cde82519ca74267754ab7212362eab5. --- include/hpipm_d_ocp_qp_ipm.h | 109 +++++++++++++++++------------------ include/hpipm_s_ocp_qp_ipm.h | 107 +++++++++++++++++----------------- 2 files changed, 107 insertions(+), 109 deletions(-) diff --git a/include/hpipm_d_ocp_qp_ipm.h b/include/hpipm_d_ocp_qp_ipm.h index 5a067d56..b117dac3 100644 --- a/include/hpipm_d_ocp_qp_ipm.h +++ b/include/hpipm_d_ocp_qp_ipm.h @@ -48,64 +48,63 @@ extern "C" { struct d_ocp_qp_ipm_arg - { - double mu0; // initial value for duality measure - double alpha_min; // exit cond on step length - double res_g_max; // exit cond on inf norm of residuals - double res_b_max; // exit cond on inf norm of residuals - double res_d_max; // exit cond on inf norm of residuals - double res_m_max; // exit cond on inf norm of residuals - double reg_prim; // reg of primal hessian - double lam_min; // min value in lam vector - double t_min; // min value in t vector - int iter_max; // exit cond in iter number - int stat_max; // iterations saved in stat - int pred_corr; // use Mehrotra's predictor-corrector IPM algirthm - int cond_pred_corr; // conditional Mehrotra's predictor-corrector - int itref_pred_max; // max number of iterative refinement steps for predictor step - int itref_corr_max; // max number of iterative refinement steps for corrector step - int warm_start; // 0 no warm start, 1 warm start primal sol - int lq_fact; // 0 syrk+potrf, 1 mix, 2 lq - int abs_form; // absolute IPM formulation - int comp_dual_sol; // dual solution (only for abs_form==1) - int comp_res_exit; // compute residuals on exit (only for abs_form==1 and comp_dual_sol==1) - int memsize; - int print_level; - }; + { + double mu0; // initial value for duality measure + double alpha_min; // exit cond on step length + double res_g_max; // exit cond on inf norm of residuals + double res_b_max; // exit cond on inf norm of residuals + double res_d_max; // exit cond on inf norm of residuals + double res_m_max; // exit cond on inf norm of residuals + double reg_prim; // reg of primal hessian + double lam_min; // min value in lam vector + double t_min; // min value in t vector + int iter_max; // exit cond in iter number + int stat_max; // iterations saved in stat + int pred_corr; // use Mehrotra's predictor-corrector IPM algirthm + int cond_pred_corr; // conditional Mehrotra's predictor-corrector + int itref_pred_max; // max number of iterative refinement steps for predictor step + int itref_corr_max; // max number of iterative refinement steps for corrector step + int warm_start; // 0 no warm start, 1 warm start primal sol + int lq_fact; // 0 syrk+potrf, 1 mix, 2 lq + int abs_form; // absolute IPM formulation + int comp_dual_sol; // dual solution (only for abs_form==1) + int comp_res_exit; // compute residuals on exit (only for abs_form==1 and comp_dual_sol==1) + int memsize; + }; struct d_ocp_qp_ipm_workspace - { - struct d_core_qp_ipm_workspace *core_workspace; - struct d_ocp_qp_res_workspace *res_workspace; - struct d_ocp_qp_sol *sol_step; - struct d_ocp_qp_sol *sol_itref; - struct d_ocp_qp *qp_step; - struct d_ocp_qp *qp_itref; - struct d_ocp_qp_res *res_itref; - struct d_ocp_qp_res *res; - struct blasfeo_dvec *Gamma; // hessian update - struct blasfeo_dvec *gamma; // hessian update - struct blasfeo_dvec *tmp_nxM; // work space of size nxM - struct blasfeo_dvec *tmp_nbgM; // work space of size nbM+ngM - struct blasfeo_dvec *tmp_nsM; // work space of size nsM - struct blasfeo_dvec *Pb; // Pb - struct blasfeo_dvec *Zs_inv; - struct blasfeo_dmat *L; - struct blasfeo_dmat *Lh; - struct blasfeo_dmat *AL; - struct blasfeo_dmat *lq0; - struct blasfeo_dvec *tmp_m; - double *stat; // convergence statistics - int *use_hess_fact; - void *lq_work0; - double qp_res[4]; // infinity norm of residuals - int iter; // iteration number - int stat_max; // iterations saved in stat - int use_Pb; - int memsize; - }; + { + struct d_core_qp_ipm_workspace *core_workspace; + struct d_ocp_qp_res_workspace *res_workspace; + struct d_ocp_qp_sol *sol_step; + struct d_ocp_qp_sol *sol_itref; + struct d_ocp_qp *qp_step; + struct d_ocp_qp *qp_itref; + struct d_ocp_qp_res *res_itref; + struct d_ocp_qp_res *res; + struct blasfeo_dvec *Gamma; // hessian update + struct blasfeo_dvec *gamma; // hessian update + struct blasfeo_dvec *tmp_nxM; // work space of size nxM + struct blasfeo_dvec *tmp_nbgM; // work space of size nbM+ngM + struct blasfeo_dvec *tmp_nsM; // work space of size nsM + struct blasfeo_dvec *Pb; // Pb + struct blasfeo_dvec *Zs_inv; + struct blasfeo_dmat *L; + struct blasfeo_dmat *Lh; + struct blasfeo_dmat *AL; + struct blasfeo_dmat *lq0; + struct blasfeo_dvec *tmp_m; + double *stat; // convergence statistics + int *use_hess_fact; + void *lq_work0; + double qp_res[4]; // infinity norm of residuals + int iter; // iteration number + int stat_max; // iterations saved in stat + int use_Pb; + int memsize; + }; @@ -128,7 +127,7 @@ int d_sizeof_ocp_qp_ipm_arg(); int d_sizeof_ocp_qp_ipm_workspace(); #ifdef __cplusplus -} // #extern "C" +} // #extern "C" #endif diff --git a/include/hpipm_s_ocp_qp_ipm.h b/include/hpipm_s_ocp_qp_ipm.h index 6232304e..7f4ac4fc 100644 --- a/include/hpipm_s_ocp_qp_ipm.h +++ b/include/hpipm_s_ocp_qp_ipm.h @@ -50,64 +50,63 @@ extern "C" { struct s_ocp_qp_ipm_arg - { - float mu0; // initial value for duality measure - float alpha_min; // exit cond on step length - float res_g_max; // exit cond on inf norm of residuals - float res_b_max; // exit cond on inf norm of residuals - float res_d_max; // exit cond on inf norm of residuals - float res_m_max; // exit cond on inf norm of residuals - float reg_prim; // reg of primal hessian - float lam_min; // min value in lam vector - float t_min; // min value in t vector - int iter_max; // exit cond in iter number - int stat_max; // iterations saved in stat - int pred_corr; // use Mehrotra's predictor-corrector IPM algirthm - int cond_pred_corr; // conditional Mehrotra's predictor-corrector - int itref_pred_max; // max number of iterative refinement steps for predictor step - int itref_corr_max; // max number of iterative refinement steps for corrector step - int warm_start; // 0 no warm start, 1 warm start primal sol - int lq_fact; // 0 syrk+potrf, 1 mix, 2 lq - int abs_form; // absolute IPM formulation - int comp_dual_sol; // dual solution (only for abs_form==1) - int comp_res_exit; // compute residuals on exit (only for abs_form==1 and comp_dual_sol==1) - int memsize; - int print_level; - }; + { + float mu0; // initial value for duality measure + float alpha_min; // exit cond on step length + float res_g_max; // exit cond on inf norm of residuals + float res_b_max; // exit cond on inf norm of residuals + float res_d_max; // exit cond on inf norm of residuals + float res_m_max; // exit cond on inf norm of residuals + float reg_prim; // reg of primal hessian + float lam_min; // min value in lam vector + float t_min; // min value in t vector + int iter_max; // exit cond in iter number + int stat_max; // iterations saved in stat + int pred_corr; // use Mehrotra's predictor-corrector IPM algirthm + int cond_pred_corr; // conditional Mehrotra's predictor-corrector + int itref_pred_max; // max number of iterative refinement steps for predictor step + int itref_corr_max; // max number of iterative refinement steps for corrector step + int warm_start; // 0 no warm start, 1 warm start primal sol + int lq_fact; // 0 syrk+potrf, 1 mix, 2 lq + int abs_form; // absolute IPM formulation + int comp_dual_sol; // dual solution (only for abs_form==1) + int comp_res_exit; // compute residuals on exit (only for abs_form==1 and comp_dual_sol==1) + int memsize; + }; struct s_ocp_qp_ipm_workspace - { - struct s_core_qp_ipm_workspace *core_workspace; - struct s_ocp_qp_res_workspace *res_workspace; - struct s_ocp_qp_sol *sol_step; - struct s_ocp_qp_sol *sol_itref; - struct s_ocp_qp *qp_step; - struct s_ocp_qp *qp_itref; - struct s_ocp_qp_res *res; - struct s_ocp_qp_res *res_itref; - struct blasfeo_svec *Gamma; // hessian update - struct blasfeo_svec *gamma; // hessian update - struct blasfeo_svec *tmp_nxM; // work space of size nxM - struct blasfeo_svec *tmp_nbgM; // work space of size nbM+ngM - struct blasfeo_svec *tmp_nsM; // work space of size nsM - struct blasfeo_svec *Pb; // Pb - struct blasfeo_svec *Zs_inv; - struct blasfeo_smat *L; - struct blasfeo_smat *Lh; - struct blasfeo_smat *AL; - struct blasfeo_smat *lq0; - struct blasfeo_svec *tmp_m; - float *stat; // convergence statistics - int *use_hess_fact; - void *lq_work0; - float qp_res[4]; // infinity norm of residuals - int iter; // iteration number - int stat_max; // iterations saved in stat - int use_Pb; - int memsize; - }; + { + struct s_core_qp_ipm_workspace *core_workspace; + struct s_ocp_qp_res_workspace *res_workspace; + struct s_ocp_qp_sol *sol_step; + struct s_ocp_qp_sol *sol_itref; + struct s_ocp_qp *qp_step; + struct s_ocp_qp *qp_itref; + struct s_ocp_qp_res *res; + struct s_ocp_qp_res *res_itref; + struct blasfeo_svec *Gamma; // hessian update + struct blasfeo_svec *gamma; // hessian update + struct blasfeo_svec *tmp_nxM; // work space of size nxM + struct blasfeo_svec *tmp_nbgM; // work space of size nbM+ngM + struct blasfeo_svec *tmp_nsM; // work space of size nsM + struct blasfeo_svec *Pb; // Pb + struct blasfeo_svec *Zs_inv; + struct blasfeo_smat *L; + struct blasfeo_smat *Lh; + struct blasfeo_smat *AL; + struct blasfeo_smat *lq0; + struct blasfeo_svec *tmp_m; + float *stat; // convergence statistics + int *use_hess_fact; + void *lq_work0; + float qp_res[4]; // infinity norm of residuals + int iter; // iteration number + int stat_max; // iterations saved in stat + int use_Pb; + int memsize; + }; From c5fe9526fb9f0ea9e143c47e728fe5d2c1ee55f2 Mon Sep 17 00:00:00 2001 From: zanellia Date: Mon, 30 Jul 2018 20:23:35 +0200 Subject: [PATCH 14/16] Revert "adding print level field to args, retabbing" This reverts commit 70c0ced68e356ba2581442b989d6a856e6ea222c. --- include/hpipm_d_dense_qp_ipm.h | 1 - include/hpipm_s_dense_qp_ipm.h | 121 ++++++++++++++++----------------- 2 files changed, 60 insertions(+), 62 deletions(-) diff --git a/include/hpipm_d_dense_qp_ipm.h b/include/hpipm_d_dense_qp_ipm.h index 0157b312..688a1abc 100644 --- a/include/hpipm_d_dense_qp_ipm.h +++ b/include/hpipm_d_dense_qp_ipm.h @@ -72,7 +72,6 @@ struct d_dense_qp_ipm_arg int abs_form; // absolute IPM formulation int comp_res_exit; // compute residuals on exit (only for abs_form==1) int memsize; - int print_level; }; diff --git a/include/hpipm_s_dense_qp_ipm.h b/include/hpipm_s_dense_qp_ipm.h index 3255dd9c..1e6ca38c 100644 --- a/include/hpipm_s_dense_qp_ipm.h +++ b/include/hpipm_s_dense_qp_ipm.h @@ -50,71 +50,70 @@ extern "C" { struct s_dense_qp_ipm_arg - { - float mu0; // initial value for duality measure - float alpha_min; // exit cond on step length - float res_g_max; // exit cond on inf norm of residuals - float res_b_max; // exit cond on inf norm of residuals - float res_d_max; // exit cond on inf norm of residuals - float res_m_max; // exit cond on inf norm of residuals - float reg_prim; // reg of primal hessian - float reg_dual; // reg of dual hessian - float lam_min; // min value in lam vector - float t_min; // min value in t vector - int iter_max; // exit cond in iter number - int stat_max; // iterations saved in stat - int pred_corr; // Mehrotra's predictor-corrector IPM algirthm - int cond_pred_corr; // conditional Mehrotra's predictor-corrector - int scale; // scale hessian - int itref_pred_max; // max number of iterative refinement steps for predictor step - int itref_corr_max; // max number of iterative refinement steps for corrector step - int warm_start; // 0 no warm start, 1 warm start primal sol - int lq_fact; // 0 syrk+potrf, 1 mix, 2 lq - int abs_form; // absolute IPM formulation - int comp_res_exit; // compute residuals on exit (only for abs_form==1) - int memsize; - int print_level; - }; + { + float mu0; // initial value for duality measure + float alpha_min; // exit cond on step length + float res_g_max; // exit cond on inf norm of residuals + float res_b_max; // exit cond on inf norm of residuals + float res_d_max; // exit cond on inf norm of residuals + float res_m_max; // exit cond on inf norm of residuals + float reg_prim; // reg of primal hessian + float reg_dual; // reg of dual hessian + float lam_min; // min value in lam vector + float t_min; // min value in t vector + int iter_max; // exit cond in iter number + int stat_max; // iterations saved in stat + int pred_corr; // Mehrotra's predictor-corrector IPM algirthm + int cond_pred_corr; // conditional Mehrotra's predictor-corrector + int scale; // scale hessian + int itref_pred_max; // max number of iterative refinement steps for predictor step + int itref_corr_max; // max number of iterative refinement steps for corrector step + int warm_start; // 0 no warm start, 1 warm start primal sol + int lq_fact; // 0 syrk+potrf, 1 mix, 2 lq + int abs_form; // absolute IPM formulation + int comp_res_exit; // compute residuals on exit (only for abs_form==1) + int memsize; + }; struct s_dense_qp_ipm_workspace - { - struct s_core_qp_ipm_workspace *core_workspace; - struct s_dense_qp_res_workspace *res_workspace; - struct s_dense_qp_sol *sol_step; - struct s_dense_qp_sol *sol_itref; - struct s_dense_qp *qp_step; - struct s_dense_qp *qp_itref; - struct s_dense_qp_res *res; - struct s_dense_qp_res *res_itref; - struct blasfeo_svec *Gamma; // - struct blasfeo_svec *gamma; // - struct blasfeo_svec *Zs_inv; // - struct blasfeo_smat *Lv; // - struct blasfeo_smat *AL; // - struct blasfeo_smat *Le; // - struct blasfeo_smat *Ctx; // - struct blasfeo_svec *lv; // - struct blasfeo_svec *sv; // scale for Lv - struct blasfeo_svec *se; // scale for Le - struct blasfeo_svec *tmp_nbg; // work space of size nb+ng - struct blasfeo_svec *tmp_ns; // work space of size ns - struct blasfeo_smat *lq0; - struct blasfeo_smat *lq1; - struct blasfeo_svec *tmp_m; - float *stat; // convergence statistics -// int *ipiv_v; -// int *ipiv_e; - void *lq_work0; - void *lq_work1; - float qp_res[4]; // infinity norm of residuals - int iter; // iteration number - int stat_max; // iterations saved in stat - int scale; - int use_hess_fact; - int memsize; // memory size (in bytes) of workspace - }; + { + struct s_core_qp_ipm_workspace *core_workspace; + struct s_dense_qp_res_workspace *res_workspace; + struct s_dense_qp_sol *sol_step; + struct s_dense_qp_sol *sol_itref; + struct s_dense_qp *qp_step; + struct s_dense_qp *qp_itref; + struct s_dense_qp_res *res; + struct s_dense_qp_res *res_itref; + struct blasfeo_svec *Gamma; // + struct blasfeo_svec *gamma; // + struct blasfeo_svec *Zs_inv; // + struct blasfeo_smat *Lv; // + struct blasfeo_smat *AL; // + struct blasfeo_smat *Le; // + struct blasfeo_smat *Ctx; // + struct blasfeo_svec *lv; // + struct blasfeo_svec *sv; // scale for Lv + struct blasfeo_svec *se; // scale for Le + struct blasfeo_svec *tmp_nbg; // work space of size nb+ng + struct blasfeo_svec *tmp_ns; // work space of size ns + struct blasfeo_smat *lq0; + struct blasfeo_smat *lq1; + struct blasfeo_svec *tmp_m; + float *stat; // convergence statistics +// int *ipiv_v; +// int *ipiv_e; + void *lq_work0; + void *lq_work1; + float qp_res[4]; // infinity norm of residuals + int iter; // iteration number + int stat_max; // iterations saved in stat + int scale; + int use_hess_fact; + int memsize; // memory size (in bytes) of workspace + }; From a424987abacca7ec9f4514e7f3c282bd165cb534 Mon Sep 17 00:00:00 2001 From: zanellia Date: Mon, 30 Jul 2018 20:26:04 +0200 Subject: [PATCH 15/16] readded print level after reverting --- include/hpipm_d_dense_qp_ipm.h | 1 + include/hpipm_d_ocp_qp_ipm.h | 1 + include/hpipm_s_dense_qp_ipm.h | 1 + include/hpipm_s_ocp_qp_ipm.h | 1 + 4 files changed, 4 insertions(+) diff --git a/include/hpipm_d_dense_qp_ipm.h b/include/hpipm_d_dense_qp_ipm.h index 688a1abc..0157b312 100644 --- a/include/hpipm_d_dense_qp_ipm.h +++ b/include/hpipm_d_dense_qp_ipm.h @@ -72,6 +72,7 @@ struct d_dense_qp_ipm_arg int abs_form; // absolute IPM formulation int comp_res_exit; // compute residuals on exit (only for abs_form==1) int memsize; + int print_level; }; diff --git a/include/hpipm_d_ocp_qp_ipm.h b/include/hpipm_d_ocp_qp_ipm.h index b117dac3..ca0c3765 100644 --- a/include/hpipm_d_ocp_qp_ipm.h +++ b/include/hpipm_d_ocp_qp_ipm.h @@ -70,6 +70,7 @@ struct d_ocp_qp_ipm_arg int comp_dual_sol; // dual solution (only for abs_form==1) int comp_res_exit; // compute residuals on exit (only for abs_form==1 and comp_dual_sol==1) int memsize; + int print_level; }; diff --git a/include/hpipm_s_dense_qp_ipm.h b/include/hpipm_s_dense_qp_ipm.h index 1e6ca38c..f057b9a0 100644 --- a/include/hpipm_s_dense_qp_ipm.h +++ b/include/hpipm_s_dense_qp_ipm.h @@ -73,6 +73,7 @@ struct s_dense_qp_ipm_arg int abs_form; // absolute IPM formulation int comp_res_exit; // compute residuals on exit (only for abs_form==1) int memsize; + int print_level; }; diff --git a/include/hpipm_s_ocp_qp_ipm.h b/include/hpipm_s_ocp_qp_ipm.h index 7f4ac4fc..6e3dc1c4 100644 --- a/include/hpipm_s_ocp_qp_ipm.h +++ b/include/hpipm_s_ocp_qp_ipm.h @@ -72,6 +72,7 @@ struct s_ocp_qp_ipm_arg int comp_dual_sol; // dual solution (only for abs_form==1) int comp_res_exit; // compute residuals on exit (only for abs_form==1 and comp_dual_sol==1) int memsize; + int print_level; }; From 1117f5cb3263dc5a1a001ad6f31461474c6dcfa3 Mon Sep 17 00:00:00 2001 From: zanellia Date: Tue, 31 Jul 2018 11:17:44 +0200 Subject: [PATCH 16/16] merged with blessed/master --- ocp_qp/x_ocp_qp_ipm.c | 1956 ++++++++++++++++++++--------------------- 1 file changed, 978 insertions(+), 978 deletions(-) diff --git a/ocp_qp/x_ocp_qp_ipm.c b/ocp_qp/x_ocp_qp_ipm.c index ad06b844..da8c5aa8 100644 --- a/ocp_qp/x_ocp_qp_ipm.c +++ b/ocp_qp/x_ocp_qp_ipm.c @@ -35,133 +35,133 @@ int SIZEOF_OCP_QP_IPM_ARG() int MEMSIZE_OCP_QP_IPM_ARG(struct OCP_QP_DIM *dim) - { + { - return 0; + return 0; - } + } void CREATE_OCP_QP_IPM_ARG(struct OCP_QP_DIM *dim, struct OCP_QP_IPM_ARG *arg, void *mem) - { + { - arg->memsize = 0; + arg->memsize = 0; - return; + return; - } + } void SET_DEFAULT_OCP_QP_IPM_ARG(enum HPIPM_MODE mode, struct OCP_QP_IPM_ARG *arg) - { + { // set common default arguments arg->print_level = 1; // set mode-specific default arguments - if(mode==SPEED_ABS) - { - arg->mu0 = 1e1; - arg->alpha_min = 1e-12; - arg->res_g_max = 1e0; // not used - arg->res_b_max = 1e0; // not used - arg->res_d_max = 1e0; // not used - arg->res_m_max = 1e-8; - arg->iter_max = 15; - arg->stat_max = 15; - arg->pred_corr = 1; - arg->cond_pred_corr = 0; // not used - arg->itref_pred_max = 0; // not used - arg->itref_corr_max = 0; // not used - arg->reg_prim = 1e-15; - arg->lq_fact = 0; // not used - arg->lam_min = 1e-30; - arg->t_min = 1e-30; - arg->warm_start = 0; - arg->abs_form = 1; - arg->comp_dual_sol = 0; - arg->comp_res_exit = 0; - } - else if(mode==SPEED) - { - arg->mu0 = 1e1; - arg->alpha_min = 1e-12; - arg->res_g_max = 1e-6; - arg->res_b_max = 1e-8; - arg->res_d_max = 1e-8; - arg->res_m_max = 1e-8; - arg->iter_max = 15; - arg->stat_max = 15; - arg->pred_corr = 1; - arg->cond_pred_corr = 1; - arg->itref_pred_max = 0; - arg->itref_corr_max = 0; - arg->reg_prim = 1e-15; - arg->lq_fact = 0; - arg->lam_min = 1e-30; - arg->t_min = 1e-30; - arg->warm_start = 0; - arg->abs_form = 0; - arg->comp_dual_sol = 1; - arg->comp_res_exit = 1; - } - else if(mode==BALANCE) - { - arg->mu0 = 1e1; - arg->alpha_min = 1e-12; - arg->res_g_max = 1e-6; - arg->res_b_max = 1e-8; - arg->res_d_max = 1e-8; - arg->res_m_max = 1e-8; - arg->iter_max = 30; - arg->stat_max = 30; - arg->pred_corr = 1; - arg->cond_pred_corr = 1; - arg->itref_pred_max = 0; - arg->itref_corr_max = 2; - arg->reg_prim = 1e-15; - arg->lq_fact = 1; - arg->lam_min = 1e-30; - arg->t_min = 1e-30; - arg->warm_start = 0; - arg->abs_form = 0; - arg->comp_dual_sol = 1; - arg->comp_res_exit = 1; - } - else if(mode==ROBUST) - { - arg->mu0 = 1e2; - arg->alpha_min = 1e-12; - arg->res_g_max = 1e-6; - arg->res_b_max = 1e-8; - arg->res_d_max = 1e-8; - arg->res_m_max = 1e-8; - arg->iter_max = 100; - arg->stat_max = 100; - arg->pred_corr = 1; - arg->cond_pred_corr = 1; - arg->itref_pred_max = 0; - arg->itref_corr_max = 4; - arg->reg_prim = 1e-15; - arg->lq_fact = 2; - arg->lam_min = 1e-30; - arg->t_min = 1e-30; - arg->warm_start = 0; - arg->abs_form = 0; - arg->comp_dual_sol = 1; - arg->comp_res_exit = 1; - } - else - { - printf("\nwrong set default mode\n"); - exit(1); - } + if(mode==SPEED_ABS) + { + arg->mu0 = 1e1; + arg->alpha_min = 1e-12; + arg->res_g_max = 1e0; // not used + arg->res_b_max = 1e0; // not used + arg->res_d_max = 1e0; // not used + arg->res_m_max = 1e-8; + arg->iter_max = 15; + arg->stat_max = 15; + arg->pred_corr = 1; + arg->cond_pred_corr = 0; // not used + arg->itref_pred_max = 0; // not used + arg->itref_corr_max = 0; // not used + arg->reg_prim = 1e-15; + arg->lq_fact = 0; // not used + arg->lam_min = 1e-30; + arg->t_min = 1e-30; + arg->warm_start = 0; + arg->abs_form = 1; + arg->comp_dual_sol = 0; + arg->comp_res_exit = 0; + } + else if(mode==SPEED) + { + arg->mu0 = 1e1; + arg->alpha_min = 1e-12; + arg->res_g_max = 1e-6; + arg->res_b_max = 1e-8; + arg->res_d_max = 1e-8; + arg->res_m_max = 1e-8; + arg->iter_max = 15; + arg->stat_max = 15; + arg->pred_corr = 1; + arg->cond_pred_corr = 1; + arg->itref_pred_max = 0; + arg->itref_corr_max = 0; + arg->reg_prim = 1e-15; + arg->lq_fact = 0; + arg->lam_min = 1e-30; + arg->t_min = 1e-30; + arg->warm_start = 0; + arg->abs_form = 0; + arg->comp_dual_sol = 1; + arg->comp_res_exit = 1; + } + else if(mode==BALANCE) + { + arg->mu0 = 1e1; + arg->alpha_min = 1e-12; + arg->res_g_max = 1e-6; + arg->res_b_max = 1e-8; + arg->res_d_max = 1e-8; + arg->res_m_max = 1e-8; + arg->iter_max = 30; + arg->stat_max = 30; + arg->pred_corr = 1; + arg->cond_pred_corr = 1; + arg->itref_pred_max = 0; + arg->itref_corr_max = 2; + arg->reg_prim = 1e-15; + arg->lq_fact = 1; + arg->lam_min = 1e-30; + arg->t_min = 1e-30; + arg->warm_start = 0; + arg->abs_form = 0; + arg->comp_dual_sol = 1; + arg->comp_res_exit = 1; + } + else if(mode==ROBUST) + { + arg->mu0 = 1e2; + arg->alpha_min = 1e-12; + arg->res_g_max = 1e-6; + arg->res_b_max = 1e-8; + arg->res_d_max = 1e-8; + arg->res_m_max = 1e-8; + arg->iter_max = 100; + arg->stat_max = 100; + arg->pred_corr = 1; + arg->cond_pred_corr = 1; + arg->itref_pred_max = 0; + arg->itref_corr_max = 4; + arg->reg_prim = 1e-15; + arg->lq_fact = 2; + arg->lam_min = 1e-30; + arg->t_min = 1e-30; + arg->warm_start = 0; + arg->abs_form = 0; + arg->comp_dual_sol = 1; + arg->comp_res_exit = 1; + } + else + { + printf("\nwrong set default mode\n"); + exit(1); + } - return; + return; - } + } @@ -322,658 +322,658 @@ int MEMSIZE_OCP_QP_IPM(struct OCP_QP_DIM *dim, struct OCP_QP_IPM_ARG *arg) void CREATE_OCP_QP_IPM(struct OCP_QP_DIM *dim, struct OCP_QP_IPM_ARG *arg, struct OCP_QP_IPM_WORKSPACE *workspace, void *mem) - { - - // loop index - int ii; - - // extract ocp qp size - int N = dim->N; - int *nx = dim->nx; - int *nu = dim->nu; - int *nb = dim->nb; - int *ng = dim->ng; - int *ns = dim->ns; - - - // compute core qp size and max size - int nvt = 0; - int net = 0; - int nct = 0; - int nxM = 0; - int nuM = 0; - int nbM = 0; - int ngM = 0; - int nsM = 0; - for(ii=0; iinxM ? nx[ii] : nxM; - nuM = nu[ii]>nuM ? nu[ii] : nuM; - nbM = nb[ii]>nbM ? nb[ii] : nbM; - ngM = ng[ii]>ngM ? ng[ii] : ngM; - nsM = ns[ii]>nsM ? ns[ii] : nsM; - } - nvt += nx[ii]+nu[ii]+2*ns[ii]; - nct += 2*nb[ii]+2*ng[ii]+2*ns[ii]; - nxM = nx[ii]>nxM ? nx[ii] : nxM; - nuM = nu[ii]>nuM ? nu[ii] : nuM; - nbM = nb[ii]>nbM ? nb[ii] : nbM; - ngM = ng[ii]>ngM ? ng[ii] : ngM; - nsM = ns[ii]>nsM ? ns[ii] : nsM; + { + // loop index + int ii; - // core struct - struct CORE_QP_IPM_WORKSPACE *sr_ptr = mem; + // extract ocp qp size + int N = dim->N; + int *nx = dim->nx; + int *nu = dim->nu; + int *nb = dim->nb; + int *ng = dim->ng; + int *ns = dim->ns; - // core workspace - workspace->core_workspace = sr_ptr; - sr_ptr += 1; - struct CORE_QP_IPM_WORKSPACE *cws = workspace->core_workspace; + // compute core qp size and max size + int nvt = 0; + int net = 0; + int nct = 0; + int nxM = 0; + int nuM = 0; + int nbM = 0; + int ngM = 0; + int nsM = 0; + for(ii=0; iinxM ? nx[ii] : nxM; + nuM = nu[ii]>nuM ? nu[ii] : nuM; + nbM = nb[ii]>nbM ? nb[ii] : nbM; + ngM = ng[ii]>ngM ? ng[ii] : ngM; + nsM = ns[ii]>nsM ? ns[ii] : nsM; + } + nvt += nx[ii]+nu[ii]+2*ns[ii]; + nct += 2*nb[ii]+2*ng[ii]+2*ns[ii]; + nxM = nx[ii]>nxM ? nx[ii] : nxM; + nuM = nu[ii]>nuM ? nu[ii] : nuM; + nbM = nb[ii]>nbM ? nb[ii] : nbM; + ngM = ng[ii]>ngM ? ng[ii] : ngM; + nsM = ns[ii]>nsM ? ns[ii] : nsM; - // res struct - struct OCP_QP_RES *res_ptr = (struct OCP_QP_RES *) sr_ptr; - workspace->res = res_ptr; - res_ptr += 1; - workspace->res_itref = res_ptr; - res_ptr += 1; + // core struct + struct CORE_QP_IPM_WORKSPACE *sr_ptr = mem; - // res workspace struct - struct OCP_QP_RES_WORKSPACE *res_ws_ptr = (struct OCP_QP_RES_WORKSPACE *) res_ptr; - workspace->res_workspace = res_ws_ptr; - res_ws_ptr += 1; + // core workspace + workspace->core_workspace = sr_ptr; + sr_ptr += 1; + struct CORE_QP_IPM_WORKSPACE *cws = workspace->core_workspace; - // qp sol struct - struct OCP_QP_SOL *qp_sol_ptr = (struct OCP_QP_SOL *) res_ws_ptr; + // res struct + struct OCP_QP_RES *res_ptr = (struct OCP_QP_RES *) sr_ptr; + workspace->res = res_ptr; + res_ptr += 1; + workspace->res_itref = res_ptr; + res_ptr += 1; - workspace->sol_step = qp_sol_ptr; - qp_sol_ptr += 1; - workspace->sol_itref = qp_sol_ptr; - qp_sol_ptr += 1; + // res workspace struct + struct OCP_QP_RES_WORKSPACE *res_ws_ptr = (struct OCP_QP_RES_WORKSPACE *) res_ptr; + workspace->res_workspace = res_ws_ptr; + res_ws_ptr += 1; - // qp struct - struct OCP_QP *qp_ptr = (struct OCP_QP *) qp_sol_ptr; - workspace->qp_step = qp_ptr; - qp_ptr += 1; - workspace->qp_itref = qp_ptr; - qp_ptr += 1; + // qp sol struct + struct OCP_QP_SOL *qp_sol_ptr = (struct OCP_QP_SOL *) res_ws_ptr; + workspace->sol_step = qp_sol_ptr; + qp_sol_ptr += 1; + workspace->sol_itref = qp_sol_ptr; + qp_sol_ptr += 1; - // matrix struct - struct STRMAT *sm_ptr = (struct STRMAT *) qp_ptr; - workspace->L = sm_ptr; - sm_ptr += N+1; - if(arg->lq_fact>0) - { - workspace->Lh = sm_ptr; - sm_ptr += N+1; - } - workspace->AL = sm_ptr; - sm_ptr += 2; - if(arg->lq_fact>0) - { - workspace->lq0 = sm_ptr; - sm_ptr += 1; - } + // qp struct + struct OCP_QP *qp_ptr = (struct OCP_QP *) qp_sol_ptr; + workspace->qp_step = qp_ptr; + qp_ptr += 1; + workspace->qp_itref = qp_ptr; + qp_ptr += 1; - // vector struct - struct STRVEC *sv_ptr = (struct STRVEC *) sm_ptr; - - workspace->sol_step->ux = sv_ptr; - sv_ptr += N+1; - workspace->sol_step->pi = sv_ptr; - sv_ptr += N; - workspace->sol_step->lam = sv_ptr; - sv_ptr += N+1; - workspace->sol_step->t = sv_ptr; - sv_ptr += N+1; - workspace->res->res_g = sv_ptr; - sv_ptr += N+1; - workspace->res->res_b = sv_ptr; - sv_ptr += N; - workspace->res->res_d = sv_ptr; - sv_ptr += N+1; - workspace->res->res_m = sv_ptr; - sv_ptr += N+1; - workspace->Gamma = sv_ptr; - sv_ptr += N+1; - workspace->gamma = sv_ptr; - sv_ptr += N+1; - workspace->Pb = sv_ptr; - sv_ptr += N; - workspace->Zs_inv = sv_ptr; - sv_ptr += N+1; - workspace->tmp_nxM = sv_ptr; - sv_ptr += 1; - workspace->tmp_nbgM = sv_ptr; - sv_ptr += 4; - workspace->res_workspace->tmp_nbgM = sv_ptr; - sv_ptr += 2; - workspace->tmp_nsM = sv_ptr; - sv_ptr += 1; - workspace->res_workspace->tmp_nsM = sv_ptr; - sv_ptr += 1; - workspace->tmp_m = sv_ptr; - sv_ptr += 1; - - - // double/float stuff - REAL *d_ptr = (REAL *) sv_ptr; - - workspace->stat = d_ptr; - d_ptr += 5*arg->stat_max; - - // int stuff - int *i_ptr = (int *) d_ptr; - - workspace->use_hess_fact = i_ptr; - i_ptr += N+1; - - - // align to typicl cache line size - size_t s_ptr = (size_t) i_ptr; - s_ptr = (s_ptr+63)/64*64; - - - // void stuf - char *c_ptr = (char *) s_ptr; - - CREATE_OCP_QP_SOL(dim, workspace->sol_itref, c_ptr); - c_ptr += workspace->sol_itref->memsize; - - CREATE_OCP_QP_RES(dim, workspace->res_itref, c_ptr); - c_ptr += workspace->res_itref->memsize; - - for(ii=0; ii<=N; ii++) - { - CREATE_STRMAT(nu[ii]+nx[ii]+1, nu[ii]+nx[ii], workspace->L+ii, c_ptr); - c_ptr += (workspace->L+ii)->memsize; - } - if(arg->lq_fact>0) - { - for(ii=0; ii<=N; ii++) - { - CREATE_STRMAT(nu[ii]+nx[ii]+1, nu[ii]+nx[ii], workspace->Lh+ii, c_ptr); - c_ptr += (workspace->Lh+ii)->memsize; - } - } + // matrix struct + struct STRMAT *sm_ptr = (struct STRMAT *) qp_ptr; - CREATE_STRMAT(nuM+nxM+1, nxM+ngM, workspace->AL+0, c_ptr); - c_ptr += (workspace->AL+0)->memsize; + workspace->L = sm_ptr; + sm_ptr += N+1; + if(arg->lq_fact>0) + { + workspace->Lh = sm_ptr; + sm_ptr += N+1; + } + workspace->AL = sm_ptr; + sm_ptr += 2; + if(arg->lq_fact>0) + { + workspace->lq0 = sm_ptr; + sm_ptr += 1; + } - CREATE_STRMAT(nuM+nxM+1, nxM+ngM, workspace->AL+1, c_ptr); - c_ptr += (workspace->AL+1)->memsize; - if(arg->lq_fact>0) - { - CREATE_STRMAT(nuM+nxM, 2*nuM+3*nxM+ngM, workspace->lq0, c_ptr); - c_ptr += (workspace->lq0)->memsize; - } + // vector struct + struct STRVEC *sv_ptr = (struct STRVEC *) sm_ptr; + + workspace->sol_step->ux = sv_ptr; + sv_ptr += N+1; + workspace->sol_step->pi = sv_ptr; + sv_ptr += N; + workspace->sol_step->lam = sv_ptr; + sv_ptr += N+1; + workspace->sol_step->t = sv_ptr; + sv_ptr += N+1; + workspace->res->res_g = sv_ptr; + sv_ptr += N+1; + workspace->res->res_b = sv_ptr; + sv_ptr += N; + workspace->res->res_d = sv_ptr; + sv_ptr += N+1; + workspace->res->res_m = sv_ptr; + sv_ptr += N+1; + workspace->Gamma = sv_ptr; + sv_ptr += N+1; + workspace->gamma = sv_ptr; + sv_ptr += N+1; + workspace->Pb = sv_ptr; + sv_ptr += N; + workspace->Zs_inv = sv_ptr; + sv_ptr += N+1; + workspace->tmp_nxM = sv_ptr; + sv_ptr += 1; + workspace->tmp_nbgM = sv_ptr; + sv_ptr += 4; + workspace->res_workspace->tmp_nbgM = sv_ptr; + sv_ptr += 2; + workspace->tmp_nsM = sv_ptr; + sv_ptr += 1; + workspace->res_workspace->tmp_nsM = sv_ptr; + sv_ptr += 1; + workspace->tmp_m = sv_ptr; + sv_ptr += 1; + + + // double/float stuff + REAL *d_ptr = (REAL *) sv_ptr; + + workspace->stat = d_ptr; + d_ptr += 5*arg->stat_max; + + // int stuff + int *i_ptr = (int *) d_ptr; + + workspace->use_hess_fact = i_ptr; + i_ptr += N+1; + + + // align to typicl cache line size + size_t s_ptr = (size_t) i_ptr; + s_ptr = (s_ptr+63)/64*64; + + + // void stuf + char *c_ptr = (char *) s_ptr; + + CREATE_OCP_QP_SOL(dim, workspace->sol_itref, c_ptr); + c_ptr += workspace->sol_itref->memsize; + + CREATE_OCP_QP_RES(dim, workspace->res_itref, c_ptr); + c_ptr += workspace->res_itref->memsize; + + for(ii=0; ii<=N; ii++) + { + CREATE_STRMAT(nu[ii]+nx[ii]+1, nu[ii]+nx[ii], workspace->L+ii, c_ptr); + c_ptr += (workspace->L+ii)->memsize; + } - for(ii=0; iiPb+ii, c_ptr); - c_ptr += (workspace->Pb+ii)->memsize; - } + if(arg->lq_fact>0) + { + for(ii=0; ii<=N; ii++) + { + CREATE_STRMAT(nu[ii]+nx[ii]+1, nu[ii]+nx[ii], workspace->Lh+ii, c_ptr); + c_ptr += (workspace->Lh+ii)->memsize; + } + } - for(ii=0; iiZs_inv+ii, c_ptr); - c_ptr += (workspace->Zs_inv+ii)->memsize; - } + CREATE_STRMAT(nuM+nxM+1, nxM+ngM, workspace->AL+0, c_ptr); + c_ptr += (workspace->AL+0)->memsize; - CREATE_STRVEC(nxM, workspace->tmp_nxM, c_ptr); - c_ptr += workspace->tmp_nxM->memsize; + CREATE_STRMAT(nuM+nxM+1, nxM+ngM, workspace->AL+1, c_ptr); + c_ptr += (workspace->AL+1)->memsize; - CREATE_STRVEC(nbM+ngM, workspace->tmp_nbgM+0, c_ptr); - CREATE_STRVEC(nbM+ngM, workspace->res_workspace->tmp_nbgM+0, c_ptr); - c_ptr += (workspace->tmp_nbgM+0)->memsize; + if(arg->lq_fact>0) + { + CREATE_STRMAT(nuM+nxM, 2*nuM+3*nxM+ngM, workspace->lq0, c_ptr); + c_ptr += (workspace->lq0)->memsize; + } - CREATE_STRVEC(nbM+ngM, workspace->tmp_nbgM+1, c_ptr); - CREATE_STRVEC(nbM+ngM, workspace->res_workspace->tmp_nbgM+1, c_ptr); - c_ptr += (workspace->tmp_nbgM+1)->memsize; + for(ii=0; iiPb+ii, c_ptr); + c_ptr += (workspace->Pb+ii)->memsize; + } - CREATE_STRVEC(nbM+ngM, workspace->tmp_nbgM+2, c_ptr); - c_ptr += (workspace->tmp_nbgM+2)->memsize; + for(ii=0; iiZs_inv+ii, c_ptr); + c_ptr += (workspace->Zs_inv+ii)->memsize; + } - CREATE_STRVEC(nbM+ngM, workspace->tmp_nbgM+3, c_ptr); - c_ptr += (workspace->tmp_nbgM+3)->memsize; + CREATE_STRVEC(nxM, workspace->tmp_nxM, c_ptr); + c_ptr += workspace->tmp_nxM->memsize; - CREATE_STRVEC(nsM, workspace->tmp_nsM+0, c_ptr); - CREATE_STRVEC(nsM, workspace->res_workspace->tmp_nsM+0, c_ptr); - c_ptr += (workspace->tmp_nsM+0)->memsize; + CREATE_STRVEC(nbM+ngM, workspace->tmp_nbgM+0, c_ptr); + CREATE_STRVEC(nbM+ngM, workspace->res_workspace->tmp_nbgM+0, c_ptr); + c_ptr += (workspace->tmp_nbgM+0)->memsize; - CREATE_STRVEC(nct, workspace->tmp_m, c_ptr); - c_ptr += SIZE_STRVEC(nct); + CREATE_STRVEC(nbM+ngM, workspace->tmp_nbgM+1, c_ptr); + CREATE_STRVEC(nbM+ngM, workspace->res_workspace->tmp_nbgM+1, c_ptr); + c_ptr += (workspace->tmp_nbgM+1)->memsize; - CREATE_CORE_QP_IPM(nvt, net, nct, cws, c_ptr); - c_ptr += workspace->core_workspace->memsize; + CREATE_STRVEC(nbM+ngM, workspace->tmp_nbgM+2, c_ptr); + c_ptr += (workspace->tmp_nbgM+2)->memsize; - if(arg->lq_fact>0) - { - workspace->lq_work0 = c_ptr; - c_ptr += GELQF_WORKSIZE(nuM+nxM, 2*nuM+3*nxM+ngM); - } + CREATE_STRVEC(nbM+ngM, workspace->tmp_nbgM+3, c_ptr); + c_ptr += (workspace->tmp_nbgM+3)->memsize; + CREATE_STRVEC(nsM, workspace->tmp_nsM+0, c_ptr); + CREATE_STRVEC(nsM, workspace->res_workspace->tmp_nsM+0, c_ptr); + c_ptr += (workspace->tmp_nsM+0)->memsize; + + CREATE_STRVEC(nct, workspace->tmp_m, c_ptr); + c_ptr += SIZE_STRVEC(nct); + + CREATE_CORE_QP_IPM(nvt, net, nct, cws, c_ptr); + c_ptr += workspace->core_workspace->memsize; + + if(arg->lq_fact>0) + { + workspace->lq_work0 = c_ptr; + c_ptr += GELQF_WORKSIZE(nuM+nxM, 2*nuM+3*nxM+ngM); + } - // alias members of workspace and core_workspace - // - c_ptr = (char *) cws->dv; - for(ii=0; ii<=N; ii++) - { - CREATE_STRVEC(nu[ii]+nx[ii]+2*ns[ii], workspace->sol_step->ux+ii, c_ptr); - c_ptr += (nu[ii]+nx[ii])*sizeof(REAL); - c_ptr += ns[ii]*sizeof(REAL); - c_ptr += ns[ii]*sizeof(REAL); - } - // - c_ptr = (char *) cws->dpi; - for(ii=0; iisol_step->pi+ii, c_ptr); - c_ptr += (nx[ii+1])*sizeof(REAL); - } - // - c_ptr = (char *) cws->dlam; - for(ii=0; ii<=N; ii++) - { - CREATE_STRVEC(2*nb[ii]+2*ng[ii]+2*ns[ii], workspace->sol_step->lam+ii, c_ptr); - c_ptr += nb[ii]*sizeof(REAL); - c_ptr += ng[ii]*sizeof(REAL); - c_ptr += nb[ii]*sizeof(REAL); - c_ptr += ng[ii]*sizeof(REAL); - c_ptr += ns[ii]*sizeof(REAL); - c_ptr += ns[ii]*sizeof(REAL); - } - // - c_ptr = (char *) cws->dt; - for(ii=0; ii<=N; ii++) - { - CREATE_STRVEC(2*nb[ii]+2*ng[ii]+2*ns[ii], workspace->sol_step->t+ii, c_ptr); - c_ptr += nb[ii]*sizeof(REAL); - c_ptr += ng[ii]*sizeof(REAL); - c_ptr += nb[ii]*sizeof(REAL); - c_ptr += ng[ii]*sizeof(REAL); - c_ptr += ns[ii]*sizeof(REAL); - c_ptr += ns[ii]*sizeof(REAL); - } - // - c_ptr = (char *) cws->res_g; - for(ii=0; ii<=N; ii++) - { - CREATE_STRVEC(nu[ii]+nx[ii]+2*ns[ii], workspace->res->res_g+ii, c_ptr); - c_ptr += (nu[ii]+nx[ii])*sizeof(REAL); - c_ptr += ns[ii]*sizeof(REAL); - c_ptr += ns[ii]*sizeof(REAL); - } - // - c_ptr = (char *) cws->res_b; - for(ii=0; iires->res_b+ii, c_ptr); - c_ptr += (nx[ii+1])*sizeof(REAL); - } - // - c_ptr = (char *) cws->res_d; - for(ii=0; ii<=N; ii++) - { - CREATE_STRVEC(2*nb[ii]+2*ng[ii]+2*ns[ii], workspace->res->res_d+ii, c_ptr); - c_ptr += nb[ii]*sizeof(REAL); - c_ptr += ng[ii]*sizeof(REAL); - c_ptr += nb[ii]*sizeof(REAL); - c_ptr += ng[ii]*sizeof(REAL); - c_ptr += ns[ii]*sizeof(REAL); - c_ptr += ns[ii]*sizeof(REAL); - } - // - c_ptr = (char *) cws->res_m; - for(ii=0; ii<=N; ii++) - { - CREATE_STRVEC(2*nb[ii]+2*ng[ii]+2*ns[ii], workspace->res->res_m+ii, c_ptr); - c_ptr += nb[ii]*sizeof(REAL); - c_ptr += ng[ii]*sizeof(REAL); - c_ptr += nb[ii]*sizeof(REAL); - c_ptr += ng[ii]*sizeof(REAL); - c_ptr += ns[ii]*sizeof(REAL); - c_ptr += ns[ii]*sizeof(REAL); - } - // - c_ptr = (char *) cws->Gamma; - for(ii=0; ii<=N; ii++) - { - CREATE_STRVEC(2*nb[ii]+2*ng[ii]+2*ns[ii], workspace->Gamma+ii, c_ptr); - c_ptr += nb[ii]*sizeof(REAL); - c_ptr += ng[ii]*sizeof(REAL); - c_ptr += nb[ii]*sizeof(REAL); - c_ptr += ng[ii]*sizeof(REAL); - c_ptr += ns[ii]*sizeof(REAL); - c_ptr += ns[ii]*sizeof(REAL); - } - // - c_ptr = (char *) cws->gamma; - for(ii=0; ii<=N; ii++) - { - CREATE_STRVEC(2*nb[ii]+2*ng[ii]+2*ns[ii], workspace->gamma+ii, c_ptr); - c_ptr += nb[ii]*sizeof(REAL); - c_ptr += ng[ii]*sizeof(REAL); - c_ptr += nb[ii]*sizeof(REAL); - c_ptr += ng[ii]*sizeof(REAL); - c_ptr += ns[ii]*sizeof(REAL); - c_ptr += ns[ii]*sizeof(REAL); - } + + // alias members of workspace and core_workspace + // + c_ptr = (char *) cws->dv; + for(ii=0; ii<=N; ii++) + { + CREATE_STRVEC(nu[ii]+nx[ii]+2*ns[ii], workspace->sol_step->ux+ii, c_ptr); + c_ptr += (nu[ii]+nx[ii])*sizeof(REAL); + c_ptr += ns[ii]*sizeof(REAL); + c_ptr += ns[ii]*sizeof(REAL); + } + // + c_ptr = (char *) cws->dpi; + for(ii=0; iisol_step->pi+ii, c_ptr); + c_ptr += (nx[ii+1])*sizeof(REAL); + } + // + c_ptr = (char *) cws->dlam; + for(ii=0; ii<=N; ii++) + { + CREATE_STRVEC(2*nb[ii]+2*ng[ii]+2*ns[ii], workspace->sol_step->lam+ii, c_ptr); + c_ptr += nb[ii]*sizeof(REAL); + c_ptr += ng[ii]*sizeof(REAL); + c_ptr += nb[ii]*sizeof(REAL); + c_ptr += ng[ii]*sizeof(REAL); + c_ptr += ns[ii]*sizeof(REAL); + c_ptr += ns[ii]*sizeof(REAL); + } + // + c_ptr = (char *) cws->dt; + for(ii=0; ii<=N; ii++) + { + CREATE_STRVEC(2*nb[ii]+2*ng[ii]+2*ns[ii], workspace->sol_step->t+ii, c_ptr); + c_ptr += nb[ii]*sizeof(REAL); + c_ptr += ng[ii]*sizeof(REAL); + c_ptr += nb[ii]*sizeof(REAL); + c_ptr += ng[ii]*sizeof(REAL); + c_ptr += ns[ii]*sizeof(REAL); + c_ptr += ns[ii]*sizeof(REAL); + } + // + c_ptr = (char *) cws->res_g; + for(ii=0; ii<=N; ii++) + { + CREATE_STRVEC(nu[ii]+nx[ii]+2*ns[ii], workspace->res->res_g+ii, c_ptr); + c_ptr += (nu[ii]+nx[ii])*sizeof(REAL); + c_ptr += ns[ii]*sizeof(REAL); + c_ptr += ns[ii]*sizeof(REAL); + } + // + c_ptr = (char *) cws->res_b; + for(ii=0; iires->res_b+ii, c_ptr); + c_ptr += (nx[ii+1])*sizeof(REAL); + } + // + c_ptr = (char *) cws->res_d; + for(ii=0; ii<=N; ii++) + { + CREATE_STRVEC(2*nb[ii]+2*ng[ii]+2*ns[ii], workspace->res->res_d+ii, c_ptr); + c_ptr += nb[ii]*sizeof(REAL); + c_ptr += ng[ii]*sizeof(REAL); + c_ptr += nb[ii]*sizeof(REAL); + c_ptr += ng[ii]*sizeof(REAL); + c_ptr += ns[ii]*sizeof(REAL); + c_ptr += ns[ii]*sizeof(REAL); + } + // + c_ptr = (char *) cws->res_m; + for(ii=0; ii<=N; ii++) + { + CREATE_STRVEC(2*nb[ii]+2*ng[ii]+2*ns[ii], workspace->res->res_m+ii, c_ptr); + c_ptr += nb[ii]*sizeof(REAL); + c_ptr += ng[ii]*sizeof(REAL); + c_ptr += nb[ii]*sizeof(REAL); + c_ptr += ng[ii]*sizeof(REAL); + c_ptr += ns[ii]*sizeof(REAL); + c_ptr += ns[ii]*sizeof(REAL); + } + // + c_ptr = (char *) cws->Gamma; + for(ii=0; ii<=N; ii++) + { + CREATE_STRVEC(2*nb[ii]+2*ng[ii]+2*ns[ii], workspace->Gamma+ii, c_ptr); + c_ptr += nb[ii]*sizeof(REAL); + c_ptr += ng[ii]*sizeof(REAL); + c_ptr += nb[ii]*sizeof(REAL); + c_ptr += ng[ii]*sizeof(REAL); + c_ptr += ns[ii]*sizeof(REAL); + c_ptr += ns[ii]*sizeof(REAL); + } + // + c_ptr = (char *) cws->gamma; + for(ii=0; ii<=N; ii++) + { + CREATE_STRVEC(2*nb[ii]+2*ng[ii]+2*ns[ii], workspace->gamma+ii, c_ptr); + c_ptr += nb[ii]*sizeof(REAL); + c_ptr += ng[ii]*sizeof(REAL); + c_ptr += nb[ii]*sizeof(REAL); + c_ptr += ng[ii]*sizeof(REAL); + c_ptr += ns[ii]*sizeof(REAL); + c_ptr += ns[ii]*sizeof(REAL); + } - workspace->res->dim = dim; + workspace->res->dim = dim; - workspace->stat_max = arg->stat_max; + workspace->stat_max = arg->stat_max; - for(ii=0; ii<=N; ii++) - workspace->use_hess_fact[ii] = 0; - - workspace->use_Pb = 0; + for(ii=0; ii<=N; ii++) + workspace->use_hess_fact[ii] = 0; + + workspace->use_Pb = 0; - workspace->memsize = MEMSIZE_OCP_QP_IPM(dim, arg); + workspace->memsize = MEMSIZE_OCP_QP_IPM(dim, arg); #if defined(RUNTIME_CHECKS) - if(c_ptr > ((char *) mem) + workspace->memsize) - { - printf("\nCreate_ocp_qp_ipm: outside memory bounds!\n\n"); - exit(1); - } + if(c_ptr > ((char *) mem) + workspace->memsize) + { + printf("\nCreate_ocp_qp_ipm: outside memory bounds!\n\n"); + exit(1); + } #endif - return; + return; - } + } int SOLVE_OCP_QP_IPM(struct OCP_QP *qp, struct OCP_QP_SOL *qp_sol, struct OCP_QP_IPM_ARG *arg, struct OCP_QP_IPM_WORKSPACE *ws) - { + { #if 0 - int N = qp->dim->N; - int *nx = qp->dim->nx; - int *nu = qp->dim->nu; - int *nb = qp->dim->nb; - int *ng = qp->dim->ng; - int *ns = qp->dim->ns; + int N = qp->dim->N; + int *nx = qp->dim->nx; + int *nu = qp->dim->nu; + int *nb = qp->dim->nb; + int *ng = qp->dim->ng; + int *ns = qp->dim->ns; - int ii; + int ii; #if 1 - printf("\nnx\n"); - int_print_mat(1, N+1, nx, 1); - printf("\nnu\n"); - int_print_mat(1, N+1, nu, 1); - printf("\nnb\n"); - int_print_mat(1, N+1, nb, 1); - printf("\nng\n"); - int_print_mat(1, N+1, ng, 1); - printf("\nns\n"); - int_print_mat(1, N+1, ns, 1); + printf("\nnx\n"); + int_print_mat(1, N+1, nx, 1); + printf("\nnu\n"); + int_print_mat(1, N+1, nu, 1); + printf("\nnb\n"); + int_print_mat(1, N+1, nb, 1); + printf("\nng\n"); + int_print_mat(1, N+1, ng, 1); + printf("\nns\n"); + int_print_mat(1, N+1, ns, 1); #endif - printf("\nBAt\n"); - for(ii=0; iiBAbt+ii, 0, 0); - printf("\nb\n"); - for(ii=0; iib+ii, 0); - printf("\nRSQ\n"); - for(ii=0; ii<=N; ii++) - blasfeo_print_dmat(nu[ii]+nx[ii], nu[ii]+nx[ii], qp->RSQrq+ii, 0, 0); - printf("\nrq\n"); - for(ii=0; ii<=N; ii++) - blasfeo_print_tran_dvec(nu[ii]+nx[ii], qp->rqz+ii, 0); - printf("\nidxb\n"); - for(ii=0; ii<=N; ii++) - int_print_mat(1, nb[ii], qp->idxb[ii], 1); - printf("\nDCt\n"); - for(ii=0; ii<=N; ii++) - blasfeo_print_dmat(nu[ii]+nx[ii], ng[ii], qp->DCt+ii, 0, 0); - printf("\nd\n"); - for(ii=0; ii<=N; ii++) - blasfeo_print_tran_dvec(2*nb[ii]+2*ng[ii]+2*ns[ii], qp->d+ii, 0); + printf("\nBAt\n"); + for(ii=0; iiBAbt+ii, 0, 0); + printf("\nb\n"); + for(ii=0; iib+ii, 0); + printf("\nRSQ\n"); + for(ii=0; ii<=N; ii++) + blasfeo_print_dmat(nu[ii]+nx[ii], nu[ii]+nx[ii], qp->RSQrq+ii, 0, 0); + printf("\nrq\n"); + for(ii=0; ii<=N; ii++) + blasfeo_print_tran_dvec(nu[ii]+nx[ii], qp->rqz+ii, 0); + printf("\nidxb\n"); + for(ii=0; ii<=N; ii++) + int_print_mat(1, nb[ii], qp->idxb[ii], 1); + printf("\nDCt\n"); + for(ii=0; ii<=N; ii++) + blasfeo_print_dmat(nu[ii]+nx[ii], ng[ii], qp->DCt+ii, 0, 0); + printf("\nd\n"); + for(ii=0; ii<=N; ii++) + blasfeo_print_tran_dvec(2*nb[ii]+2*ng[ii]+2*ns[ii], qp->d+ii, 0); #if 0 - printf("\nlb\n"); - for(ii=0; ii<=N; ii++) - blasfeo_print_tran_dvec(nb[ii], qp->d+ii, 0); - printf("\nlg\n"); - for(ii=0; ii<=N; ii++) - blasfeo_print_tran_dvec(ng[ii], qp->d+ii, nb[ii]); - printf("\nub\n"); - for(ii=0; ii<=N; ii++) - blasfeo_print_tran_dvec(nb[ii], qp->d+ii, nb[ii]+ng[ii]); - printf("\nug\n"); - for(ii=0; ii<=N; ii++) - blasfeo_print_tran_dvec(ng[ii], qp->d+ii, 2*nb[ii]+ng[ii]); - printf("\nls\n"); - for(ii=0; ii<=N; ii++) - blasfeo_print_tran_dvec(ns[ii], qp->d+ii, 2*nb[ii]+2*ng[ii]); - printf("\nus\n"); - for(ii=0; ii<=N; ii++) - blasfeo_print_tran_dvec(ns[ii], qp->d+ii, 2*nb[ii]+2*ng[ii]+ns[ii]); - printf("\nidxb\n"); - for(ii=0; ii<=N; ii++) - int_print_mat(1, nb[ii], qp->idxb[ii], 1); - printf("\nidxs\n"); - for(ii=0; ii<=N; ii++) - int_print_mat(1, ns[ii], qp->idxs[ii], 1); - printf("\nZ\n"); - for(ii=0; ii<=N; ii++) - blasfeo_print_tran_dvec(2*ns[ii], qp->Z+ii, 0); - printf("\nrqz\n"); - for(ii=0; ii<=N; ii++) - blasfeo_print_tran_dvec(nu[ii]+nx[ii]+2*ns[ii], qp->rqz+ii, 0); + printf("\nlb\n"); + for(ii=0; ii<=N; ii++) + blasfeo_print_tran_dvec(nb[ii], qp->d+ii, 0); + printf("\nlg\n"); + for(ii=0; ii<=N; ii++) + blasfeo_print_tran_dvec(ng[ii], qp->d+ii, nb[ii]); + printf("\nub\n"); + for(ii=0; ii<=N; ii++) + blasfeo_print_tran_dvec(nb[ii], qp->d+ii, nb[ii]+ng[ii]); + printf("\nug\n"); + for(ii=0; ii<=N; ii++) + blasfeo_print_tran_dvec(ng[ii], qp->d+ii, 2*nb[ii]+ng[ii]); + printf("\nls\n"); + for(ii=0; ii<=N; ii++) + blasfeo_print_tran_dvec(ns[ii], qp->d+ii, 2*nb[ii]+2*ng[ii]); + printf("\nus\n"); + for(ii=0; ii<=N; ii++) + blasfeo_print_tran_dvec(ns[ii], qp->d+ii, 2*nb[ii]+2*ng[ii]+ns[ii]); + printf("\nidxb\n"); + for(ii=0; ii<=N; ii++) + int_print_mat(1, nb[ii], qp->idxb[ii], 1); + printf("\nidxs\n"); + for(ii=0; ii<=N; ii++) + int_print_mat(1, ns[ii], qp->idxs[ii], 1); + printf("\nZ\n"); + for(ii=0; ii<=N; ii++) + blasfeo_print_tran_dvec(2*ns[ii], qp->Z+ii, 0); + printf("\nrqz\n"); + for(ii=0; ii<=N; ii++) + blasfeo_print_tran_dvec(nu[ii]+nx[ii]+2*ns[ii], qp->rqz+ii, 0); #endif #endif - struct CORE_QP_IPM_WORKSPACE *cws = ws->core_workspace; - - // arg to core workspace - cws->lam_min = arg->lam_min; - cws->t_min = arg->t_min; - - // alias qp vectors into qp_sol - cws->v = qp_sol->ux->pa; - cws->pi = qp_sol->pi->pa; - cws->lam = qp_sol->lam->pa; - cws->t = qp_sol->t->pa; - - // alias members of qp_step - ws->qp_step->dim = qp->dim; - ws->qp_step->RSQrq = qp->RSQrq; - ws->qp_step->BAbt = qp->BAbt; - ws->qp_step->DCt = qp->DCt; - ws->qp_step->Z = qp->Z; - ws->qp_step->idxb = qp->idxb; - ws->qp_step->idxs = qp->idxs; - ws->qp_step->rqz = ws->res->res_g; - ws->qp_step->b = ws->res->res_b; - ws->qp_step->d = ws->res->res_d; - ws->qp_step->m = ws->res->res_m; - - // alias members of qp_step - ws->qp_itref->dim = qp->dim; - ws->qp_itref->RSQrq = qp->RSQrq; - ws->qp_itref->BAbt = qp->BAbt; - ws->qp_itref->DCt = qp->DCt; - ws->qp_itref->Z = qp->Z; - ws->qp_itref->idxb = qp->idxb; - ws->qp_itref->idxs = qp->idxs; - ws->qp_itref->rqz = ws->res_itref->res_g; - ws->qp_itref->b = ws->res_itref->res_b; - ws->qp_itref->d = ws->res_itref->res_d; - ws->qp_itref->m = ws->res_itref->res_m; - - // alias members of qp_itref - - // no constraints - if(cws->nc==0) - { - FACT_SOLVE_KKT_UNCONSTR_OCP_QP(qp, qp_sol, arg, ws); - COMPUTE_RES_OCP_QP(qp, qp_sol, ws->res, ws->res_workspace); - cws->mu = ws->res->res_mu; - ws->iter = 0; - return 0; - } - - // blasfeo alias for residuals - struct STRVEC str_res_g; - struct STRVEC str_res_b; - struct STRVEC str_res_d; - struct STRVEC str_res_m; - str_res_g.m = cws->nv; - str_res_b.m = cws->ne; - str_res_d.m = cws->nc; - str_res_m.m = cws->nc; - str_res_g.pa = cws->res_g; - str_res_b.pa = cws->res_b; - str_res_d.pa = cws->res_d; - str_res_m.pa = cws->res_m; - - REAL *qp_res = ws->qp_res; - qp_res[0] = 0; - qp_res[1] = 0; - qp_res[2] = 0; - qp_res[3] = 0; - - int N = qp->dim->N; - int *nx = qp->dim->nx; - int *nu = qp->dim->nu; - int *nb = qp->dim->nb; - int *ng = qp->dim->ng; - int *ns = qp->dim->ns; - - int kk, ii, itref0=0, itref1=0, iter_ref_step; - REAL tmp; - REAL mu_aff0, mu; - - // init solver - INIT_VAR_OCP_QP(qp, qp_sol, arg, ws); - - cws->alpha = 1.0; - - - - // absolute IPM formulation - - if(arg->abs_form) - { - - // alias members of qp_step - ws->qp_step->dim = qp->dim; - ws->qp_step->RSQrq = qp->RSQrq; - ws->qp_step->BAbt = qp->BAbt; - ws->qp_step->DCt = qp->DCt; - ws->qp_step->Z = qp->Z; - ws->qp_step->idxb = qp->idxb; - ws->qp_step->idxs = qp->idxs; - ws->qp_step->rqz = qp->rqz; - ws->qp_step->b = qp->b; - ws->qp_step->d = qp->d; - ws->qp_step->m = ws->tmp_m; - - // alias core workspace - cws->res_m = ws->qp_step->m->pa; - cws->res_m_bkp = ws->qp_step->m->pa; - - mu = VECMULDOT(cws->nc, qp_sol->lam, 0, qp_sol->t, 0, ws->tmp_m, 0); - mu /= cws->nc; - cws->mu = mu; - - // IPM loop (absolute formulation) - for(kk=0; \ - kkiter_max & \ - cws->alpha>arg->alpha_min & \ - mu>arg->res_m_max; kk++) - { - - VECSC(cws->nc, -1.0, ws->tmp_m, 0); - - // fact solve - FACT_SOLVE_KKT_STEP_OCP_QP(ws->qp_step, ws->sol_step, arg, ws); -//blasfeo_print_tran_dvec(cws->nv, ws->sol_step->ux, 0); - - // compute step - AXPY(cws->nv, -1.0, qp_sol->ux, 0, ws->sol_step->ux, 0, ws->sol_step->ux, 0); - AXPY(cws->ne, -1.0, qp_sol->pi, 0, ws->sol_step->pi, 0, ws->sol_step->pi, 0); - AXPY(cws->nc, -1.0, qp_sol->lam, 0, ws->sol_step->lam, 0, ws->sol_step->lam, 0); - AXPY(cws->nc, -1.0, qp_sol->t, 0, ws->sol_step->t, 0, ws->sol_step->t, 0); - - // alpha - COMPUTE_ALPHA_QP(cws); - if(kkstat_max) - ws->stat[5*kk+0] = cws->alpha; + struct CORE_QP_IPM_WORKSPACE *cws = ws->core_workspace; + + // arg to core workspace + cws->lam_min = arg->lam_min; + cws->t_min = arg->t_min; + + // alias qp vectors into qp_sol + cws->v = qp_sol->ux->pa; + cws->pi = qp_sol->pi->pa; + cws->lam = qp_sol->lam->pa; + cws->t = qp_sol->t->pa; + + // alias members of qp_step + ws->qp_step->dim = qp->dim; + ws->qp_step->RSQrq = qp->RSQrq; + ws->qp_step->BAbt = qp->BAbt; + ws->qp_step->DCt = qp->DCt; + ws->qp_step->Z = qp->Z; + ws->qp_step->idxb = qp->idxb; + ws->qp_step->idxs = qp->idxs; + ws->qp_step->rqz = ws->res->res_g; + ws->qp_step->b = ws->res->res_b; + ws->qp_step->d = ws->res->res_d; + ws->qp_step->m = ws->res->res_m; + + // alias members of qp_step + ws->qp_itref->dim = qp->dim; + ws->qp_itref->RSQrq = qp->RSQrq; + ws->qp_itref->BAbt = qp->BAbt; + ws->qp_itref->DCt = qp->DCt; + ws->qp_itref->Z = qp->Z; + ws->qp_itref->idxb = qp->idxb; + ws->qp_itref->idxs = qp->idxs; + ws->qp_itref->rqz = ws->res_itref->res_g; + ws->qp_itref->b = ws->res_itref->res_b; + ws->qp_itref->d = ws->res_itref->res_d; + ws->qp_itref->m = ws->res_itref->res_m; + + // alias members of qp_itref + + // no constraints + if(cws->nc==0) + { + FACT_SOLVE_KKT_UNCONSTR_OCP_QP(qp, qp_sol, arg, ws); + COMPUTE_RES_OCP_QP(qp, qp_sol, ws->res, ws->res_workspace); + cws->mu = ws->res->res_mu; + ws->iter = 0; + return 0; + } - // Mehrotra's predictor-corrector - if(arg->pred_corr==1) - { - // mu_aff - COMPUTE_MU_AFF_QP(cws); - if(kkstat_max) - ws->stat[5*kk+1] = cws->mu_aff; + // blasfeo alias for residuals + struct STRVEC str_res_g; + struct STRVEC str_res_b; + struct STRVEC str_res_d; + struct STRVEC str_res_m; + str_res_g.m = cws->nv; + str_res_b.m = cws->ne; + str_res_d.m = cws->nc; + str_res_m.m = cws->nc; + str_res_g.pa = cws->res_g; + str_res_b.pa = cws->res_b; + str_res_d.pa = cws->res_d; + str_res_m.pa = cws->res_m; - tmp = cws->mu_aff/cws->mu; - cws->sigma = tmp*tmp*tmp; - if(kkstat_max) - ws->stat[5*kk+2] = cws->sigma; + REAL *qp_res = ws->qp_res; + qp_res[0] = 0; + qp_res[1] = 0; + qp_res[2] = 0; + qp_res[3] = 0; - COMPUTE_CENTERING_CORRECTION_QP(cws); + int N = qp->dim->N; + int *nx = qp->dim->nx; + int *nu = qp->dim->nu; + int *nb = qp->dim->nb; + int *ng = qp->dim->ng; + int *ns = qp->dim->ns; - // fact and solve kkt - ws->use_Pb = 1; - SOLVE_KKT_STEP_OCP_QP(ws->qp_step, ws->sol_step, arg, ws); + int kk, ii, itref0=0, itref1=0, iter_ref_step; + REAL tmp; + REAL mu_aff0, mu; - // compute step - AXPY(cws->nv, -1.0, qp_sol->ux, 0, ws->sol_step->ux, 0, ws->sol_step->ux, 0); - AXPY(cws->ne, -1.0, qp_sol->pi, 0, ws->sol_step->pi, 0, ws->sol_step->pi, 0); - AXPY(cws->nc, -1.0, qp_sol->lam, 0, ws->sol_step->lam, 0, ws->sol_step->lam, 0); - AXPY(cws->nc, -1.0, qp_sol->t, 0, ws->sol_step->t, 0, ws->sol_step->t, 0); + // init solver + INIT_VAR_OCP_QP(qp, qp_sol, arg, ws); - // alpha - COMPUTE_ALPHA_QP(cws); - if(kkstat_max) - ws->stat[5*kk+3] = cws->alpha; + cws->alpha = 1.0; - } - // - UPDATE_VAR_QP(cws); - // compute mu - mu = VECMULDOT(cws->nc, qp_sol->lam, 0, qp_sol->t, 0, ws->tmp_m, 0); - mu /= cws->nc; - cws->mu = mu; - if(kkstat_max) - ws->stat[5*kk+4] = mu; + // absolute IPM formulation - // exit(1); - - } + if(arg->abs_form) + { - if(arg->comp_res_exit & arg->comp_dual_sol) - { - // compute residuals - COMPUTE_RES_OCP_QP(qp, qp_sol, ws->res, ws->res_workspace); + // alias members of qp_step + ws->qp_step->dim = qp->dim; + ws->qp_step->RSQrq = qp->RSQrq; + ws->qp_step->BAbt = qp->BAbt; + ws->qp_step->DCt = qp->DCt; + ws->qp_step->Z = qp->Z; + ws->qp_step->idxb = qp->idxb; + ws->qp_step->idxs = qp->idxs; + ws->qp_step->rqz = qp->rqz; + ws->qp_step->b = qp->b; + ws->qp_step->d = qp->d; + ws->qp_step->m = ws->tmp_m; + + // alias core workspace + cws->res_m = ws->qp_step->m->pa; + cws->res_m_bkp = ws->qp_step->m->pa; + + mu = VECMULDOT(cws->nc, qp_sol->lam, 0, qp_sol->t, 0, ws->tmp_m, 0); + mu /= cws->nc; + cws->mu = mu; + + // IPM loop (absolute formulation) + for(kk=0; \ + kkiter_max & \ + cws->alpha>arg->alpha_min & \ + mu>arg->res_m_max; kk++) + { + + VECSC(cws->nc, -1.0, ws->tmp_m, 0); + + // fact solve + FACT_SOLVE_KKT_STEP_OCP_QP(ws->qp_step, ws->sol_step, arg, ws); +//blasfeo_print_tran_dvec(cws->nv, ws->sol_step->ux, 0); - // compute infinity norm of residuals - VECNRM_INF(cws->nv, &str_res_g, 0, &qp_res[0]); - VECNRM_INF(cws->ne, &str_res_b, 0, &qp_res[1]); - VECNRM_INF(cws->nc, &str_res_d, 0, &qp_res[2]); - VECNRM_INF(cws->nc, &str_res_m, 0, &qp_res[3]); + // compute step + AXPY(cws->nv, -1.0, qp_sol->ux, 0, ws->sol_step->ux, 0, ws->sol_step->ux, 0); + AXPY(cws->ne, -1.0, qp_sol->pi, 0, ws->sol_step->pi, 0, ws->sol_step->pi, 0); + AXPY(cws->nc, -1.0, qp_sol->lam, 0, ws->sol_step->lam, 0, ws->sol_step->lam, 0); + AXPY(cws->nc, -1.0, qp_sol->t, 0, ws->sol_step->t, 0, ws->sol_step->t, 0); + + // alpha + COMPUTE_ALPHA_QP(cws); + if(kkstat_max) + ws->stat[5*kk+0] = cws->alpha; + + // Mehrotra's predictor-corrector + if(arg->pred_corr==1) + { + // mu_aff + COMPUTE_MU_AFF_QP(cws); + if(kkstat_max) + ws->stat[5*kk+1] = cws->mu_aff; + + tmp = cws->mu_aff/cws->mu; + cws->sigma = tmp*tmp*tmp; + if(kkstat_max) + ws->stat[5*kk+2] = cws->sigma; + + COMPUTE_CENTERING_CORRECTION_QP(cws); + + // fact and solve kkt + ws->use_Pb = 1; + SOLVE_KKT_STEP_OCP_QP(ws->qp_step, ws->sol_step, arg, ws); + + // compute step + AXPY(cws->nv, -1.0, qp_sol->ux, 0, ws->sol_step->ux, 0, ws->sol_step->ux, 0); + AXPY(cws->ne, -1.0, qp_sol->pi, 0, ws->sol_step->pi, 0, ws->sol_step->pi, 0); + AXPY(cws->nc, -1.0, qp_sol->lam, 0, ws->sol_step->lam, 0, ws->sol_step->lam, 0); + AXPY(cws->nc, -1.0, qp_sol->t, 0, ws->sol_step->t, 0, ws->sol_step->t, 0); + + // alpha + COMPUTE_ALPHA_QP(cws); + if(kkstat_max) + ws->stat[5*kk+3] = cws->alpha; + + } + + // + UPDATE_VAR_QP(cws); + + // compute mu + mu = VECMULDOT(cws->nc, qp_sol->lam, 0, qp_sol->t, 0, ws->tmp_m, 0); + mu /= cws->nc; + cws->mu = mu; + if(kkstat_max) + ws->stat[5*kk+4] = mu; + + // exit(1); + + } + + if(arg->comp_res_exit & arg->comp_dual_sol) + { + // compute residuals + COMPUTE_RES_OCP_QP(qp, qp_sol, ws->res, ws->res_workspace); + + // compute infinity norm of residuals + VECNRM_INF(cws->nv, &str_res_g, 0, &qp_res[0]); + VECNRM_INF(cws->ne, &str_res_b, 0, &qp_res[1]); + VECNRM_INF(cws->nc, &str_res_d, 0, &qp_res[2]); + VECNRM_INF(cws->nc, &str_res_m, 0, &qp_res[3]); if(arg->print_level > 0) { @@ -985,9 +985,9 @@ int SOLVE_OCP_QP_IPM(struct OCP_QP *qp, struct OCP_QP_SOL *qp_sol, struct OCP_QP } printf("%-10d %-15e %-15e %-15e %-15e %-15e %-15e\n", kk, qp_res[0], qp_res[1], qp_res[2], qp_res[3], cws->alpha, cws->mu); } - } + } - ws->iter = kk; + ws->iter = kk; // max iteration number reached if(kk == arg->iter_max) { @@ -1023,38 +1023,38 @@ int SOLVE_OCP_QP_IPM(struct OCP_QP *qp, struct OCP_QP_SOL *qp_sol, struct OCP_QP printf(" \n -> SOLUTION FOUND. \n\n"); return 0; - } + } - // compute residuals - COMPUTE_RES_OCP_QP(qp, qp_sol, ws->res, ws->res_workspace); - BACKUP_RES_M(cws); - cws->mu = ws->res->res_mu; + // compute residuals + COMPUTE_RES_OCP_QP(qp, qp_sol, ws->res, ws->res_workspace); + BACKUP_RES_M(cws); + cws->mu = ws->res->res_mu; - // compute infinity norm of residuals - VECNRM_INF(cws->nv, &str_res_g, 0, &qp_res[0]); - VECNRM_INF(cws->ne, &str_res_b, 0, &qp_res[1]); - VECNRM_INF(cws->nc, &str_res_d, 0, &qp_res[2]); - VECNRM_INF(cws->nc, &str_res_m, 0, &qp_res[3]); + // compute infinity norm of residuals + VECNRM_INF(cws->nv, &str_res_g, 0, &qp_res[0]); + VECNRM_INF(cws->ne, &str_res_b, 0, &qp_res[1]); + VECNRM_INF(cws->nc, &str_res_d, 0, &qp_res[2]); + VECNRM_INF(cws->nc, &str_res_m, 0, &qp_res[3]); // printf("\niter %d\t%e\t%e\t%e\t%e\n", -1, qp_res[0], qp_res[1], qp_res[2], qp_res[3]); - REAL itref_qp_norm[4] = {0,0,0,0}; - REAL itref_qp_norm0[4] = {0,0,0,0}; - int ndp0, ndp1; + REAL itref_qp_norm[4] = {0,0,0,0}; + REAL itref_qp_norm0[4] = {0,0,0,0}; + int ndp0, ndp1; - int force_lq = 0; + int force_lq = 0; #if 0 - // IPM loop (absolute formulation) - for(kk=0; kkiter_max; kk++) - { + // IPM loop (absolute formulation) + for(kk=0; kkiter_max; kk++) + { - // fact solve - FACT_SOLVE_KKT_STEP_OCP_QP(qp, ws->sol_step, arg, ws); + // fact solve + FACT_SOLVE_KKT_STEP_OCP_QP(qp, ws->sol_step, arg, ws); #if 0 blasfeo_print_tran_dvec(cws->nv, ws->sol_step->ux, 0); @@ -1063,11 +1063,11 @@ blasfeo_print_tran_dvec(cws->nc, ws->sol_step->lam, 0); blasfeo_print_tran_dvec(cws->nc, ws->sol_step->t, 0); #endif - // compute step - AXPY(cws->nv, -1.0, qp_sol->ux, 0, ws->sol_step->ux, 0, ws->sol_step->ux, 0); - AXPY(cws->ne, -1.0, qp_sol->pi, 0, ws->sol_step->pi, 0, ws->sol_step->pi, 0); - AXPY(cws->nc, -1.0, qp_sol->lam, 0, ws->sol_step->lam, 0, ws->sol_step->lam, 0); - AXPY(cws->nc, -1.0, qp_sol->t, 0, ws->sol_step->t, 0, ws->sol_step->t, 0); + // compute step + AXPY(cws->nv, -1.0, qp_sol->ux, 0, ws->sol_step->ux, 0, ws->sol_step->ux, 0); + AXPY(cws->ne, -1.0, qp_sol->pi, 0, ws->sol_step->pi, 0, ws->sol_step->pi, 0); + AXPY(cws->nc, -1.0, qp_sol->lam, 0, ws->sol_step->lam, 0, ws->sol_step->lam, 0); + AXPY(cws->nc, -1.0, qp_sol->t, 0, ws->sol_step->t, 0, ws->sol_step->t, 0); #if 0 blasfeo_print_tran_dvec(cws->nv, ws->sol_step->ux, 0); @@ -1076,82 +1076,82 @@ blasfeo_print_tran_dvec(cws->nc, ws->sol_step->lam, 0); blasfeo_print_tran_dvec(cws->nc, ws->sol_step->t, 0); #endif - // alpha - COMPUTE_ALPHA_QP(cws); - if(kkstat_max) - ws->stat[5*kk+0] = cws->alpha; + // alpha + COMPUTE_ALPHA_QP(cws); + if(kkstat_max) + ws->stat[5*kk+0] = cws->alpha; - // - UPDATE_VAR_QP(cws); + // + UPDATE_VAR_QP(cws); - // compute residuals - COMPUTE_RES_OCP_QP(qp, qp_sol, ws->res, ws->res_workspace); - BACKUP_RES_M(cws); - cws->mu = ws->res->res_mu; - if(kkstat_max) - ws->stat[5*kk+4] = ws->res->res_mu; + // compute residuals + COMPUTE_RES_OCP_QP(qp, qp_sol, ws->res, ws->res_workspace); + BACKUP_RES_M(cws); + cws->mu = ws->res->res_mu; + if(kkstat_max) + ws->stat[5*kk+4] = ws->res->res_mu; - // compute infinity norm of residuals - VECNRM_INF(cws->nv, &str_res_g, 0, &qp_res[0]); - VECNRM_INF(cws->ne, &str_res_b, 0, &qp_res[1]); - VECNRM_INF(cws->nc, &str_res_d, 0, &qp_res[2]); - VECNRM_INF(cws->nc, &str_res_m, 0, &qp_res[3]); + // compute infinity norm of residuals + VECNRM_INF(cws->nv, &str_res_g, 0, &qp_res[0]); + VECNRM_INF(cws->ne, &str_res_b, 0, &qp_res[1]); + VECNRM_INF(cws->nc, &str_res_d, 0, &qp_res[2]); + VECNRM_INF(cws->nc, &str_res_m, 0, &qp_res[3]); -// exit(1); +// exit(1); - } + } - ws->iter = kk; + ws->iter = kk; - return 0; + return 0; #endif - // IPM loop - for(kk=0; kkiter_max & \ - cws->alpha>arg->alpha_min & \ - (qp_res[0]>arg->res_g_max | \ - qp_res[1]>arg->res_b_max | \ - qp_res[2]>arg->res_d_max | \ - qp_res[3]>arg->res_m_max); kk++) - { + // IPM loop + for(kk=0; kkiter_max & \ + cws->alpha>arg->alpha_min & \ + (qp_res[0]>arg->res_g_max | \ + qp_res[1]>arg->res_b_max | \ + qp_res[2]>arg->res_d_max | \ + qp_res[3]>arg->res_m_max); kk++) + { - // fact and solve kkt - if(arg->lq_fact==0) - { + // fact and solve kkt + if(arg->lq_fact==0) + { - // syrk+cholesky - FACT_SOLVE_KKT_STEP_OCP_QP(ws->qp_step, ws->sol_step, arg, ws); + // syrk+cholesky + FACT_SOLVE_KKT_STEP_OCP_QP(ws->qp_step, ws->sol_step, arg, ws); - } - else if(arg->lq_fact==1 & force_lq==0) - { + } + else if(arg->lq_fact==1 & force_lq==0) + { - // syrk+chol, switch to lq when needed - FACT_SOLVE_KKT_STEP_OCP_QP(ws->qp_step, ws->sol_step, arg, ws); + // syrk+chol, switch to lq when needed + FACT_SOLVE_KKT_STEP_OCP_QP(ws->qp_step, ws->sol_step, arg, ws); - // compute res of linear system - COMPUTE_LIN_RES_OCP_QP(ws->qp_step, qp_sol, ws->sol_step, ws->res_itref, ws->res_workspace); - VECNRM_INF(cws->nv, ws->res_itref->res_g, 0, &itref_qp_norm[0]); - VECNRM_INF(cws->ne, ws->res_itref->res_b, 0, &itref_qp_norm[1]); - VECNRM_INF(cws->nc, ws->res_itref->res_d, 0, &itref_qp_norm[2]); - VECNRM_INF(cws->nc, ws->res_itref->res_m, 0, &itref_qp_norm[3]); + // compute res of linear system + COMPUTE_LIN_RES_OCP_QP(ws->qp_step, qp_sol, ws->sol_step, ws->res_itref, ws->res_workspace); + VECNRM_INF(cws->nv, ws->res_itref->res_g, 0, &itref_qp_norm[0]); + VECNRM_INF(cws->ne, ws->res_itref->res_b, 0, &itref_qp_norm[1]); + VECNRM_INF(cws->nc, ws->res_itref->res_d, 0, &itref_qp_norm[2]); + VECNRM_INF(cws->nc, ws->res_itref->res_m, 0, &itref_qp_norm[3]); //printf("\n%e\t%e\t%e\t%e\n", itref_qp_norm[0], itref_qp_norm[1], itref_qp_norm[2], itref_qp_norm[3]); - // inaccurate factorization: switch to lq - if( + // inaccurate factorization: switch to lq + if( #ifdef USE_C99_MATH - ( itref_qp_norm[0]==0.0 & isnan(BLASFEO_DVECEL(ws->res_itref->res_g+0, 0)) ) | + ( itref_qp_norm[0]==0.0 & isnan(BLASFEO_DVECEL(ws->res_itref->res_g+0, 0)) ) | #else - ( itref_qp_norm[0]==0.0 & BLASFEO_DVECEL(ws->res_itref->res_g+0, 0)!=BLASFEO_DVECEL(ws->res_itref->res_g+0, 0) ) | + ( itref_qp_norm[0]==0.0 & BLASFEO_DVECEL(ws->res_itref->res_g+0, 0)!=BLASFEO_DVECEL(ws->res_itref->res_g+0, 0) ) | #endif - itref_qp_norm[0]>1e-5 | - itref_qp_norm[1]>1e-5 | - itref_qp_norm[2]>1e-5 | - itref_qp_norm[3]>1e-5 ) - { + itref_qp_norm[0]>1e-5 | + itref_qp_norm[1]>1e-5 | + itref_qp_norm[2]>1e-5 | + itref_qp_norm[3]>1e-5 ) + { #if 0 blasfeo_print_tran_dvec(cws->nv, ws->sol_step->v, 0); @@ -1160,11 +1160,11 @@ blasfeo_print_tran_dvec(cws->nc, ws->sol_step->lam, 0); blasfeo_print_tran_dvec(cws->nc, ws->sol_step->t, 0); #endif - // refactorize using lq - FACT_LQ_SOLVE_KKT_STEP_OCP_QP(ws->qp_step, ws->sol_step, arg, ws); + // refactorize using lq + FACT_LQ_SOLVE_KKT_STEP_OCP_QP(ws->qp_step, ws->sol_step, arg, ws); - // switch to lq - force_lq = 1; + // switch to lq + force_lq = 1; #if 0 blasfeo_print_tran_dvec(cws->nv, ws->sol_step->v, 0); @@ -1173,61 +1173,61 @@ blasfeo_print_tran_dvec(cws->nc, ws->sol_step->lam, 0); blasfeo_print_tran_dvec(cws->nc, ws->sol_step->t, 0); #endif - } - - } - else // arg->lq_fact==2 - { - - FACT_LQ_SOLVE_KKT_STEP_OCP_QP(ws->qp_step, ws->sol_step, arg, ws); - - } - - // iterative refinement on prediction step - for(itref0=0; itref0itref_pred_max; itref0++) - { - - COMPUTE_LIN_RES_OCP_QP(ws->qp_step, qp_sol, ws->sol_step, ws->res_itref, ws->res_workspace); - - VECNRM_INF(cws->nv, ws->res_itref->res_g, 0, &itref_qp_norm[0]); - VECNRM_INF(cws->ne, ws->res_itref->res_b, 0, &itref_qp_norm[1]); - VECNRM_INF(cws->nc, ws->res_itref->res_d, 0, &itref_qp_norm[2]); - VECNRM_INF(cws->nc, ws->res_itref->res_m, 0, &itref_qp_norm[3]); - - if(itref0==0) - { - itref_qp_norm0[0] = itref_qp_norm[0]; - itref_qp_norm0[1] = itref_qp_norm[1]; - itref_qp_norm0[2] = itref_qp_norm[2]; - itref_qp_norm0[3] = itref_qp_norm[3]; - } - - if( \ - (itref_qp_norm[0]<1e0*arg->res_g_max | itref_qp_norm[0]<1e-3*qp_res[0]) & \ - (itref_qp_norm[1]<1e0*arg->res_b_max | itref_qp_norm[1]<1e-3*qp_res[1]) & \ - (itref_qp_norm[2]<1e0*arg->res_d_max | itref_qp_norm[2]<1e-3*qp_res[2]) & \ - (itref_qp_norm[3]<1e0*arg->res_m_max | itref_qp_norm[3]<1e-3*qp_res[3]) ) -// (itref_qp_norm[0]<=arg->res_g_max) & \ - (itref_qp_norm[1]<=arg->res_b_max) & \ - (itref_qp_norm[2]<=arg->res_d_max) & \ - (itref_qp_norm[3]<=arg->res_m_max) ) - { - break; - } - - ws->use_Pb = 0; - SOLVE_KKT_STEP_OCP_QP(ws->qp_itref, ws->sol_itref, arg, ws); - - for(ii=0; ii<=N; ii++) - AXPY(nu[ii]+nx[ii]+2*ns[ii], 1.0, ws->sol_itref->ux+ii, 0, ws->sol_step->ux+ii, 0, ws->sol_step->ux+ii, 0); - for(ii=0; iisol_itref->pi+ii, 0, ws->sol_step->pi+ii, 0, ws->sol_step->pi+ii, 0); - for(ii=0; ii<=N; ii++) - AXPY(2*nb[ii]+2*ng[ii]+2*ns[ii], 1.0, ws->sol_itref->lam+ii, 0, ws->sol_step->lam+ii, 0, ws->sol_step->lam+ii, 0); - for(ii=0; ii<=N; ii++) - AXPY(2*nb[ii]+2*ng[ii]+2*ns[ii], 1.0, ws->sol_itref->t+ii, 0, ws->sol_step->t+ii, 0, ws->sol_step->t+ii, 0); - - } + } + + } + else // arg->lq_fact==2 + { + + FACT_LQ_SOLVE_KKT_STEP_OCP_QP(ws->qp_step, ws->sol_step, arg, ws); + + } + + // iterative refinement on prediction step + for(itref0=0; itref0itref_pred_max; itref0++) + { + + COMPUTE_LIN_RES_OCP_QP(ws->qp_step, qp_sol, ws->sol_step, ws->res_itref, ws->res_workspace); + + VECNRM_INF(cws->nv, ws->res_itref->res_g, 0, &itref_qp_norm[0]); + VECNRM_INF(cws->ne, ws->res_itref->res_b, 0, &itref_qp_norm[1]); + VECNRM_INF(cws->nc, ws->res_itref->res_d, 0, &itref_qp_norm[2]); + VECNRM_INF(cws->nc, ws->res_itref->res_m, 0, &itref_qp_norm[3]); + + if(itref0==0) + { + itref_qp_norm0[0] = itref_qp_norm[0]; + itref_qp_norm0[1] = itref_qp_norm[1]; + itref_qp_norm0[2] = itref_qp_norm[2]; + itref_qp_norm0[3] = itref_qp_norm[3]; + } + + if( \ + (itref_qp_norm[0]<1e0*arg->res_g_max | itref_qp_norm[0]<1e-3*qp_res[0]) & \ + (itref_qp_norm[1]<1e0*arg->res_b_max | itref_qp_norm[1]<1e-3*qp_res[1]) & \ + (itref_qp_norm[2]<1e0*arg->res_d_max | itref_qp_norm[2]<1e-3*qp_res[2]) & \ + (itref_qp_norm[3]<1e0*arg->res_m_max | itref_qp_norm[3]<1e-3*qp_res[3]) ) +// (itref_qp_norm[0]<=arg->res_g_max) & \ + (itref_qp_norm[1]<=arg->res_b_max) & \ + (itref_qp_norm[2]<=arg->res_d_max) & \ + (itref_qp_norm[3]<=arg->res_m_max) ) + { + break; + } + + ws->use_Pb = 0; + SOLVE_KKT_STEP_OCP_QP(ws->qp_itref, ws->sol_itref, arg, ws); + + for(ii=0; ii<=N; ii++) + AXPY(nu[ii]+nx[ii]+2*ns[ii], 1.0, ws->sol_itref->ux+ii, 0, ws->sol_step->ux+ii, 0, ws->sol_step->ux+ii, 0); + for(ii=0; iisol_itref->pi+ii, 0, ws->sol_step->pi+ii, 0, ws->sol_step->pi+ii, 0); + for(ii=0; ii<=N; ii++) + AXPY(2*nb[ii]+2*ng[ii]+2*ns[ii], 1.0, ws->sol_itref->lam+ii, 0, ws->sol_step->lam+ii, 0, ws->sol_step->lam+ii, 0); + for(ii=0; ii<=N; ii++) + AXPY(2*nb[ii]+2*ng[ii]+2*ns[ii], 1.0, ws->sol_itref->t+ii, 0, ws->sol_step->t+ii, 0, ws->sol_step->t+ii, 0); + + } #if 0 int N = qp->dim->N; @@ -1242,212 +1242,212 @@ int ii; //exit(1); //for(ii=0; ii<=N; ii++) -// blasfeo_print_dmat(nu[ii]+nx[ii], nu[ii]+nx[ii], ws->L+ii, 0, 0); +// blasfeo_print_dmat(nu[ii]+nx[ii], nu[ii]+nx[ii], ws->L+ii, 0, 0); //exit(1); printf("\nux\n"); for(ii=0; ii<=N; ii++) - blasfeo_print_tran_dvec(nu[ii]+nx[ii]+2*ns[ii], ws->dux+ii, 0); + blasfeo_print_tran_dvec(nu[ii]+nx[ii]+2*ns[ii], ws->dux+ii, 0); printf("\npi\n"); for(ii=0; iidpi+ii, 0); + blasfeo_print_tran_dvec(nx[ii+1], ws->dpi+ii, 0); //printf("\nlam\n"); //for(ii=0; ii<=N; ii++) -// blasfeo_print_tran_dvec(2*nb[ii]+2*ng[ii]+2*ns[ii], ws->dlam+ii, 0); +// blasfeo_print_tran_dvec(2*nb[ii]+2*ng[ii]+2*ns[ii], ws->dlam+ii, 0); printf("\nt\n"); for(ii=0; ii<=N; ii++) - blasfeo_print_tran_dvec(2*nb[ii]+2*ng[ii]+2*ns[ii], ws->dt+ii, 0); + blasfeo_print_tran_dvec(2*nb[ii]+2*ng[ii]+2*ns[ii], ws->dt+ii, 0); SOLVE_KKT_STEP_OCP_QP(qp, ws); printf("\nux\n"); for(ii=0; ii<=N; ii++) - blasfeo_print_tran_dvec(nu[ii]+nx[ii]+2*ns[ii], ws->dux+ii, 0); + blasfeo_print_tran_dvec(nu[ii]+nx[ii]+2*ns[ii], ws->dux+ii, 0); printf("\npi\n"); for(ii=0; iidpi+ii, 0); + blasfeo_print_tran_dvec(nx[ii+1], ws->dpi+ii, 0); //printf("\nlam\n"); //for(ii=0; ii<=N; ii++) -// blasfeo_print_tran_dvec(2*nb[ii]+2*ng[ii]+2*ns[ii], ws->dlam+ii, 0); +// blasfeo_print_tran_dvec(2*nb[ii]+2*ng[ii]+2*ns[ii], ws->dlam+ii, 0); printf("\nt\n"); for(ii=0; ii<=N; ii++) - blasfeo_print_tran_dvec(2*nb[ii]+2*ng[ii]+2*ns[ii], ws->dt+ii, 0); + blasfeo_print_tran_dvec(2*nb[ii]+2*ng[ii]+2*ns[ii], ws->dt+ii, 0); exit(1); #endif - // alpha - COMPUTE_ALPHA_QP(cws); - if(kkstat_max) - ws->stat[5*kk+0] = cws->alpha; + // alpha + COMPUTE_ALPHA_QP(cws); + if(kkstat_max) + ws->stat[5*kk+0] = cws->alpha; - // Mehrotra's predictor-corrector - if(arg->pred_corr==1) - { - // mu_aff - COMPUTE_MU_AFF_QP(cws); - if(kkstat_max) - ws->stat[5*kk+1] = cws->mu_aff; + // Mehrotra's predictor-corrector + if(arg->pred_corr==1) + { + // mu_aff + COMPUTE_MU_AFF_QP(cws); + if(kkstat_max) + ws->stat[5*kk+1] = cws->mu_aff; - tmp = cws->mu_aff/cws->mu; - cws->sigma = tmp*tmp*tmp; - if(kkstat_max) - ws->stat[5*kk+2] = cws->sigma; + tmp = cws->mu_aff/cws->mu; + cws->sigma = tmp*tmp*tmp; + if(kkstat_max) + ws->stat[5*kk+2] = cws->sigma; - COMPUTE_CENTERING_CORRECTION_QP(cws); + COMPUTE_CENTERING_CORRECTION_QP(cws); - // fact and solve kkt - ws->use_Pb = 1; - SOLVE_KKT_STEP_OCP_QP(ws->qp_step, ws->sol_step, arg, ws); + // fact and solve kkt + ws->use_Pb = 1; + SOLVE_KKT_STEP_OCP_QP(ws->qp_step, ws->sol_step, arg, ws); - // alpha - COMPUTE_ALPHA_QP(cws); - if(kkstat_max) - ws->stat[5*kk+3] = cws->alpha; + // alpha + COMPUTE_ALPHA_QP(cws); + if(kkstat_max) + ws->stat[5*kk+3] = cws->alpha; - // conditional Mehrotra's predictor-corrector - if(arg->cond_pred_corr==1) - { + // conditional Mehrotra's predictor-corrector + if(arg->cond_pred_corr==1) + { - // save mu_aff (from prediction step) - mu_aff0 = cws->mu_aff; + // save mu_aff (from prediction step) + mu_aff0 = cws->mu_aff; - // compute mu for predictor-corrector-centering - COMPUTE_MU_AFF_QP(cws); + // compute mu for predictor-corrector-centering + COMPUTE_MU_AFF_QP(cws); -// if(cws->mu_aff > 2.0*cws->mu) - if(cws->mu_aff > 2.0*mu_aff0) - { +// if(cws->mu_aff > 2.0*cws->mu) + if(cws->mu_aff > 2.0*mu_aff0) + { - // centering direction - COMPUTE_CENTERING_QP(cws); + // centering direction + COMPUTE_CENTERING_QP(cws); - // solve kkt - ws->use_Pb = 1; - SOLVE_KKT_STEP_OCP_QP(ws->qp_step, ws->sol_step, arg, ws); + // solve kkt + ws->use_Pb = 1; + SOLVE_KKT_STEP_OCP_QP(ws->qp_step, ws->sol_step, arg, ws); - // alpha - COMPUTE_ALPHA_QP(cws); - if(kkstat_max) - ws->stat[5*kk+3] = cws->alpha; + // alpha + COMPUTE_ALPHA_QP(cws); + if(kkstat_max) + ws->stat[5*kk+3] = cws->alpha; - } + } - } + } - iter_ref_step = 0; - for(itref1=0; itref1itref_corr_max; itref1++) - { + iter_ref_step = 0; + for(itref1=0; itref1itref_corr_max; itref1++) + { - COMPUTE_LIN_RES_OCP_QP(ws->qp_step, qp_sol, ws->sol_step, ws->res_itref, ws->res_workspace); + COMPUTE_LIN_RES_OCP_QP(ws->qp_step, qp_sol, ws->sol_step, ws->res_itref, ws->res_workspace); //for(ii=0; ii<=N; ii++) -// blasfeo_dvecse(nu[ii]+nx[ii], 0.0, ws->res_itref->res_g+ii, 0); +// blasfeo_dvecse(nu[ii]+nx[ii], 0.0, ws->res_itref->res_g+ii, 0); //for(ii=0; iires_itref->res_b+ii, 0); +// blasfeo_dvecse(nx[ii+1], 0.0, ws->res_itref->res_b+ii, 0); //for(ii=0; ii<=N; ii++) -// blasfeo_dvecse(2*nb[ii]+2*ng[ii], 0.0, ws->res_itref->res_d+ii, 0); +// blasfeo_dvecse(2*nb[ii]+2*ng[ii], 0.0, ws->res_itref->res_d+ii, 0); //for(ii=0; ii<=N; ii++) -// blasfeo_dvecse(2*nb[ii]+2*ng[ii], 0.0, ws->res_itref->res_m+ii, 0); - VECNRM_INF(cws->nv, ws->res_itref->res_g, 0, &itref_qp_norm[0]); - VECNRM_INF(cws->ne, ws->res_itref->res_b, 0, &itref_qp_norm[1]); - VECNRM_INF(cws->nc, ws->res_itref->res_d, 0, &itref_qp_norm[2]); - VECNRM_INF(cws->nc, ws->res_itref->res_m, 0, &itref_qp_norm[3]); - - if(itref1==0) - { - itref_qp_norm0[0] = itref_qp_norm[0]; - itref_qp_norm0[1] = itref_qp_norm[1]; - itref_qp_norm0[2] = itref_qp_norm[2]; - itref_qp_norm0[3] = itref_qp_norm[3]; - } +// blasfeo_dvecse(2*nb[ii]+2*ng[ii], 0.0, ws->res_itref->res_m+ii, 0); + VECNRM_INF(cws->nv, ws->res_itref->res_g, 0, &itref_qp_norm[0]); + VECNRM_INF(cws->ne, ws->res_itref->res_b, 0, &itref_qp_norm[1]); + VECNRM_INF(cws->nc, ws->res_itref->res_d, 0, &itref_qp_norm[2]); + VECNRM_INF(cws->nc, ws->res_itref->res_m, 0, &itref_qp_norm[3]); + + if(itref1==0) + { + itref_qp_norm0[0] = itref_qp_norm[0]; + itref_qp_norm0[1] = itref_qp_norm[1]; + itref_qp_norm0[2] = itref_qp_norm[2]; + itref_qp_norm0[3] = itref_qp_norm[3]; + } //printf("\nitref1 %d\t%e\t%e\t%e\t%e\n", itref1, itref_qp_norm[0], itref_qp_norm[1], itref_qp_norm[2], itref_qp_norm[3]); - if( \ - (itref_qp_norm[0]<1e0*arg->res_g_max | itref_qp_norm[0]<1e-3*qp_res[0]) & \ - (itref_qp_norm[1]<1e0*arg->res_b_max | itref_qp_norm[1]<1e-3*qp_res[1]) & \ - (itref_qp_norm[2]<1e0*arg->res_d_max | itref_qp_norm[2]<1e-3*qp_res[2]) & \ - (itref_qp_norm[3]<1e0*arg->res_m_max | itref_qp_norm[3]<1e-3*qp_res[3]) ) -// (itref_qp_norm[0]<=arg->res_g_max) & \ - (itref_qp_norm[1]<=arg->res_b_max) & \ - (itref_qp_norm[2]<=arg->res_d_max) & \ - (itref_qp_norm[3]<=arg->res_m_max) ) - { - break; - } + if( \ + (itref_qp_norm[0]<1e0*arg->res_g_max | itref_qp_norm[0]<1e-3*qp_res[0]) & \ + (itref_qp_norm[1]<1e0*arg->res_b_max | itref_qp_norm[1]<1e-3*qp_res[1]) & \ + (itref_qp_norm[2]<1e0*arg->res_d_max | itref_qp_norm[2]<1e-3*qp_res[2]) & \ + (itref_qp_norm[3]<1e0*arg->res_m_max | itref_qp_norm[3]<1e-3*qp_res[3]) ) +// (itref_qp_norm[0]<=arg->res_g_max) & \ + (itref_qp_norm[1]<=arg->res_b_max) & \ + (itref_qp_norm[2]<=arg->res_d_max) & \ + (itref_qp_norm[3]<=arg->res_m_max) ) + { + break; + } //printf("\nres_g\n"); //ii = 0; //blasfeo_print_exp_tran_dvec(nu[ii]+nx[ii], ws->res_itref->res_g+ii, 0); //for(ii=0; ii<=N; ii++) -// blasfeo_print_exp_tran_dvec(nu[ii]+nx[ii], ws->res_itref->res_g+ii, 0); +// blasfeo_print_exp_tran_dvec(nu[ii]+nx[ii], ws->res_itref->res_g+ii, 0); //printf("\nres_b\n"); //for(ii=0; iires_itref->res_b+ii, 0); +// blasfeo_print_exp_tran_dvec(nx[ii+1], ws->res_itref->res_b+ii, 0); //printf("\nres_d\n"); //for(ii=0; ii<=N; ii++) -// blasfeo_print_exp_tran_dvec(2*nb[ii]+2*ng[ii], ws->res_itref->res_d+ii, 0); +// blasfeo_print_exp_tran_dvec(2*nb[ii]+2*ng[ii], ws->res_itref->res_d+ii, 0); //printf("\nres_m\n"); //for(ii=0; ii<=N; ii++) -// blasfeo_print_exp_tran_dvec(2*nb[ii]+2*ng[ii], ws->res_itref->res_m+ii, 0); +// blasfeo_print_exp_tran_dvec(2*nb[ii]+2*ng[ii], ws->res_itref->res_m+ii, 0); - ws->use_Pb = 0; - SOLVE_KKT_STEP_OCP_QP(ws->qp_itref, ws->sol_itref, arg, ws); -// FACT_SOLVE_LQ_KKT_STEP_OCP_QP(ws->qp_itref, ws->sol_itref, arg, ws); - iter_ref_step = 1; + ws->use_Pb = 0; + SOLVE_KKT_STEP_OCP_QP(ws->qp_itref, ws->sol_itref, arg, ws); +// FACT_SOLVE_LQ_KKT_STEP_OCP_QP(ws->qp_itref, ws->sol_itref, arg, ws); + iter_ref_step = 1; //printf("\nux_corr\n"); //ii = 0; //blasfeo_print_exp_tran_dvec(nu[ii]+nx[ii], ws->sol_itref->ux+ii, 0); //for(ii=0; ii<=N; ii++) -// blasfeo_print_exp_tran_dvec(nu[ii]+nx[ii], ws->sol_itref->ux+ii, 0); +// blasfeo_print_exp_tran_dvec(nu[ii]+nx[ii], ws->sol_itref->ux+ii, 0); //printf("\npi_corr\n"); //for(ii=0; iisol_itref->pi+ii, 0); +// blasfeo_print_exp_tran_dvec(nx[ii+1], ws->sol_itref->pi+ii, 0); //printf("\nlam_corr\n"); //for(ii=0; ii<=N; ii++) -// blasfeo_print_exp_tran_dvec(2*nb[ii]+2*ng[ii], ws->sol_itref->lam+ii, 0); +// blasfeo_print_exp_tran_dvec(2*nb[ii]+2*ng[ii], ws->sol_itref->lam+ii, 0); //printf("\nt_corr\n"); //for(ii=0; ii<=N; ii++) -// blasfeo_print_exp_tran_dvec(2*nb[ii]+2*ng[ii], ws->sol_itref->t+ii, 0); - - for(ii=0; ii<=N; ii++) - AXPY(nu[ii]+nx[ii]+2*ns[ii], 1.0, ws->sol_itref->ux+ii, 0, ws->sol_step->ux+ii, 0, ws->sol_step->ux+ii, 0); - for(ii=0; iisol_itref->pi+ii, 0, ws->sol_step->pi+ii, 0, ws->sol_step->pi+ii, 0); - for(ii=0; ii<=N; ii++) - AXPY(2*nb[ii]+2*ng[ii]+2*ns[ii], 1.0, ws->sol_itref->lam+ii, 0, ws->sol_step->lam+ii, 0, ws->sol_step->lam+ii, 0); - for(ii=0; ii<=N; ii++) - AXPY(2*nb[ii]+2*ng[ii]+2*ns[ii], 1.0, ws->sol_itref->t+ii, 0, ws->sol_step->t+ii, 0, ws->sol_step->t+ii, 0); - - } - - if(iter_ref_step) - { - // alpha - COMPUTE_ALPHA_QP(cws); - if(kkstat_max) - ws->stat[5*kk+3] = cws->alpha; - } - - } - - // - UPDATE_VAR_QP(cws); - - // compute residuals - COMPUTE_RES_OCP_QP(qp, qp_sol, ws->res, ws->res_workspace); - BACKUP_RES_M(cws); - cws->mu = ws->res->res_mu; - if(kkstat_max) - ws->stat[5*kk+4] = ws->res->res_mu; - - // compute infinity norm of residuals - VECNRM_INF(cws->nv, &str_res_g, 0, &qp_res[0]); - VECNRM_INF(cws->ne, &str_res_b, 0, &qp_res[1]); - VECNRM_INF(cws->nc, &str_res_d, 0, &qp_res[2]); - VECNRM_INF(cws->nc, &str_res_m, 0, &qp_res[3]); +// blasfeo_print_exp_tran_dvec(2*nb[ii]+2*ng[ii], ws->sol_itref->t+ii, 0); + + for(ii=0; ii<=N; ii++) + AXPY(nu[ii]+nx[ii]+2*ns[ii], 1.0, ws->sol_itref->ux+ii, 0, ws->sol_step->ux+ii, 0, ws->sol_step->ux+ii, 0); + for(ii=0; iisol_itref->pi+ii, 0, ws->sol_step->pi+ii, 0, ws->sol_step->pi+ii, 0); + for(ii=0; ii<=N; ii++) + AXPY(2*nb[ii]+2*ng[ii]+2*ns[ii], 1.0, ws->sol_itref->lam+ii, 0, ws->sol_step->lam+ii, 0, ws->sol_step->lam+ii, 0); + for(ii=0; ii<=N; ii++) + AXPY(2*nb[ii]+2*ng[ii]+2*ns[ii], 1.0, ws->sol_itref->t+ii, 0, ws->sol_step->t+ii, 0, ws->sol_step->t+ii, 0); + + } + + if(iter_ref_step) + { + // alpha + COMPUTE_ALPHA_QP(cws); + if(kkstat_max) + ws->stat[5*kk+3] = cws->alpha; + } + + } + + // + UPDATE_VAR_QP(cws); + + // compute residuals + COMPUTE_RES_OCP_QP(qp, qp_sol, ws->res, ws->res_workspace); + BACKUP_RES_M(cws); + cws->mu = ws->res->res_mu; + if(kkstat_max) + ws->stat[5*kk+4] = ws->res->res_mu; + + // compute infinity norm of residuals + VECNRM_INF(cws->nv, &str_res_g, 0, &qp_res[0]); + VECNRM_INF(cws->ne, &str_res_b, 0, &qp_res[1]); + VECNRM_INF(cws->nc, &str_res_d, 0, &qp_res[2]); + VECNRM_INF(cws->nc, &str_res_m, 0, &qp_res[3]); if(arg->print_level > 0) { @@ -1459,49 +1459,49 @@ exit(1); } printf("%-10d %-15e %-15e %-15e %-15e %-15e %-15e\n", kk, qp_res[0], qp_res[1], qp_res[2], qp_res[3], cws->alpha, cws->mu); } - } + } - ws->iter = kk; + ws->iter = kk; #if 0 - printf("\nux\n"); - for(ii=0; ii<=N; ii++) - blasfeo_print_tran_dvec(nu[ii]+nx[ii], qp_sol->ux+ii, 0); + printf("\nux\n"); + for(ii=0; ii<=N; ii++) + blasfeo_print_tran_dvec(nu[ii]+nx[ii], qp_sol->ux+ii, 0); #endif - // max iteration number reached - if(kk == arg->iter_max) { + // max iteration number reached + if(kk == arg->iter_max) { if (arg->print_level > 0) printf(" \n -> ERROR: maximum number of iterations reached, exiting.\n\n"); - return 1; + return 1; } - // min step lenght - if(cws->alpha <= arg->alpha_min) { + // min step lenght + if(cws->alpha <= arg->alpha_min) { if (arg->print_level > 0) printf(" \n -> ERROR: minimum step size reached, exiting.\n\n"); - return 2; + return 2; } - // NaN in the solution + // NaN in the solution #ifdef USE_C99_MATH - if(isnan(cws->mu)) { + if(isnan(cws->mu)) { if (arg->print_level > 0) printf(" \n -> ERROR: NaN detected, exiting.\n\n"); - return 3; + return 3; } #else - if(cws->mu != cws->mu) { + if(cws->mu != cws->mu) { if (arg->print_level > 0) printf(" \n -> ERROR: NaN detected, exiting.\n\n"); - return 3; + return 3; } #endif - // normal return + // normal return if (arg->print_level > 0) printf(" \n -> SOLUTION FOUND. \n\n"); - return 0; + return 0; - } + }