From b140f3633ef383cdf481ba4e8c94bdc21596c47b Mon Sep 17 00:00:00 2001 From: giaf Date: Wed, 1 Jun 2022 17:00:05 +0200 Subject: [PATCH] add obj computation to tree_ocp_{qp,qcqp}_res --- include/hpipm_d_tree_ocp_qcqp_res.h | 1 + include/hpipm_d_tree_ocp_qp_res.h | 1 + include/hpipm_s_tree_ocp_qcqp_res.h | 1 + include/hpipm_s_tree_ocp_qp_res.h | 1 + tree_ocp_qp/d_tree_ocp_qp_res.c | 1 + tree_ocp_qp/s_tree_ocp_qp_res.c | 1 + tree_ocp_qp/x_tree_ocp_qcqp_res.c | 12 ++++++++++-- tree_ocp_qp/x_tree_ocp_qp_res.c | 12 ++++++++++-- 8 files changed, 26 insertions(+), 4 deletions(-) diff --git a/include/hpipm_d_tree_ocp_qcqp_res.h b/include/hpipm_d_tree_ocp_qcqp_res.h index 69eebb1b..52a71273 100644 --- a/include/hpipm_d_tree_ocp_qcqp_res.h +++ b/include/hpipm_d_tree_ocp_qcqp_res.h @@ -63,6 +63,7 @@ struct d_tree_ocp_qcqp_res struct blasfeo_dvec *res_m; // m-residuals double res_max[4]; // max of residuals double res_mu; // mu-residual + double obj; // (primal) objective hpipm_size_t memsize; }; diff --git a/include/hpipm_d_tree_ocp_qp_res.h b/include/hpipm_d_tree_ocp_qp_res.h index fe499080..c77ac006 100644 --- a/include/hpipm_d_tree_ocp_qp_res.h +++ b/include/hpipm_d_tree_ocp_qp_res.h @@ -63,6 +63,7 @@ struct d_tree_ocp_qp_res struct blasfeo_dvec *res_m; // m-residuals double res_max[4]; // max of residuals double res_mu; // mu-residual + double obj; // (primal) objective hpipm_size_t memsize; }; diff --git a/include/hpipm_s_tree_ocp_qcqp_res.h b/include/hpipm_s_tree_ocp_qcqp_res.h index d0d84a6f..36267182 100644 --- a/include/hpipm_s_tree_ocp_qcqp_res.h +++ b/include/hpipm_s_tree_ocp_qcqp_res.h @@ -63,6 +63,7 @@ struct s_tree_ocp_qcqp_res struct blasfeo_svec *res_m; // m-residuals float res_max[4]; // max of residuals float res_mu; // mu-residual + float obj; // (primal) objective hpipm_size_t memsize; }; diff --git a/include/hpipm_s_tree_ocp_qp_res.h b/include/hpipm_s_tree_ocp_qp_res.h index 5c679720..4221a393 100644 --- a/include/hpipm_s_tree_ocp_qp_res.h +++ b/include/hpipm_s_tree_ocp_qp_res.h @@ -63,6 +63,7 @@ struct s_tree_ocp_qp_res struct blasfeo_svec *res_m; // m-residuals float res_max[4]; // max of residuals float res_mu; // mu-residual + float obj; // (primal) objective hpipm_size_t memsize; }; diff --git a/tree_ocp_qp/d_tree_ocp_qp_res.c b/tree_ocp_qp/d_tree_ocp_qp_res.c index 1cbfbd84..957dba27 100644 --- a/tree_ocp_qp/d_tree_ocp_qp_res.c +++ b/tree_ocp_qp/d_tree_ocp_qp_res.c @@ -56,6 +56,7 @@ #define AXPY blasfeo_daxpy #define CREATE_STRVEC blasfeo_create_dvec +#define DOT blasfeo_ddot #define GEMV_DIAG blasfeo_dgemv_d #define GEMV_NT blasfeo_dgemv_nt #define UNPACK_VEC blasfeo_unpack_dvec diff --git a/tree_ocp_qp/s_tree_ocp_qp_res.c b/tree_ocp_qp/s_tree_ocp_qp_res.c index d177b66c..ad86800b 100644 --- a/tree_ocp_qp/s_tree_ocp_qp_res.c +++ b/tree_ocp_qp/s_tree_ocp_qp_res.c @@ -56,6 +56,7 @@ #define AXPY blasfeo_saxpy #define CREATE_STRVEC blasfeo_create_svec +#define DOT blasfeo_sdot #define GEMV_DIAG blasfeo_sgemv_d #define GEMV_NT blasfeo_sgemv_nt #define UNPACK_VEC blasfeo_unpack_svec diff --git a/tree_ocp_qp/x_tree_ocp_qcqp_res.c b/tree_ocp_qp/x_tree_ocp_qcqp_res.c index 78798489..565fd2b8 100644 --- a/tree_ocp_qp/x_tree_ocp_qcqp_res.c +++ b/tree_ocp_qp/x_tree_ocp_qcqp_res.c @@ -443,6 +443,7 @@ void TREE_OCP_QCQP_RES_COMPUTE(struct TREE_OCP_QCQP *qp, struct TREE_OCP_QCQP_SO // REAL tmp; REAL mu = 0.0; + res->obj = 0.0; // loop over nodes for(ii=0; iiobj += 0.5*DOT(nu0+nx0, res_g+ii, 0, ux+ii, 0); + AXPY(nu0+nx0, -1.0, rqz+ii, 0, res_g+ii, 0, res_g+ii, 0); // if not root if(ii>0) @@ -507,7 +511,11 @@ void TREE_OCP_QCQP_RES_COMPUTE(struct TREE_OCP_QCQP *qp, struct TREE_OCP_QCQP_SO if(ns0>0) { // res_g - GEMV_DIAG(2*ns0, 1.0, Z+ii, 0, ux+ii, nu0+nx0, 1.0, rqz+ii, nu0+nx0, res_g+ii, nu0+nx0); +// GEMV_DIAG(2*ns0, 1.0, Z+ii, 0, ux+ii, nu0+nx0, 1.0, rqz+ii, nu0+nx0, res_g+ii, nu0+nx0); + GEMV_DIAG(2*ns0, 1.0, Z+ii, 0, ux+ii, nu0+nx0, 2.0, rqz+ii, nu0+nx0, res_g+ii, nu0+nx0); + res->obj += 0.5*DOT(2*ns0, res_g+ii, nu0+nx0, ux+ii, nu0+nx0); + AXPY(2*ns0, -1.0, rqz+ii, nu0+nx0, res_g+ii, nu0+nx0, res_g+ii, nu0+nx0); + AXPY(2*ns0, -1.0, lam+ii, 2*nb0+2*ng0+2*nq0, res_g+ii, nu0+nx0, res_g+ii, nu0+nx0); for(jj=0; jjobj = 0.0; // loop over nodes for(ii=0; iiobj += 0.5*DOT(nu0+nx0, res_g+ii, 0, ux+ii, 0); + AXPY(nu0+nx0, -1.0, rqz+ii, 0, res_g+ii, 0, res_g+ii, 0); // if not root if(ii>0) @@ -485,7 +489,11 @@ void TREE_OCP_QP_RES_COMPUTE(struct TREE_OCP_QP *qp, struct TREE_OCP_QP_SOL *qp_ if(ns0>0) { // res_g - GEMV_DIAG(2*ns0, 1.0, Z+ii, 0, ux+ii, nu0+nx0, 1.0, rqz+ii, nu0+nx0, res_g+ii, nu0+nx0); +// GEMV_DIAG(2*ns0, 1.0, Z+ii, 0, ux+ii, nu0+nx0, 1.0, rqz+ii, nu0+nx0, res_g+ii, nu0+nx0); + GEMV_DIAG(2*ns0, 1.0, Z+ii, 0, ux+ii, nu0+nx0, 2.0, rqz+ii, nu0+nx0, res_g+ii, nu0+nx0); + res->obj += 0.5*DOT(2*ns0, res_g+ii, nu0+nx0, ux+ii, nu0+nx0); + AXPY(2*ns0, -1.0, rqz+ii, nu0+nx0, res_g+ii, nu0+nx0, res_g+ii, nu0+nx0); + AXPY(2*ns0, -1.0, lam+ii, 2*nb0+2*ng0, res_g+ii, nu0+nx0, res_g+ii, nu0+nx0); for(jj=0; jj