Skip to content

Commit

Permalink
add ocp_qp_ipm_get_obj
Browse files Browse the repository at this point in the history
  • Loading branch information
@giaf
giaf committed May 1, 2022
1 parent 0e78532 commit d0df588
Show file tree
Hide file tree
Showing 21 changed files with 112 additions and 60 deletions.
2 changes: 1 addition & 1 deletion examples/c/example_d_ocp_qcqp_part_cond.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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);
Expand Down
2 changes: 1 addition & 1 deletion examples/c/example_d_ocp_qcqp_x0emb_part_cond.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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);
Expand Down
2 changes: 1 addition & 1 deletion examples/c/example_d_ocp_qp.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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);
Expand Down
2 changes: 1 addition & 1 deletion examples/c/example_d_ocp_qp_part_cond.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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);
Expand Down
2 changes: 1 addition & 1 deletion examples/c/example_d_ocp_qp_part_cond_sens.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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);
Expand Down
2 changes: 1 addition & 1 deletion examples/c/example_d_ocp_qp_sens.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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);
Expand Down
2 changes: 1 addition & 1 deletion examples/c/example_d_ocp_qp_unconstr.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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);
Expand Down
2 changes: 1 addition & 1 deletion examples/c/example_d_ocp_qp_x0emb.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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);
Expand Down
2 changes: 1 addition & 1 deletion examples/c/example_d_ocp_qp_x0emb_cond.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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);
Expand Down
2 changes: 1 addition & 1 deletion examples/c/example_d_ocp_qp_x0emb_part_cond.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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);
Expand Down
2 changes: 2 additions & 0 deletions include/hpipm_d_ocp_qp_ipm.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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);
Expand Down
2 changes: 2 additions & 0 deletions include/hpipm_s_ocp_qp_ipm.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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);
Expand Down
1 change: 1 addition & 0 deletions ocp_qp/d_ocp_qp_ipm.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
1 change: 1 addition & 0 deletions ocp_qp/s_ocp_qp_ipm.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
17 changes: 17 additions & 0 deletions ocp_qp/x_ocp_qcqp_ipm.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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);
Expand Down Expand Up @@ -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);
Expand Down Expand Up @@ -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;
}
Expand Down Expand Up @@ -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;
}
}

Expand Down Expand Up @@ -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;
}


Expand Down Expand Up @@ -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;
}

}
Expand Down
68 changes: 42 additions & 26 deletions ocp_qp/x_ocp_qp_ipm.c
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down Expand Up @@ -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
Expand Down Expand Up @@ -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);
Expand Down Expand Up @@ -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;
Expand Down Expand Up @@ -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)
{
Expand Down Expand Up @@ -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(
Expand Down Expand Up @@ -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;

}

Expand All @@ -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;

}

Expand All @@ -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
Expand All @@ -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)
Expand Down Expand Up @@ -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);
Expand Down Expand Up @@ -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)
Expand Down Expand Up @@ -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];
}
}
}
Expand All @@ -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

Expand Down Expand Up @@ -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;
}
Expand Down Expand Up @@ -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;
}
}

Expand Down Expand Up @@ -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;
}


Expand Down Expand Up @@ -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;
}

}
Expand Down
Loading

0 comments on commit d0df588

Please sign in to comment.