Squashed 'third_party/hpipm/' content from commit c2c0261
Change-Id: I05e186a6e1e1f075aa629092e7ad86e6116ca711
git-subtree-dir: third_party/hpipm
git-subtree-split: c2c0261e8ded36f636d9c4390a2899bdc4c999cc
diff --git a/include/hpipm_d_cond.h b/include/hpipm_d_cond.h
new file mode 100644
index 0000000..c5bdbb8
--- /dev/null
+++ b/include/hpipm_d_cond.h
@@ -0,0 +1,58 @@
+/**************************************************************************************************
+* *
+* This file is part of HPIPM. *
+* *
+* HPIPM -- High Performance Interior Point Method. *
+* Copyright (C) 2017 by Gianluca Frison. *
+* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. *
+* All rights reserved. *
+* *
+* HPMPC is free software; you can redistribute it and/or *
+* modify it under the terms of the GNU Lesser General Public *
+* License as published by the Free Software Foundation; either *
+* version 2.1 of the License, or (at your option) any later version. *
+* *
+* HPMPC is distributed in the hope that it will be useful, *
+* but WITHOUT ANY WARRANTY; without even the implied warranty of *
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. *
+* See the GNU Lesser General Public License for more details. *
+* *
+* You should have received a copy of the GNU Lesser General Public *
+* License along with HPMPC; if not, write to the Free Software *
+* Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA *
+* *
+* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de *
+* *
+**************************************************************************************************/
+
+
+
+#include <blasfeo_target.h>
+#include <blasfeo_common.h>
+
+
+
+struct d_cond_qp_ocp2dense_workspace
+ {
+ struct d_strmat *Gamma;
+ struct d_strmat *L;
+ struct d_strmat *Lx;
+ struct d_strmat *AL;
+ struct d_strvec *Gammab;
+ struct d_strvec *tmp_ngM;
+ struct d_strvec *tmp_nuxM;
+ int memsize;
+ };
+
+
+
+//
+void d_compute_qp_size_ocp2dense(int N, int *nx, int *nu, int *nb, int **idxb, int *ng, int *nvc, int *nec, int *nbc, int *ngc);
+//
+int d_memsize_cond_qp_ocp2dense(struct d_ocp_qp *ocp_qp, struct d_dense_qp *dense_qp); // XXX + args for algorithm type ???
+//
+void d_create_cond_qp_ocp2dense(struct d_ocp_qp *ocp_qp, struct d_dense_qp *dense_qp, struct d_cond_qp_ocp2dense_workspace *cond_ws, void *mem);
+//
+void d_cond_qp_ocp2dense(struct d_ocp_qp *ocp_qp, struct d_dense_qp *dense_qp, struct d_cond_qp_ocp2dense_workspace *cond_ws);
+//
+void d_expand_sol_dense2ocp(struct d_ocp_qp *ocp_qp, struct d_dense_qp_sol *dense_qp_sol, struct d_ocp_qp_sol *ocp_qp_sol, struct d_cond_qp_ocp2dense_workspace *cond_ws);
diff --git a/include/hpipm_d_cond_aux.h b/include/hpipm_d_cond_aux.h
new file mode 100644
index 0000000..88548b7
--- /dev/null
+++ b/include/hpipm_d_cond_aux.h
@@ -0,0 +1,42 @@
+/**************************************************************************************************
+* *
+* This file is part of HPIPM. *
+* *
+* HPIPM -- High Performance Interior Point Method. *
+* Copyright (C) 2017 by Gianluca Frison. *
+* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. *
+* All rights reserved. *
+* *
+* HPMPC is free software; you can redistribute it and/or *
+* modify it under the terms of the GNU Lesser General Public *
+* License as published by the Free Software Foundation; either *
+* version 2.1 of the License, or (at your option) any later version. *
+* *
+* HPMPC is distributed in the hope that it will be useful, *
+* but WITHOUT ANY WARRANTY; without even the implied warranty of *
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. *
+* See the GNU Lesser General Public License for more details. *
+* *
+* You should have received a copy of the GNU Lesser General Public *
+* License along with HPMPC; if not, write to the Free Software *
+* Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA *
+* *
+* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de *
+* *
+**************************************************************************************************/
+
+
+
+#include <blasfeo_target.h>
+#include <blasfeo_common.h>
+
+
+
+//
+void d_compute_Gamma(struct d_ocp_qp *ocp_qp, struct d_cond_qp_ocp2dense_workspace *cond_ws);
+//
+void d_cond_RSQrq_N2nx3(struct d_ocp_qp *ocp_qp, struct d_strmat *RSQrq2, struct d_strvec *rq2, struct d_cond_qp_ocp2dense_workspace *cond_ws);
+//
+void d_cond_DCtd(struct d_ocp_qp *ocp_qp, int *idxb2, struct d_strvec *d_lb2, struct d_strvec *d_ub2, struct d_strmat *DCt2, struct d_strvec *d_lg2, struct d_strvec *d_ug2, struct d_cond_qp_ocp2dense_workspace *cond_ws);
+//
+void d_expand_sol(struct d_ocp_qp *ocp_qp, struct d_dense_qp_sol *dense_qp_sol, struct d_strvec *ux, struct d_strvec *pi, struct d_strvec *lam_lb, struct d_strvec *lam_ub, struct d_strvec *lam_lg, struct d_strvec *lam_ug, struct d_strvec *t_lb, struct d_strvec *t_ub, struct d_strvec *t_lg, struct d_strvec *t_ug, struct d_strvec *tmp_nuxM, struct d_strvec *tmp_ngM);
diff --git a/include/hpipm_d_core_qp_ipm_hard.h b/include/hpipm_d_core_qp_ipm_hard.h
new file mode 100644
index 0000000..ce200ca
--- /dev/null
+++ b/include/hpipm_d_core_qp_ipm_hard.h
@@ -0,0 +1,109 @@
+/**************************************************************************************************
+* *
+* This file is part of HPIPM. *
+* *
+* HPIPM -- High Performance Interior Point Method. *
+* Copyright (C) 2017 by Gianluca Frison. *
+* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. *
+* All rights reserved. *
+* *
+* HPMPC is free software; you can redistribute it and/or *
+* modify it under the terms of the GNU Lesser General Public *
+* License as published by the Free Software Foundation; either *
+* version 2.1 of the License, or (at your option) any later version. *
+* *
+* HPMPC is distributed in the hope that it will be useful, *
+* but WITHOUT ANY WARRANTY; without even the implied warranty of *
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. *
+* See the GNU Lesser General Public License for more details. *
+* *
+* You should have received a copy of the GNU Lesser General Public *
+* License along with HPMPC; if not, write to the Free Software *
+* Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA *
+* *
+* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de *
+* *
+**************************************************************************************************/
+
+
+
+struct d_ipm_hard_core_qp_workspace
+ {
+ double *d; // constraints
+ double *d_lb; // lower box constraints
+ double *d_ub; // upper box constraints
+ double *d_lg; // lower general constraints
+ double *d_ug; // upper general constraints
+ double *v; // primal variables
+ double *pi; // equality constraints multipliers
+ double *lam; // inequality constraints multipliers
+ double *lam_lb; // lower bounds multipliers
+ double *lam_lg; // lower general constraints multipliers
+ double *lam_ub; // upper bounds multipliers
+ double *lam_ug; // upper general constraints multipliers
+ double *t; // inequality constraints slacks
+ double *t_lb; // lower box constraints slacks
+ double *t_lg; // lower general constraints slacks
+ double *t_ub; // upper box constraints slacks
+ double *t_ug; // upper general constraints slacks
+ double *t_inv; // inverse of t
+ double *t_inv_lb; // inverse of t
+ double *t_inv_ub; // inverse of t
+ double *t_inv_lg; // inverse of t
+ double *t_inv_ug; // inverse of t
+ double *dv; // step in v
+ double *dpi; // step in pi
+ double *dlam; // step in lam
+ double *dlam_lb; //
+ double *dlam_lg; //
+ double *dlam_ub; //
+ double *dlam_ug; //
+ double *dt; // step in t
+ double *dt_lb; // step in t_lb
+ double *dt_ub; // step in t_ub
+ double *dt_lg; // step in t_lg
+ double *dt_ug; // step in t_ug
+ double *res_g; // q-residuals
+ double *res_b; // b-residuals
+ double *res_d; // d-residuals
+ double *res_d_lb; // d-residuals
+ double *res_d_ub; // d-residuals
+ double *res_d_lg; // d-residuals
+ double *res_d_ug; // d-residuals
+ double *res_m; // m-residuals
+ double *res_m_lb; // m-residuals
+ double *res_m_ub; // m-residuals
+ double *res_m_lg; // m-residuals
+ double *res_m_ug; // m-residuals
+ double *Qx; // Hessian update
+ double *Qx_lb; // Hessian update
+ double *Qx_lg; // Hessian update
+ double *qx; // gradient update
+ double *qx_lb; // gradient update
+ double *qx_lg; // gradient update
+ double *stat; // convergence statistics
+ double alpha; // step length
+ double alpha_min; // exit cond on step lenght
+ double sigma; // centering XXX
+ double mu; // duality measuere
+ double mu_aff; // affine duality measuere
+ double mu0; // initial duality measuere
+ double mu_max; // exit cond on mu
+ double nt_inv; // 1.0/nt, where nt is the total number of constraints
+ int nv; // number of primal variables
+ int ne; // number of equality constraints
+ int nb; // number of two-sized bounds
+ int ng; // number of two-sized constraints
+ int iter_max; // exit cond on iter mumber
+ int memsize; // memory size (in bytes) of workspace
+ };
+
+
+
+//
+int d_memsize_ipm_hard_core_qp(int nv, int ne, int nb, int ng, int iter_max);
+//
+
+void d_create_ipm_hard_core_qp(struct d_ipm_hard_core_qp_workspace *workspace, void *mem);
+//
+void d_ipm_hard_core_qp(struct d_ipm_hard_core_qp_workspace *workspace);
diff --git a/include/hpipm_d_core_qp_ipm_hard_aux.h b/include/hpipm_d_core_qp_ipm_hard_aux.h
new file mode 100644
index 0000000..9d59737
--- /dev/null
+++ b/include/hpipm_d_core_qp_ipm_hard_aux.h
@@ -0,0 +1,41 @@
+/**************************************************************************************************
+* *
+* This file is part of HPIPM. *
+* *
+* HPIPM -- High Performance Interior Point Method. *
+* Copyright (C) 2017 by Gianluca Frison. *
+* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. *
+* All rights reserved. *
+* *
+* HPMPC is free software; you can redistribute it and/or *
+* modify it under the terms of the GNU Lesser General Public *
+* License as published by the Free Software Foundation; either *
+* version 2.1 of the License, or (at your option) any later version. *
+* *
+* HPMPC is distributed in the hope that it will be useful, *
+* but WITHOUT ANY WARRANTY; without even the implied warranty of *
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. *
+* See the GNU Lesser General Public License for more details. *
+* *
+* You should have received a copy of the GNU Lesser General Public *
+* License along with HPMPC; if not, write to the Free Software *
+* Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA *
+* *
+* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de *
+* *
+**************************************************************************************************/
+
+//
+void d_compute_Qx_qx_hard_qp(struct d_ipm_hard_core_qp_workspace *rws);
+//
+void d_compute_lam_t_hard_qp(struct d_ipm_hard_core_qp_workspace *rws);
+//
+void d_compute_alpha_hard_qp(struct d_ipm_hard_core_qp_workspace *rws);
+//
+void d_update_var_hard_qp(struct d_ipm_hard_core_qp_workspace *rws);
+//
+void d_compute_mu_aff_hard_qp(struct d_ipm_hard_core_qp_workspace *rws);
+//
+void d_compute_centering_correction_hard_qp(struct d_ipm_hard_core_qp_workspace *rws);
+//
+void d_compute_qx_hard_qp(struct d_ipm_hard_core_qp_workspace *rws);
diff --git a/include/hpipm_d_dense_qp.h b/include/hpipm_d_dense_qp.h
new file mode 100644
index 0000000..ba60ab4
--- /dev/null
+++ b/include/hpipm_d_dense_qp.h
@@ -0,0 +1,73 @@
+/**************************************************************************************************
+* *
+* This file is part of HPIPM. *
+* *
+* HPIPM -- High Performance Interior Point Method. *
+* Copyright (C) 2017 by Gianluca Frison. *
+* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. *
+* All rights reserved. *
+* *
+* HPMPC is free software; you can redistribute it and/or *
+* modify it under the terms of the GNU Lesser General Public *
+* License as published by the Free Software Foundation; either *
+* version 2.1 of the License, or (at your option) any later version. *
+* *
+* HPMPC is distributed in the hope that it will be useful, *
+* but WITHOUT ANY WARRANTY; without even the implied warranty of *
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. *
+* See the GNU Lesser General Public License for more details. *
+* *
+* You should have received a copy of the GNU Lesser General Public *
+* License along with HPMPC; if not, write to the Free Software *
+* Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA *
+* *
+* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de *
+* *
+**************************************************************************************************/
+
+
+
+#include <blasfeo_target.h>
+#include <blasfeo_common.h>
+
+
+
+struct d_dense_qp
+ {
+ struct d_strmat *Hg; // hessian & gradient
+ struct d_strmat *A; // dynamics matrix
+ struct d_strmat *Ct; // constraints matrix
+ struct d_strvec *g; // gradient
+ struct d_strvec *b; // dynamics vector
+ struct d_strvec *d; // constraint
+ struct d_strvec *d_lb; // lower box constraint
+ struct d_strvec *d_ub; // upper box constraint
+ struct d_strvec *d_lg; // lower general constraint
+ struct d_strvec *d_ug; // upper general constraint
+ int *idxb; // index of box constraints
+ int nv; // number of variables
+ int ne; // number of equality constraints
+ int nb; // number of box constraints
+ int ng; // number of general constraints
+ int memsize; // memory size in bytes
+ };
+
+
+
+//
+int d_memsize_dense_qp(int nv, int ne, int nb, int ng);
+//
+void d_create_dense_qp(int nv, int ne, int nb, int ng, struct d_dense_qp *qp, void *memory);
+//
+void d_cvt_colmaj_to_dense_qp(double *H, double *g, double *A, double *b, int *idxb, double *d_lb, double *d_ub, double *C, double *d_lg, double *d_ug, struct d_dense_qp *qp);
+//
+void d_cvt_dense_qp_to_colmaj(struct d_dense_qp *qp, double *H, double *g, double *A, double *b, int *idxb, double *d_lb, double *d_ub, double *C, double *d_lg, double *d_ug);
+//
+void d_cvt_rowmaj_to_dense_qp(double *H, double *g, double *A, double *b, int *idxb, double *d_lb, double *d_ub, double *C, double *d_lg, double *d_ug, struct d_dense_qp *qp);
+//
+void d_cvt_dense_qp_to_rowmaj(struct d_dense_qp *qp, double *H, double *g, double *A, double *b, int *idxb, double *d_lb, double *d_ub, double *C, double *d_lg, double *d_ug);
+//
+void d_cvt_libstr_to_dense_qp(struct d_strmat *H, struct d_strmat *A, struct d_strmat *C, struct d_strvec *g, struct d_strvec *b, struct d_strvec *d_lb, struct d_strvec *d_ub, struct d_strvec *d_lg, struct d_strvec *d_ug, int *idxb, struct d_dense_qp *qp);
+//
+void d_cvt_dense_qp_to_libstr(struct d_dense_qp *qp, struct d_strmat *H, struct d_strmat *A, struct d_strmat *C, struct d_strvec *g, struct d_strvec *b, struct d_strvec *d_lb, struct d_strvec *d_ub, struct d_strvec *d_lg, struct d_strvec *d_ug, int *idxb);
+
diff --git a/include/hpipm_d_dense_qp_ipm_hard.h b/include/hpipm_d_dense_qp_ipm_hard.h
new file mode 100644
index 0000000..e5ef65b
--- /dev/null
+++ b/include/hpipm_d_dense_qp_ipm_hard.h
@@ -0,0 +1,89 @@
+/**************************************************************************************************
+* *
+* This file is part of HPIPM. *
+* *
+* HPIPM -- High Performance Interior Point Method. *
+* Copyright (C) 2017 by Gianluca Frison. *
+* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. *
+* All rights reserved. *
+* *
+* HPMPC is free software; you can redistribute it and/or *
+* modify it under the terms of the GNU Lesser General Public *
+* License as published by the Free Software Foundation; either *
+* version 2.1 of the License, or (at your option) any later version. *
+* *
+* HPMPC is distributed in the hope that it will be useful, *
+* but WITHOUT ANY WARRANTY; without even the implied warranty of *
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. *
+* See the GNU Lesser General Public License for more details. *
+* *
+* You should have received a copy of the GNU Lesser General Public *
+* License along with HPMPC; if not, write to the Free Software *
+* Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA *
+* *
+* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de *
+* *
+**************************************************************************************************/
+
+
+
+#include <blasfeo_target.h>
+#include <blasfeo_common.h>
+
+
+
+struct d_ipm_hard_dense_qp_workspace
+ {
+ struct d_ipm_hard_core_qp_workspace *core_workspace;
+ struct d_strvec *dv; // step in v
+ struct d_strvec *dpi; // step in pi
+ struct d_strvec *dlam; // step in lam XXX needed ???
+ struct d_strvec *dt; // step in t XXX needed ???
+ struct d_strvec *dt_lb; //
+ struct d_strvec *dt_ub; // XXX needed ???
+ struct d_strvec *dt_lg; //
+ struct d_strvec *dt_ug; // XXX needed ???
+ struct d_strvec *res_g; // q-residuals
+ struct d_strvec *res_b; // b-residuals
+ struct d_strvec *res_d; // d-residuals
+ struct d_strvec *res_d_lb; // d-residuals
+ struct d_strvec *res_d_ub; // d-residuals
+ struct d_strvec *res_d_lg; // d-residuals
+ struct d_strvec *res_d_ug; // d-residuals
+ struct d_strvec *res_m; // m-residuals
+ struct d_strvec *Qx; // hessian update
+ struct d_strvec *qx; // gradient update
+ struct d_strmat *Lv; //
+ struct d_strmat *AL; //
+ struct d_strmat *Le; //
+ struct d_strmat *Ctx; //
+ struct d_strvec *lv; //
+ struct d_strvec *tmp_nb; // work space of size nb
+ struct d_strvec *tmp_ng0; // work space of size nb
+ struct d_strvec *tmp_ng1; // work space of size nb
+ double *stat; // convergence statistics
+ double res_mu; // mu-residual
+ int iter; // iteration number
+ int memsize; // memory size (in bytes) of workspace
+ };
+
+
+
+struct d_ipm_hard_dense_qp_arg
+ {
+ double alpha_min; // exit cond on step length
+ double mu_max; // exit cond on duality measure
+ double mu0; // initial value for duality measure
+ int iter_max; // exit cond in iter number
+ };
+
+
+
+//
+int d_memsize_ipm_hard_dense_qp(struct d_dense_qp *qp, struct d_ipm_hard_dense_qp_arg *arg);
+//
+void d_create_ipm_hard_dense_qp(struct d_dense_qp *qp, struct d_ipm_hard_dense_qp_arg *arg, struct d_ipm_hard_dense_qp_workspace *ws, void *mem);
+//
+void d_solve_ipm_hard_dense_qp(struct d_dense_qp *qp, struct d_dense_qp_sol *qp_sol, struct d_ipm_hard_dense_qp_workspace *ws);
+//
+void d_solve_ipm2_hard_dense_qp(struct d_dense_qp *qp, struct d_dense_qp_sol *qp_sol, struct d_ipm_hard_dense_qp_workspace *ws);
diff --git a/include/hpipm_d_dense_qp_kkt.h b/include/hpipm_d_dense_qp_kkt.h
new file mode 100644
index 0000000..4721657
--- /dev/null
+++ b/include/hpipm_d_dense_qp_kkt.h
@@ -0,0 +1,39 @@
+/**************************************************************************************************
+* *
+* This file is part of HPIPM. *
+* *
+* HPIPM -- High Performance Interior Point Method. *
+* Copyright (C) 2017 by Gianluca Frison. *
+* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. *
+* All rights reserved. *
+* *
+* HPMPC is free software; you can redistribute it and/or *
+* modify it under the terms of the GNU Lesser General Public *
+* License as published by the Free Software Foundation; either *
+* version 2.1 of the License, or (at your option) any later version. *
+* *
+* HPMPC is distributed in the hope that it will be useful, *
+* but WITHOUT ANY WARRANTY; without even the implied warranty of *
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. *
+* See the GNU Lesser General Public License for more details. *
+* *
+* You should have received a copy of the GNU Lesser General Public *
+* License along with HPMPC; if not, write to the Free Software *
+* Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA *
+* *
+* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de *
+* *
+**************************************************************************************************/
+
+
+
+//
+void d_init_var_hard_dense_qp(struct d_dense_qp *qp, struct d_dense_qp_sol *qp_sol, struct d_ipm_hard_dense_qp_workspace *ws);
+//
+void d_compute_res_hard_dense_qp(struct d_dense_qp *qp, struct d_dense_qp_sol *qp_sol, struct d_ipm_hard_dense_qp_workspace *ws);
+//
+void d_fact_solve_kkt_unconstr_dense_qp(struct d_dense_qp *qp, struct d_dense_qp_sol *qp_sol, struct d_ipm_hard_dense_qp_workspace *ws);
+//
+void d_fact_solve_kkt_step_hard_dense_qp(struct d_dense_qp *qp, struct d_ipm_hard_dense_qp_workspace *ws);
+//
+void d_solve_kkt_step_hard_dense_qp(struct d_dense_qp *qp, struct d_ipm_hard_dense_qp_workspace *ws);
diff --git a/include/hpipm_d_dense_qp_sol.h b/include/hpipm_d_dense_qp_sol.h
new file mode 100644
index 0000000..0348e10
--- /dev/null
+++ b/include/hpipm_d_dense_qp_sol.h
@@ -0,0 +1,61 @@
+/**************************************************************************************************
+* *
+* This file is part of HPIPM. *
+* *
+* HPIPM -- High Performance Interior Point Method. *
+* Copyright (C) 2017 by Gianluca Frison. *
+* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. *
+* All rights reserved. *
+* *
+* HPMPC is free software; you can redistribute it and/or *
+* modify it under the terms of the GNU Lesser General Public *
+* License as published by the Free Software Foundation; either *
+* version 2.1 of the License, or (at your option) any later version. *
+* *
+* HPMPC is distributed in the hope that it will be useful, *
+* but WITHOUT ANY WARRANTY; without even the implied warranty of *
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. *
+* See the GNU Lesser General Public License for more details. *
+* *
+* You should have received a copy of the GNU Lesser General Public *
+* License along with HPMPC; if not, write to the Free Software *
+* Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA *
+* *
+* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de *
+* *
+**************************************************************************************************/
+
+
+
+#include <blasfeo_target.h>
+#include <blasfeo_common.h>
+
+
+
+struct d_dense_qp_sol
+ {
+ struct d_strvec *v;
+ struct d_strvec *pi;
+ struct d_strvec *lam_lb;
+ struct d_strvec *lam_ub;
+ struct d_strvec *lam_lg;
+ struct d_strvec *lam_ug;
+ struct d_strvec *t_lb;
+ struct d_strvec *t_ub;
+ struct d_strvec *t_lg;
+ struct d_strvec *t_ug;
+ int memsize;
+ };
+
+
+
+//
+int d_memsize_dense_qp_sol(int nv, int ne, int nb, int ng);
+//
+void d_create_dense_qp_sol(int nv, int ne, int nb, int ng, struct d_dense_qp_sol *qp_sol, void *memory);
+//
+void d_cvt_dense_qp_sol_to_colmaj(struct d_dense_qp *qp, struct d_dense_qp_sol *qp_sol, double *v, double *pi, double *lam_lb, double *lam_ub, double *lam_lg, double *lam_ug);
+//
+void d_cvt_dense_qp_sol_to_rowmaj(struct d_dense_qp *qp, struct d_dense_qp_sol *qp_sol, double *v, double *pi, double *lam_lb, double *lam_ub, double *lam_lg, double *lam_ug);
+//
+void d_cvt_dense_qp_sol_to_libstr(struct d_dense_qp *qp, struct d_dense_qp_sol *qp_sol, struct d_strvec *v, struct d_strvec *pi, struct d_strvec *lam_lb, struct d_strvec *lam_ub, struct d_strvec *lam_lg, struct d_strvec *lam_ug);
diff --git a/include/hpipm_d_ocp_qp.h b/include/hpipm_d_ocp_qp.h
new file mode 100644
index 0000000..ec4f767
--- /dev/null
+++ b/include/hpipm_d_ocp_qp.h
@@ -0,0 +1,68 @@
+/**************************************************************************************************
+* *
+* This file is part of HPIPM. *
+* *
+* HPIPM -- High Performance Interior Point Method. *
+* Copyright (C) 2017 by Gianluca Frison. *
+* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. *
+* All rights reserved. *
+* *
+* HPMPC is free software; you can redistribute it and/or *
+* modify it under the terms of the GNU Lesser General Public *
+* License as published by the Free Software Foundation; either *
+* version 2.1 of the License, or (at your option) any later version. *
+* *
+* HPMPC is distributed in the hope that it will be useful, *
+* but WITHOUT ANY WARRANTY; without even the implied warranty of *
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. *
+* See the GNU Lesser General Public License for more details. *
+* *
+* You should have received a copy of the GNU Lesser General Public *
+* License along with HPMPC; if not, write to the Free Software *
+* Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA *
+* *
+* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de *
+* *
+**************************************************************************************************/
+
+
+
+#include <blasfeo_target.h>
+#include <blasfeo_common.h>
+
+
+
+struct d_ocp_qp
+ {
+ struct d_strmat *BAbt;
+ struct d_strvec *b;
+ struct d_strmat *RSQrq;
+ struct d_strvec *rq;
+ struct d_strmat *DCt;
+ struct d_strvec *d_lb;
+ struct d_strvec *d_ub;
+ struct d_strvec *d_lg;
+ struct d_strvec *d_ug;
+ int *nx; // number of states
+ int *nu; // number of inputs
+ int *nb; // number of box constraints
+ int **idxb; // index of box constraints
+ int *ng; // number of general constraints
+ int N; // hotizon lenght
+ int memsize; // memory size in bytes
+ };
+
+
+
+//
+int d_memsize_ocp_qp(int N, int *nx, int *nu, int *nb, int *ng);
+//
+void d_create_ocp_qp(int N, int *nx, int *nu, int *nb, int *ng, struct d_ocp_qp *qp, void *memory);
+//
+void d_cvt_colmaj_to_ocp_qp(double **A, double **B, double **b, double **Q, double **S, double **R, double **q, double **r, int **idxb, double **lb, double **ub, double **C, double **D, double **lg, double **ug, struct d_ocp_qp *qp);
+//
+void d_cvt_rowmaj_to_ocp_qp(double **A, double **B, double **b, double **Q, double **S, double **R, double **q, double **r, int **idxb, double **lb, double **ub, double **C, double **D, double **lg, double **ug, struct d_ocp_qp *qp);
+//
+//void d_cast_ocp_qp(int N, int *nx, int *nu, int *nb, int **idxb, int *ng, struct d_strmat *sBAbt, struct d_strvec *sb, struct d_strmat *sRSQrq, struct d_strvec *srq, struct d_strmat *sDCt, struct d_strvec *slb, struct d_strvec *sub, struct d_strvec *slg, struct d_strvec *sug, struct d_ocp_qp *str_out);
+//
+void d_copy_ocp_qp(struct d_ocp_qp *qp_in, struct d_ocp_qp *qp_out);
diff --git a/include/hpipm_d_ocp_qp_ipm_hard.h b/include/hpipm_d_ocp_qp_ipm_hard.h
new file mode 100644
index 0000000..e3660d6
--- /dev/null
+++ b/include/hpipm_d_ocp_qp_ipm_hard.h
@@ -0,0 +1,91 @@
+/**************************************************************************************************
+* *
+* This file is part of HPIPM. *
+* *
+* HPIPM -- High Performance Interior Point Method. *
+* Copyright (C) 2017 by Gianluca Frison. *
+* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. *
+* All rights reserved. *
+* *
+* HPMPC is free software; you can redistribute it and/or *
+* modify it under the terms of the GNU Lesser General Public *
+* License as published by the Free Software Foundation; either *
+* version 2.1 of the License, or (at your option) any later version. *
+* *
+* HPMPC is distributed in the hope that it will be useful, *
+* but WITHOUT ANY WARRANTY; without even the implied warranty of *
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. *
+* See the GNU Lesser General Public License for more details. *
+* *
+* You should have received a copy of the GNU Lesser General Public *
+* License along with HPMPC; if not, write to the Free Software *
+* Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA *
+* *
+* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de *
+* *
+**************************************************************************************************/
+
+
+
+#include <blasfeo_target.h>
+#include <blasfeo_common.h>
+
+
+
+
+struct d_ipm_hard_ocp_qp_workspace
+ {
+ struct d_ipm_hard_core_qp_workspace *core_workspace;
+ struct d_strvec *dux;
+ struct d_strvec *dpi;
+ struct d_strvec *dt_lb;
+ struct d_strvec *dt_lg;
+ struct d_strvec *res_g; // q-residuals
+ struct d_strvec *res_b; // b-residuals
+ struct d_strvec *res_d; // d-residuals XXX remove ???
+ struct d_strvec *res_d_lb; // d-residuals
+ struct d_strvec *res_d_ub; // d-residuals
+ struct d_strvec *res_d_lg; // d-residuals
+ struct d_strvec *res_d_ug; // d-residuals
+ struct d_strvec *res_m; // m-residuals
+ struct d_strvec *res_m_lb; // m-residuals
+ struct d_strvec *res_m_ub; // m-residuals
+ struct d_strvec *res_m_lg; // m-residuals
+ struct d_strvec *res_m_ug; // m-residuals
+ struct d_strvec *Qx_lb; // hessian update
+ struct d_strvec *Qx_lg; // hessian update
+ struct d_strvec *qx_lb; // gradient update
+ struct d_strvec *qx_lg; // gradient update
+ struct d_strvec *tmp_nbM; // work space of size nbM
+ struct d_strvec *tmp_nxM; // work space of size nxM
+ struct d_strvec *tmp_ngM; // work space of size ngM
+ struct d_strvec *Pb; // Pb
+ struct d_strmat *L;
+ struct d_strmat *AL;
+ double *stat; // convergence statistics
+ double res_mu; // mu-residual
+ int iter; // iteration number
+ int memsize;
+ };
+
+
+
+struct d_ipm_hard_ocp_qp_arg
+ {
+ double alpha_min; // exit cond on step length
+ double mu_max; // exit cond on duality measure
+ double mu0; // initial value for duality measure
+ int iter_max; // exit cond in iter number
+ };
+
+
+
+//
+int d_memsize_ipm_hard_ocp_qp(struct d_ocp_qp *qp, struct d_ipm_hard_ocp_qp_arg *arg);
+//
+void d_create_ipm_hard_ocp_qp(struct d_ocp_qp *qp, struct d_ipm_hard_ocp_qp_arg *arg, struct d_ipm_hard_ocp_qp_workspace *ws, void *mem);
+//
+void d_solve_ipm_hard_ocp_qp(struct d_ocp_qp *qp, struct d_ocp_qp_sol *qp_sol, struct d_ipm_hard_ocp_qp_workspace *ws);
+//
+void d_solve_ipm2_hard_ocp_qp(struct d_ocp_qp *qp, struct d_ocp_qp_sol *qp_sol, struct d_ipm_hard_ocp_qp_workspace *ws);
+
diff --git a/include/hpipm_d_ocp_qp_kkt.h b/include/hpipm_d_ocp_qp_kkt.h
new file mode 100644
index 0000000..eb41bd4
--- /dev/null
+++ b/include/hpipm_d_ocp_qp_kkt.h
@@ -0,0 +1,40 @@
+/**************************************************************************************************
+* *
+* This file is part of HPIPM. *
+* *
+* HPIPM -- High Performance Interior Point Method. *
+* Copyright (C) 2017 by Gianluca Frison. *
+* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. *
+* All rights reserved. *
+* *
+* HPMPC is free software; you can redistribute it and/or *
+* modify it under the terms of the GNU Lesser General Public *
+* License as published by the Free Software Foundation; either *
+* version 2.1 of the License, or (at your option) any later version. *
+* *
+* HPMPC is distributed in the hope that it will be useful, *
+* but WITHOUT ANY WARRANTY; without even the implied warranty of *
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. *
+* See the GNU Lesser General Public License for more details. *
+* *
+* You should have received a copy of the GNU Lesser General Public *
+* License along with HPMPC; if not, write to the Free Software *
+* Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA *
+* *
+* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de *
+* *
+**************************************************************************************************/
+
+
+
+
+//
+void d_init_var_hard_ocp_qp(struct d_ocp_qp *qp, struct d_ocp_qp_sol *qp_sol, struct d_ipm_hard_ocp_qp_workspace *ws);
+//
+void d_compute_res_hard_ocp_qp(struct d_ocp_qp *qp, struct d_ocp_qp_sol *qp_sol, struct d_ipm_hard_ocp_qp_workspace *ws);
+//
+void d_fact_solve_kkt_unconstr_ocp_qp(struct d_ocp_qp *qp, struct d_ocp_qp_sol *qp_sol, struct d_ipm_hard_ocp_qp_workspace *ws);
+//
+void d_fact_solve_kkt_step_hard_ocp_qp(struct d_ocp_qp *qp, struct d_ipm_hard_ocp_qp_workspace *ws);
+//
+void d_solve_kkt_step_hard_ocp_qp(struct d_ocp_qp *qp, struct d_ipm_hard_ocp_qp_workspace *ws);
diff --git a/include/hpipm_d_ocp_qp_sol.h b/include/hpipm_d_ocp_qp_sol.h
new file mode 100644
index 0000000..73519ca
--- /dev/null
+++ b/include/hpipm_d_ocp_qp_sol.h
@@ -0,0 +1,61 @@
+/**************************************************************************************************
+* *
+* This file is part of HPIPM. *
+* *
+* HPIPM -- High Performance Interior Point Method. *
+* Copyright (C) 2017 by Gianluca Frison. *
+* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. *
+* All rights reserved. *
+* *
+* HPMPC is free software; you can redistribute it and/or *
+* modify it under the terms of the GNU Lesser General Public *
+* License as published by the Free Software Foundation; either *
+* version 2.1 of the License, or (at your option) any later version. *
+* *
+* HPMPC is distributed in the hope that it will be useful, *
+* but WITHOUT ANY WARRANTY; without even the implied warranty of *
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. *
+* See the GNU Lesser General Public License for more details. *
+* *
+* You should have received a copy of the GNU Lesser General Public *
+* License along with HPMPC; if not, write to the Free Software *
+* Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA *
+* *
+* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de *
+* *
+**************************************************************************************************/
+
+
+
+#include <blasfeo_target.h>
+#include <blasfeo_common.h>
+
+
+
+struct d_ocp_qp_sol
+ {
+ struct d_strvec *ux;
+ struct d_strvec *pi;
+ struct d_strvec *lam_lb;
+ struct d_strvec *lam_ub;
+ struct d_strvec *lam_lg;
+ struct d_strvec *lam_ug;
+ struct d_strvec *t_lb;
+ struct d_strvec *t_ub;
+ struct d_strvec *t_lg;
+ struct d_strvec *t_ug;
+ int memsize; // memory size in bytes
+ };
+
+
+
+//
+int d_memsize_ocp_qp_sol(int N, int *nx, int *nu, int *nb, int *ng);
+//
+void d_create_ocp_qp_sol(int N, int *nx, int *nu, int *nb, int *ng, struct d_ocp_qp_sol *qp_sol, void *memory);
+//
+void d_cvt_ocp_qp_sol_to_colmaj(struct d_ocp_qp *qp, struct d_ocp_qp_sol *qp_sol, double **u, double **x, double **pi, double **lam_lb, double **lam_ub, double **lam_lg, double **lam_ug);
+//
+void d_cvt_ocp_qp_sol_to_rowmaj(struct d_ocp_qp *qp, struct d_ocp_qp_sol *qp_sol, double **u, double **x, double **pi, double **lam_lb, double **lam_ub, double **lam_lg, double **lam_ug);
+//
+void d_cvt_ocp_qp_sol_to_libstr(struct d_ocp_qp *qp, struct d_ocp_qp_sol *qp_sol, struct d_strvec *u, struct d_strvec *x, struct d_strvec *pi, struct d_strvec *lam_lb, struct d_strvec *lam_ub, struct d_strvec *lam_lg, struct d_strvec *lam_ug);
diff --git a/include/hpipm_m_ocp_qp.h b/include/hpipm_m_ocp_qp.h
new file mode 100644
index 0000000..bd9090b
--- /dev/null
+++ b/include/hpipm_m_ocp_qp.h
@@ -0,0 +1,36 @@
+/**************************************************************************************************
+* *
+* This file is part of HPIPM. *
+* *
+* HPIPM -- High Performance Interior Point Method. *
+* Copyright (C) 2017 by Gianluca Frison. *
+* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. *
+* All rights reserved. *
+* *
+* HPMPC is free software; you can redistribute it and/or *
+* modify it under the terms of the GNU Lesser General Public *
+* License as published by the Free Software Foundation; either *
+* version 2.1 of the License, or (at your option) any later version. *
+* *
+* HPMPC is distributed in the hope that it will be useful, *
+* but WITHOUT ANY WARRANTY; without even the implied warranty of *
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. *
+* See the GNU Lesser General Public License for more details. *
+* *
+* You should have received a copy of the GNU Lesser General Public *
+* License along with HPMPC; if not, write to the Free Software *
+* Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA *
+* *
+* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de *
+* *
+**************************************************************************************************/
+
+
+
+
+#include <blasfeo_target.h>
+#include <blasfeo_common.h>
+
+
+
+void m_cvt_d_ocp_qp_to_s_ocp_qp(struct d_ocp_qp *d_qp, struct s_ocp_qp *s_qp);
diff --git a/include/hpipm_m_ocp_qp_ipm_hard.h b/include/hpipm_m_ocp_qp_ipm_hard.h
new file mode 100644
index 0000000..29a6124
--- /dev/null
+++ b/include/hpipm_m_ocp_qp_ipm_hard.h
@@ -0,0 +1,99 @@
+/**************************************************************************************************
+* *
+* This file is part of HPIPM. *
+* *
+* HPIPM -- High Performance Interior Point Method. *
+* Copyright (C) 2017 by Gianluca Frison. *
+* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. *
+* All rights reserved. *
+* *
+* HPMPC is free software; you can redistribute it and/or *
+* modify it under the terms of the GNU Lesser General Public *
+* License as published by the Free Software Foundation; either *
+* version 2.1 of the License, or (at your option) any later version. *
+* *
+* HPMPC is distributed in the hope that it will be useful, *
+* but WITHOUT ANY WARRANTY; without even the implied warranty of *
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. *
+* See the GNU Lesser General Public License for more details. *
+* *
+* You should have received a copy of the GNU Lesser General Public *
+* License along with HPMPC; if not, write to the Free Software *
+* Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA *
+* *
+* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de *
+* *
+**************************************************************************************************/
+
+
+
+#include <blasfeo_target.h>
+#include <blasfeo_common.h>
+
+
+
+
+struct m_ipm_hard_ocp_qp_workspace
+ {
+ struct d_ipm_hard_core_qp_workspace *core_workspace;
+ struct d_strvec *dux;
+ struct d_strvec *dpi;
+ struct d_strvec *dt_lb;
+ struct d_strvec *dt_lg;
+ struct s_strvec *sdux; // XXX
+ struct s_strvec *sdpi; // XXX
+ struct d_strvec *res_g; // q-residuals
+ struct d_strvec *res_b; // b-residuals
+ struct d_strvec *res_d; // d-residuals XXX remove ???
+ struct d_strvec *res_d_lb; // d-residuals
+ struct d_strvec *res_d_ub; // d-residuals
+ struct d_strvec *res_d_lg; // d-residuals
+ struct d_strvec *res_d_ug; // d-residuals
+ struct d_strvec *res_m; // m-residuals
+ struct d_strvec *res_m_lb; // m-residuals
+ struct d_strvec *res_m_ub; // m-residuals
+ struct d_strvec *res_m_lg; // m-residuals
+ struct d_strvec *res_m_ug; // m-residuals
+ struct s_strvec *sres_g; // q-residuals // XXX
+ struct s_strvec *sres_b; // b-residuals // XXX
+ struct d_strvec *Qx_lb; // hessian update
+ struct d_strvec *Qx_lg; // hessian update
+ struct d_strvec *qx_lb; // gradient update
+ struct d_strvec *qx_lg; // gradient update
+ struct s_strvec *sQx_lb; // hessian update // XXX
+ struct s_strvec *sQx_lg; // hessian update // XXX
+ struct s_strvec *sqx_lb; // gradient update // XXX
+ struct s_strvec *sqx_lg; // gradient update // XXX
+ struct d_strvec *tmp_nbM; // work space of size nbM
+ struct s_strvec *tmp_nxM; // work space of size nxM // XXX
+ struct d_strvec *tmp_ngM; // work space of size ngM
+ struct s_strvec *Pb; // Pb // XXX
+ struct s_strmat *L; // XXX
+ struct s_strmat *AL; // XXX
+ double *stat; // convergence statistics
+ double res_mu; // mu-residual
+ int iter; // iteration number
+ int compute_Pb;
+ };
+
+
+
+struct m_ipm_hard_ocp_qp_arg
+ {
+ double alpha_min; // exit cond on step length
+ double mu_max; // exit cond on duality measure
+ double mu0; // initial value for duality measure
+ int iter_max; // exit cond in iter number
+ };
+
+
+
+//
+int m_memsize_ipm_hard_ocp_qp(struct d_ocp_qp *d_qp, struct s_ocp_qp *s_qp, struct m_ipm_hard_ocp_qp_arg *arg);
+//
+void m_create_ipm_hard_ocp_qp(struct d_ocp_qp *d_qp, struct s_ocp_qp *s_qp, struct m_ipm_hard_ocp_qp_arg *arg, struct m_ipm_hard_ocp_qp_workspace *ws, void *mem);
+//
+void m_solve_ipm_hard_ocp_qp(struct d_ocp_qp *d_qp, struct s_ocp_qp *s_qp, struct d_ocp_qp_sol *qp_sol, struct m_ipm_hard_ocp_qp_workspace *ws);
+//
+void m_solve_ipm2_hard_ocp_qp(struct d_ocp_qp *d_qp, struct s_ocp_qp *s_qp, struct d_ocp_qp_sol *qp_sol, struct m_ipm_hard_ocp_qp_workspace *ws);
+
diff --git a/include/hpipm_m_ocp_qp_kkt.h b/include/hpipm_m_ocp_qp_kkt.h
new file mode 100644
index 0000000..d335cfe
--- /dev/null
+++ b/include/hpipm_m_ocp_qp_kkt.h
@@ -0,0 +1,29 @@
+/**************************************************************************************************
+* *
+* This file is part of HPIPM. *
+* *
+* HPIPM -- High Performance Interior Point Method. *
+* Copyright (C) 2017 by Gianluca Frison. *
+* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. *
+* All rights reserved. *
+* *
+* HPMPC is free software; you can redistribute it and/or *
+* modify it under the terms of the GNU Lesser General Public *
+* License as published by the Free Software Foundation; either *
+* version 2.1 of the License, or (at your option) any later version. *
+* *
+* HPMPC is distributed in the hope that it will be useful, *
+* but WITHOUT ANY WARRANTY; without even the implied warranty of *
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. *
+* See the GNU Lesser General Public License for more details. *
+* *
+* You should have received a copy of the GNU Lesser General Public *
+* License along with HPMPC; if not, write to the Free Software *
+* Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA *
+* *
+* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de *
+* *
+**************************************************************************************************/
+
+void m_fact_solve_kkt_step_hard_ocp_qp(struct d_ocp_qp *d_qp, struct s_ocp_qp *s_qp, struct m_ipm_hard_ocp_qp_workspace *ws);
+void m_solve_kkt_step_hard_ocp_qp(struct d_ocp_qp *d_qp, struct s_ocp_qp *s_qp, struct m_ipm_hard_ocp_qp_workspace *ws);
diff --git a/include/hpipm_s_core_qp_ipm_hard.h b/include/hpipm_s_core_qp_ipm_hard.h
new file mode 100644
index 0000000..4c1fa39
--- /dev/null
+++ b/include/hpipm_s_core_qp_ipm_hard.h
@@ -0,0 +1,110 @@
+/**************************************************************************************************
+* *
+* This file is part of HPIPM. *
+* *
+* HPIPM -- High Performance Interior Point Method. *
+* Copyright (C) 2017 by Gianluca Frison. *
+* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. *
+* All rights reserved. *
+* *
+* HPMPC is free software; you can redistribute it and/or *
+* modify it under the terms of the GNU Lesser General Public *
+* License as published by the Free Software Foundation; either *
+* version 2.1 of the License, or (at your option) any later version. *
+* *
+* HPMPC is distributed in the hope that it will be useful, *
+* but WITHOUT ANY WARRANTY; without even the implied warranty of *
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. *
+* See the GNU Lesser General Public License for more details. *
+* *
+* You should have received a copy of the GNU Lesser General Public *
+* License along with HPMPC; if not, write to the Free Software *
+* Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA *
+* *
+* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de *
+* *
+**************************************************************************************************/
+
+
+
+struct s_ipm_hard_core_qp_workspace
+ {
+ float *d; // constraints
+ float *d_lb; // lower box constraints
+ float *d_ub; // upper box constraints
+ float *d_lg; // lower general constraints
+ float *d_ug; // upper general constraints
+ float *v; // primal variables
+ float *pi; // equality constraints multipliers
+ float *lam; // inequality constraints multipliers
+ float *lam_lb; // lower bounds multipliers
+ float *lam_lg; // lower general constraints multipliers
+ float *lam_ub; // upper bounds multipliers
+ float *lam_ug; // upper general constraints multipliers
+ float *t; // inequality constraints slacks
+ float *t_lb; // lower box constraints slacks
+ float *t_lg; // lower general constraints slacks
+ float *t_ub; // upper box constraints slacks
+ float *t_ug; // upper general constraints slacks
+ float *t_inv; // inverse of t
+ float *t_inv_lb; // inverse of t
+ float *t_inv_ub; // inverse of t
+ float *t_inv_lg; // inverse of t
+ float *t_inv_ug; // inverse of t
+ float *dv; // step in v
+ float *dpi; // step in pi
+ float *dlam; // step in lam
+ float *dlam_lb; //
+ float *dlam_lg; //
+ float *dlam_ub; //
+ float *dlam_ug; //
+ float *dt; // step in t
+ float *dt_lb; // step in t_lb
+ float *dt_ub; // step in t_ub
+ float *dt_lg; // step in t_lg
+ float *dt_ug; // step in t_ug
+ float *res_g; // q-residuals
+ float *res_b; // b-residuals
+ float *res_d; // d-residuals
+ float *res_d_lb; // d-residuals
+ float *res_d_ub; // d-residuals
+ float *res_d_lg; // d-residuals
+ float *res_d_ug; // d-residuals
+ float *res_m; // m-residuals
+ float *res_m_lb; // m-residuals
+ float *res_m_ub; // m-residuals
+ float *res_m_lg; // m-residuals
+ float *res_m_ug; // m-residuals
+ float *Qx; // Hessian update
+ float *Qx_lb; // Hessian update
+ float *Qx_lg; // Hessian update
+ float *qx; // gradient update
+ float *qx_lb; // gradient update
+ float *qx_lg; // gradient update
+ float *stat; // convergence statistics
+ float alpha; // step length
+ float alpha_min; // exit cond on step lenght
+ float sigma; // centering XXX
+ float mu; // duality measuere
+ float mu_aff; // affine duality measuere
+ float mu0; // initial duality measuere
+ float mu_max; // exit cond on mu
+ float nt_inv; // 1.0/nt, where nt is the total number of constraints
+ int nv; // number of primal variables
+ int ne; // number of equality constraints
+ int nb; // number of two-sized bounds
+ int ng; // number of two-sized constraints
+ int memsize; // memory size (in bytes) of workspace
+ int iter_max; // exit cond on iter mumber
+ };
+
+
+
+//
+int s_memsize_ipm_hard_core_qp(int nv, int ne, int nb, int ng, int iter_max);
+//
+
+void s_create_ipm_hard_core_qp(struct s_ipm_hard_core_qp_workspace *workspace, void *mem);
+//
+void s_ipm_hard_core_qp(struct s_ipm_hard_core_qp_workspace *workspace);
+
diff --git a/include/hpipm_s_core_qp_ipm_hard_aux.h b/include/hpipm_s_core_qp_ipm_hard_aux.h
new file mode 100644
index 0000000..fb79bf8
--- /dev/null
+++ b/include/hpipm_s_core_qp_ipm_hard_aux.h
@@ -0,0 +1,42 @@
+/**************************************************************************************************
+* *
+* This file is part of HPIPM. *
+* *
+* HPIPM -- High Performance Interior Point Method. *
+* Copyright (C) 2017 by Gianluca Frison. *
+* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. *
+* All rights reserved. *
+* *
+* HPMPC is free software; you can redistribute it and/or *
+* modify it under the terms of the GNU Lesser General Public *
+* License as published by the Free Software Foundation; either *
+* version 2.1 of the License, or (at your option) any later version. *
+* *
+* HPMPC is distributed in the hope that it will be useful, *
+* but WITHOUT ANY WARRANTY; without even the implied warranty of *
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. *
+* See the GNU Lesser General Public License for more details. *
+* *
+* You should have received a copy of the GNU Lesser General Public *
+* License along with HPMPC; if not, write to the Free Software *
+* Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA *
+* *
+* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de *
+* *
+**************************************************************************************************/
+
+//
+void s_compute_Qx_qx_hard_qp(struct s_ipm_hard_core_qp_workspace *rws);
+//
+void s_compute_lam_t_hard_qp(struct s_ipm_hard_core_qp_workspace *rws);
+//
+void s_compute_alpha_hard_qp(struct s_ipm_hard_core_qp_workspace *rws);
+//
+void s_update_var_hard_qp(struct s_ipm_hard_core_qp_workspace *rws);
+//
+void s_compute_mu_aff_hard_qp(struct s_ipm_hard_core_qp_workspace *rws);
+//
+void s_compute_centering_correction_hard_qp(struct s_ipm_hard_core_qp_workspace *rws);
+//
+void s_compute_qx_hard_qp(struct s_ipm_hard_core_qp_workspace *rws);
+
diff --git a/include/hpipm_s_dense_qp.h b/include/hpipm_s_dense_qp.h
new file mode 100644
index 0000000..857017f
--- /dev/null
+++ b/include/hpipm_s_dense_qp.h
@@ -0,0 +1,75 @@
+
+/**************************************************************************************************
+* *
+* This file is part of HPIPM. *
+* *
+* HPIPM -- High Performance Interior Point Method. *
+* Copyright (C) 2017 by Gianluca Frison. *
+* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. *
+* All rights reserved. *
+* *
+* HPMPC is free software; you can redistribute it and/or *
+* modify it under the terms of the GNU Lesser General Public *
+* License as published by the Free Software Foundation; either *
+* version 2.1 of the License, or (at your option) any later version. *
+* *
+* HPMPC is distributed in the hope that it will be useful, *
+* but WITHOUT ANY WARRANTY; without even the implied warranty of *
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. *
+* See the GNU Lesser General Public License for more details. *
+* *
+* You should have received a copy of the GNU Lesser General Public *
+* License along with HPMPC; if not, write to the Free Software *
+* Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA *
+* *
+* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de *
+* *
+**************************************************************************************************/
+
+
+
+#include <blasfeo_target.h>
+#include <blasfeo_common.h>
+
+
+
+struct s_dense_qp
+ {
+ struct s_strmat *Hg; // hessian & gradient
+ struct s_strmat *A; // dynamics matrix
+ struct s_strmat *Ct; // constraints matrix
+ struct s_strvec *g; // gradient
+ struct s_strvec *b; // dynamics vector
+ struct s_strvec *d; // constraint
+ struct s_strvec *d_lb; // lower box constraint
+ struct s_strvec *d_ub; // upper box constraint
+ struct s_strvec *d_lg; // lower general constraint
+ struct s_strvec *d_ug; // upper general constraint
+ int *idxb; // index of box constraints
+ int nv; // number of variables
+ int ne; // number of equality constraints
+ int nb; // number of box constraints
+ int ng; // number of general constraints
+ int memsize; // memory size in bytes
+ };
+
+
+
+//
+int s_memsize_dense_qp(int nv, int ne, int nb, int ng);
+//
+void s_create_dense_qp(int nv, int ne, int nb, int ng, struct s_dense_qp *qp, void *memory);
+//
+void s_cvt_colmaj_to_dense_qp(float *H, float *g, float *A, float *b, int *idxb, float *d_lb, float *d_ub, float *C, float *d_lg, float *d_ug, struct s_dense_qp *qp);
+//
+void s_cvt_dense_qp_to_colmaj(struct s_dense_qp *qp, float *H, float *g, float *A, float *b, int *idxb, float *d_lb, float *d_ub, float *C, float *d_lg, float *d_ug);
+//
+void s_cvt_rowmaj_to_dense_qp(float *H, float *g, float *A, float *b, int *idxb, float *d_lb, float *d_ub, float *C, float *d_lg, float *d_ug, struct s_dense_qp *qp);
+//
+void s_cvt_dense_qp_to_rowmaj(struct s_dense_qp *qp, float *H, float *g, float *A, float *b, int *idxb, float *d_lb, float *d_ub, float *C, float *d_lg, float *d_ug);
+//
+void s_cvt_libstr_to_dense_qp(struct s_strmat *H, struct s_strmat *A, struct s_strmat *C, struct s_strvec *g, struct s_strvec *b, struct s_strvec *d_lb, struct s_strvec *d_ub, struct s_strvec *d_lg, struct s_strvec *d_ug, int *idxb, struct s_dense_qp *qp);
+//
+void s_cvt_dense_qp_to_libstr(struct s_dense_qp *qp, struct s_strmat *H, struct s_strmat *A, struct s_strmat *C, struct s_strvec *g, struct s_strvec *b, struct s_strvec *d_lb, struct s_strvec *d_ub, struct s_strvec *d_lg, struct s_strvec *d_ug, int *idxb);
+
+
diff --git a/include/hpipm_s_dense_qp_ipm_hard.h b/include/hpipm_s_dense_qp_ipm_hard.h
new file mode 100644
index 0000000..519b0e8
--- /dev/null
+++ b/include/hpipm_s_dense_qp_ipm_hard.h
@@ -0,0 +1,90 @@
+/**************************************************************************************************
+* *
+* This file is part of HPIPM. *
+* *
+* HPIPM -- High Performance Interior Point Method. *
+* Copyright (C) 2017 by Gianluca Frison. *
+* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. *
+* All rights reserved. *
+* *
+* HPMPC is free software; you can redistribute it and/or *
+* modify it under the terms of the GNU Lesser General Public *
+* License as published by the Free Software Foundation; either *
+* version 2.1 of the License, or (at your option) any later version. *
+* *
+* HPMPC is distributed in the hope that it will be useful, *
+* but WITHOUT ANY WARRANTY; without even the implied warranty of *
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. *
+* See the GNU Lesser General Public License for more details. *
+* *
+* You should have received a copy of the GNU Lesser General Public *
+* License along with HPMPC; if not, write to the Free Software *
+* Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA *
+* *
+* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de *
+* *
+**************************************************************************************************/
+
+
+
+#include <blasfeo_target.h>
+#include <blasfeo_common.h>
+
+
+
+struct s_ipm_hard_dense_qp_workspace
+ {
+ struct s_ipm_hard_core_qp_workspace *core_workspace;
+ struct s_strvec *dv; // step in v
+ struct s_strvec *dpi; // step in pi
+ struct s_strvec *dlam; // step in lam XXX needed ???
+ struct s_strvec *dt; // step in t XXX needed ???
+ struct s_strvec *dt_lb; //
+ struct s_strvec *dt_ub; // XXX needed ???
+ struct s_strvec *dt_lg; //
+ struct s_strvec *dt_ug; // XXX needed ???
+ struct s_strvec *res_g; // q-residuals
+ struct s_strvec *res_b; // b-residuals
+ struct s_strvec *res_d; // d-residuals
+ struct s_strvec *res_d_lb; // d-residuals
+ struct s_strvec *res_d_ub; // d-residuals
+ struct s_strvec *res_d_lg; // d-residuals
+ struct s_strvec *res_d_ug; // d-residuals
+ struct s_strvec *res_m; // m-residuals
+ struct s_strvec *Qx; // hessian update
+ struct s_strvec *qx; // gradient update
+ struct s_strmat *Lv; //
+ struct s_strmat *AL; //
+ struct s_strmat *Le; //
+ struct s_strmat *Ctx; //
+ struct s_strvec *lv; //
+ struct s_strvec *tmp_nb; // work space of size nb
+ struct s_strvec *tmp_ng0; // work space of size nb
+ struct s_strvec *tmp_ng1; // work space of size nb
+ float *stat; // convergence statistics
+ float res_mu; // mu-residual
+ int iter; // iteration number
+ int memsize; // memory size (in bytes) of workspace
+ };
+
+
+
+struct s_ipm_hard_dense_qp_arg
+ {
+ float alpha_min; // exit cond on step length
+ float mu_max; // exit cond on duality measure
+ float mu0; // initial value for duality measure
+ int iter_max; // exit cond in iter number
+ };
+
+
+
+//
+int s_memsize_ipm_hard_dense_qp(struct s_dense_qp *qp, struct s_ipm_hard_dense_qp_arg *arg);
+//
+void s_create_ipm_hard_dense_qp(struct s_dense_qp *qp, struct s_ipm_hard_dense_qp_arg *arg, struct s_ipm_hard_dense_qp_workspace *ws, void *mem);
+//
+void s_solve_ipm_hard_dense_qp(struct s_dense_qp *qp, struct s_dense_qp_sol *qp_sol, struct s_ipm_hard_dense_qp_workspace *ws);
+//
+void s_solve_ipm2_hard_dense_qp(struct s_dense_qp *qp, struct s_dense_qp_sol *qp_sol, struct s_ipm_hard_dense_qp_workspace *ws);
+
diff --git a/include/hpipm_s_dense_qp_kkt.h b/include/hpipm_s_dense_qp_kkt.h
new file mode 100644
index 0000000..b3d5a75
--- /dev/null
+++ b/include/hpipm_s_dense_qp_kkt.h
@@ -0,0 +1,40 @@
+/**************************************************************************************************
+* *
+* This file is part of HPIPM. *
+* *
+* HPIPM -- High Performance Interior Point Method. *
+* Copyright (C) 2017 by Gianluca Frison. *
+* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. *
+* All rights reserved. *
+* *
+* HPMPC is free software; you can redistribute it and/or *
+* modify it under the terms of the GNU Lesser General Public *
+* License as published by the Free Software Foundation; either *
+* version 2.1 of the License, or (at your option) any later version. *
+* *
+* HPMPC is distributed in the hope that it will be useful, *
+* but WITHOUT ANY WARRANTY; without even the implied warranty of *
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. *
+* See the GNU Lesser General Public License for more details. *
+* *
+* You should have received a copy of the GNU Lesser General Public *
+* License along with HPMPC; if not, write to the Free Software *
+* Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA *
+* *
+* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de *
+* *
+**************************************************************************************************/
+
+
+
+//
+void s_init_var_hard_dense_qp(struct s_dense_qp *qp, struct s_dense_qp_sol *qp_sol, struct s_ipm_hard_dense_qp_workspace *ws);
+//
+void s_compute_res_hard_dense_qp(struct s_dense_qp *qp, struct s_dense_qp_sol *qp_sol, struct s_ipm_hard_dense_qp_workspace *ws);
+//
+void s_fact_solve_kkt_unconstr_dense_qp(struct s_dense_qp *qp, struct s_dense_qp_sol *qp_sol, struct s_ipm_hard_dense_qp_workspace *ws);
+//
+void s_fact_solve_kkt_step_hard_dense_qp(struct s_dense_qp *qp, struct s_ipm_hard_dense_qp_workspace *ws);
+//
+void s_solve_kkt_step_hard_dense_qp(struct s_dense_qp *qp, struct s_ipm_hard_dense_qp_workspace *ws);
+
diff --git a/include/hpipm_s_dense_qp_sol.h b/include/hpipm_s_dense_qp_sol.h
new file mode 100644
index 0000000..1d37341
--- /dev/null
+++ b/include/hpipm_s_dense_qp_sol.h
@@ -0,0 +1,61 @@
+/**************************************************************************************************
+* *
+* This file is part of HPIPM. *
+* *
+* HPIPM -- High Performance Interior Point Method. *
+* Copyright (C) 2017 by Gianluca Frison. *
+* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. *
+* All rights reserved. *
+* *
+* HPMPC is free software; you can redistribute it and/or *
+* modify it under the terms of the GNU Lesser General Public *
+* License as published by the Free Software Foundation; either *
+* version 2.1 of the License, or (at your option) any later version. *
+* *
+* HPMPC is distributed in the hope that it will be useful, *
+* but WITHOUT ANY WARRANTY; without even the implied warranty of *
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. *
+* See the GNU Lesser General Public License for more details. *
+* *
+* You should have received a copy of the GNU Lesser General Public *
+* License along with HPMPC; if not, write to the Free Software *
+* Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA *
+* *
+* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de *
+* *
+**************************************************************************************************/
+
+
+
+#include <blasfeo_target.h>
+#include <blasfeo_common.h>
+
+
+
+struct s_dense_qp_sol
+ {
+ struct s_strvec *v;
+ struct s_strvec *pi;
+ struct s_strvec *lam_lb;
+ struct s_strvec *lam_ub;
+ struct s_strvec *lam_lg;
+ struct s_strvec *lam_ug;
+ struct s_strvec *t_lb;
+ struct s_strvec *t_ub;
+ struct s_strvec *t_lg;
+ struct s_strvec *t_ug;
+ int memsize;
+ };
+
+
+
+//
+int s_memsize_dense_qp_sol(int nv, int ne, int nb, int ng);
+//
+void s_create_dense_qp_sol(int nv, int ne, int nb, int ng, struct s_dense_qp_sol *qp_sol, void *memory);
+//
+void s_cvt_dense_qp_sol_to_colmaj(struct s_dense_qp *qp, struct s_dense_qp_sol *qp_sol, float *v, float *pi, float *lam_lb, float *lam_ub, float *lam_lg, float *lam_ug);
+//
+void s_cvt_dense_qp_sol_to_rowmaj(struct s_dense_qp *qp, struct s_dense_qp_sol *qp_sol, float *v, float *pi, float *lam_lb, float *lam_ub, float *lam_lg, float *lam_ug);
+//
+void s_cvt_dense_qp_sol_to_libstr(struct s_dense_qp *qp, struct s_dense_qp_sol *qp_sol, struct s_strvec *v, struct s_strvec *pi, struct s_strvec *lam_lb, struct s_strvec *lam_ub, struct s_strvec *lam_lg, struct s_strvec *lam_ug);
diff --git a/include/hpipm_s_ocp_qp.h b/include/hpipm_s_ocp_qp.h
new file mode 100644
index 0000000..618726e
--- /dev/null
+++ b/include/hpipm_s_ocp_qp.h
@@ -0,0 +1,68 @@
+/**************************************************************************************************
+* *
+* This file is part of HPIPM. *
+* *
+* HPIPM -- High Performance Interior Point Method. *
+* Copyright (C) 2017 by Gianluca Frison. *
+* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. *
+* All rights reserved. *
+* *
+* HPMPC is free software; you can redistribute it and/or *
+* modify it under the terms of the GNU Lesser General Public *
+* License as published by the Free Software Foundation; either *
+* version 2.1 of the License, or (at your option) any later version. *
+* *
+* HPMPC is distributed in the hope that it will be useful, *
+* but WITHOUT ANY WARRANTY; without even the implied warranty of *
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. *
+* See the GNU Lesser General Public License for more details. *
+* *
+* You should have received a copy of the GNU Lesser General Public *
+* License along with HPMPC; if not, write to the Free Software *
+* Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA *
+* *
+* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de *
+* *
+**************************************************************************************************/
+
+
+
+#include <blasfeo_target.h>
+#include <blasfeo_common.h>
+
+
+
+struct s_ocp_qp
+ {
+ struct s_strmat *BAbt;
+ struct s_strvec *b;
+ struct s_strmat *RSQrq;
+ struct s_strvec *rq;
+ struct s_strmat *DCt;
+ struct s_strvec *d_lb;
+ struct s_strvec *d_ub;
+ struct s_strvec *d_lg;
+ struct s_strvec *d_ug;
+ int *nx; // number of states
+ int *nu; // number of inputs
+ int *nb; // number of box constraints
+ int **idxb; // index of box constraints
+ int *ng; // number of general constraints
+ int N; // hotizon lenght
+ int memsize; // memory size in bytes
+ };
+
+
+
+//
+int s_memsize_ocp_qp(int N, int *nx, int *nu, int *nb, int *ng);
+//
+void s_create_ocp_qp(int N, int *nx, int *nu, int *nb, int *ng, struct s_ocp_qp *qp, void *memory);
+//
+void s_cvt_colmaj_to_ocp_qp(float **A, float **B, float **b, float **Q, float **S, float **R, float **q, float **r, int **idxb, float **lb, float **ub, float **C, float **D, float **lg, float **ug, struct s_ocp_qp *qp);
+//
+void s_cvt_rowmaj_to_ocp_qp(float **A, float **B, float **b, float **Q, float **S, float **R, float **q, float **r, int **idxb, float **lb, float **ub, float **C, float **D, float **lg, float **ug, struct s_ocp_qp *qp);
+//
+//void s_cast_ocp_qp(int N, int *nx, int *nu, int *nb, int **idxb, int *ng, struct s_strmat *sBAbt, struct s_strvec *sb, struct s_strmat *sRSQrq, struct s_strvec *srq, struct s_strmat *sDCt, struct s_strvec *slb, struct s_strvec *sub, struct s_strvec *slg, struct s_strvec *sug, struct s_ocp_qp *str_out);
+//
+void s_copy_ocp_qp(struct s_ocp_qp *qp_in, struct s_ocp_qp *qp_out);
diff --git a/include/hpipm_s_ocp_qp_ipm_hard.h b/include/hpipm_s_ocp_qp_ipm_hard.h
new file mode 100644
index 0000000..a8e3ca3
--- /dev/null
+++ b/include/hpipm_s_ocp_qp_ipm_hard.h
@@ -0,0 +1,92 @@
+/**************************************************************************************************
+* *
+* This file is part of HPIPM. *
+* *
+* HPIPM -- High Performance Interior Point Method. *
+* Copyright (C) 2017 by Gianluca Frison. *
+* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. *
+* All rights reserved. *
+* *
+* HPMPC is free software; you can redistribute it and/or *
+* modify it under the terms of the GNU Lesser General Public *
+* License as published by the Free Software Foundation; either *
+* version 2.1 of the License, or (at your option) any later version. *
+* *
+* HPMPC is distributed in the hope that it will be useful, *
+* but WITHOUT ANY WARRANTY; without even the implied warranty of *
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. *
+* See the GNU Lesser General Public License for more details. *
+* *
+* You should have received a copy of the GNU Lesser General Public *
+* License along with HPMPC; if not, write to the Free Software *
+* Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA *
+* *
+* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de *
+* *
+**************************************************************************************************/
+
+
+
+#include <blasfeo_target.h>
+#include <blasfeo_common.h>
+
+
+
+
+struct s_ipm_hard_ocp_qp_workspace
+ {
+ struct s_ipm_hard_core_qp_workspace *core_workspace;
+ struct s_strvec *dux;
+ struct s_strvec *dpi;
+ struct s_strvec *dt_lb;
+ struct s_strvec *dt_lg;
+ struct s_strvec *res_g; // q-residuals
+ struct s_strvec *res_b; // b-residuals
+ struct s_strvec *res_d; // d-residuals XXX remove ???
+ struct s_strvec *res_d_lb; // d-residuals
+ struct s_strvec *res_d_ub; // d-residuals
+ struct s_strvec *res_d_lg; // d-residuals
+ struct s_strvec *res_d_ug; // d-residuals
+ struct s_strvec *res_m; // m-residuals
+ struct s_strvec *res_m_lb; // m-residuals
+ struct s_strvec *res_m_ub; // m-residuals
+ struct s_strvec *res_m_lg; // m-residuals
+ struct s_strvec *res_m_ug; // m-residuals
+ struct s_strvec *Qx_lb; // hessian update
+ struct s_strvec *Qx_lg; // hessian update
+ struct s_strvec *qx_lb; // gradient update
+ struct s_strvec *qx_lg; // gradient update
+ struct s_strvec *tmp_nbM; // work space of size nbM
+ struct s_strvec *tmp_nxM; // work space of size nxM
+ struct s_strvec *tmp_ngM; // work space of size ngM
+ struct s_strvec *Pb; // Pb
+ struct s_strmat *L;
+ struct s_strmat *AL;
+ float *stat; // convergence statistics
+ float res_mu; // mu-residual
+ int iter; // iteration number
+ int memsize;
+ };
+
+
+
+struct s_ipm_hard_ocp_qp_arg
+ {
+ float alpha_min; // exit cond on step length
+ float mu_max; // exit cond on duality measure
+ float mu0; // initial value for duality measure
+ int iter_max; // exit cond in iter number
+ };
+
+
+
+//
+int s_memsize_ipm_hard_ocp_qp(struct s_ocp_qp *qp, struct s_ipm_hard_ocp_qp_arg *arg);
+//
+void s_create_ipm_hard_ocp_qp(struct s_ocp_qp *qp, struct s_ipm_hard_ocp_qp_arg *arg, struct s_ipm_hard_ocp_qp_workspace *ws, void *mem);
+//
+void s_solve_ipm_hard_ocp_qp(struct s_ocp_qp *qp, struct s_ocp_qp_sol *qp_sol, struct s_ipm_hard_ocp_qp_workspace *ws);
+//
+void s_solve_ipm2_hard_ocp_qp(struct s_ocp_qp *qp, struct s_ocp_qp_sol *qp_sol, struct s_ipm_hard_ocp_qp_workspace *ws);
+
+
diff --git a/include/hpipm_s_ocp_qp_kkt.h b/include/hpipm_s_ocp_qp_kkt.h
new file mode 100644
index 0000000..214d9cc
--- /dev/null
+++ b/include/hpipm_s_ocp_qp_kkt.h
@@ -0,0 +1,41 @@
+/**************************************************************************************************
+* *
+* This file is part of HPIPM. *
+* *
+* HPIPM -- High Performance Interior Point Method. *
+* Copyright (C) 2017 by Gianluca Frison. *
+* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. *
+* All rights reserved. *
+* *
+* HPMPC is free software; you can redistribute it and/or *
+* modify it under the terms of the GNU Lesser General Public *
+* License as published by the Free Software Foundation; either *
+* version 2.1 of the License, or (at your option) any later version. *
+* *
+* HPMPC is distributed in the hope that it will be useful, *
+* but WITHOUT ANY WARRANTY; without even the implied warranty of *
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. *
+* See the GNU Lesser General Public License for more details. *
+* *
+* You should have received a copy of the GNU Lesser General Public *
+* License along with HPMPC; if not, write to the Free Software *
+* Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA *
+* *
+* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de *
+* *
+**************************************************************************************************/
+
+
+
+
+//
+void s_init_var_hard_ocp_qp(struct s_ocp_qp *qp, struct s_ocp_qp_sol *qp_sol, struct s_ipm_hard_ocp_qp_workspace *ws);
+//
+void s_compute_res_hard_ocp_qp(struct s_ocp_qp *qp, struct s_ocp_qp_sol *qp_sol, struct s_ipm_hard_ocp_qp_workspace *ws);
+//
+void s_fact_solve_kkt_unconstr_ocp_qp(struct s_ocp_qp *qp, struct s_ocp_qp_sol *qp_sol, struct s_ipm_hard_ocp_qp_workspace *ws);
+//
+void s_fact_solve_kkt_step_hard_ocp_qp(struct s_ocp_qp *qp, struct s_ipm_hard_ocp_qp_workspace *ws);
+//
+void s_solve_kkt_step_hard_ocp_qp(struct s_ocp_qp *qp, struct s_ipm_hard_ocp_qp_workspace *ws);
+
diff --git a/include/hpipm_s_ocp_qp_sol.h b/include/hpipm_s_ocp_qp_sol.h
new file mode 100644
index 0000000..39759dc
--- /dev/null
+++ b/include/hpipm_s_ocp_qp_sol.h
@@ -0,0 +1,61 @@
+/**************************************************************************************************
+* *
+* This file is part of HPIPM. *
+* *
+* HPIPM -- High Performance Interior Point Method. *
+* Copyright (C) 2017 by Gianluca Frison. *
+* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. *
+* All rights reserved. *
+* *
+* HPMPC is free software; you can redistribute it and/or *
+* modify it under the terms of the GNU Lesser General Public *
+* License as published by the Free Software Foundation; either *
+* version 2.1 of the License, or (at your option) any later version. *
+* *
+* HPMPC is distributed in the hope that it will be useful, *
+* but WITHOUT ANY WARRANTY; without even the implied warranty of *
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. *
+* See the GNU Lesser General Public License for more details. *
+* *
+* You should have received a copy of the GNU Lesser General Public *
+* License along with HPMPC; if not, write to the Free Software *
+* Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA *
+* *
+* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de *
+* *
+**************************************************************************************************/
+
+
+
+#include <blasfeo_target.h>
+#include <blasfeo_common.h>
+
+
+
+struct s_ocp_qp_sol
+ {
+ struct s_strvec *ux;
+ struct s_strvec *pi;
+ struct s_strvec *lam_lb;
+ struct s_strvec *lam_ub;
+ struct s_strvec *lam_lg;
+ struct s_strvec *lam_ug;
+ struct s_strvec *t_lb;
+ struct s_strvec *t_ub;
+ struct s_strvec *t_lg;
+ struct s_strvec *t_ug;
+ int memsize; // memory size in bytes
+ };
+
+
+
+//
+int s_memsize_ocp_qp_sol(int N, int *nx, int *nu, int *nb, int *ng);
+//
+void s_create_ocp_qp_sol(int N, int *nx, int *nu, int *nb, int *ng, struct s_ocp_qp_sol *qp_sol, void *memory);
+//
+void s_cvt_ocp_qp_sol_to_colmaj(struct s_ocp_qp *qp, struct s_ocp_qp_sol *qp_sol, float **u, float **x, float **pi, float **lam_lb, float **lam_ub, float **lam_lg, float **lam_ug);
+//
+void s_cvt_ocp_qp_sol_to_rowmaj(struct s_ocp_qp *qp, struct s_ocp_qp_sol *qp_sol, float **u, float **x, float **pi, float **lam_lb, float **lam_ub, float **lam_lg, float **lam_ug);
+//
+void s_cvt_ocp_qp_sol_to_libstr(struct s_ocp_qp *qp, struct s_ocp_qp_sol *qp_sol, struct s_strvec *u, struct s_strvec *x, struct s_strvec *pi, struct s_strvec *lam_lb, struct s_strvec *lam_ub, struct s_strvec *lam_lg, struct s_strvec *lam_ug);