From d0df588e89b0cd78dbc0343e9df5b2613008dddd Mon Sep 17 00:00:00 2001 From: giaf Date: Sun, 1 May 2022 16:29:24 +0200 Subject: [PATCH] add ocp_qp_ipm_get_obj --- examples/c/example_d_ocp_qcqp_part_cond.c | 2 +- .../c/example_d_ocp_qcqp_x0emb_part_cond.c | 2 +- examples/c/example_d_ocp_qp.c | 2 +- examples/c/example_d_ocp_qp_part_cond.c | 2 +- examples/c/example_d_ocp_qp_part_cond_sens.c | 2 +- examples/c/example_d_ocp_qp_sens.c | 2 +- examples/c/example_d_ocp_qp_unconstr.c | 2 +- examples/c/example_d_ocp_qp_x0emb.c | 2 +- examples/c/example_d_ocp_qp_x0emb_cond.c | 2 +- examples/c/example_d_ocp_qp_x0emb_part_cond.c | 2 +- include/hpipm_d_ocp_qp_ipm.h | 2 + include/hpipm_s_ocp_qp_ipm.h | 2 + ocp_qp/d_ocp_qp_ipm.c | 1 + ocp_qp/s_ocp_qp_ipm.c | 1 + ocp_qp/x_ocp_qcqp_ipm.c | 17 +++++ ocp_qp/x_ocp_qp_ipm.c | 68 ++++++++++++------- test_problems/test_d_ocp.c | 38 ++++++----- test_problems/test_d_ocp_phase_I.c | 2 +- test_problems/test_d_ocp_qcqp.c | 6 +- test_problems/test_d_part_cond.c | 2 +- test_problems/test_d_part_cond_qcqp.c | 13 +++- 21 files changed, 112 insertions(+), 60 deletions(-) diff --git a/examples/c/example_d_ocp_qcqp_part_cond.c b/examples/c/example_d_ocp_qcqp_part_cond.c index d9489139..62c51367 100644 --- 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 index 34204bf8..86d358d6 100644 --- 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 index 9193bf9f..491d273e 100644 --- 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 index 5e75d622..9a266915 100644 --- 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 index a9b3c72d..a169a91a 100644 --- 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 index bda59116..99469d95 100644 --- 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 index ae1d00d2..9c558997 100644 --- 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 index 9c8db9f2..a73ed0c7 100644 --- 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 index e51fc67d..4d8d4ffd 100644 --- 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 index 4cd43e41..6d0d51a8 100644 --- 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 index d2a2833f..d0129530 100644 --- 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 index 11f3c47b..838cbf92 100644 --- 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 index f23c64c5..bb81c6b8 100644 --- 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 index 0c867262..482ae442 100644 --- 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 index 7daae8b4..a52c425f 100644 --- 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 index a0594c0b..d73c785d 100644 --- 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+1stat_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+1stat_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+1stat_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+1stat_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+1stat_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+1stat_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+1stat_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+1stat_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+1stat_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+1stat_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+1stat_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+1stat_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 index e0e6d30b..43920cd1 100644 --- a/test_problems/test_d_ocp.c +++ b/test_problems/test_d_ocp.c @@ -283,7 +283,6 @@ int main() nx[0] = nx_; for(ii=1; ii<=N; ii++) nx[ii] = nx_; -// nx[N] = 0; int *nu = (int *) malloc((N+1)*sizeof(int)); for(ii=0; ii