add ocp_qp_ipm_get_obj

giaf · giaf · commit d0df588e89b0 · 2022-05-01T16:29:24.000+02:00
diff --git a/examples/c/example_d_ocp_qcqp_part_cond.c b/examples/c/example_d_ocp_qcqp_part_cond.c
@@ -446,7 +446,7 @@ int main()
 	printf("\nipm residuals max: res_g = %e, res_b = %e, res_d = %e, res_m = %e\n", res_stat, res_eq, res_ineq, res_comp);
 
 	printf("\nipm iter = %d\n", iter);
-	printf("\nalpha_aff\tmu_aff\t\tsigma\t\talpha_prim\talpha_dual\tmu\t\tres_stat\tres_eq\t\tres_ineq\tres_comp\tlq fact\t\titref pred\titref corr\tlin res stat\tlin res eq\tlin res ineq\tlin res comp\n");
+	printf("\nalpha_aff\tmu_aff\t\tsigma\t\talpha_prim\talpha_dual\tmu\t\tres_stat\tres_eq\t\tres_ineq\tres_comp\tobj\t\tlq fact\t\titref pred\titref corr\tlin res stat\tlin res eq\tlin res ineq\tlin res comp\n");
 	d_print_exp_tran_mat(stat_m, iter+1, stat, stat_m);
 
 	printf("\npart cond time = %e [s]\n\n", time_cond);
diff --git a/examples/c/example_d_ocp_qcqp_x0emb_part_cond.c b/examples/c/example_d_ocp_qcqp_x0emb_part_cond.c
@@ -573,7 +573,7 @@ int main()
 	printf("\nipm residuals max: res_g = %e, res_b = %e, res_d = %e, res_m = %e\n", res_stat, res_eq, res_ineq, res_comp);
 
 	printf("\nipm iter = %d\n", iter);
-	printf("\nalpha_aff\tmu_aff\t\tsigma\t\talpha_prim\talpha_dual\tmu\t\tres_stat\tres_eq\t\tres_ineq\tres_comp\tlq fact\t\titref pred\titref corr\tlin res stat\tlin res eq\tlin res ineq\tlin res comp\n");
+	printf("\nalpha_aff\tmu_aff\t\tsigma\t\talpha_prim\talpha_dual\tmu\t\tres_stat\tres_eq\t\tres_ineq\tres_comp\tobj\t\tlq fact\t\titref pred\titref corr\tlin res stat\tlin res eq\tlin res ineq\tlin res comp\n");
 	d_print_exp_tran_mat(stat_m, iter+1, stat, stat_m);
 
 	printf("\nred eq for time = %e [s]\n\n", time_red_eq_dof);
diff --git a/examples/c/example_d_ocp_qp.c b/examples/c/example_d_ocp_qp.c
@@ -326,7 +326,7 @@ int main()
 	printf("\nipm residuals max: res_g = %e, res_b = %e, res_d = %e, res_m = %e\n", res_stat, res_eq, res_ineq, res_comp);
 
 	printf("\nipm iter = %d\n", iter);
-	printf("\nalpha_aff\tmu_aff\t\tsigma\t\talpha_prim\talpha_dual\tmu\t\tres_stat\tres_eq\t\tres_ineq\tres_comp\tlq fact\t\titref pred\titref corr\tlin res stat\tlin res eq\tlin res ineq\tlin res comp\n");
+	printf("\nalpha_aff\tmu_aff\t\tsigma\t\talpha_prim\talpha_dual\tmu\t\tres_stat\tres_eq\t\tres_ineq\tres_comp\tobj\t\tlq fact\t\titref pred\titref corr\tlin res stat\tlin res eq\tlin res ineq\tlin res comp\n");
 	d_print_exp_tran_mat(stat_m, iter+1, stat, stat_m);
 
 	printf("\nocp ipm time = %e [s]\n\n", time_ipm);
diff --git a/examples/c/example_d_ocp_qp_part_cond.c b/examples/c/example_d_ocp_qp_part_cond.c
@@ -390,7 +390,7 @@ int main()
 	printf("\nipm residuals max: res_g = %e, res_b = %e, res_d = %e, res_m = %e\n", res_stat, res_eq, res_ineq, res_comp);
 
 	printf("\nipm iter = %d\n", iter);
-	printf("\nalpha_aff\tmu_aff\t\tsigma\t\talpha_prim\talpha_dual\tmu\t\tres_stat\tres_eq\t\tres_ineq\tres_comp\tlq fact\t\titref pred\titref corr\tlin res stat\tlin res eq\tlin res ineq\tlin res comp\n");
+	printf("\nalpha_aff\tmu_aff\t\tsigma\t\talpha_prim\talpha_dual\tmu\t\tres_stat\tres_eq\t\tres_ineq\tres_comp\tobj\t\tlq fact\t\titref pred\titref corr\tlin res stat\tlin res eq\tlin res ineq\tlin res comp\n");
 	d_print_exp_tran_mat(stat_m, iter+1, stat, stat_m);
 
 	printf("\npart cond time = %e [s]\n\n", time_cond);
diff --git a/examples/c/example_d_ocp_qp_part_cond_sens.c b/examples/c/example_d_ocp_qp_part_cond_sens.c
@@ -379,7 +379,7 @@ int main()
 	printf("\nipm residuals max: res_g = %e, res_b = %e, res_d = %e, res_m = %e\n", res_stat, res_eq, res_ineq, res_comp);
 
 	printf("\nipm iter = %d\n", iter);
-	printf("\nalpha_aff\tmu_aff\t\tsigma\t\talpha_prim\talpha_dual\tmu\t\tres_stat\tres_eq\t\tres_ineq\tres_comp\tlq fact\t\titref pred\titref corr\tlin res stat\tlin res eq\tlin res ineq\tlin res comp\n");
+	printf("\nalpha_aff\tmu_aff\t\tsigma\t\talpha_prim\talpha_dual\tmu\t\tres_stat\tres_eq\t\tres_ineq\tres_comp\tobj\t\tlq fact\t\titref pred\titref corr\tlin res stat\tlin res eq\tlin res ineq\tlin res comp\n");
 	d_print_exp_tran_mat(stat_m, iter+1, stat, stat_m);
 
 	printf("\npart cond time = %e [s]\n\n", time_cond);
diff --git a/examples/c/example_d_ocp_qp_sens.c b/examples/c/example_d_ocp_qp_sens.c
@@ -304,7 +304,7 @@ int main()
 	printf("\nipm residuals max: res_g = %e, res_b = %e, res_d = %e, res_m = %e\n", res_stat, res_eq, res_ineq, res_comp);
 
 	printf("\nipm iter = %d\n", iter);
-	printf("\nalpha_aff\tmu_aff\t\tsigma\t\talpha_prim\talpha_dual\tmu\t\tres_stat\tres_eq\t\tres_ineq\tres_comp\tlq fact\t\titref pred\titref corr\tlin res stat\tlin res eq\tlin res ineq\tlin res comp\n");
+	printf("\nalpha_aff\tmu_aff\t\tsigma\t\talpha_prim\talpha_dual\tmu\t\tres_stat\tres_eq\t\tres_ineq\tres_comp\tobj\t\tlq fact\t\titref pred\titref corr\tlin res stat\tlin res eq\tlin res ineq\tlin res comp\n");
 	d_print_exp_tran_mat(stat_m, iter+1, stat, stat_m);
 
 	printf("\nocp ipm time = %e [s]\n\n", time_ipm);
diff --git a/examples/c/example_d_ocp_qp_unconstr.c b/examples/c/example_d_ocp_qp_unconstr.c
@@ -368,7 +368,7 @@ int main()
 	printf("\nipm residuals max: res_g = %e, res_b = %e, res_d = %e, res_m = %e\n", res_stat, res_eq, res_ineq, res_comp);
 
 	printf("\nipm iter = %d\n", iter);
-	printf("\nalpha_aff\tmu_aff\t\tsigma\t\talpha_prim\talpha_dual\tmu\t\tres_stat\tres_eq\t\tres_ineq\tres_comp\tlq fact\t\titref pred\titref corr\tlin res stat\tlin res eq\tlin res ineq\tlin res comp\n");
+	printf("\nalpha_aff\tmu_aff\t\tsigma\t\talpha_prim\talpha_dual\tmu\t\tres_stat\tres_eq\t\tres_ineq\tres_comp\tobj\t\tlq fact\t\titref pred\titref corr\tlin res stat\tlin res eq\tlin res ineq\tlin res comp\n");
 	d_print_exp_tran_mat(stat_m, iter+1, stat, stat_m);
 
 	printf("\nocp ipm time = %e [s]\n\n", time_ipm);
diff --git a/examples/c/example_d_ocp_qp_x0emb.c b/examples/c/example_d_ocp_qp_x0emb.c
@@ -420,7 +420,7 @@ int main()
 	printf("\nipm residuals max: res_g = %e, res_b = %e, res_d = %e, res_m = %e\n", res_stat, res_eq, res_ineq, res_comp);
 
 	printf("\nipm iter = %d\n", iter);
-	printf("\nalpha_aff\tmu_aff\t\tsigma\t\talpha_prim\talpha_dual\tmu\t\tres_stat\tres_eq\t\tres_ineq\tres_comp\tlq fact\t\titref pred\titref corr\tlin res stat\tlin res eq\tlin res ineq\tlin res comp\n");
+	printf("\nalpha_aff\tmu_aff\t\tsigma\t\talpha_prim\talpha_dual\tmu\t\tres_stat\tres_eq\t\tres_ineq\tres_comp\tobj\t\tlq fact\t\titref pred\titref corr\tlin res stat\tlin res eq\tlin res ineq\tlin res comp\n");
 	d_print_exp_tran_mat(stat_m, iter+1, stat, stat_m);
 
 	printf("\neq dof red time = %e [s]\n\n", time_red_eq_dof);
diff --git a/examples/c/example_d_ocp_qp_x0emb_cond.c b/examples/c/example_d_ocp_qp_x0emb_cond.c
@@ -486,7 +486,7 @@ int main()
 	printf("\nipm residuals max: res_g = %e, res_b = %e, res_d = %e, res_m = %e\n", res_stat, res_eq, res_ineq, res_comp);
 
 	printf("\nipm iter = %d\n", iter);
-	printf("\nalpha_aff\tmu_aff\t\tsigma\t\talpha_prim\talpha_dual\tmu\t\tres_stat\tres_eq\t\tres_ineq\tres_comp\tlq fact\t\titref pred\titref corr\tlin res stat\tlin res eq\tlin res ineq\tlin res comp\n");
+	printf("\nalpha_aff\tmu_aff\t\tsigma\t\talpha_prim\talpha_dual\tmu\t\tres_stat\tres_eq\t\tres_ineq\tres_comp\tobj\t\tlq fact\t\titref pred\titref corr\tlin res stat\tlin res eq\tlin res ineq\tlin res comp\n");
 	d_print_exp_tran_mat(stat_m, iter+1, stat, stat_m);
 
 	printf("\neq dof red time = %e [s]\n\n", time_red_eq_dof);
diff --git a/examples/c/example_d_ocp_qp_x0emb_part_cond.c b/examples/c/example_d_ocp_qp_x0emb_part_cond.c
@@ -493,7 +493,7 @@ int main()
 	printf("\nipm residuals max: res_g = %e, res_b = %e, res_d = %e, res_m = %e\n", res_stat, res_eq, res_ineq, res_comp);
 
 	printf("\nipm iter = %d\n", iter);
-	printf("\nalpha_aff\tmu_aff\t\tsigma\t\talpha_prim\talpha_dual\tmu\t\tres_stat\tres_eq\t\tres_ineq\tres_comp\tlq fact\t\titref pred\titref corr\tlin res stat\tlin res eq\tlin res ineq\tlin res comp\n");
+	printf("\nalpha_aff\tmu_aff\t\tsigma\t\talpha_prim\talpha_dual\tmu\t\tres_stat\tres_eq\t\tres_ineq\tres_comp\tobj\t\tlq fact\t\titref pred\titref corr\tlin res stat\tlin res eq\tlin res ineq\tlin res comp\n");
 	d_print_exp_tran_mat(stat_m, iter+1, stat, stat_m);
 
 	printf("\neq dof red time = %e [s]\n\n", time_red_eq_dof);
diff --git a/include/hpipm_d_ocp_qp_ipm.h b/include/hpipm_d_ocp_qp_ipm.h
@@ -209,6 +209,8 @@ void d_ocp_qp_ipm_get_max_res_ineq(struct d_ocp_qp_ipm_ws *ws, double *res_ineq)
 //
 void d_ocp_qp_ipm_get_max_res_comp(struct d_ocp_qp_ipm_ws *ws, double *res_comp);
 //
+void d_ocp_qp_ipm_get_obj(struct d_ocp_qp_ipm_ws *ws, double *obj);
+//
 void d_ocp_qp_ipm_get_stat(struct d_ocp_qp_ipm_ws *ws, double **stat);
 //
 void d_ocp_qp_ipm_get_stat_m(struct d_ocp_qp_ipm_ws *ws, int *stat_m);
diff --git a/include/hpipm_s_ocp_qp_ipm.h b/include/hpipm_s_ocp_qp_ipm.h
@@ -209,6 +209,8 @@ void s_ocp_qp_ipm_get_max_res_ineq(struct s_ocp_qp_ipm_ws *ws, float *res_ineq);
 //
 void s_ocp_qp_ipm_get_max_res_comp(struct s_ocp_qp_ipm_ws *ws, float *res_comp);
 //
+void s_ocp_qp_ipm_get_obj(struct s_ocp_qp_ipm_ws *ws, float *obj);
+//
 void s_ocp_qp_ipm_get_stat(struct s_ocp_qp_ipm_ws *ws, float **stat);
 //
 void s_ocp_qp_ipm_get_stat_m(struct s_ocp_qp_ipm_ws *ws, int *stat_m);
diff --git a/ocp_qp/d_ocp_qp_ipm.c b/ocp_qp/d_ocp_qp_ipm.c
@@ -173,6 +173,7 @@
 #define OCP_QP_IPM_GET_MAX_RES_EQ d_ocp_qp_ipm_get_max_res_eq
 #define OCP_QP_IPM_GET_MAX_RES_INEQ d_ocp_qp_ipm_get_max_res_ineq
 #define OCP_QP_IPM_GET_MAX_RES_COMP d_ocp_qp_ipm_get_max_res_comp
+#define OCP_QP_IPM_GET_OBJ d_ocp_qp_ipm_get_obj
 #define OCP_QP_IPM_GET_STAT d_ocp_qp_ipm_get_stat
 #define OCP_QP_IPM_GET_STAT_M d_ocp_qp_ipm_get_stat_m
 #define OCP_QP_IPM_GET_RIC_LR d_ocp_qp_ipm_get_ric_Lr
diff --git a/ocp_qp/s_ocp_qp_ipm.c b/ocp_qp/s_ocp_qp_ipm.c
@@ -173,6 +173,7 @@
 #define OCP_QP_IPM_GET_MAX_RES_EQ s_ocp_qp_ipm_get_max_res_eq
 #define OCP_QP_IPM_GET_MAX_RES_INEQ s_ocp_qp_ipm_get_max_res_ineq
 #define OCP_QP_IPM_GET_MAX_RES_COMP s_ocp_qp_ipm_get_max_res_comp
+#define OCP_QP_IPM_GET_OBJ s_ocp_qp_ipm_get_obj
 #define OCP_QP_IPM_GET_STAT s_ocp_qp_ipm_get_stat
 #define OCP_QP_IPM_GET_STAT_M s_ocp_qp_ipm_get_stat_m
 #define OCP_QP_IPM_GET_RIC_LR s_ocp_qp_ipm_get_ric_Lr
diff --git a/ocp_qp/x_ocp_qcqp_ipm.c b/ocp_qp/x_ocp_qcqp_ipm.c
@@ -682,6 +682,10 @@ void OCP_QCQP_IPM_GET(char *field, struct OCP_QCQP_IPM_WS *ws, void *value)
 		{ 
 		OCP_QCQP_IPM_GET_MAX_RES_COMP(ws, value);
 		}
+//	else if(hpipm_strcmp(field, "obj"))
+//		{ 
+//		OCP_QCQP_IPM_GET_OBJ(ws, value);
+//		}
 	else if(hpipm_strcmp(field, "stat"))
 		{ 
 		OCP_QCQP_IPM_GET_STAT(ws, value);
@@ -748,6 +752,15 @@ void OCP_QCQP_IPM_GET_MAX_RES_COMP(struct OCP_QCQP_IPM_WS *ws, REAL *res_comp)
 
 
 
+// TODO fix once implemented !!!!!!!!!!!!!!!!!!!!!!!
+//void OCP_QCQP_IPM_GET_OBJ(struct OCP_QCQP_IPM_WS *ws, REAL *res_comp)
+//	{
+//	*res_comp = ws->qcqp_res->obj;
+//	return;
+//	}
+
+
+
 void OCP_QCQP_IPM_GET_STAT(struct OCP_QCQP_IPM_WS *ws, REAL **stat)
 	{
 	OCP_QP_IPM_GET_STAT(ws->qp_ws, stat);
@@ -1579,6 +1592,7 @@ void OCP_QCQP_IPM_SOLVE(struct OCP_QCQP *qcqp, struct OCP_QCQP_SOL *qcqp_sol, st
 				stat[7] = qcqp_res_max[1];
 				stat[8] = qcqp_res_max[2];
 				stat[9] = qcqp_res_max[3];
+//				stat[10] = qcqp_res->obj;
 				}
 			cws->mu = qcqp_res->res_mu;
 			}
@@ -1684,6 +1698,7 @@ void OCP_QCQP_IPM_SOLVE(struct OCP_QCQP *qcqp, struct OCP_QCQP_SOL *qcqp_sol, st
 				stat[stat_m*(kk+0)+7] = qcqp_res_max[1];
 				stat[stat_m*(kk+0)+8] = qcqp_res_max[2];
 				stat[stat_m*(kk+0)+9] = qcqp_res_max[3];
+//				stat[stat_m*(kk+0)+10] = qcqp_res->obj;
 				}
 			}
 
@@ -1712,6 +1727,7 @@ void OCP_QCQP_IPM_SOLVE(struct OCP_QCQP *qcqp, struct OCP_QCQP_SOL *qcqp_sol, st
 		stat[stat_m*(0)+7] = qcqp_res_max[1];
 		stat[stat_m*(0)+8] = qcqp_res_max[2];
 		stat[stat_m*(0)+9] = qcqp_res_max[3];
+//		stat[stat_m*(0)+10] = qcqp_res->obj;
 		}
 
 
@@ -1765,6 +1781,7 @@ void OCP_QCQP_IPM_SOLVE(struct OCP_QCQP *qcqp, struct OCP_QCQP_SOL *qcqp_sol, st
 			stat[stat_m*(kk+1)+7] = qcqp_res_max[1];
 			stat[stat_m*(kk+1)+8] = qcqp_res_max[2];
 			stat[stat_m*(kk+1)+9] = qcqp_res_max[3];
+//			stat[stat_m*(kk+1)+10] = qcqp_res->obj;
 			}
 
 		}
diff --git a/ocp_qp/x_ocp_qp_ipm.c b/ocp_qp/x_ocp_qp_ipm.c
@@ -614,7 +614,7 @@ hpipm_size_t OCP_QP_IPM_WS_MEMSIZE(struct OCP_QP_DIM *dim, struct OCP_QP_IPM_ARG
 		size += 1*GELQF_WORKSIZE(nuM+nxM, 2*nuM+3*nxM+ngM); // lq_work0
 		}
 
-	int stat_m = 17;
+	int stat_m = 18;
 	size += stat_m*(1+arg->stat_max)*sizeof(REAL); // stat
 
 	size += (N+1)*sizeof(int); // use_hess_fact
@@ -792,7 +792,7 @@ void OCP_QP_IPM_WS_CREATE(struct OCP_QP_DIM *dim, struct OCP_QP_IPM_ARG *arg, st
 	REAL *d_ptr = (REAL *) sv_ptr;
 
 	workspace->stat = d_ptr;
-	int stat_m = 17;
+	int stat_m = 18;
 	d_ptr += stat_m*(1+arg->stat_max);
 
 	// int stuff
@@ -1077,6 +1077,10 @@ void OCP_QP_IPM_GET(char *field, struct OCP_QP_IPM_WS *ws, void *value)
 		{ 
 		OCP_QP_IPM_GET_MAX_RES_COMP(ws, value);
 		}
+	else if(hpipm_strcmp(field, "obj"))
+		{ 
+		OCP_QP_IPM_GET_OBJ(ws, value);
+		}
 	else if(hpipm_strcmp(field, "stat"))
 		{ 
 		OCP_QP_IPM_GET_STAT(ws, value);
@@ -1143,6 +1147,14 @@ void OCP_QP_IPM_GET_MAX_RES_COMP(struct OCP_QP_IPM_WS *ws, REAL *res_comp)
 
 
 
+void OCP_QP_IPM_GET_OBJ(struct OCP_QP_IPM_WS *ws, REAL *obj)
+	{
+	*obj = ws->res->obj;
+	return;
+	}
+
+
+
 void OCP_QP_IPM_GET_STAT(struct OCP_QP_IPM_WS *ws, REAL **stat)
 	{
 	*stat = ws->stat;
@@ -1936,7 +1948,7 @@ void OCP_QP_IPM_DELTA_STEP(int kk, struct OCP_QP *qp, struct OCP_QP_SOL *qp_sol,
 			VECMUL(cws->nc, qp->d_mask, 0, ws->sol_step->lam, 0, ws->sol_step->lam, 0);
 			}
 		if(kk+1<ws->stat_max)
-			stat[stat_m*(kk+1)+10] = 0;
+			stat[stat_m*(kk+1)+11] = 0;
 		}
 	else if(ws->lq_fact==1 & force_lq==0)
 		{
@@ -1969,7 +1981,7 @@ void OCP_QP_IPM_DELTA_STEP(int kk, struct OCP_QP *qp, struct OCP_QP_SOL *qp_sol,
 		itref_qp_norm[3] = ws->res_itref->res_max[3];
 
 		if(kk+1<ws->stat_max)
-			stat[stat_m*(kk+1)+10] = 0;
+			stat[stat_m*(kk+1)+11] = 0;
 
 		// inaccurate factorization: switch to lq
 		if(
@@ -1999,7 +2011,7 @@ void OCP_QP_IPM_DELTA_STEP(int kk, struct OCP_QP *qp, struct OCP_QP_SOL *qp_sol,
 			force_lq = 1;
 
 			if(kk+1<ws->stat_max)
-				stat[stat_m*(kk+1)+10] = 1;
+				stat[stat_m*(kk+1)+11] = 1;
 
 			}
 
@@ -2017,7 +2029,7 @@ void OCP_QP_IPM_DELTA_STEP(int kk, struct OCP_QP *qp, struct OCP_QP_SOL *qp_sol,
 			VECMUL(cws->nc, qp->d_mask, 0, ws->sol_step->lam, 0, ws->sol_step->lam, 0);
 			}
 		if(kk+1<ws->stat_max)
-			stat[stat_m*(kk+1)+10] = 1;
+			stat[stat_m*(kk+1)+11] = 1;
 
 		}
 
@@ -2026,10 +2038,10 @@ void OCP_QP_IPM_DELTA_STEP(int kk, struct OCP_QP *qp, struct OCP_QP_SOL *qp_sol,
 		{
 		if(kk+1<ws->stat_max)
 			{
-			stat[stat_m*(kk+1)+13] = 0.0;
 			stat[stat_m*(kk+1)+14] = 0.0;
 			stat[stat_m*(kk+1)+15] = 0.0;
 			stat[stat_m*(kk+1)+16] = 0.0;
+			stat[stat_m*(kk+1)+17] = 0.0;
 			}
 		}
 	else
@@ -2053,10 +2065,10 @@ void OCP_QP_IPM_DELTA_STEP(int kk, struct OCP_QP *qp, struct OCP_QP_SOL *qp_sol,
 			itref_qp_norm[3] = ws->res_itref->res_max[3];
 			if(kk+1<ws->stat_max)
 				{
-				stat[stat_m*(kk+1)+13] = itref_qp_norm[0];
-				stat[stat_m*(kk+1)+14] = itref_qp_norm[1];
-				stat[stat_m*(kk+1)+15] = itref_qp_norm[2];
-				stat[stat_m*(kk+1)+16] = itref_qp_norm[3];
+				stat[stat_m*(kk+1)+14] = itref_qp_norm[0];
+				stat[stat_m*(kk+1)+15] = itref_qp_norm[1];
+				stat[stat_m*(kk+1)+16] = itref_qp_norm[2];
+				stat[stat_m*(kk+1)+17] = itref_qp_norm[3];
 				}
 
 			if(itref0==0)
@@ -2115,16 +2127,16 @@ void OCP_QP_IPM_DELTA_STEP(int kk, struct OCP_QP *qp, struct OCP_QP_SOL *qp_sol,
 			itref_qp_norm[3] = ws->res_itref->res_max[3];
 			if(kk+1<ws->stat_max)
 				{
-				stat[stat_m*(kk+1)+13] = itref_qp_norm[0];
-				stat[stat_m*(kk+1)+14] = itref_qp_norm[1];
-				stat[stat_m*(kk+1)+15] = itref_qp_norm[2];
-				stat[stat_m*(kk+1)+16] = itref_qp_norm[3];
+				stat[stat_m*(kk+1)+14] = itref_qp_norm[0];
+				stat[stat_m*(kk+1)+15] = itref_qp_norm[1];
+				stat[stat_m*(kk+1)+16] = itref_qp_norm[2];
+				stat[stat_m*(kk+1)+17] = itref_qp_norm[3];
 				}
 			}
 		}
 
 	if(kk+1<ws->stat_max)
-		stat[stat_m*(kk+1)+11] = itref0;
+		stat[stat_m*(kk+1)+12] = itref0;
 
 	// alpha
 	COMPUTE_ALPHA_QP(cws);
@@ -2239,10 +2251,10 @@ void OCP_QP_IPM_DELTA_STEP(int kk, struct OCP_QP *qp, struct OCP_QP_SOL *qp_sol,
 				itref_qp_norm[3] = ws->res_itref->res_max[3];
 				if(kk+1<ws->stat_max)
 					{
-					stat[stat_m*(kk+1)+13] = itref_qp_norm[0];
-					stat[stat_m*(kk+1)+14] = itref_qp_norm[1];
-					stat[stat_m*(kk+1)+15] = itref_qp_norm[2];
-					stat[stat_m*(kk+1)+16] = itref_qp_norm[3];
+					stat[stat_m*(kk+1)+14] = itref_qp_norm[0];
+					stat[stat_m*(kk+1)+15] = itref_qp_norm[1];
+					stat[stat_m*(kk+1)+16] = itref_qp_norm[2];
+					stat[stat_m*(kk+1)+17] = itref_qp_norm[3];
 					}
 
 				if(itref1==0)
@@ -2302,10 +2314,10 @@ void OCP_QP_IPM_DELTA_STEP(int kk, struct OCP_QP *qp, struct OCP_QP_SOL *qp_sol,
 				itref_qp_norm[3] = ws->res_itref->res_max[3];
 				if(kk+1<ws->stat_max)
 					{
-					stat[stat_m*(kk+1)+13] = itref_qp_norm[0];
-					stat[stat_m*(kk+1)+14] = itref_qp_norm[1];
-					stat[stat_m*(kk+1)+15] = itref_qp_norm[2];
-					stat[stat_m*(kk+1)+16] = itref_qp_norm[3];
+					stat[stat_m*(kk+1)+14] = itref_qp_norm[0];
+					stat[stat_m*(kk+1)+15] = itref_qp_norm[1];
+					stat[stat_m*(kk+1)+16] = itref_qp_norm[2];
+					stat[stat_m*(kk+1)+17] = itref_qp_norm[3];
 					}
 				}
 			}
@@ -2326,14 +2338,14 @@ void OCP_QP_IPM_DELTA_STEP(int kk, struct OCP_QP *qp, struct OCP_QP_SOL *qp_sol,
 		{
 		if(kk+1<ws->stat_max)
 			{
-			stat[stat_m*(kk+1)+13] = 0.0;
 			stat[stat_m*(kk+1)+14] = 0.0;
 			stat[stat_m*(kk+1)+15] = 0.0;
 			stat[stat_m*(kk+1)+16] = 0.0;
+			stat[stat_m*(kk+1)+17] = 0.0;
 			}
 		}
 	if(kk+1<ws->stat_max)
-		stat[stat_m*(kk+1)+12] = itref1;
+		stat[stat_m*(kk+1)+13] = itref1;
 
 	// TODO step length computation
 
@@ -2471,6 +2483,7 @@ void OCP_QP_IPM_SOLVE(struct OCP_QP *qp, struct OCP_QP_SOL *qp_sol, struct OCP_Q
 				stat[7] = qp_res_max[1];
 				stat[8] = qp_res_max[2];
 				stat[9] = qp_res_max[3];
+				stat[10] = ws->res->obj;
 				}
 			cws->mu = ws->res->res_mu;
 			}
@@ -2584,6 +2597,7 @@ void OCP_QP_IPM_SOLVE(struct OCP_QP *qp, struct OCP_QP_SOL *qp_sol, struct OCP_Q
 				stat[stat_m*(kk+0)+7] = qp_res_max[1];
 				stat[stat_m*(kk+0)+8] = qp_res_max[2];
 				stat[stat_m*(kk+0)+9] = qp_res_max[3];
+				stat[stat_m*(kk+0)+10] = ws->res->obj;
 				}
 			}
 
@@ -2612,6 +2626,7 @@ void OCP_QP_IPM_SOLVE(struct OCP_QP *qp, struct OCP_QP_SOL *qp_sol, struct OCP_Q
 		stat[stat_m*(0)+7] = qp_res_max[1];
 		stat[stat_m*(0)+8] = qp_res_max[2];
 		stat[stat_m*(0)+9] = qp_res_max[3];
+		stat[stat_m*(0)+10] = ws->res->obj;
 		}
 
 
@@ -2652,6 +2667,7 @@ void OCP_QP_IPM_SOLVE(struct OCP_QP *qp, struct OCP_QP_SOL *qp_sol, struct OCP_Q
 			stat[stat_m*(kk+1)+7] = qp_res_max[1];
 			stat[stat_m*(kk+1)+8] = qp_res_max[2];
 			stat[stat_m*(kk+1)+9] = qp_res_max[3];
+			stat[stat_m*(kk+1)+10] = ws->res->obj;
 			}
 
 		}
diff --git a/test_problems/test_d_ocp.c b/test_problems/test_d_ocp.c
diff --git a/test_problems/test_d_ocp_phase_I.c b/test_problems/test_d_ocp_phase_I.c
diff --git a/test_problems/test_d_ocp_qcqp.c b/test_problems/test_d_ocp_qcqp.c
diff --git a/test_problems/test_d_part_cond.c b/test_problems/test_d_part_cond.c
diff --git a/test_problems/test_d_part_cond_qcqp.c b/test_problems/test_d_part_cond_qcqp.c
