add obj computation to tree_ocp_{qp,qcqp}_res

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

Hide file tree

Showing 8 changed files with 26 additions and 4 deletions.
diff --git 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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -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
@@ -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; ii<Nn; ii++)
@@ -455,7 +456,10 @@ void TREE_OCP_QCQP_RES_COMPUTE(struct TREE_OCP_QCQP *qp, struct TREE_OCP_QCQP_SO
 		nq0 = nq[ii];
 		ns0 = ns[ii];
 
-		SYMV_L(nu0+nx0, 1.0, RSQrq+ii, 0, 0, ux+ii, 0, 1.0, rqz+ii, 0, res_g+ii, 0);
+//		SYMV_L(nu0+nx0, 1.0, RSQrq+ii, 0, 0, ux+ii, 0, 1.0, rqz+ii, 0, res_g+ii, 0);
+		SYMV_L(nu0+nx0, 1.0, RSQrq+ii, 0, 0, ux+ii, 0, 2.0, rqz+ii, 0, res_g+ii, 0);
+		res->obj += 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; jj<nb0+ng0+nq0; jj++)
 				{

diff --git a/tree_ocp_qp/x_tree_ocp_qp_res.c b/tree_ocp_qp/x_tree_ocp_qp_res.c
@@ -446,6 +446,7 @@ void TREE_OCP_QP_RES_COMPUTE(struct TREE_OCP_QP *qp, struct TREE_OCP_QP_SOL *qp_
 
 	//
 	REAL mu = 0.0;
+	res->obj = 0.0;
 
 	// loop over nodes
 	for(ii=0; ii<Nn; ii++)
@@ -457,7 +458,10 @@ void TREE_OCP_QP_RES_COMPUTE(struct TREE_OCP_QP *qp, struct TREE_OCP_QP_SOL *qp_
 		ng0 = ng[ii];
 		ns0 = ns[ii];
 
-		SYMV_L(nu0+nx0, 1.0, RSQrq+ii, 0, 0, ux+ii, 0, 1.0, rqz+ii, 0, res_g+ii, 0);
+//		SYMV_L(nu0+nx0, 1.0, RSQrq+ii, 0, 0, ux+ii, 0, 1.0, rqz+ii, 0, res_g+ii, 0);
+		SYMV_L(nu0+nx0, 1.0, RSQrq+ii, 0, 0, ux+ii, 0, 2.0, rqz+ii, 0, res_g+ii, 0);
+		res->obj += 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<nb0+ng0; jj++)
 				{