Skip to content

Commit

Permalink
add dense qp example & some small interface edit
Browse files Browse the repository at this point in the history
  • Loading branch information
giaf committed Aug 10, 2023
1 parent a4d8266 commit 6b19249
Show file tree
Hide file tree
Showing 8 changed files with 176 additions and 51 deletions.
8 changes: 4 additions & 4 deletions dense_qp/d_dense_qp.c
Original file line number Diff line number Diff line change
Expand Up @@ -104,10 +104,10 @@
#define DENSE_QP_SET_ZZU d_dense_qp_set_Zu
#define DENSE_QP_SET_ZL d_dense_qp_set_zl
#define DENSE_QP_SET_ZU d_dense_qp_set_zu
#define DENSE_QP_SET_LS d_dense_qp_set_ls
#define DENSE_QP_SET_LS_MASK d_dense_qp_set_ls_mask
#define DENSE_QP_SET_US d_dense_qp_set_us
#define DENSE_QP_SET_US_MASK d_dense_qp_set_us_mask
#define DENSE_QP_SET_LLS d_dense_qp_set_lls
#define DENSE_QP_SET_LLS_MASK d_dense_qp_set_lls_mask
#define DENSE_QP_SET_LUS d_dense_qp_set_lus
#define DENSE_QP_SET_LUS_MASK d_dense_qp_set_lus_mask
#define DENSE_QP_GET_H d_dense_qp_get_H
#define DENSE_QP_GET_G d_dense_qp_get_g
#define DENSE_QP_GET_A d_dense_qp_get_A
Expand Down
8 changes: 4 additions & 4 deletions dense_qp/s_dense_qp.c
Original file line number Diff line number Diff line change
Expand Up @@ -107,10 +107,10 @@
#define DENSE_QP_SET_ZZU s_dense_qp_set_Zu
#define DENSE_QP_SET_ZL s_dense_qp_set_zl
#define DENSE_QP_SET_ZU s_dense_qp_set_zu
#define DENSE_QP_SET_LS s_dense_qp_set_ls
#define DENSE_QP_SET_LS_MASK s_dense_qp_set_ls_mask
#define DENSE_QP_SET_US s_dense_qp_set_us
#define DENSE_QP_SET_US_MASK s_dense_qp_set_us_mask
#define DENSE_QP_SET_LLS s_dense_qp_set_lls
#define DENSE_QP_SET_LLS_MASK s_dense_qp_set_lls_mask
#define DENSE_QP_SET_LUS s_dense_qp_set_lus
#define DENSE_QP_SET_LUS_MASK s_dense_qp_set_lus_mask
#define DENSE_QP_GET_H s_dense_qp_get_H
#define DENSE_QP_GET_G s_dense_qp_get_g
#define DENSE_QP_GET_A s_dense_qp_get_A
Expand Down
64 changes: 32 additions & 32 deletions dense_qp/x_dense_qp.c
Original file line number Diff line number Diff line change
Expand Up @@ -394,19 +394,19 @@ void DENSE_QP_SET(char *field, void *value, struct DENSE_QP *qp)
}
else if(hpipm_strcmp(field, "lls"))
{
DENSE_QP_SET_LS(value, qp);
DENSE_QP_SET_LLS(value, qp);
}
else if(hpipm_strcmp(field, "lls_mask"))
{
DENSE_QP_SET_LS_MASK(value, qp);
DENSE_QP_SET_LLS_MASK(value, qp);
}
else if(hpipm_strcmp(field, "lus"))
{
DENSE_QP_SET_US(value, qp);
DENSE_QP_SET_LUS(value, qp);
}
else if(hpipm_strcmp(field, "lus_mask"))
{
DENSE_QP_SET_US_MASK(value, qp);
DENSE_QP_SET_LUS_MASK(value, qp);
}
// int
else if(hpipm_strcmp(field, "idxb"))
Expand Down Expand Up @@ -558,13 +558,13 @@ void DENSE_QP_SET_LB_MASK(REAL *lb_mask, struct DENSE_QP *qp)
for(ii=0; ii<nb; ii++)
if(lb_mask[ii]==0.0)
#ifdef DOUBLE_PRECISION
BLASFEO_DVECEL(qp->d_mask, ii) = 0;
BLASFEO_DVECEL(qp->d_mask, ii) = 0.0;
else
BLASFEO_DVECEL(qp->d_mask, ii) = 1;
BLASFEO_DVECEL(qp->d_mask, ii) = 1.0;
#else
BLASFEO_SVECEL(qp->d_mask, ii) = 0;
BLASFEO_SVECEL(qp->d_mask, ii) = 0.0;
else
BLASFEO_SVECEL(qp->d_mask, ii) = 1;
BLASFEO_SVECEL(qp->d_mask, ii) = 1.0;
#endif

return;
Expand Down Expand Up @@ -600,13 +600,13 @@ void DENSE_QP_SET_UB_MASK(REAL *ub_mask, struct DENSE_QP *qp)
for(ii=0; ii<nb; ii++)
if(ub_mask[ii]==0.0)
#ifdef DOUBLE_PRECISION
BLASFEO_DVECEL(qp->d_mask, nb+ng+ii) = 0;
BLASFEO_DVECEL(qp->d_mask, nb+ng+ii) = 0.0;
else
BLASFEO_DVECEL(qp->d_mask, nb+ng+ii) = 1;
BLASFEO_DVECEL(qp->d_mask, nb+ng+ii) = 1.0;
#else
BLASFEO_SVECEL(qp->d_mask, nb+ng+ii) = 0;
BLASFEO_SVECEL(qp->d_mask, nb+ng+ii) = 0.0;
else
BLASFEO_SVECEL(qp->d_mask, nb+ng+ii) = 1;
BLASFEO_SVECEL(qp->d_mask, nb+ng+ii) = 1.0;
#endif

return;
Expand Down Expand Up @@ -655,13 +655,13 @@ void DENSE_QP_SET_LG_MASK(REAL *lg_mask, struct DENSE_QP *qp)
for(ii=0; ii<ng; ii++)
if(lg_mask[ii]==0.0)
#ifdef DOUBLE_PRECISION
BLASFEO_DVECEL(qp->d_mask, nb+ii) = 0;
BLASFEO_DVECEL(qp->d_mask, nb+ii) = 0.0;
else
BLASFEO_DVECEL(qp->d_mask, nb+ii) = 1;
BLASFEO_DVECEL(qp->d_mask, nb+ii) = 1.0;
#else
BLASFEO_SVECEL(qp->d_mask, nb+ii) = 0;
BLASFEO_SVECEL(qp->d_mask, nb+ii) = 0.0;
else
BLASFEO_SVECEL(qp->d_mask, nb+ii) = 1;
BLASFEO_SVECEL(qp->d_mask, nb+ii) = 1.0;
#endif

return;
Expand Down Expand Up @@ -697,13 +697,13 @@ void DENSE_QP_SET_UG_MASK(REAL *ug_mask, struct DENSE_QP *qp)
for(ii=0; ii<ng; ii++)
if(ug_mask[ii]==0.0)
#ifdef DOUBLE_PRECISION
BLASFEO_DVECEL(qp->d_mask, 2*nb+ng+ii) = 0;
BLASFEO_DVECEL(qp->d_mask, 2*nb+ng+ii) = 0.0;
else
BLASFEO_DVECEL(qp->d_mask, 2*nb+ng+ii) = 1;
BLASFEO_DVECEL(qp->d_mask, 2*nb+ng+ii) = 1.0;
#else
BLASFEO_SVECEL(qp->d_mask, 2*nb+ng+ii) = 0;
BLASFEO_SVECEL(qp->d_mask, 2*nb+ng+ii) = 0.0;
else
BLASFEO_SVECEL(qp->d_mask, 2*nb+ng+ii) = 1;
BLASFEO_SVECEL(qp->d_mask, 2*nb+ng+ii) = 1.0;
#endif

return;
Expand Down Expand Up @@ -855,7 +855,7 @@ void DENSE_QP_SET_ZU(REAL *zu, struct DENSE_QP *qp)
}


void DENSE_QP_SET_LS(REAL *ls, struct DENSE_QP *qp)
void DENSE_QP_SET_LLS(REAL *ls, struct DENSE_QP *qp)
{

int ns = qp->dim->ns;
Expand All @@ -871,7 +871,7 @@ void DENSE_QP_SET_LS(REAL *ls, struct DENSE_QP *qp)



void DENSE_QP_SET_LS_MASK(REAL *ls_mask, struct DENSE_QP *qp)
void DENSE_QP_SET_LLS_MASK(REAL *ls_mask, struct DENSE_QP *qp)
{

int nb = qp->dim->nb;
Expand All @@ -883,13 +883,13 @@ void DENSE_QP_SET_LS_MASK(REAL *ls_mask, struct DENSE_QP *qp)
for(ii=0; ii<ns; ii++)
if(ls_mask[ii]==0.0)
#ifdef DOUBLE_PRECISION
BLASFEO_DVECEL(qp->d_mask, 2*nb+2*ng+ii) = 0;
BLASFEO_DVECEL(qp->d_mask, 2*nb+2*ng+ii) = 0.0;
else
BLASFEO_DVECEL(qp->d_mask, 2*nb+2*ng+ii) = 1;
BLASFEO_DVECEL(qp->d_mask, 2*nb+2*ng+ii) = 1.0;
#else
BLASFEO_SVECEL(qp->d_mask, 2*nb+2*ng+ii) = 0;
BLASFEO_SVECEL(qp->d_mask, 2*nb+2*ng+ii) = 0.0;
else
BLASFEO_SVECEL(qp->d_mask, 2*nb+2*ng+ii) = 1;
BLASFEO_SVECEL(qp->d_mask, 2*nb+2*ng+ii) = 1.0;
#endif

return;
Expand All @@ -898,7 +898,7 @@ void DENSE_QP_SET_LS_MASK(REAL *ls_mask, struct DENSE_QP *qp)



void DENSE_QP_SET_US(REAL *us, struct DENSE_QP *qp)
void DENSE_QP_SET_LUS(REAL *us, struct DENSE_QP *qp)
{

int ns = qp->dim->ns;
Expand All @@ -914,7 +914,7 @@ void DENSE_QP_SET_US(REAL *us, struct DENSE_QP *qp)



void DENSE_QP_SET_US_MASK(REAL *us_mask, struct DENSE_QP *qp)
void DENSE_QP_SET_LUS_MASK(REAL *us_mask, struct DENSE_QP *qp)
{

int nb = qp->dim->nb;
Expand All @@ -926,13 +926,13 @@ void DENSE_QP_SET_US_MASK(REAL *us_mask, struct DENSE_QP *qp)
for(ii=0; ii<ns; ii++)
if(us_mask[ii]==0.0)
#ifdef DOUBLE_PRECISION
BLASFEO_DVECEL(qp->d_mask, 2*nb+2*ng+ns+ii) = 0;
BLASFEO_DVECEL(qp->d_mask, 2*nb+2*ng+ns+ii) = 0.0;
else
BLASFEO_DVECEL(qp->d_mask, 2*nb+2*ng+ns+ii) = 1;
BLASFEO_DVECEL(qp->d_mask, 2*nb+2*ng+ns+ii) = 1.0;
#else
BLASFEO_SVECEL(qp->d_mask, 2*nb+2*ng+ns+ii) = 0;
BLASFEO_SVECEL(qp->d_mask, 2*nb+2*ng+ns+ii) = 0.0;
else
BLASFEO_SVECEL(qp->d_mask, 2*nb+2*ng+ns+ii) = 1;
BLASFEO_SVECEL(qp->d_mask, 2*nb+2*ng+ns+ii) = 1.0;
#endif

return;
Expand Down
9 changes: 8 additions & 1 deletion examples/c/Makefile
Original file line number Diff line number Diff line change
Expand Up @@ -49,7 +49,7 @@ OCP_QP_DATA = data/mass_spring_qp_data.o
#OCP_QCQP_DATA = data/pmsm_ocp_qcqp_data.o
#OCP_QCQP_DATA = data/ocp_qcqp_data.o
#OCP_QCQP_DATA = data/mass_spring_qcqp_data.o

#DENSE_QP_DATA = data/getting_started_dense_qp_data.o

### select one single example ###

Expand All @@ -63,6 +63,7 @@ OBJS_EXMP = $(OCP_QP_DATA) example_d_ocp_qp_x0emb.o
#OBJS_EXMP = $(OCP_QCQP_DATA) example_d_ocp_qcqp_x0emb_part_cond.o
#OBJS_EXMP = $(OCP_QP_DATA) example_d_ocp_qp_sens.o
#OBJS_EXMP = $(OCP_QP_DATA) example_d_ocp_qp_part_cond_sens.o
#OBJS_EXMP = $(DENSE_QP_DATA) example_d_dense_qp.o

obj: $(OBJS_EXMP)
$(CC) -o example.out $(OBJS_EXMP) $(LIBS)
Expand All @@ -86,6 +87,12 @@ example_d_ocp_qcqp_part_cond \
$(CC) -o $@.out $^ $(LIBS)
./$@.out

# dense qp data
example_d_dense_qp \
: %: $(DENSE_QP_DATA) %.o
$(CC) -o $@.out $^ $(LIBS)
./$@.out


### clean ###
clean:
Expand Down
118 changes: 118 additions & 0 deletions examples/c/data/getting_started_dense_qp_data.c
Original file line number Diff line number Diff line change
@@ -0,0 +1,118 @@
/***************
* dim
***************/
/* nv */
int nv = 2;
/* ne */
int ne = 1;
/* nb */
int nb = 2;
/* ng */
int ng = 0;
/* nsb */
int nsb = 0;
/* nsg */
int nsg = 0;
/* ns */
int ns = 0;
/***************
* qp
***************/
/* H */
static double HH[] = {4.000000000000000e+00, 1.000000000000000e+00, 1.000000000000000e+00, 2.000000000000000e+00, };
double *H = HH;
/* A */
static double AA[] = {1.000000000000000e+00, 1.000000000000000e+00, };
double *A = AA;
/* C */
static double CC[] = {};
double *C = CC;
/* idxb */
static int iidxb[] = {0, 1, };
int *idxb = iidxb;
/* g */
static double gg[] = {1.000000000000000e+00, 1.000000000000000e+00, };
double *g = gg;
/* zl */
static double zzl[] = {};
double *zl = zzl;
/* zu */
static double zzu[] = {};
double *zu = zzu;
/* b */
static double bb[] = {1.000000000000000e+00, };
double *b = bb;
/* lb */
static double llb[] = {-1.000000000000000e+00, -1.000000000000000e+00, };
double *lb = llb;
/* lb_mask */
static double llb_mask[] = {1.000000000000000e+00, 1.000000000000000e+00, };
double *lb_mask = llb_mask;
/* ub */
static double uub[] = {1.500000000000000e+00, 5.000000000000000e-01, };
double *ub = uub;
/* ub_mask */
static double uub_mask[] = {1.000000000000000e+00, 1.000000000000000e+00, };
double *ub_mask = uub_mask;
/* lg */
static double llg[] = {};
double *lg = llg;
/* lg_mask */
static double llg_mask[] = {};
double *lg_mask = llg_mask;
/* ug */
static double uug[] = {};
double *ug = uug;
/* ug_mask */
static double uug_mask[] = {};
double *ug_mask = uug_mask;
/* lls */
static double llls[] = {};
double *lls = llls;
/* lls_mask */
static double llls_mask[] = {};
double *lls_mask = llls_mask;
/* lus */
static double llus[] = {};
double *lus = llus;
/* lus_mask */
static double llus_mask[] = {};
double *lus_mask = llus_mask;
/* Zl */
static double ZZl[] = {};
double *Zl = ZZl;
/* Zu */
static double ZZu[] = {};
double *Zu = ZZu;
/* idxs_rev */
static int iidxs_rev[] = {-1, -1, };
int *idxs_rev = iidxs_rev;
/***************
* arg
***************/
/* mode */
int mode = 1;
/* iter_max */
int iter_max = 20;
/* alpha_min */
double alpha_min = 1.000000000000000e-12;
/* mu0 */
double mu0 = 1.000000000000000e+01;
/* tol_stat */
double tol_stat = 1.000000000000000e-12;
/* tol_eq */
double tol_eq = 1.000000000000000e-12;
/* tol_ineq */
double tol_ineq = 1.000000000000000e-12;
/* tol_comp */
double tol_comp = 1.000000000000000e-12;
/* reg_prim */
double reg_prim = 1.000000000000000e-15;
/* reg_dual */
double reg_dual = 1.000000000000000e-15;
/* warm_start */
int warm_start = 0;
/* pred_corr */
int pred_corr = 1;
/* split_step */
int split_step = 1;
8 changes: 4 additions & 4 deletions include/hpipm_d_dense_qp.h
Original file line number Diff line number Diff line change
Expand Up @@ -133,13 +133,13 @@ void d_dense_qp_set_zl(double *zl, struct d_dense_qp *qp);
//
void d_dense_qp_set_zu(double *zu, struct d_dense_qp *qp);
//
void d_dense_qp_set_ls(double *ls, struct d_dense_qp *qp);
void d_dense_qp_set_lls(double *ls, struct d_dense_qp *qp);
//
void d_dense_qp_set_ls_mask(double *ls, struct d_dense_qp *qp);
void d_dense_qp_set_lls_mask(double *ls, struct d_dense_qp *qp);
//
void d_dense_qp_set_us(double *us, struct d_dense_qp *qp);
void d_dense_qp_set_lus(double *us, struct d_dense_qp *qp);
//
void d_dense_qp_set_us_mask(double *us, struct d_dense_qp *qp);
void d_dense_qp_set_lus_mask(double *us, struct d_dense_qp *qp);

// getters - colmaj
//
Expand Down
8 changes: 4 additions & 4 deletions include/hpipm_s_dense_qp.h
Original file line number Diff line number Diff line change
Expand Up @@ -131,13 +131,13 @@ void s_dense_qp_set_zl(float *zl, struct s_dense_qp *qp);
//
void s_dense_qp_set_zu(float *zu, struct s_dense_qp *qp);
//
void s_dense_qp_set_ls(float *ls, struct s_dense_qp *qp);
void s_dense_qp_set_lls(float *ls, struct s_dense_qp *qp);
//
void s_dense_qp_set_ls_mask(float *ls, struct s_dense_qp *qp);
void s_dense_qp_set_lls_mask(float *ls, struct s_dense_qp *qp);
//
void s_dense_qp_set_us(float *us, struct s_dense_qp *qp);
void s_dense_qp_set_lus(float *us, struct s_dense_qp *qp);
//
void s_dense_qp_set_us_mask(float *us, struct s_dense_qp *qp);
void s_dense_qp_set_lus_mask(float *us, struct s_dense_qp *qp);

// getters - colmaj
//
Expand Down
4 changes: 2 additions & 2 deletions test_problems/test_d_dense.c
Original file line number Diff line number Diff line change
Expand Up @@ -319,8 +319,8 @@ int main()
d_dense_qp_set_zl(zl, &qp);
d_dense_qp_set_zu(zu, &qp);
d_dense_qp_set_idxs(idxs, &qp);
d_dense_qp_set_ls(d_ls, &qp);
d_dense_qp_set_us(d_us, &qp);
d_dense_qp_set_lls(d_ls, &qp);
d_dense_qp_set_lus(d_us, &qp);
#else
d_dense_qp_set_all(H, g, A, b, idxb, d_lb, d_ub, C, d_lg, d_ug, Zl, Zu, zl, zu, idxs, d_ls, d_us, &qp);
#endif
Expand Down

0 comments on commit 6b19249

Please sign in to comment.