add obj computation dense_{qp,qcqp_}res

giaf · Jun 1, 2022 · cd78586 · cd78586
1 parent 4afa9e5
commit cd78586
Show file tree

Hide file tree

Showing 6 changed files with 24 additions and 4 deletions.
diff --git a/dense_qp/x_dense_qcqp_res.c b/dense_qp/x_dense_qcqp_res.c
@@ -301,9 +301,13 @@ void DENSE_QCQP_RES_COMPUTE(struct DENSE_QCQP *qp, struct DENSE_QCQP_SOL *qp_sol
 	struct STRVEC *tmp_ns = ws->tmp_ns;
 
 	REAL mu, tmp;
+	res->obj = 0.0;
 
 	// res g
-	SYMV_L(nv, 1.0, Hg, 0, 0, v, 0, 1.0, gz, 0, res_g, 0);
+//	SYMV_L(nv, 1.0, Hg, 0, 0, v, 0, 1.0, gz, 0, res_g, 0);
+	SYMV_L(nv, 1.0, Hg, 0, 0, v, 0, 2.0, gz, 0, res_g, 0);
+	res->obj += 0.5*DOT(nv, res_g, 0, v, 0);
+	AXPY(nv, -1.0, gz, 0, res_g, 0, res_g, 0);
 
 	if(nb+ng+nq>0)
 		{
@@ -360,7 +364,11 @@ void DENSE_QCQP_RES_COMPUTE(struct DENSE_QCQP *qp, struct DENSE_QCQP_SOL *qp_sol
 	if(ns>0)
 		{
 		// res_g
-		GEMV_DIAG(2*ns, 1.0, Z, 0, v, nv, 1.0, gz, nv, res_g, nv);
+//		GEMV_DIAG(2*ns, 1.0, Z, 0, v, nv, 1.0, gz, nv, res_g, nv);
+		GEMV_DIAG(2*ns, 1.0, Z, 0, v, nv, 2.0, gz, nv, res_g, nv);
+		res->obj += 0.5*DOT(2*ns, res_g, nv, v, nv);
+		AXPY(2*ns, -1.0, gz, nv, res_g, nv, res_g, nv);
+
 		AXPY(2*ns, -1.0, lam, 2*nb+2*ng+2*nq, res_g, nv, res_g, nv);
 		for(ii=0; ii<nb+ng+nq; ii++)
 			{

diff --git a/dense_qp/x_dense_qp_res.c b/dense_qp/x_dense_qp_res.c
@@ -295,9 +295,13 @@ void DENSE_QP_RES_COMPUTE(struct DENSE_QP *qp, struct DENSE_QP_SOL *qp_sol, stru
 	struct STRVEC *tmp_ns = ws->tmp_ns;
 
 	REAL mu, tmp;
+	res->obj = 0.0;
 
 	// res g
-	SYMV_L(nv, 1.0, Hg, 0, 0, v, 0, 1.0, gz, 0, res_g, 0);
+//	SYMV_L(nv, 1.0, Hg, 0, 0, v, 0, 1.0, gz, 0, res_g, 0);
+	SYMV_L(nv, 1.0, Hg, 0, 0, v, 0, 2.0, gz, 0, res_g, 0);
+	res->obj += 0.5*DOT(nv, res_g, 0, v, 0);
+	AXPY(nv, -1.0, gz, 0, res_g, 0, res_g, 0);
 
 	if(nb+ng>0)
 		{
@@ -322,7 +326,11 @@ void DENSE_QP_RES_COMPUTE(struct DENSE_QP *qp, struct DENSE_QP_SOL *qp_sol, stru
 	if(ns>0)
 		{
 		// res_g
-		GEMV_DIAG(2*ns, 1.0, Z, 0, v, nv, 1.0, gz, nv, res_g, nv);
+//		GEMV_DIAG(2*ns, 1.0, Z, 0, v, nv, 1.0, gz, nv, res_g, nv);
+		GEMV_DIAG(2*ns, 1.0, Z, 0, v, nv, 2.0, gz, nv, res_g, nv);
+		res->obj += 0.5*DOT(2*ns, res_g, nv, v, nv);
+		AXPY(2*ns, -1.0, gz, nv, res_g, nv, res_g, nv);
+
 		AXPY(2*ns, -1.0, lam, 2*nb+2*ng, res_g, nv, res_g, nv);
 		for(ii=0; ii<nb+ng; ii++)
 			{

diff --git a/include/hpipm_d_dense_qcqp_res.h b/include/hpipm_d_dense_qcqp_res.h
@@ -62,6 +62,7 @@ struct d_dense_qcqp_res
 	struct blasfeo_dvec *res_m; // m-residuals
 	double res_max[4]; // infinity norm of residuals
 	double res_mu; // mu-residual
+	double obj; // (primal) objective
 	hpipm_size_t memsize;
 	};
 

diff --git a/include/hpipm_d_dense_qp_res.h b/include/hpipm_d_dense_qp_res.h
@@ -62,6 +62,7 @@ struct d_dense_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_dense_qcqp_res.h b/include/hpipm_s_dense_qcqp_res.h
@@ -62,6 +62,7 @@ struct s_dense_qcqp_res
 	struct blasfeo_svec *res_m; // m-residuals
 	float res_max[4]; // infinity norm of residuals
 	float res_mu; // mu-residual
+	float obj; // (primal) objective
 	hpipm_size_t memsize;
 	};
 

diff --git a/include/hpipm_s_dense_qp_res.h b/include/hpipm_s_dense_qp_res.h
@@ -62,6 +62,7 @@ struct s_dense_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;
 	};