add obj computation to ocp_qp_res

giaf · Apr 28, 2022 · 0e78532 · 0e78532
1 parent f38a216
commit 0e78532
Show file tree

Hide file tree

Showing 5 changed files with 18 additions and 8 deletions.
diff --git a/include/hpipm_d_ocp_qp_res.h b/include/hpipm_d_ocp_qp_res.h
@@ -63,14 +63,15 @@ struct d_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;
 	};
 
 
 
 struct d_ocp_qp_res_ws
 	{
-	struct blasfeo_dvec *tmp_nbgM; // work space of size nbM+ngM
+	struct blasfeo_dvec *tmp_nbgM; // work space of size nbgM
 	struct blasfeo_dvec *tmp_nsM; // work space of size nsM
 	hpipm_size_t memsize;
 	};

diff --git a/include/hpipm_s_ocp_qp_res.h b/include/hpipm_s_ocp_qp_res.h
@@ -63,14 +63,15 @@ struct s_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;
 	};
 
 
 
 struct s_ocp_qp_res_ws
 	{
-	struct blasfeo_svec *tmp_nbgM; // work space of size nbM+ngM
+	struct blasfeo_svec *tmp_nbgM; // work space of size nbgM
 	struct blasfeo_svec *tmp_nsM; // work space of size nsM
 	hpipm_size_t memsize;
 	};

diff --git a/ocp_qp/d_ocp_qp_res.c b/ocp_qp/d_ocp_qp_res.c
@@ -55,6 +55,7 @@
 
 #define AXPY blasfeo_daxpy
 #define CREATE_STRVEC blasfeo_create_dvec
+#define DOT blasfeo_ddot
 #define UNPACK_VEC blasfeo_unpack_dvec
 #define GEMV_DIAG blasfeo_dgemv_d
 #define GEMV_NT blasfeo_dgemv_nt

diff --git a/ocp_qp/s_ocp_qp_res.c b/ocp_qp/s_ocp_qp_res.c
@@ -55,6 +55,7 @@
 
 #define AXPY blasfeo_saxpy
 #define CREATE_STRVEC blasfeo_create_svec
+#define DOT blasfeo_sdot
 #define UNPACK_VEC blasfeo_unpack_svec
 #define GEMV_DIAG blasfeo_sgemv_d
 #define GEMV_NT blasfeo_sgemv_nt

diff --git a/ocp_qp/x_ocp_qp_res.c b/ocp_qp/x_ocp_qp_res.c
@@ -225,18 +225,16 @@ hpipm_size_t OCP_QP_RES_WS_MEMSIZE(struct OCP_QP_DIM *dim)
 	int *ns = dim->ns;
 
 	// compute core qp size and max size
+	int nuxM = 0;
 	int nbM = 0;
 	int ngM = 0;
 	int nsM = 0;
-	for(ii=0; ii<N; ii++)
+	for(ii=0; ii<=N; ii++)
 		{
 		nbM = nb[ii]>nbM ? nb[ii] : nbM;
 		ngM = ng[ii]>ngM ? ng[ii] : ngM;
 		nsM = ns[ii]>nsM ? ns[ii] : nsM;
 		}
-	nbM = nb[ii]>nbM ? nb[ii] : nbM;
-	ngM = ng[ii]>ngM ? ng[ii] : ngM;
-	nsM = ns[ii]>nsM ? ns[ii] : nsM;
 
 	hpipm_size_t size = 0;
 
@@ -381,6 +379,7 @@ void OCP_QP_RES_COMPUTE(struct OCP_QP *qp, struct OCP_QP_SOL *qp_sol, struct OCP
 
 	//
 	REAL mu = 0.0;
+	res->obj = 0.0;
 
 	// loop over stages
 	for(ii=0; ii<=N; ii++)
@@ -392,7 +391,10 @@ void OCP_QP_RES_COMPUTE(struct OCP_QP *qp, struct OCP_QP_SOL *qp_sol, struct OCP
 		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(ii>0)
 			AXPY(nx0, -1.0, pi+(ii-1), 0, res_g+ii, nu0, res_g+ii, nu0);
@@ -421,7 +423,11 @@ void OCP_QP_RES_COMPUTE(struct OCP_QP *qp, struct OCP_QP_SOL *qp_sol, struct OCP
 		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++)
 				{