Skip to content

Commit e119a4f

Browse files
giafgiaf
committed
ocp_qp: change to ipm_sens api
1 parent b25b9ae commit e119a4f

File tree

6 files changed

+143
-27
lines changed

6 files changed

+143
-27
lines changed

‎examples/c/example_d_ocp_qp_sens.c‎

Lines changed: 72 additions & 15 deletions
Original file line numberDiff line numberDiff line change
@@ -259,28 +259,28 @@ int main()
259259
* print solution info
260260
************************************************/
261261

262-
printf("\nHPIPM returned with flag %i.\n", hpipm_status);
263-
if(hpipm_status == 0)
262+
printf("\nHPIPM returned with flag %i.\n", hpipm_status);
263+
if(hpipm_status == 0)
264264
{
265-
printf("\n -> QP solved!\n");
265+
printf("\n -> QP solved!\n");
266266
}
267267
else if(hpipm_status==1)
268268
{
269-
printf("\n -> Solver failed! Maximum number of iterations reached\n");
269+
printf("\n -> Solver failed! Maximum number of iterations reached\n");
270270
}
271271
else if(hpipm_status==2)
272272
{
273-
printf("\n -> Solver failed! Minimum step lenght reached\n");
273+
printf("\n -> Solver failed! Minimum step lenght reached\n");
274274
}
275275
else if(hpipm_status==2)
276276
{
277-
printf("\n -> Solver failed! NaN in computations\n");
277+
printf("\n -> Solver failed! NaN in computations\n");
278278
}
279279
else
280280
{
281-
printf("\n -> Solver failed! Unknown return flag\n");
281+
printf("\n -> Solver failed! Unknown return flag\n");
282282
}
283-
printf("\nAverage solution time over %i runs: %e [s]\n", nrep, time_ipm);
283+
printf("\nAverage solution time over %i runs: %e [s]\n", nrep, time_ipm);
284284
printf("\n\n");
285285

286286
/************************************************
@@ -390,7 +390,7 @@ int main()
390390

391391
double time_pred = hpipm_toc(&timer) / nrep;
392392

393-
printf("\nAverage prediction time over %i runs: %e [s]\n", nrep, time_pred);
393+
printf("\nAverage prediction time over %i runs: %e [s]\n", nrep, time_pred);
394394
printf("\n\n");
395395

396396
// predicted solution
@@ -429,9 +429,62 @@ int main()
429429
* sensitivity of solution of QP
430430
************************************************/
431431

432+
#if 1
433+
434+
// new sol struct
435+
void *seed_mem = malloc(qp_sol_size);
436+
struct d_ocp_qp_sol seed;
437+
d_ocp_qp_sol_create(&dim, &seed, seed_mem);
438+
439+
void *sens_mem = malloc(qp_sol_size);
440+
struct d_ocp_qp_sol sens;
441+
d_ocp_qp_sol_create(&dim, &sens, sens_mem);
442+
443+
// set seeds to zero
444+
d_ocp_qp_sol_set_zero(&seed);
445+
446+
// set I to param
447+
double *seed_x0 = malloc(nx[0]*sizeof(double));
448+
for(ii=0; ii<nx[0]; ii++)
449+
seed_x0[ii] = 0.0;
450+
int index = 0;
451+
seed_x0[index] = 1.0;
452+
int stage = 0;
453+
d_ocp_qp_sol_set_lam_lbx(stage, seed_x0, &seed);
454+
d_ocp_qp_sol_set_lam_ubx(stage, seed_x0, &seed);
455+
456+
// sensitivity solution
457+
d_ocp_qp_ipm_sens(&qp, &seed, &sens, &arg, &workspace);
458+
459+
// u
460+
printf("\nu_sens = \n");
461+
for(ii=0; ii<=N; ii++)
462+
{
463+
d_ocp_qp_sol_get_u(ii, &sens, u);
464+
d_print_mat(1, nu[ii], u, 1);
465+
}
466+
467+
// x
468+
printf("\nx_sens = \n");
469+
for(ii=0; ii<=N; ii++)
470+
{
471+
d_ocp_qp_sol_get_x(ii, &sens, x);
472+
d_print_mat(1, nx[ii], x, 1);
473+
}
474+
475+
// pi
476+
printf("\npi_sens = \n");
477+
for(ii=0; ii<N; ii++)
478+
{
479+
d_ocp_qp_sol_get_pi(ii, &sens, pi);
480+
d_print_mat(1, nx[ii+1], pi, 1);
481+
}
482+
483+
#else
484+
432485
void *qp2_mem = malloc(qp_size);
433486
struct d_ocp_qp qp2;
434-
d_ocp_qp_create(&dim, &qp2, qp1_mem);
487+
d_ocp_qp_create(&dim, &qp2, qp2_mem);
435488

436489
// new sol struct
437490
void *qp_sol2_mem = malloc(qp_sol_size);
@@ -483,17 +536,21 @@ int main()
483536
d_print_mat(1, nx[ii+1], pi, 1);
484537
}
485538

539+
#endif
540+
486541
/************************************************
487542
* free memory and return
488543
************************************************/
489544

490-
free(dim_mem);
491-
free(qp_mem);
492-
free(qp1_mem);
493-
free(qp2_mem);
545+
free(dim_mem);
546+
free(qp_mem);
547+
free(qp1_mem);
548+
//free(qp2_mem);
494549
free(qp_sol_mem);
495550
free(qp_sol1_mem);
496-
free(qp_sol2_mem);
551+
//free(qp_sol2_mem);
552+
free(seed_mem);
553+
free(sens_mem);
497554
free(ipm_arg_mem);
498555
free(ipm_mem);
499556

‎include/hpipm_d_ocp_qp_ipm.h‎

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -265,8 +265,8 @@ void d_ocp_qp_ipm_delta_step(int kk, struct d_ocp_qp *qp, struct d_ocp_qp_sol *q
265265
void d_ocp_qp_ipm_solve(struct d_ocp_qp *qp, struct d_ocp_qp_sol *qp_sol, struct d_ocp_qp_ipm_arg *arg, struct d_ocp_qp_ipm_ws *ws);
266266
//
267267
void d_ocp_qp_ipm_predict(struct d_ocp_qp *qp, struct d_ocp_qp_sol *qp_sol, struct d_ocp_qp_ipm_arg *arg, struct d_ocp_qp_ipm_ws *ws);
268-
//
269-
void d_ocp_qp_ipm_sens(struct d_ocp_qp *qp, struct d_ocp_qp_sol *qp_sol, struct d_ocp_qp_ipm_arg *arg, struct d_ocp_qp_ipm_ws *ws);
268+
// forward solution sensitivities
269+
void d_ocp_qp_ipm_sens_for(struct d_ocp_qp *qp, struct d_ocp_qp_sol *seed, struct d_ocp_qp_sol *sens, struct d_ocp_qp_ipm_arg *arg, struct d_ocp_qp_ipm_ws *ws);
270270
//
271271
void d_ocp_qp_ipm_sens_adj(struct d_ocp_qp *qp, struct d_ocp_qp_sol *qp_sol, struct d_ocp_qp_ipm_arg *arg, struct d_ocp_qp_ipm_ws *ws);
272272

‎include/hpipm_s_ocp_qp_ipm.h‎

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -265,8 +265,8 @@ void s_ocp_qp_ipm_delta_step(int kk, struct s_ocp_qp *qp, struct s_ocp_qp_sol *q
265265
void s_ocp_qp_ipm_solve(struct s_ocp_qp *qp, struct s_ocp_qp_sol *qp_sol, struct s_ocp_qp_ipm_arg *arg, struct s_ocp_qp_ipm_ws *ws);
266266
//
267267
void s_ocp_qp_ipm_predict(struct s_ocp_qp *qp, struct s_ocp_qp_sol *qp_sol, struct s_ocp_qp_ipm_arg *arg, struct s_ocp_qp_ipm_ws *ws);
268-
//
269-
void s_ocp_qp_ipm_sens(struct s_ocp_qp *qp, struct s_ocp_qp_sol *qp_sol, struct s_ocp_qp_ipm_arg *arg, struct s_ocp_qp_ipm_ws *ws);
268+
// forward solution sensitivities
269+
void s_ocp_qp_ipm_sens_for(struct s_ocp_qp *qp, struct s_ocp_qp_sol *seed, struct s_ocp_qp_sol *sens, struct s_ocp_qp_ipm_arg *arg, struct s_ocp_qp_ipm_ws *ws);
270270
//
271271
void s_ocp_qp_ipm_sens_adj(struct s_ocp_qp *qp, struct s_ocp_qp_sol *qp_sol, struct s_ocp_qp_ipm_arg *arg, struct s_ocp_qp_ipm_ws *ws);
272272

‎ocp_qp/d_ocp_qp_ipm.c‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -201,7 +201,7 @@
201201
#define OCP_QP_IPM_DELTA_STEP d_ocp_qp_ipm_delta_step
202202
#define OCP_QP_IPM_SOLVE d_ocp_qp_ipm_solve
203203
#define OCP_QP_IPM_PREDICT d_ocp_qp_ipm_predict
204-
#define OCP_QP_IPM_SENS d_ocp_qp_ipm_sens
204+
#define OCP_QP_IPM_SENS_FOR d_ocp_qp_ipm_sens_for
205205
#define OCP_QP_IPM_SENS_ADJ d_ocp_qp_ipm_sens_adj
206206

207207

‎ocp_qp/s_ocp_qp_ipm.c‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -201,7 +201,7 @@
201201
#define OCP_QP_IPM_DELTA_STEP s_ocp_qp_ipm_delta_step
202202
#define OCP_QP_IPM_SOLVE s_ocp_qp_ipm_solve
203203
#define OCP_QP_IPM_PREDICT s_ocp_qp_ipm_predict
204-
#define OCP_QP_IPM_SENS s_ocp_qp_ipm_sens
204+
#define OCP_QP_IPM_SENS_FOR s_ocp_qp_ipm_sens_for
205205
#define OCP_QP_IPM_SENS_ADJ s_ocp_qp_ipm_sens_adj
206206

207207

‎ocp_qp/x_ocp_qp_ipm.c‎

Lines changed: 65 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -3172,16 +3172,25 @@ void OCP_QP_IPM_PREDICT(struct OCP_QP *qp, struct OCP_QP_SOL *qp_sol, struct OCP
31723172

31733173

31743174

3175-
void OCP_QP_IPM_SENS(struct OCP_QP *qp, struct OCP_QP_SOL *qp_sol, struct OCP_QP_IPM_ARG *arg, struct OCP_QP_IPM_WS *ws)
3175+
void OCP_QP_IPM_SENS_FOR(struct OCP_QP *qp, struct OCP_QP_SOL *seed, struct OCP_QP_SOL *sens, struct OCP_QP_IPM_ARG *arg, struct OCP_QP_IPM_WS *ws)
31763176
{
31773177

31783178
#if 0
31793179
OCP_QP_DIM_PRINT(qp->dim);
31803180
OCP_QP_PRINT(qp->dim, qp);
3181+
OCP_QP_SOL_PRINT(qp->dim, seed);
31813182
#endif
31823183

31833184
int ii;
31843185

3186+
// dim
3187+
int N = qp->dim->N;
3188+
int *nx = qp->dim->nx;
3189+
int *nu = qp->dim->nu;
3190+
int *nb = qp->dim->nb;
3191+
int *ng = qp->dim->ng;
3192+
int *ns = qp->dim->ns;
3193+
31853194
struct CORE_QP_IPM_WORKSPACE *cws = ws->core_workspace;
31863195

31873196
// arg to core workspace
@@ -3191,10 +3200,10 @@ void OCP_QP_IPM_SENS(struct OCP_QP *qp, struct OCP_QP_SOL *qp_sol, struct OCP_QP
31913200
cws->t_lam_min = arg->t_lam_min;
31923201

31933202
// alias qp vectors into qp_sol
3194-
cws->v = qp_sol->ux->pa;
3195-
cws->pi = qp_sol->pi->pa;
3196-
cws->lam = qp_sol->lam->pa;
3197-
cws->t = qp_sol->t->pa;
3203+
cws->v = sens->ux->pa;
3204+
cws->pi = sens->pi->pa;
3205+
cws->lam = sens->lam->pa;
3206+
cws->t = sens->t->pa;
31983207

31993208
// load sol from bkp
32003209
for(ii=0; ii<cws->nv; ii++)
@@ -3206,9 +3215,59 @@ void OCP_QP_IPM_SENS(struct OCP_QP *qp, struct OCP_QP_SOL *qp_sol, struct OCP_QP
32063215
for(ii=0; ii<cws->nc; ii++)
32073216
cws->t[ii] = cws->t_bkp[ii];
32083217

3218+
// flip sign of seed lam
3219+
for(ii=0; ii<=N; ii++)
3220+
VECSC(nb[ii]+ng[ii], -1.0, seed->lam+ii, nb[ii]+ng[ii]);
3221+
3222+
// swap qp rhs with seed
3223+
{
3224+
struct STRVEC *tmp;
3225+
//
3226+
tmp = qp->rqz;
3227+
qp->rqz = seed->ux;
3228+
seed->ux = tmp;
3229+
//
3230+
tmp = qp->b;
3231+
qp->b = seed->pi;
3232+
seed->pi = tmp;
3233+
//
3234+
tmp = qp->d;
3235+
qp->d = seed->lam;
3236+
seed->lam = tmp;
3237+
//
3238+
tmp = qp->m;
3239+
qp->m = seed->t;
3240+
seed->t = tmp;
3241+
}
3242+
32093243
// solve kkt
32103244
ws->use_Pb = 0;
3211-
OCP_QP_SOLVE_KKT_STEP(qp, qp_sol, arg, ws);
3245+
OCP_QP_SOLVE_KKT_STEP(qp, sens, arg, ws);
3246+
3247+
// swap qp rhs with seed
3248+
{
3249+
struct STRVEC *tmp;
3250+
//
3251+
tmp = qp->rqz;
3252+
qp->rqz = seed->ux;
3253+
seed->ux = tmp;
3254+
//
3255+
tmp = qp->b;
3256+
qp->b = seed->pi;
3257+
seed->pi = tmp;
3258+
//
3259+
tmp = qp->d;
3260+
qp->d = seed->lam;
3261+
seed->lam = tmp;
3262+
//
3263+
tmp = qp->m;
3264+
qp->m = seed->t;
3265+
seed->t = tmp;
3266+
}
3267+
3268+
// restore sign of seed lam
3269+
for(ii=0; ii<=N; ii++)
3270+
VECSC(nb[ii]+ng[ii], -1.0, seed->lam+ii, nb[ii]+ng[ii]);
32123271

32133272
return;
32143273

0 commit comments

Comments
 (0)