Squashed 'third_party/hpipm/' content from commit c2c0261
Change-Id: I05e186a6e1e1f075aa629092e7ad86e6116ca711
git-subtree-dir: third_party/hpipm
git-subtree-split: c2c0261e8ded36f636d9c4390a2899bdc4c999cc
diff --git a/ocp_qp/Makefile b/ocp_qp/Makefile
new file mode 100644
index 0000000..272eedd
--- /dev/null
+++ b/ocp_qp/Makefile
@@ -0,0 +1,53 @@
+###################################################################################################
+# #
+# 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 ../Makefile.rule
+
+OBJS =
+
+ifeq ($(TARGET), GENERIC)
+OBJS +=
+endif
+
+OBJS += d_ocp_qp.o d_ocp_qp_sol.o d_ocp_qp_kkt.o d_ocp_qp_ipm_hard.o
+OBJS += s_ocp_qp.o s_ocp_qp_sol.o s_ocp_qp_kkt.o s_ocp_qp_ipm_hard.o
+OBJS += m_ocp_qp.o m_ocp_qp_kkt.o m_ocp_qp_ipm_hard.o
+
+obj: $(OBJS)
+
+clean:
+ rm -f *.o
+ rm -f *.s
+
+d_ocp_qp.o: d_ocp_qp.c x_ocp_qp.c
+s_ocp_qp.o: s_ocp_qp.c x_ocp_qp.c
+d_ocp_qp_sol.o: d_ocp_qp_sol.c x_ocp_qp_sol.c
+s_ocp_qp_sol.o: s_ocp_qp_sol.c x_ocp_qp_sol.c
+d_ocp_qp_kkt.o: d_ocp_qp_kkt.c x_ocp_qp_kkt.c
+s_ocp_qp_kkt.o: s_ocp_qp_kkt.c x_ocp_qp_kkt.c
+d_ocp_qp_ipm_hard.o: d_ocp_qp_ipm_hard.c x_ocp_qp_ipm_hard.c
+s_ocp_qp_ipm_hard.o: s_ocp_qp_ipm_hard.c x_ocp_qp_ipm_hard.c
diff --git a/ocp_qp/d_ocp_qp.c b/ocp_qp/d_ocp_qp.c
new file mode 100644
index 0000000..3358da9
--- /dev/null
+++ b/ocp_qp/d_ocp_qp.c
@@ -0,0 +1,65 @@
+/**************************************************************************************************
+* *
+* 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 *
+* *
+**************************************************************************************************/
+
+
+
+#if defined(RUNTIME_CHECKS)
+#include <stdlib.h>
+#endif
+
+#include <blasfeo_target.h>
+#include <blasfeo_common.h>
+#include <blasfeo_d_aux.h>
+
+#include "../include/hpipm_d_ocp_qp.h"
+
+
+
+#define CREATE_STRMAT d_create_strmat
+#define CREATE_STRVEC d_create_strvec
+#define CVT_MAT2STRMAT d_cvt_mat2strmat
+#define CVT_TRAN_MAT2STRMAT d_cvt_tran_mat2strmat
+#define CVT_VEC2STRVEC d_cvt_vec2strvec
+#define GECP_LIBSTR dgecp_libstr
+#define OCP_QP d_ocp_qp
+#define REAL double
+#define STRMAT d_strmat
+#define STRVEC d_strvec
+#define SIZE_STRMAT d_size_strmat
+#define SIZE_STRVEC d_size_strvec
+#define VECCP_LIBSTR dveccp_libstr
+
+#define CAST_OCP_QP d_cast_ocp_qp
+#define COPY_OCP_QP d_copy_ocp_qp
+#define CREATE_OCP_QP d_create_ocp_qp
+#define CVT_COLMAJ_TO_OCP_QP d_cvt_colmaj_to_ocp_qp
+#define CVT_ROWMAJ_TO_OCP_QP d_cvt_rowmaj_to_ocp_qp
+#define MEMSIZE_OCP_QP d_memsize_ocp_qp
+
+
+
+#include "x_ocp_qp.c"
diff --git a/ocp_qp/d_ocp_qp_ipm_hard.c b/ocp_qp/d_ocp_qp_ipm_hard.c
new file mode 100644
index 0000000..0b44315
--- /dev/null
+++ b/ocp_qp/d_ocp_qp_ipm_hard.c
@@ -0,0 +1,82 @@
+/**************************************************************************************************
+* *
+* 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>
+#include <blasfeo_d_aux.h>
+
+#include "../include/hpipm_d_ocp_qp.h"
+#include "../include/hpipm_d_ocp_qp_sol.h"
+#include "../include/hpipm_d_ocp_qp_ipm_hard.h"
+#include "../include/hpipm_d_core_qp_ipm_hard.h"
+#include "../include/hpipm_d_core_qp_ipm_hard_aux.h"
+#include "../include/hpipm_d_ocp_qp_kkt.h"
+
+
+
+#define COMPUTE_ALPHA_HARD_QP d_compute_alpha_hard_qp
+#define COMPUTE_CENTERING_CORRECTION_HARD_QP d_compute_centering_correction_hard_qp
+#define COMPUTE_MU_AFF_HARD_QP d_compute_mu_aff_hard_qp
+#define COMPUTE_RES_HARD_OCP_QP d_compute_res_hard_ocp_qp
+#define CREATE_IPM_HARD_CORE_QP d_create_ipm_hard_core_qp
+#define CREATE_STRMAT d_create_strmat
+#define CREATE_STRVEC d_create_strvec
+#define FACT_SOLVE_KKT_STEP_HARD_OCP_QP d_fact_solve_kkt_step_hard_ocp_qp
+#define FACT_SOLVE_KKT_UNCONSTR_OCP_QP d_fact_solve_kkt_unconstr_ocp_qp
+#define INIT_VAR_HARD_OCP_QP d_init_var_hard_ocp_qp
+#define IPM_HARD_CORE_QP_WORKSPACE d_ipm_hard_core_qp_workspace
+#define IPM_HARD_OCP_QP_WORKSPACE d_ipm_hard_ocp_qp_workspace
+#define IPM_HARD_OCP_QP_ARG d_ipm_hard_ocp_qp_arg
+#define MEMSIZE_IPM_HARD_CORE_QP d_memsize_ipm_hard_core_qp
+#define OCP_QP d_ocp_qp
+#define OCP_QP_SOL d_ocp_qp_sol
+#define PRINT_E_MAT d_print_e_mat
+#define PRINT_E_STRVEC d_print_e_strvec
+#define PRINT_E_TRAN_STRVEC d_print_e_tran_strvec
+#define PRINT_STRMAT d_print_strmat
+#define PRINT_STRVEC d_print_strvec
+#define PRINT_TRAN_STRVEC d_print_tran_strvec
+#define REAL double
+#define SIZE_STRMAT d_size_strmat
+#define SIZE_STRVEC d_size_strvec
+#define SOLVE_KKT_STEP_HARD_OCP_QP d_solve_kkt_step_hard_ocp_qp
+#define STRMAT d_strmat
+#define STRVEC d_strvec
+#define UPDATE_VAR_HARD_QP d_update_var_hard_qp
+
+
+
+#define MEMSIZE_IPM_HARD_OCP_QP d_memsize_ipm_hard_ocp_qp
+#define CREATE_IPM_HARD_OCP_QP d_create_ipm_hard_ocp_qp
+#define SOLVE_IPM_HARD_OCP_QP d_solve_ipm_hard_ocp_qp
+#define SOLVE_IPM2_HARD_OCP_QP d_solve_ipm2_hard_ocp_qp
+
+
+
+#include "x_ocp_qp_ipm_hard.c"
diff --git a/ocp_qp/d_ocp_qp_kkt.c b/ocp_qp/d_ocp_qp_kkt.c
new file mode 100644
index 0000000..7b63620
--- /dev/null
+++ b/ocp_qp/d_ocp_qp_kkt.c
@@ -0,0 +1,101 @@
+/**************************************************************************************************
+* *
+* 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 <math.h>
+
+#include <blasfeo_target.h>
+#include <blasfeo_common.h>
+#include <blasfeo_d_aux.h>
+#include <blasfeo_d_blas.h>
+
+#include "../include/hpipm_d_ocp_qp.h"
+#include "../include/hpipm_d_ocp_qp_sol.h"
+#include "../include/hpipm_d_ocp_qp_ipm_hard.h"
+#include "../include/hpipm_d_core_qp_ipm_hard.h"
+#include "../include/hpipm_d_core_qp_ipm_hard_aux.h"
+
+
+
+#define DOUBLE_PRECISION
+
+#define AXPY_LIBSTR daxpy_libstr
+#define COMPUTE_LAM_T_HARD_QP d_compute_lam_t_hard_qp
+#define COMPUTE_QX_HARD_QP d_compute_qx_hard_qp
+#define COMPUTE_QX_QX_HARD_QP d_compute_Qx_qx_hard_qp
+#define DIAAD_SP_LIBSTR ddiaad_sp_libstr
+#define GEAD_LIBSTR dgead_libstr
+#define GECP_LIBSTR dgecp_libstr
+#define GEMM_R_DIAG_LIBSTR dgemm_r_diag_libstr
+#define GEMV_N_LIBSTR dgemv_n_libstr
+#define GEMV_NT_LIBSTR dgemv_nt_libstr
+#define GEMV_T_LIBSTR dgemv_t_libstr
+#define IPM_HARD_CORE_QP_WORKSPACE d_ipm_hard_core_qp_workspace
+#define IPM_HARD_OCP_QP_WORKSPACE d_ipm_hard_ocp_qp_workspace
+#define OCP_QP d_ocp_qp
+#define OCP_QP_SOL d_ocp_qp_sol
+#define POTRF_L_MN_LIBSTR dpotrf_l_mn_libstr
+#define PRINT_E_MAT d_print_e_mat
+#define PRINT_E_STRVEC d_print_e_strvec
+#define PRINT_E_TRAN_STRVEC d_print_e_tran_strvec
+#define PRINT_STRMAT d_print_strmat
+#define PRINT_STRVEC d_print_strvec
+#define PRINT_TRAN_STRVEC d_print_tran_strvec
+#define REAL double
+#define ROWAD_SP_LIBSTR drowad_sp_libstr
+#define ROWEX_LIBSTR drowex_libstr
+#define ROWIN_LIBSTR drowin_libstr
+#define STRMAT d_strmat
+#define STRVEC d_strvec
+#define SYMV_L_LIBSTR dsymv_l_libstr
+#define SYRK_POTRF_LN_LIBSTR dsyrk_dpotrf_ln_libstr
+#define TRCP_L_LIBSTR dtrcp_l_libstr
+#define TRMM_RLNN_LIBSTR dtrmm_rlnn_libstr
+#define TRMV_LNN_LIBSTR dtrmv_lnn_libstr
+#define TRMV_LTN_LIBSTR dtrmv_ltn_libstr
+#define TRSV_LNN_LIBSTR dtrsv_lnn_libstr
+#define TRSV_LTN_LIBSTR dtrsv_ltn_libstr
+#define TRSV_LNN_MN_LIBSTR dtrsv_lnn_mn_libstr
+#define TRSV_LTN_MN_LIBSTR dtrsv_ltn_mn_libstr
+#define VECAD_SP_LIBSTR dvecad_sp_libstr
+#define VECCP_LIBSTR dveccp_libstr
+#define VECEX_SP_LIBSTR dvecex_sp_libstr
+#define VECMULDOT_LIBSTR dvecmuldot_libstr
+#define VECSC_LIBSTR dvecsc_libstr
+
+
+
+#define INIT_VAR_HARD_OCP_QP d_init_var_hard_ocp_qp
+#define COMPUTE_RES_HARD_OCP_QP d_compute_res_hard_ocp_qp
+#define FACT_SOLVE_KKT_UNCONSTR_OCP_QP d_fact_solve_kkt_unconstr_ocp_qp
+#define FACT_SOLVE_KKT_STEP_HARD_OCP_QP d_fact_solve_kkt_step_hard_ocp_qp
+#define SOLVE_KKT_STEP_HARD_OCP_QP d_solve_kkt_step_hard_ocp_qp
+
+
+
+#include "x_ocp_qp_kkt.c"
diff --git a/ocp_qp/d_ocp_qp_sol.c b/ocp_qp/d_ocp_qp_sol.c
new file mode 100644
index 0000000..7770ec8
--- /dev/null
+++ b/ocp_qp/d_ocp_qp_sol.c
@@ -0,0 +1,60 @@
+/**************************************************************************************************
+* *
+* 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 *
+* *
+**************************************************************************************************/
+
+
+
+#if defined(RUNTIME_CHECKS)
+#include <stdlib.h>
+#endif
+
+#include <blasfeo_target.h>
+#include <blasfeo_common.h>
+#include <blasfeo_d_aux.h>
+
+#include "../include/hpipm_d_ocp_qp.h"
+#include "../include/hpipm_d_ocp_qp_sol.h"
+
+
+
+#define CREATE_STRVEC d_create_strvec
+#define CVT_STRVEC2VEC d_cvt_strvec2vec
+#define OCP_QP d_ocp_qp
+#define OCP_QP_SOL d_ocp_qp_sol
+#define REAL double
+#define STRVEC d_strvec
+#define SIZE_STRVEC d_size_strvec
+#define VECCP_LIBSTR dveccp_libstr
+
+#define CREATE_OCP_QP_SOL d_create_ocp_qp_sol
+#define MEMSIZE_OCP_QP_SOL d_memsize_ocp_qp_sol
+#define CVT_OCP_QP_SOL_TO_COLMAJ d_cvt_ocp_qp_sol_to_colmaj
+#define CVT_OCP_QP_SOL_TO_ROWMAJ d_cvt_ocp_qp_sol_to_rowmaj
+#define CVT_OCP_QP_SOL_TO_LIBSTR d_cvt_ocp_qp_sol_to_libstr
+
+
+
+#include "x_ocp_qp_sol.c"
diff --git a/ocp_qp/m_ocp_qp.c b/ocp_qp/m_ocp_qp.c
new file mode 100644
index 0000000..bddaadb
--- /dev/null
+++ b/ocp_qp/m_ocp_qp.c
@@ -0,0 +1,81 @@
+/**************************************************************************************************
+* *
+* 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 *
+* *
+**************************************************************************************************/
+
+
+
+#if defined(RUNTIME_CHECKS)
+#include <stdlib.h>
+#endif
+
+#include <blasfeo_target.h>
+#include <blasfeo_common.h>
+#include <blasfeo_m_aux.h>
+
+#include "../include/hpipm_d_ocp_qp.h"
+#include "../include/hpipm_s_ocp_qp.h"
+
+
+
+void m_cvt_d_ocp_qp_to_s_ocp_qp(struct d_ocp_qp *d_qp, struct s_ocp_qp *s_qp)
+ {
+
+ // TODO check that they have equal sizes !!!!!
+
+ int N = d_qp->N;
+ int *nx = d_qp->nx;
+ int *nu = d_qp->nu;
+ int *nb = d_qp->nb;
+ int *ng = d_qp->ng;
+
+ int ii, jj;
+
+ for(ii=0; ii<N; ii++)
+ {
+ m_cvt_d2s_strmat(nu[ii]+nx[ii]+1, nx[ii+1], d_qp->BAbt+ii, 0, 0, s_qp->BAbt+ii, 0, 0);
+ m_cvt_d2s_strmat(nu[ii]+nx[ii]+1, nu[ii]+nx[ii], d_qp->RSQrq+ii, 0, 0, s_qp->RSQrq+ii, 0, 0);
+ m_cvt_d2s_strmat(nu[ii]+nx[ii], ng[ii], d_qp->DCt+ii, 0, 0, s_qp->DCt+ii, 0, 0);
+ m_cvt_d2s_strvec(nx[ii+1], d_qp->b+ii, 0, s_qp->b+ii, 0);
+ m_cvt_d2s_strvec(nu[ii]+nx[ii], d_qp->rq+ii, 0, s_qp->rq+ii, 0);
+ m_cvt_d2s_strvec(nb[ii], d_qp->d_lb+ii, 0, s_qp->d_lb+ii, 0);
+ m_cvt_d2s_strvec(nb[ii], d_qp->d_ub+ii, 0, s_qp->d_ub+ii, 0);
+ m_cvt_d2s_strvec(ng[ii], d_qp->d_lg+ii, 0, s_qp->d_lg+ii, 0);
+ m_cvt_d2s_strvec(ng[ii], d_qp->d_ug+ii, 0, s_qp->d_ug+ii, 0);
+ for(jj=0; jj<nb[ii]; jj++) s_qp->idxb[ii][jj] = d_qp->idxb[ii][jj];
+ }
+ ii = N;
+ m_cvt_d2s_strmat(nu[ii]+nx[ii]+1, nu[ii]+nx[ii], d_qp->RSQrq+ii, 0, 0, s_qp->RSQrq+ii, 0, 0);
+ m_cvt_d2s_strmat(nu[ii]+nx[ii], ng[ii], d_qp->DCt+ii, 0, 0, s_qp->DCt+ii, 0, 0);
+ m_cvt_d2s_strvec(nu[ii]+nx[ii], d_qp->rq+ii, 0, s_qp->rq+ii, 0);
+ m_cvt_d2s_strvec(nb[ii], d_qp->d_lb+ii, 0, s_qp->d_lb+ii, 0);
+ m_cvt_d2s_strvec(nb[ii], d_qp->d_ub+ii, 0, s_qp->d_ub+ii, 0);
+ m_cvt_d2s_strvec(ng[ii], d_qp->d_lg+ii, 0, s_qp->d_lg+ii, 0);
+ m_cvt_d2s_strvec(ng[ii], d_qp->d_ug+ii, 0, s_qp->d_ug+ii, 0);
+ for(jj=0; jj<nb[ii]; jj++) s_qp->idxb[ii][jj] = d_qp->idxb[ii][jj];
+
+ return;
+
+ }
diff --git a/ocp_qp/m_ocp_qp_ipm_hard.c b/ocp_qp/m_ocp_qp_ipm_hard.c
new file mode 100644
index 0000000..3968a17
--- /dev/null
+++ b/ocp_qp/m_ocp_qp_ipm_hard.c
@@ -0,0 +1,835 @@
+/**************************************************************************************************
+* *
+* 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>
+#include <blasfeo_d_aux.h>
+#include <blasfeo_s_aux.h>
+
+#include "../include/hpipm_d_ocp_qp.h"
+#include "../include/hpipm_s_ocp_qp.h"
+#include "../include/hpipm_d_ocp_qp_sol.h"
+#include "../include/hpipm_d_ocp_qp_ipm_hard.h"
+#include "../include/hpipm_m_ocp_qp_ipm_hard.h"
+#include "../include/hpipm_d_core_qp_ipm_hard.h"
+#include "../include/hpipm_d_core_qp_ipm_hard_aux.h"
+#include "../include/hpipm_d_ocp_qp_kkt.h"
+#include "../include/hpipm_m_ocp_qp_kkt.h"
+
+
+
+int m_memsize_ipm_hard_ocp_qp(struct d_ocp_qp *qp, struct s_ocp_qp *s_qp, struct m_ipm_hard_ocp_qp_arg *arg)
+ {
+
+ // loop index
+ int ii;
+
+ // extract ocp qp size
+ int N = qp->N;
+ int *nx = qp->nx;
+ int *nu = qp->nu;
+ int *nb = qp->nb;
+ int *ng = qp->ng;
+
+ // compute core qp size and max size
+ int nvt = 0;
+ int net = 0;
+ int nbt = 0;
+ int ngt = 0;
+ int nxM = 0;
+ int nuM = 0;
+ int nbM = 0;
+ int ngM = 0;
+
+ for(ii=0; ii<N; ii++)
+ {
+ nvt += nx[ii]+nu[ii];
+ net += nx[ii+1];
+ nbt += nb[ii];
+ ngt += ng[ii];
+ nxM = nx[ii]>nxM ? nx[ii] : nxM;
+ nuM = nu[ii]>nuM ? nu[ii] : nuM;
+ nbM = nb[ii]>nbM ? nb[ii] : nbM;
+ ngM = ng[ii]>ngM ? ng[ii] : ngM;
+ }
+ ii = N;
+ nvt += nx[ii]+nu[ii];
+ nbt += nb[ii];
+ ngt += ng[ii];
+ nxM = nx[ii]>nxM ? nx[ii] : nxM;
+ nuM = nu[ii]>nuM ? nu[ii] : nuM;
+ nbM = nb[ii]>nbM ? nb[ii] : nbM;
+ ngM = ng[ii]>ngM ? ng[ii] : ngM;
+
+ int size = 0;
+
+ size += (4+(N+1)*18)*sizeof(struct d_strvec); // dux dpi dt_lb dt_lg res_g res_b res_d res_d_lb res_d_ub res_d_lg res_d_ug res_m res_m_lb res_m_ub res_m_lg res_m_ug Qx_lb Qx_lg qx_lb qx_lg tmp_nbM tmp_ngM
+ size += (1+(N+1)*9)*sizeof(struct s_strvec); // sdux sdpi sres_g sres_b sQx_lb sQx_lg, sqx_lb, sqx_lg tmp_nxM Pb
+ size += (1+(N+1)*1)*sizeof(struct s_strmat); // L AL
+
+ size += 1*d_size_strvec(nbM); // tmp_nbM
+ size += 1*s_size_strvec(nxM); // tmp_nxM
+ size += 2*d_size_strvec(nxM); // tmp_ngM
+ for(ii=0; ii<=N; ii++) size += 2*s_size_strvec(nu[ii]+nx[ii]); // sdux sres_g
+ for(ii=0; ii<N; ii++) size += 3*s_size_strvec(nx[ii+1]); // sdpi sres_b Pb
+ for(ii=0; ii<=N; ii++) size += 2*s_size_strvec(nb[ii]); // sQx_lb sqx_lb
+ for(ii=0; ii<=N; ii++) size += 2*s_size_strvec(ng[ii]); // sQx_lg sqx_lg
+ for(ii=0; ii<=N; ii++) size += 1*s_size_strmat(nu[ii]+nx[ii]+1, nu[ii]+nx[ii]); // L
+ size += 2*s_size_strmat(nuM+nxM+1, nxM+ngM); // AL
+
+ size += 1*sizeof(struct d_ipm_hard_core_qp_workspace);
+ size += 1*d_memsize_ipm_hard_core_qp(nvt, net, nbt, ngt, arg->iter_max);
+
+ size = (size+63)/64*64; // make multiple of typical cache line size
+ size += 1*64; // align once to typical cache line size
+
+ return size;
+
+ }
+
+
+
+void m_create_ipm_hard_ocp_qp(struct d_ocp_qp *qp, struct s_ocp_qp *s_qp, struct m_ipm_hard_ocp_qp_arg *arg, struct m_ipm_hard_ocp_qp_workspace *workspace, void *mem)
+ {
+
+ // loop index
+ int ii;
+
+ // extract ocp qp size
+ int N = qp->N;
+ int *nx = qp->nx;
+ int *nu = qp->nu;
+ int *nb = qp->nb;
+ int *ng = qp->ng;
+
+ // compute core qp size and max size
+ int nvt = 0;
+ int net = 0;
+ int nbt = 0;
+ int ngt = 0;
+ int nxM = 0;
+ int nuM = 0;
+ int nbM = 0;
+ int ngM = 0;
+
+ for(ii=0; ii<N; ii++)
+ {
+ nvt += nx[ii]+nu[ii];
+ net += nx[ii+1];
+ nbt += nb[ii];
+ ngt += ng[ii];
+ nxM = nx[ii]>nxM ? nx[ii] : nxM;
+ nuM = nu[ii]>nuM ? nu[ii] : nuM;
+ nbM = nb[ii]>nbM ? nb[ii] : nbM;
+ ngM = ng[ii]>ngM ? ng[ii] : ngM;
+ }
+ ii = N;
+ nvt += nx[ii]+nu[ii];
+ nbt += nb[ii];
+ ngt += ng[ii];
+ nxM = nx[ii]>nxM ? nx[ii] : nxM;
+ nuM = nu[ii]>nuM ? nu[ii] : nuM;
+ nbM = nb[ii]>nbM ? nb[ii] : nbM;
+ ngM = ng[ii]>ngM ? ng[ii] : ngM;
+
+
+ // core struct
+ struct d_ipm_hard_core_qp_workspace *sr_ptr = mem;
+
+ // core workspace
+ workspace->core_workspace = sr_ptr;
+ sr_ptr += 1;
+ struct d_ipm_hard_core_qp_workspace *rwork = workspace->core_workspace;
+
+
+ // s matrix struct
+ struct s_strmat *sm_ptr = (struct s_strmat *) sr_ptr;
+
+ workspace->L = sm_ptr;
+ sm_ptr += N+1;
+ workspace->AL = sm_ptr;
+ sm_ptr += 2;
+
+
+ // s vector struct
+ struct s_strvec *ssv_ptr = (struct s_strvec *) sm_ptr;
+
+ workspace->sdux = ssv_ptr;
+ ssv_ptr += N+1;
+ workspace->sdpi = ssv_ptr;
+ ssv_ptr += N+1;
+ workspace->sres_g = ssv_ptr;
+ ssv_ptr += N+1;
+ workspace->sres_b = ssv_ptr;
+ ssv_ptr += N+1;
+ workspace->sQx_lb = ssv_ptr;
+ ssv_ptr += N+1;
+ workspace->sQx_lg = ssv_ptr;
+ ssv_ptr += N+1;
+ workspace->sqx_lb = ssv_ptr;
+ ssv_ptr += N+1;
+ workspace->sqx_lg = ssv_ptr;
+ ssv_ptr += N+1;
+ workspace->Pb = ssv_ptr;
+ ssv_ptr += N+1;
+ workspace->tmp_nxM = ssv_ptr;
+ ssv_ptr += 1;
+
+
+ // d vector struct
+ struct d_strvec *sv_ptr = (struct d_strvec *) ssv_ptr;
+
+ workspace->dux = sv_ptr;
+ sv_ptr += N+1;
+ workspace->dpi = sv_ptr;
+ sv_ptr += N+1;
+ workspace->dt_lb = sv_ptr;
+ sv_ptr += N+1;
+ workspace->dt_lg = sv_ptr;
+ sv_ptr += N+1;
+ workspace->res_g = sv_ptr;
+ sv_ptr += N+1;
+ workspace->res_b = sv_ptr;
+ sv_ptr += N+1;
+ workspace->res_d = sv_ptr;
+ sv_ptr += 1;
+ workspace->res_d_lb = sv_ptr;
+ sv_ptr += N+1;
+ workspace->res_d_ub = sv_ptr;
+ sv_ptr += N+1;
+ workspace->res_d_lg = sv_ptr;
+ sv_ptr += N+1;
+ workspace->res_d_ug = sv_ptr;
+ sv_ptr += N+1;
+ workspace->res_m = sv_ptr;
+ sv_ptr += 1;
+ workspace->res_m_lb = sv_ptr;
+ sv_ptr += N+1;
+ workspace->res_m_ub = sv_ptr;
+ sv_ptr += N+1;
+ workspace->res_m_lg = sv_ptr;
+ sv_ptr += N+1;
+ workspace->res_m_ug = sv_ptr;
+ sv_ptr += N+1;
+ workspace->Qx_lb = sv_ptr;
+ sv_ptr += N+1;
+ workspace->Qx_lg = sv_ptr;
+ sv_ptr += N+1;
+ workspace->qx_lb = sv_ptr;
+ sv_ptr += N+1;
+ workspace->qx_lg = sv_ptr;
+ sv_ptr += N+1;
+// workspace->Pb = sv_ptr;
+// sv_ptr += N+1;
+ workspace->tmp_nbM = sv_ptr;
+ sv_ptr += 1;
+// workspace->tmp_nxM = sv_ptr;
+// sv_ptr += 1;
+ workspace->tmp_ngM = sv_ptr;
+ sv_ptr += 2;
+
+
+ // align to typicl cache line size
+ size_t s_ptr = (size_t) sv_ptr;
+ s_ptr = (s_ptr+63)/64*64;
+
+
+ // void stuf
+ void *v_ptr = (void *) s_ptr;
+
+ for(ii=0; ii<=N; ii++)
+ {
+ s_create_strmat(nu[ii]+nx[ii]+1, nu[ii]+nx[ii], workspace->L+ii, v_ptr);
+ v_ptr += (workspace->L+ii)->memory_size;
+ }
+
+ s_create_strmat(nuM+nxM+1, nxM+ngM, workspace->AL+0, v_ptr);
+ v_ptr += (workspace->AL+0)->memory_size;
+
+ s_create_strmat(nuM+nxM+1, nxM+ngM, workspace->AL+1, v_ptr);
+ v_ptr += (workspace->AL+1)->memory_size;
+
+ for(ii=0; ii<=N; ii++)
+ {
+ s_create_strvec(nu[ii]+nx[ii], workspace->sdux+ii, v_ptr);
+ v_ptr += (workspace->sdux+ii)->memory_size;
+ }
+
+ for(ii=0; ii<N; ii++)
+ {
+ s_create_strvec(nx[ii+1], workspace->sdpi+ii, v_ptr);
+ v_ptr += (workspace->sdpi+ii)->memory_size;
+ }
+
+ for(ii=0; ii<=N; ii++)
+ {
+ s_create_strvec(nu[ii]+nx[ii], workspace->sres_g+ii, v_ptr);
+ v_ptr += (workspace->sdux+ii)->memory_size;
+ }
+
+ for(ii=0; ii<N; ii++)
+ {
+ s_create_strvec(nx[ii+1], workspace->sres_b+ii, v_ptr);
+ v_ptr += (workspace->sdpi+ii)->memory_size;
+ }
+
+ for(ii=0; ii<N; ii++)
+ {
+ s_create_strvec(nx[ii+1], workspace->Pb+ii, v_ptr);
+ v_ptr += (workspace->Pb+ii)->memory_size;
+ }
+
+ for(ii=0; ii<=N; ii++)
+ {
+ s_create_strvec(nb[ii], workspace->sQx_lb+ii, v_ptr);
+ v_ptr += (workspace->sQx_lb+ii)->memory_size;
+ }
+
+ for(ii=0; ii<=N; ii++)
+ {
+ s_create_strvec(nb[ii], workspace->sqx_lb+ii, v_ptr);
+ v_ptr += (workspace->sqx_lb+ii)->memory_size;
+ }
+
+ for(ii=0; ii<=N; ii++)
+ {
+ s_create_strvec(ng[ii], workspace->sQx_lg+ii, v_ptr);
+ v_ptr += (workspace->sQx_lg+ii)->memory_size;
+ }
+
+ for(ii=0; ii<=N; ii++)
+ {
+ s_create_strvec(ng[ii], workspace->sqx_lg+ii, v_ptr);
+ v_ptr += (workspace->sqx_lg+ii)->memory_size;
+ }
+
+ d_create_strvec(nbM, workspace->tmp_nbM, v_ptr);
+ v_ptr += workspace->tmp_nbM->memory_size;
+
+ s_create_strvec(nxM, workspace->tmp_nxM, v_ptr);
+ v_ptr += workspace->tmp_nxM->memory_size;
+
+ d_create_strvec(ngM, workspace->tmp_ngM+0, v_ptr);
+ v_ptr += (workspace->tmp_ngM+0)->memory_size;
+
+ d_create_strvec(ngM, workspace->tmp_ngM+1, v_ptr);
+ v_ptr += (workspace->tmp_ngM+1)->memory_size;
+
+
+
+ rwork->nv = nvt;
+ rwork->ne = net;
+ rwork->nb = nbt;
+ rwork->ng = ngt;
+ rwork->iter_max = arg->iter_max;
+ d_create_ipm_hard_core_qp(rwork, v_ptr);
+ v_ptr += workspace->core_workspace->memsize;
+
+ rwork->alpha_min = arg->alpha_min;
+ rwork->mu_max = arg->mu_max;
+ rwork->mu0 = arg->mu0;
+ rwork->nt_inv = 1.0/(2*nbt+2*ngt);
+
+
+ // alias members of workspace and core_workspace
+ v_ptr = rwork->dv;
+ for(ii=0; ii<=N; ii++)
+ {
+ d_create_strvec(nu[ii]+nx[ii], workspace->dux+ii, v_ptr);
+ v_ptr += (nu[ii]+nx[ii])*sizeof(double);
+ }
+ v_ptr = rwork->dpi;
+ for(ii=0; ii<N; ii++)
+ {
+ d_create_strvec(nx[ii+1], workspace->dpi+ii, v_ptr);
+ v_ptr += (nx[ii+1])*sizeof(double);
+ }
+ v_ptr = rwork->dt_lb;
+ for(ii=0; ii<=N; ii++)
+ {
+ d_create_strvec(nb[ii], workspace->dt_lb+ii, v_ptr);
+ v_ptr += (nb[ii])*sizeof(double);
+ }
+ v_ptr = rwork->dt_lg;
+ for(ii=0; ii<=N; ii++)
+ {
+ d_create_strvec(ng[ii], workspace->dt_lg+ii, v_ptr);
+ v_ptr += (ng[ii])*sizeof(double);
+ }
+ v_ptr = rwork->res_g;
+ for(ii=0; ii<=N; ii++)
+ {
+ d_create_strvec(nu[ii]+nx[ii], workspace->res_g+ii, v_ptr);
+ v_ptr += (nu[ii]+nx[ii])*sizeof(double);
+ }
+ v_ptr = rwork->res_b;
+ for(ii=0; ii<N; ii++)
+ {
+ d_create_strvec(nx[ii+1], workspace->res_b+ii, v_ptr);
+ v_ptr += (nx[ii+1])*sizeof(double);
+ }
+ v_ptr = rwork->res_d;
+ d_create_strvec(2*nbt+2*ngt, workspace->res_d, v_ptr);
+ v_ptr = rwork->res_d_lb;
+ for(ii=0; ii<=N; ii++)
+ {
+ d_create_strvec(nb[ii], workspace->res_d_lb+ii, v_ptr);
+ v_ptr += (nb[ii])*sizeof(double);
+ }
+ v_ptr = rwork->res_d_ub;
+ for(ii=0; ii<=N; ii++)
+ {
+ d_create_strvec(nb[ii], workspace->res_d_ub+ii, v_ptr);
+ v_ptr += (nb[ii])*sizeof(double);
+ }
+ v_ptr = rwork->res_d_lg;
+ for(ii=0; ii<=N; ii++)
+ {
+ d_create_strvec(ng[ii], workspace->res_d_lg+ii, v_ptr);
+ v_ptr += (ng[ii])*sizeof(double);
+ }
+ v_ptr = rwork->res_d_ug;
+ for(ii=0; ii<=N; ii++)
+ {
+ d_create_strvec(ng[ii], workspace->res_d_ug+ii, v_ptr);
+ v_ptr += (ng[ii])*sizeof(double);
+ }
+ v_ptr = rwork->res_m;
+ d_create_strvec(2*nbt+2*ngt, workspace->res_m, v_ptr);
+ v_ptr = rwork->res_m_lb;
+ for(ii=0; ii<=N; ii++)
+ {
+ d_create_strvec(nb[ii], workspace->res_m_lb+ii, v_ptr);
+ v_ptr += (nb[ii])*sizeof(double);
+ }
+ v_ptr = rwork->res_m_ub;
+ for(ii=0; ii<=N; ii++)
+ {
+ d_create_strvec(nb[ii], workspace->res_m_ub+ii, v_ptr);
+ v_ptr += (nb[ii])*sizeof(double);
+ }
+ v_ptr = rwork->res_m_lg;
+ for(ii=0; ii<=N; ii++)
+ {
+ d_create_strvec(ng[ii], workspace->res_m_lg+ii, v_ptr);
+ v_ptr += (ng[ii])*sizeof(double);
+ }
+ v_ptr = rwork->res_m_ug;
+ for(ii=0; ii<=N; ii++)
+ {
+ d_create_strvec(ng[ii], workspace->res_m_ug+ii, v_ptr);
+ v_ptr += (ng[ii])*sizeof(double);
+ }
+ v_ptr = rwork->Qx_lb;
+ for(ii=0; ii<=N; ii++)
+ {
+ d_create_strvec(nb[ii], workspace->Qx_lb+ii, v_ptr);
+ v_ptr += (nb[ii])*sizeof(double);
+ }
+ v_ptr = rwork->Qx_lg;
+ for(ii=0; ii<=N; ii++)
+ {
+ d_create_strvec(ng[ii], workspace->Qx_lg+ii, v_ptr);
+ v_ptr += (ng[ii])*sizeof(double);
+ }
+ v_ptr = rwork->qx_lb;
+ for(ii=0; ii<=N; ii++)
+ {
+ d_create_strvec(nb[ii], workspace->qx_lb+ii, v_ptr);
+ v_ptr += (nb[ii])*sizeof(double);
+ }
+ v_ptr = rwork->qx_lg;
+ for(ii=0; ii<=N; ii++)
+ {
+ d_create_strvec(ng[ii], workspace->qx_lg+ii, v_ptr);
+ v_ptr += (ng[ii])*sizeof(double);
+ }
+ workspace->stat = rwork->stat;
+
+ return;
+
+ }
+
+
+
+void m_solve_ipm_hard_ocp_qp(struct d_ocp_qp *qp, struct s_ocp_qp *s_qp, struct d_ocp_qp_sol *qp_sol, struct m_ipm_hard_ocp_qp_workspace *ws)
+ {
+
+ struct d_ipm_hard_core_qp_workspace *cws = ws->core_workspace;
+
+ // alias d_ocp_workspace to m_ocp_workspace
+ struct d_ipm_hard_ocp_qp_workspace dws;
+ dws.core_workspace = ws->core_workspace;
+ dws.dux = ws->dux;
+ dws.dpi = ws->dpi;
+ dws.dt_lb = ws->dt_lb;
+ dws.dt_lg = ws->dt_lg;
+ dws.res_g = ws->res_g;
+ dws.res_b = ws->res_b;
+ dws.res_d = ws->res_d;
+ dws.res_d_lb = ws->res_d_lb;
+ dws.res_d_ub = ws->res_d_ub;
+ dws.res_d_lg = ws->res_d_lg;
+ dws.res_d_ug = ws->res_d_ug;
+ dws.res_m = ws->res_m;
+ dws.res_m_lb = ws->res_m_lb;
+ dws.res_m_ub = ws->res_m_ub;
+ dws.res_m_lg = ws->res_m_lg;
+ dws.res_m_ug = ws->res_m_ug;
+ dws.tmp_nbM = ws->tmp_nbM;
+ dws.tmp_ngM = ws->tmp_ngM;
+
+ // alias qp vectors into qp
+ cws->d_lb = qp->d_lb->pa;
+ cws->d_ub = qp->d_ub->pa;
+ cws->d_lg = qp->d_lg->pa;
+ cws->d_ug = qp->d_ug->pa;
+
+ // alias qp vectors into qp_sol
+ cws->v = qp_sol->ux->pa;
+ cws->pi = qp_sol->pi->pa;
+ cws->lam = qp_sol->lam_lb->pa;
+ cws->lam_lb = qp_sol->lam_lb->pa;
+ cws->lam_ub = qp_sol->lam_ub->pa;
+ cws->lam_lg = qp_sol->lam_lg->pa;
+ cws->lam_ug = qp_sol->lam_ug->pa;
+ cws->t = qp_sol->t_lb->pa;
+ cws->t_lb = qp_sol->t_lb->pa;
+ cws->t_ub = qp_sol->t_ub->pa;
+ cws->t_lg = qp_sol->t_lg->pa;
+ cws->t_ug = qp_sol->t_ug->pa;
+
+ if(cws->nb+cws->ng==0)
+ {
+ //
+ d_init_var_hard_ocp_qp(qp, qp_sol, &dws);
+ //
+ d_compute_res_hard_ocp_qp(qp, qp_sol, &dws);
+ cws->mu = dws.res_mu;
+ ws->res_mu = dws.res_mu;
+ //
+ m_fact_solve_kkt_step_hard_ocp_qp(qp, s_qp, ws);
+ //
+ cws->alpha = 1.0;
+ d_update_var_hard_qp(cws);
+ //
+ d_compute_res_hard_ocp_qp(qp, qp_sol, &dws);
+ cws->mu = dws.res_mu;
+ ws->res_mu = dws.res_mu;
+ //
+ ws->compute_Pb = 1;
+ m_solve_kkt_step_hard_ocp_qp(qp, s_qp, ws);
+ //
+ cws->alpha = 1.0;
+ d_update_var_hard_qp(cws);
+ //
+ d_compute_res_hard_ocp_qp(qp, qp_sol, &dws);
+ cws->mu = dws.res_mu;
+ ws->res_mu = dws.res_mu;
+ //
+ ws->iter = 0;
+ return;
+ }
+
+ // init solver
+ d_init_var_hard_ocp_qp(qp, qp_sol, &dws);
+
+ // compute residuals
+ d_compute_res_hard_ocp_qp(qp, qp_sol, &dws);
+ cws->mu = dws.res_mu;
+ ws->res_mu = dws.res_mu;
+
+ int kk;
+ for(kk=0; kk<cws->iter_max & cws->mu>cws->mu_max; kk++)
+ {
+
+ // fact and solve kkt
+ m_fact_solve_kkt_step_hard_ocp_qp(qp, s_qp, ws);
+
+ // alpha
+ d_compute_alpha_hard_qp(cws);
+ cws->stat[5*kk+0] = cws->alpha;
+
+ //
+ d_update_var_hard_qp(cws);
+
+ // compute residuals
+ d_compute_res_hard_ocp_qp(qp, qp_sol, &dws);
+ cws->mu = dws.res_mu;
+ ws->res_mu = dws.res_mu;
+ cws->stat[5*kk+1] = ws->res_mu;
+
+ }
+
+ ws->iter = kk;
+
+ return;
+
+ }
+
+
+
+void m_solve_ipm2_hard_ocp_qp(struct d_ocp_qp *qp, struct s_ocp_qp *s_qp, struct d_ocp_qp_sol *qp_sol, struct m_ipm_hard_ocp_qp_workspace *ws)
+ {
+
+ struct d_ipm_hard_core_qp_workspace *cws = ws->core_workspace;
+
+ // alias d_ocp_workspace to m_ocp_workspace
+ struct d_ipm_hard_ocp_qp_workspace dws;
+ dws.core_workspace = ws->core_workspace;
+ dws.dux = ws->dux;
+ dws.dpi = ws->dpi;
+ dws.dt_lb = ws->dt_lb;
+ dws.dt_lg = ws->dt_lg;
+ dws.res_g = ws->res_g;
+ dws.res_b = ws->res_b;
+ dws.res_d = ws->res_d;
+ dws.res_d_lb = ws->res_d_lb;
+ dws.res_d_ub = ws->res_d_ub;
+ dws.res_d_lg = ws->res_d_lg;
+ dws.res_d_ug = ws->res_d_ug;
+ dws.res_m = ws->res_m;
+ dws.res_m_lb = ws->res_m_lb;
+ dws.res_m_ub = ws->res_m_ub;
+ dws.res_m_lg = ws->res_m_lg;
+ dws.res_m_ug = ws->res_m_ug;
+ dws.tmp_nbM = ws->tmp_nbM;
+ dws.tmp_ngM = ws->tmp_ngM;
+
+ // alias qp vectors into qp
+ cws->d_lb = qp->d_lb->pa;
+ cws->d_ub = qp->d_ub->pa;
+ cws->d_lg = qp->d_lg->pa;
+ cws->d_ug = qp->d_ug->pa;
+
+ // alias qp vectors into qp_sol
+ cws->v = qp_sol->ux->pa;
+ cws->pi = qp_sol->pi->pa;
+ cws->lam = qp_sol->lam_lb->pa;
+ cws->lam_lb = qp_sol->lam_lb->pa;
+ cws->lam_ub = qp_sol->lam_ub->pa;
+ cws->lam_lg = qp_sol->lam_lg->pa;
+ cws->lam_ug = qp_sol->lam_ug->pa;
+ cws->t = qp_sol->t_lb->pa;
+ cws->t_lb = qp_sol->t_lb->pa;
+ cws->t_ub = qp_sol->t_ub->pa;
+ cws->t_lg = qp_sol->t_lg->pa;
+ cws->t_ug = qp_sol->t_ug->pa;
+
+ double tmp;
+
+ if(cws->nb+cws->ng==0)
+ {
+ //
+ d_init_var_hard_ocp_qp(qp, qp_sol, &dws);
+ //
+ d_compute_res_hard_ocp_qp(qp, qp_sol, &dws);
+ cws->mu = dws.res_mu;
+ ws->res_mu = dws.res_mu;
+ //
+ m_fact_solve_kkt_step_hard_ocp_qp(qp, s_qp, ws);
+ //
+ cws->alpha = 1.0;
+ d_update_var_hard_qp(cws);
+ //
+ d_compute_res_hard_ocp_qp(qp, qp_sol, &dws);
+ cws->mu = dws.res_mu;
+ ws->res_mu = dws.res_mu;
+ //
+ ws->compute_Pb = 1;
+ m_solve_kkt_step_hard_ocp_qp(qp, s_qp, ws);
+ //
+ cws->alpha = 1.0;
+ d_update_var_hard_qp(cws);
+ //
+ d_compute_res_hard_ocp_qp(qp, qp_sol, &dws);
+ cws->mu = dws.res_mu;
+ ws->res_mu = dws.res_mu;
+ //
+ ws->iter = 0;
+ return;
+ }
+
+ // init solver
+ d_init_var_hard_ocp_qp(qp, qp_sol, &dws);
+
+ // compute residuals
+ d_compute_res_hard_ocp_qp(qp, qp_sol, &dws);
+ cws->mu = dws.res_mu;
+ ws->res_mu = dws.res_mu;
+
+#if 0
+ printf("\nres_g\n");
+ for(int ii=0; ii<=qp->N; ii++)
+ {
+ PRINT_E_TRAN_STRVEC(qp->nx[ii]+qp->nu[ii], ws->res_g+ii, 0);
+ }
+ printf("\nres_b\n");
+ for(int ii=0; ii<qp->N; ii++)
+ {
+ PRINT_E_TRAN_STRVEC(qp->nx[ii+1], ws->res_b+ii, 0);
+ }
+ printf("\nres_d_lb\n");
+ for(int ii=0; ii<qp->N; ii++)
+ {
+ PRINT_E_TRAN_STRVEC(qp->nb[ii], ws->res_d_lb+ii, 0);
+ }
+ printf("\nres_d_ub\n");
+ for(int ii=0; ii<qp->N; ii++)
+ {
+ PRINT_E_TRAN_STRVEC(qp->nb[ii], ws->res_d_ub+ii, 0);
+ }
+ printf("\nres_d_lg\n");
+ for(int ii=0; ii<qp->N; ii++)
+ {
+ PRINT_E_TRAN_STRVEC(qp->ng[ii], ws->res_d_lg+ii, 0);
+ }
+ printf("\nres_d_ug\n");
+ for(int ii=0; ii<qp->N; ii++)
+ {
+ PRINT_E_TRAN_STRVEC(qp->ng[ii], ws->res_d_ug+ii, 0);
+ }
+ printf("\nres_m_lb\n");
+ for(int ii=0; ii<qp->N; ii++)
+ {
+ PRINT_E_TRAN_STRVEC(qp->nb[ii], ws->res_m_lb+ii, 0);
+ }
+ printf("\nres_m_ub\n");
+ for(int ii=0; ii<qp->N; ii++)
+ {
+ PRINT_E_TRAN_STRVEC(qp->nb[ii], ws->res_m_ub+ii, 0);
+ }
+ printf("\nres_m_lg\n");
+ for(int ii=0; ii<qp->N; ii++)
+ {
+ PRINT_E_TRAN_STRVEC(qp->ng[ii], ws->res_m_lg+ii, 0);
+ }
+ printf("\nres_m_ug\n");
+ for(int ii=0; ii<qp->N; ii++)
+ {
+ PRINT_E_TRAN_STRVEC(qp->ng[ii], ws->res_m_ug+ii, 0);
+ }
+ exit(1);
+#endif
+
+#if 0
+ int ii;
+ for(ii=0; ii<1; ii++)
+ {
+ cws->sigma = 1.0;
+ cws->stat[5*kk+2] = cws->sigma;
+ COMPUTE_CENTERING_CORRECTION_HARD_QP(cws);
+ FACT_SOLVE_KKT_STEP_HARD_OCP_QP(qp, ws);
+ COMPUTE_ALPHA_HARD_QP(cws);
+ cws->stat[5*kk+3] = cws->alpha;
+ UPDATE_VAR_HARD_QP(cws);
+ COMPUTE_RES_HARD_OCP_QP(qp, qp_sol, ws);
+ cws->mu = ws->res_mu;
+ cws->stat[5*kk+4] = ws->res_mu;
+ kk++;
+ }
+// ws->iter = kk;
+// return;
+#endif
+
+ int kk = 0;
+ for(; kk<cws->iter_max & cws->mu>cws->mu_max; kk++)
+ {
+
+ // fact and solve kkt
+ m_fact_solve_kkt_step_hard_ocp_qp(qp, s_qp, ws);
+
+#if 0
+ printf("\ndux\n");
+ for(int ii=0; ii<=qp->N; ii++)
+ {
+ PRINT_E_TRAN_STRVEC(qp->nx[ii]+qp->nu[ii], ws->dux+ii, 0);
+ }
+ printf("\ndpi\n");
+ for(int ii=0; ii<qp->N; ii++)
+ {
+ PRINT_E_TRAN_STRVEC(qp->nx[ii+1], ws->dpi+ii, 0);
+ }
+ printf("\ndt\n");
+ for(int ii=0; ii<qp->N; ii++)
+ {
+ PRINT_E_TRAN_STRVEC(2*qp->nb[ii]+2*qp->ng[ii], ws->dt_lb+ii, 0);
+ }
+ exit(1);
+#endif
+ // alpha
+ d_compute_alpha_hard_qp(cws);
+ cws->stat[5*kk+0] = cws->alpha;
+
+ // mu_aff
+ d_compute_mu_aff_hard_qp(cws);
+ cws->stat[5*kk+1] = cws->mu_aff;
+
+ tmp = cws->mu_aff/cws->mu;
+ cws->sigma = tmp*tmp*tmp;
+ cws->stat[5*kk+2] = cws->sigma;
+
+ d_compute_centering_correction_hard_qp(cws);
+
+ // fact and solve kkt
+ ws->compute_Pb = 0;
+ m_solve_kkt_step_hard_ocp_qp(qp, s_qp, ws);
+
+#if 0
+int ii;
+for(ii=0; ii<=qp->N; ii++)
+ d_print_tran_strvec(qp->nu[ii]+qp->nx[ii], ws->dux+ii, 0);
+for(ii=0; ii<qp->N; ii++)
+ d_print_tran_strvec(qp->nx[ii+1], ws->dpi+ii, 0);
+exit(1);
+#endif
+ // alpha
+ d_compute_alpha_hard_qp(cws);
+ cws->stat[5*kk+3] = cws->alpha;
+
+ //
+ d_update_var_hard_qp(cws);
+
+ // compute residuals
+ d_compute_res_hard_ocp_qp(qp, qp_sol, &dws);
+ cws->mu = dws.res_mu;
+ ws->res_mu = dws.res_mu;
+ cws->stat[5*kk+4] = ws->res_mu;
+
+ }
+
+ ws->iter = kk;
+
+ return;
+
+ }
+
+
+
+
diff --git a/ocp_qp/m_ocp_qp_kkt.c b/ocp_qp/m_ocp_qp_kkt.c
new file mode 100644
index 0000000..b3b5a71
--- /dev/null
+++ b/ocp_qp/m_ocp_qp_kkt.c
@@ -0,0 +1,426 @@
+/**************************************************************************************************
+* *
+* 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 <math.h>
+
+#include <blasfeo_target.h>
+#include <blasfeo_common.h>
+#include <blasfeo_d_aux.h>
+#include <blasfeo_s_aux.h>
+#include <blasfeo_m_aux.h>
+#include <blasfeo_d_blas.h>
+#include <blasfeo_s_blas.h>
+
+#include "../include/hpipm_d_ocp_qp.h"
+#include "../include/hpipm_s_ocp_qp.h"
+#include "../include/hpipm_d_ocp_qp_sol.h"
+#include "../include/hpipm_d_ocp_qp_ipm_hard.h"
+#include "../include/hpipm_m_ocp_qp_ipm_hard.h"
+#include "../include/hpipm_d_core_qp_ipm_hard.h"
+#include "../include/hpipm_d_core_qp_ipm_hard_aux.h"
+
+
+
+// backward Riccati recursion
+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)
+ {
+
+ int N = s_qp->N;
+ int *nx = s_qp->nx;
+ int *nu = s_qp->nu;
+ int *nb = s_qp->nb;
+ int *ng = s_qp->ng;
+
+ struct s_strmat *BAbt = s_qp->BAbt;
+ struct s_strmat *RSQrq = s_qp->RSQrq;
+ struct s_strmat *DCt = s_qp->DCt;
+ struct d_strmat *d_DCt = d_qp->DCt;
+ int **idxb = s_qp->idxb;
+ int **d_idxb = d_qp->idxb;
+
+ struct s_strmat *L = ws->L;
+ struct s_strmat *AL = ws->AL;
+ struct d_strvec *d_res_b = ws->res_b;
+ struct d_strvec *d_res_g = ws->res_g;
+ struct s_strvec *res_b = ws->sres_b;
+ struct s_strvec *res_g = ws->sres_g;
+ struct d_strvec *d_dux = ws->dux;
+ struct d_strvec *d_dpi = ws->dpi;
+ struct s_strvec *dux = ws->sdux;
+ struct s_strvec *dpi = ws->sdpi;
+ struct d_strvec *d_dt_lb = ws->dt_lb;
+ struct d_strvec *d_dt_lg = ws->dt_lg;
+ struct d_strvec *d_Qx_lg = ws->Qx_lg;
+ struct d_strvec *d_Qx_lb = ws->Qx_lb;
+ struct d_strvec *d_qx_lg = ws->qx_lg;
+ struct d_strvec *d_qx_lb = ws->qx_lb;
+ struct s_strvec *Qx_lg = ws->sQx_lg;
+ struct s_strvec *Qx_lb = ws->sQx_lb;
+ struct s_strvec *qx_lg = ws->sqx_lg;
+ struct s_strvec *qx_lb = ws->sqx_lb;
+ struct s_strvec *Pb = ws->Pb;
+ struct s_strvec *tmp_nxM = ws->tmp_nxM;
+
+ //
+ int ii;
+
+ struct d_ipm_hard_core_qp_workspace *cws = ws->core_workspace;
+
+// if(nb>0 | ng>0)
+// {
+ d_compute_Qx_qx_hard_qp(cws);
+// }
+
+
+
+ // cvt double => single
+ for(ii=0; ii<N; ii++)
+ {
+ m_cvt_d2s_strvec(nu[ii]+nx[ii], d_res_g+ii, 0, res_g+ii, 0);
+ m_cvt_d2s_strvec(nx[ii+1], d_res_b+ii, 0, res_b+ii, 0);
+ m_cvt_d2s_strvec(nb[ii], d_Qx_lb+ii, 0, Qx_lb+ii, 0);
+ m_cvt_d2s_strvec(nb[ii], d_qx_lb+ii, 0, qx_lb+ii, 0);
+ m_cvt_d2s_strvec(ng[ii], d_Qx_lg+ii, 0, Qx_lg+ii, 0);
+ m_cvt_d2s_strvec(ng[ii], d_qx_lg+ii, 0, qx_lg+ii, 0);
+ }
+ ii = N;
+ m_cvt_d2s_strvec(nu[ii]+nx[ii], d_res_g+ii, 0, res_g+ii, 0);
+ m_cvt_d2s_strvec(nb[ii], d_Qx_lb+ii, 0, Qx_lb+ii, 0);
+ m_cvt_d2s_strvec(nb[ii], d_qx_lb+ii, 0, qx_lb+ii, 0);
+ m_cvt_d2s_strvec(ng[ii], d_Qx_lg+ii, 0, Qx_lg+ii, 0);
+ m_cvt_d2s_strvec(ng[ii], d_qx_lg+ii, 0, qx_lg+ii, 0);
+
+
+
+ // factorization and backward substitution
+
+ // last stage
+#if defined(DOUBLE_PRECISION)
+ strcp_l_libstr(nu[N]+nx[N], RSQrq+N, 0, 0, L+N, 0, 0); // TODO dtrcp_l_libstr with m and n, for m>=n
+#else
+ sgecp_libstr(nu[N]+nx[N], nu[N]+nx[N], RSQrq+N, 0, 0, L+N, 0, 0); // TODO dtrcp_l_libstr with m and n, for m>=n
+#endif
+ srowin_libstr(nu[N]+nx[N], 1.0, res_g+N, 0, L+N, nu[N]+nx[N], 0);
+ if(nb[N]>0)
+ {
+ sdiaad_sp_libstr(nb[N], 1.0, Qx_lb+N, 0, idxb[N], L+N, 0, 0);
+ srowad_sp_libstr(nb[N], 1.0, qx_lb+N, 0, idxb[N], L+N, nu[N]+nx[N], 0);
+ }
+ if(ng[N]>0)
+ {
+ sgemm_r_diag_libstr(nu[N]+nx[N], ng[N], 1.0, DCt+N, 0, 0, Qx_lg+N, 0, 0.0, AL+0, 0, 0, AL+0, 0, 0);
+ srowin_libstr(ng[N], 1.0, qx_lg+N, 0, AL+0, nu[N]+nx[N], 0);
+ ssyrk_spotrf_ln_libstr(nu[N]+nx[N]+1, nu[N]+nx[N], ng[N], AL+0, 0, 0, DCt+N, 0, 0, L+N, 0, 0, L+N, 0, 0);
+ }
+ else
+ {
+ spotrf_l_mn_libstr(nu[N]+nx[N]+1, nu[N]+nx[N], L+N, 0, 0, L+N, 0, 0);
+ }
+
+ // middle stages
+ for(ii=0; ii<N; ii++)
+ {
+ sgecp_libstr(nu[N-ii-1]+nx[N-ii-1], nx[N-ii], BAbt+(N-ii-1), 0, 0, AL, 0, 0);
+ srowin_libstr(nx[N-ii], 1.0, res_b+(N-ii-1), 0, AL, nu[N-ii-1]+nx[N-ii-1], 0);
+ strmm_rlnn_libstr(nu[N-ii-1]+nx[N-ii-1]+1, nx[N-ii], 1.0, L+(N-ii), nu[N-ii], nu[N-ii], AL, 0, 0, AL, 0, 0);
+ srowex_libstr(nx[N-ii], 1.0, AL, nu[N-ii-1]+nx[N-ii-1], 0, tmp_nxM, 0);
+ strmv_lnn_libstr(nx[N-ii], nx[N-ii], L+(N-ii), nu[N-ii], nu[N-ii], tmp_nxM, 0, Pb+(N-ii-1), 0);
+ sgead_libstr(1, nx[N-ii], 1.0, L+(N-ii), nu[N-ii]+nx[N-ii], nu[N-ii], AL, nu[N-ii-1]+nx[N-ii-1], 0);
+
+#if defined(DOUBLE_PRECISION)
+ strcp_l_libstr(nu[N-ii-1]+nx[N-ii-1], RSQrq+(N-ii-1), 0, 0, L+(N-ii-1), 0, 0);
+#else
+ sgecp_libstr(nu[N-ii-1]+nx[N-ii-1], nu[N-ii-1]+nx[N-ii-1], RSQrq+(N-ii-1), 0, 0, L+(N-ii-1), 0, 0);
+#endif
+ srowin_libstr(nu[N-ii-1]+nx[N-ii-1], 1.0, res_g+(N-ii-1), 0, L+(N-ii-1), nu[N-ii-1]+nx[N-ii-1], 0);
+
+ if(nb[N-ii-1]>0)
+ {
+ sdiaad_sp_libstr(nb[N-ii-1], 1.0, Qx_lb+(N-ii-1), 0, idxb[N-ii-1], L+(N-ii-1), 0, 0);
+ srowad_sp_libstr(nb[N-ii-1], 1.0, qx_lb+(N-ii-1), 0, idxb[N-ii-1], L+(N-ii-1), nu[N-ii-1]+nx[N-ii-1], 0);
+ }
+
+ if(ng[N-ii-1]>0)
+ {
+ sgemm_r_diag_libstr(nu[N-ii-1]+nx[N-ii-1], ng[N-ii-1], 1.0, DCt+N-ii-1, 0, 0, Qx_lg+N-ii-1, 0, 0.0, AL+0, 0, nx[N-ii], AL+0, 0, nx[N-ii]);
+ srowin_libstr(ng[N-ii-1], 1.0, qx_lg+N-ii-1, 0, AL+0, nu[N-ii-1]+nx[N-ii-1], nx[N-ii]);
+ sgecp_libstr(nu[N-ii-1]+nx[N-ii-1], nx[N-ii], AL+0, 0, 0, AL+1, 0, 0);
+ sgecp_libstr(nu[N-ii-1]+nx[N-ii-1], ng[N-ii-1], DCt+N-ii-1, 0, 0, AL+1, 0, nx[N-ii]);
+ ssyrk_spotrf_ln_libstr(nu[N-ii-1]+nx[N-ii-1]+1, nu[N-ii-1]+nx[N-ii-1], nx[N-ii]+ng[N-ii-1], AL+0, 0, 0, AL+1, 0, 0, L+N-ii-1, 0, 0, L+N-ii-1, 0, 0);
+ }
+ else
+ {
+ ssyrk_spotrf_ln_libstr(nu[N-ii-1]+nx[N-ii-1]+1, nu[N-ii-1]+nx[N-ii-1], nx[N-ii], AL, 0, 0, AL, 0, 0, L+(N-ii-1), 0, 0, L+(N-ii-1), 0, 0);
+ }
+
+// d_print_strmat(nu[N-ii-1]+nx[N-ii-1]+1, nu[N-ii-1]+nx[N-ii-1], L+(N-ii-1), 0, 0);
+ }
+
+ // forward substitution
+
+ // first stage
+ ii = 0;
+ srowex_libstr(nu[ii]+nx[ii], -1.0, L+(ii), nu[ii]+nx[ii], 0, dux+ii, 0);
+ strsv_ltn_libstr(nu[ii]+nx[ii], L+ii, 0, 0, dux+ii, 0, dux+ii, 0);
+ sgemv_t_libstr(nu[ii]+nx[ii], nx[ii+1], 1.0, BAbt+ii, 0, 0, dux+ii, 0, 1.0, res_b+ii, 0, dux+(ii+1), nu[ii+1]);
+ srowex_libstr(nx[ii+1], 1.0, L+(ii+1), nu[ii+1]+nx[ii+1], nu[ii+1], tmp_nxM, 0);
+ strmv_ltn_libstr(nx[ii+1], nx[ii+1], L+(ii+1), nu[ii+1], nu[ii+1], dux+(ii+1), nu[ii+1], dpi+ii, 0);
+ saxpy_libstr(nx[ii+1], 1.0, tmp_nxM, 0, dpi+ii, 0, dpi+ii, 0);
+ strmv_lnn_libstr(nx[ii+1], nx[ii+1], L+(ii+1), nu[ii+1], nu[ii+1], dpi+ii, 0, dpi+ii, 0);
+
+// d_print_tran_strvec(nu[ii]+nx[ii], dux+ii, 0);
+
+ // middle stages
+ for(ii=1; ii<N; ii++)
+ {
+ srowex_libstr(nu[ii], -1.0, L+(ii), nu[ii]+nx[ii], 0, dux+ii, 0);
+ strsv_ltn_mn_libstr(nu[ii]+nx[ii], nu[ii], L+ii, 0, 0, dux+ii, 0, dux+ii, 0);
+ sgemv_t_libstr(nu[ii]+nx[ii], nx[ii+1], 1.0, BAbt+ii, 0, 0, dux+ii, 0, 1.0, res_b+ii, 0, dux+(ii+1), nu[ii+1]);
+ srowex_libstr(nx[ii+1], 1.0, L+(ii+1), nu[ii+1]+nx[ii+1], nu[ii+1], tmp_nxM, 0);
+ strmv_ltn_libstr(nx[ii+1], nx[ii+1], L+(ii+1), nu[ii+1], nu[ii+1], dux+(ii+1), nu[ii+1], dpi+ii, 0);
+ saxpy_libstr(nx[ii+1], 1.0, tmp_nxM, 0, dpi+ii, 0, dpi+ii, 0);
+ strmv_lnn_libstr(nx[ii+1], nx[ii+1], L+(ii+1), nu[ii+1], nu[ii+1], dpi+ii, 0, dpi+ii, 0);
+
+// d_print_tran_strvec(nu[ii]+nx[ii], dux+ii, 0);
+ }
+
+
+
+ // cvt single => double
+ for(ii=0; ii<N; ii++)
+ {
+ m_cvt_s2d_strvec(nu[ii]+nx[ii], dux+ii, 0, d_dux+ii, 0);
+ m_cvt_s2d_strvec(nx[ii+1], dpi+ii, 0, d_dpi+ii, 0);
+ }
+ ii = N;
+ m_cvt_s2d_strvec(nu[ii]+nx[ii], dux+ii, 0, d_dux+ii, 0);
+
+
+
+// if(nb>0)
+// {
+ for(ii=0; ii<=N; ii++)
+ dvecex_sp_libstr(nb[ii], 1.0, d_idxb[ii], d_dux+ii, 0, d_dt_lb+ii, 0);
+// }
+
+// if(ng>0)
+// {
+ for(ii=0; ii<=N; ii++)
+ dgemv_t_libstr(nu[ii]+nx[ii], ng[ii], 1.0, d_DCt+ii, 0, 0, d_dux+ii, 0, 0.0, d_dt_lg+ii, 0, d_dt_lg+ii, 0);
+// }
+
+// if(nb>0 | ng>0)
+// {
+ d_compute_lam_t_hard_qp(cws);
+// }
+
+ return;
+
+ }
+
+
+
+// backward Riccati recursion
+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)
+ {
+
+ int N = s_qp->N;
+ int *nx = s_qp->nx;
+ int *nu = s_qp->nu;
+ int *nb = s_qp->nb;
+ int *ng = s_qp->ng;
+
+ struct s_strmat *BAbt = s_qp->BAbt;
+ struct s_strmat *RSQrq = s_qp->RSQrq;
+ struct s_strmat *DCt = s_qp->DCt;
+ struct d_strmat *d_DCt = d_qp->DCt;
+ int **idxb = s_qp->idxb;
+ int **d_idxb = d_qp->idxb;
+
+ struct s_strmat *L = ws->L;
+ struct s_strmat *AL = ws->AL;
+ struct d_strvec *d_res_b = ws->res_b;
+ struct d_strvec *d_res_g = ws->res_g;
+ struct s_strvec *res_b = ws->sres_b;
+ struct s_strvec *res_g = ws->sres_g;
+ struct d_strvec *d_dux = ws->dux;
+ struct d_strvec *d_dpi = ws->dpi;
+ struct s_strvec *dux = ws->sdux;
+ struct s_strvec *dpi = ws->sdpi;
+ struct d_strvec *d_dt_lb = ws->dt_lb;
+ struct d_strvec *d_dt_lg = ws->dt_lg;
+ struct d_strvec *d_qx_lg = ws->qx_lg;
+ struct d_strvec *d_qx_lb = ws->qx_lb;
+ struct s_strvec *qx_lg = ws->sqx_lg;
+ struct s_strvec *qx_lb = ws->sqx_lb;
+ struct s_strvec *Pb = ws->Pb;
+ struct s_strvec *tmp_nxM = ws->tmp_nxM;
+
+ //
+ int ii;
+
+ struct d_ipm_hard_core_qp_workspace *cws = ws->core_workspace;
+
+// if(nb>0 | ng>0)
+// {
+ d_compute_qx_hard_qp(cws);
+// }
+
+
+
+ // cvt double => single
+ for(ii=0; ii<N; ii++)
+ {
+ m_cvt_d2s_strvec(nu[ii]+nx[ii], d_res_g+ii, 0, res_g+ii, 0);
+ m_cvt_d2s_strvec(nx[ii+1], d_res_b+ii, 0, res_b+ii, 0);
+ m_cvt_d2s_strvec(nb[ii], d_qx_lb+ii, 0, qx_lb+ii, 0);
+ m_cvt_d2s_strvec(ng[ii], d_qx_lg+ii, 0, qx_lg+ii, 0);
+ }
+ ii = N;
+ m_cvt_d2s_strvec(nu[ii]+nx[ii], d_res_g+ii, 0, res_g+ii, 0);
+ m_cvt_d2s_strvec(nb[ii], d_qx_lb+ii, 0, qx_lb+ii, 0);
+ m_cvt_d2s_strvec(ng[ii], d_qx_lg+ii, 0, qx_lg+ii, 0);
+
+
+
+ // backward substitution
+
+ // last stage
+ sveccp_libstr(nu[N]+nx[N], res_g+N, 0, dux+N, 0);
+ if(nb[N]>0)
+ {
+ svecad_sp_libstr(nb[N], 1.0, qx_lb+N, 0, idxb[N], dux+N, 0);
+ }
+ // general constraints
+ if(ng[N]>0)
+ {
+ sgemv_n_libstr(nu[N]+nx[N], ng[N], 1.0, DCt+N, 0, 0, qx_lg+N, 0, 1.0, dux+N, 0, dux+N, 0);
+ }
+
+ // middle stages
+ for(ii=0; ii<N-1; ii++)
+ {
+ sveccp_libstr(nu[N-ii-1]+nx[N-ii-1], res_g+N-ii-1, 0, dux+N-ii-1, 0);
+ if(nb[N-ii-1]>0)
+ {
+ svecad_sp_libstr(nb[N-ii-1], 1.0, qx_lb+N-ii-1, 0, idxb[N-ii-1], dux+N-ii-1, 0);
+ }
+ if(ng[N-ii-1]>0)
+ {
+ sgemv_n_libstr(nu[N-ii-1]+nx[N-ii-1], ng[N-ii-1], 1.0, DCt+N-ii-1, 0, 0, qx_lg+N-ii-1, 0, 1.0, dux+N-ii-1, 0, dux+N-ii-1, 0);
+ }
+ if(ws->compute_Pb)
+ {
+ strmv_ltn_libstr(nx[N-ii], nx[N-ii], L+(N-ii), nu[N-ii], nu[N-ii], res_b+N-ii-1, 0, Pb+(N-ii-1), 0);
+ strmv_lnn_libstr(nx[N-ii], nx[N-ii], L+(N-ii), nu[N-ii], nu[N-ii], Pb+(N-ii-1), 0, Pb+(N-ii-1), 0);
+ }
+ saxpy_libstr(nx[N-ii], 1.0, dux+N-ii, nu[N-ii], Pb+N-ii-1, 0, tmp_nxM, 0);
+ sgemv_n_libstr(nu[N-ii-1]+nx[N-ii-1], nx[N-ii], 1.0, BAbt+N-ii-1, 0, 0, tmp_nxM, 0, 1.0, dux+N-ii-1, 0, dux+N-ii-1, 0);
+ strsv_lnn_mn_libstr(nu[N-ii-1]+nx[N-ii-1], nu[N-ii-1], L+N-ii-1, 0, 0, dux+N-ii-1, 0, dux+N-ii-1, 0);
+ }
+
+ // first stage
+ ii = N-1;
+ sveccp_libstr(nu[N-ii-1]+nx[N-ii-1], res_g+N-ii-1, 0, dux+N-ii-1, 0);
+ if(nb[N-ii-1]>0)
+ {
+ svecad_sp_libstr(nb[N-ii-1], 1.0, qx_lb+N-ii-1, 0, idxb[N-ii-1], dux+N-ii-1, 0);
+ }
+ if(ng[N-ii-1]>0)
+ {
+ sgemv_n_libstr(nu[N-ii-1]+nx[N-ii-1], ng[N-ii-1], 1.0, DCt+N-ii-1, 0, 0, qx_lg+N-ii-1, 0, 1.0, dux+N-ii-1, 0, dux+N-ii-1, 0);
+ }
+ if(ws->compute_Pb)
+ {
+ strmv_ltn_libstr(nx[N-ii], nx[N-ii], L+(N-ii), nu[N-ii], nu[N-ii], res_b+N-ii-1, 0, Pb+(N-ii-1), 0);
+ strmv_lnn_libstr(nx[N-ii], nx[N-ii], L+(N-ii), nu[N-ii], nu[N-ii], Pb+(N-ii-1), 0, Pb+(N-ii-1), 0);
+ }
+ saxpy_libstr(nx[N-ii], 1.0, dux+N-ii, nu[N-ii], Pb+N-ii-1, 0, tmp_nxM, 0);
+ sgemv_n_libstr(nu[N-ii-1]+nx[N-ii-1], nx[N-ii], 1.0, BAbt+N-ii-1, 0, 0, tmp_nxM, 0, 1.0, dux+N-ii-1, 0, dux+N-ii-1, 0);
+ strsv_lnn_libstr(nu[N-ii-1]+nx[N-ii-1], L+N-ii-1, 0, 0, dux+N-ii-1, 0, dux+N-ii-1, 0);
+
+ // first stage
+ ii = 0;
+ sveccp_libstr(nx[ii+1], dux+ii+1, nu[ii+1], dpi+ii, 0);
+ svecsc_libstr(nu[ii]+nx[ii], -1.0, dux+ii, 0);
+ strsv_ltn_libstr(nu[ii]+nx[ii], L+ii, 0, 0, dux+ii, 0, dux+ii, 0);
+ sgemv_t_libstr(nu[ii]+nx[ii], nx[ii+1], 1.0, BAbt+ii, 0, 0, dux+ii, 0, 1.0, res_b+ii, 0, dux+ii+1, nu[ii+1]);
+ sveccp_libstr(nx[ii+1], dux+ii+1, nu[ii+1], tmp_nxM, 0);
+ strmv_ltn_libstr(nx[ii+1], nx[ii+1], L+ii+1, nu[ii+1], nu[ii+1], tmp_nxM, 0, tmp_nxM, 0);
+ strmv_lnn_libstr(nx[ii+1], nx[ii+1], L+ii+1, nu[ii+1], nu[ii+1], tmp_nxM, 0, tmp_nxM, 0);
+ saxpy_libstr(nx[ii+1], 1.0, tmp_nxM, 0, dpi+ii, 0, dpi+ii, 0);
+
+ // middle stages
+ for(ii=1; ii<N; ii++)
+ {
+ sveccp_libstr(nx[ii+1], dux+ii+1, nu[ii+1], dpi+ii, 0);
+ svecsc_libstr(nu[ii], -1.0, dux+ii, 0);
+ strsv_ltn_mn_libstr(nu[ii]+nx[ii], nu[ii], L+ii, 0, 0, dux+ii, 0, dux+ii, 0);
+ sgemv_t_libstr(nu[ii]+nx[ii], nx[ii+1], 1.0, BAbt+ii, 0, 0, dux+ii, 0, 1.0, res_b+ii, 0, dux+ii+1, nu[ii+1]);
+ sveccp_libstr(nx[ii+1], dux+ii+1, nu[ii+1], tmp_nxM, 0);
+ strmv_ltn_libstr(nx[ii+1], nx[ii+1], L+ii+1, nu[ii+1], nu[ii+1], tmp_nxM, 0, tmp_nxM, 0);
+ strmv_lnn_libstr(nx[ii+1], nx[ii+1], L+ii+1, nu[ii+1], nu[ii+1], tmp_nxM, 0, tmp_nxM, 0);
+ saxpy_libstr(nx[ii+1], 1.0, tmp_nxM, 0, dpi+ii, 0, dpi+ii, 0);
+ }
+
+
+
+ // cvt single => double
+ for(ii=0; ii<N; ii++)
+ {
+ m_cvt_s2d_strvec(nu[ii]+nx[ii], dux+ii, 0, d_dux+ii, 0);
+ m_cvt_s2d_strvec(nx[ii+1], dpi+ii, 0, d_dpi+ii, 0);
+ }
+ ii = N;
+ m_cvt_s2d_strvec(nu[ii]+nx[ii], dux+ii, 0, d_dux+ii, 0);
+
+
+
+// if(nb>0)
+// {
+ for(ii=0; ii<=N; ii++)
+ dvecex_sp_libstr(nb[ii], 1.0, d_idxb[ii], d_dux+ii, 0, d_dt_lb+ii, 0);
+// }
+
+// if(ng>0)
+// {
+ for(ii=0; ii<=N; ii++)
+ dgemv_t_libstr(nu[ii]+nx[ii], ng[ii], 1.0, d_DCt+ii, 0, 0, d_dux+ii, 0, 0.0, d_dt_lg+ii, 0, d_dt_lg+ii, 0);
+// }
+
+// if(nb>0 | ng>0)
+// {
+ d_compute_lam_t_hard_qp(cws);
+// }
+
+ return;
+
+ }
+
+
diff --git a/ocp_qp/s_ocp_qp.c b/ocp_qp/s_ocp_qp.c
new file mode 100644
index 0000000..0ebb972
--- /dev/null
+++ b/ocp_qp/s_ocp_qp.c
@@ -0,0 +1,66 @@
+/**************************************************************************************************
+* *
+* 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 *
+* *
+**************************************************************************************************/
+
+
+
+#if defined(RUNTIME_CHECKS)
+#include <stdlib.h>
+#endif
+
+#include <blasfeo_target.h>
+#include <blasfeo_common.h>
+#include <blasfeo_s_aux.h>
+
+#include "../include/hpipm_s_ocp_qp.h"
+
+
+
+#define CREATE_STRMAT s_create_strmat
+#define CREATE_STRVEC s_create_strvec
+#define CVT_MAT2STRMAT s_cvt_mat2strmat
+#define CVT_TRAN_MAT2STRMAT s_cvt_tran_mat2strmat
+#define CVT_VEC2STRVEC s_cvt_vec2strvec
+#define GECP_LIBSTR sgecp_libstr
+#define OCP_QP s_ocp_qp
+#define REAL float
+#define STRMAT s_strmat
+#define STRVEC s_strvec
+#define SIZE_STRMAT s_size_strmat
+#define SIZE_STRVEC s_size_strvec
+#define VECCP_LIBSTR sveccp_libstr
+
+#define CAST_OCP_QP s_cast_ocp_qp
+#define COPY_OCP_QP s_copy_ocp_qp
+#define CREATE_OCP_QP s_create_ocp_qp
+#define CVT_COLMAJ_TO_OCP_QP s_cvt_colmaj_to_ocp_qp
+#define CVT_ROWMAJ_TO_OCP_QP s_cvt_rowmaj_to_ocp_qp
+#define MEMSIZE_OCP_QP s_memsize_ocp_qp
+
+
+
+#include "x_ocp_qp.c"
+
diff --git a/ocp_qp/s_ocp_qp_ipm_hard.c b/ocp_qp/s_ocp_qp_ipm_hard.c
new file mode 100644
index 0000000..20ad0b4
--- /dev/null
+++ b/ocp_qp/s_ocp_qp_ipm_hard.c
@@ -0,0 +1,83 @@
+/**************************************************************************************************
+* *
+* 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>
+#include <blasfeo_s_aux.h>
+
+#include "../include/hpipm_s_ocp_qp.h"
+#include "../include/hpipm_s_ocp_qp_sol.h"
+#include "../include/hpipm_s_ocp_qp_ipm_hard.h"
+#include "../include/hpipm_s_core_qp_ipm_hard.h"
+#include "../include/hpipm_s_core_qp_ipm_hard_aux.h"
+#include "../include/hpipm_s_ocp_qp_kkt.h"
+
+
+
+#define COMPUTE_ALPHA_HARD_QP s_compute_alpha_hard_qp
+#define COMPUTE_CENTERING_CORRECTION_HARD_QP s_compute_centering_correction_hard_qp
+#define COMPUTE_MU_AFF_HARD_QP s_compute_mu_aff_hard_qp
+#define COMPUTE_RES_HARD_OCP_QP s_compute_res_hard_ocp_qp
+#define CREATE_IPM_HARD_CORE_QP s_create_ipm_hard_core_qp
+#define CREATE_STRMAT s_create_strmat
+#define CREATE_STRVEC s_create_strvec
+#define FACT_SOLVE_KKT_STEP_HARD_OCP_QP s_fact_solve_kkt_step_hard_ocp_qp
+#define FACT_SOLVE_KKT_UNCONSTR_OCP_QP s_fact_solve_kkt_unconstr_ocp_qp
+#define INIT_VAR_HARD_OCP_QP s_init_var_hard_ocp_qp
+#define IPM_HARD_CORE_QP_WORKSPACE s_ipm_hard_core_qp_workspace
+#define IPM_HARD_OCP_QP_WORKSPACE s_ipm_hard_ocp_qp_workspace
+#define IPM_HARD_OCP_QP_ARG s_ipm_hard_ocp_qp_arg
+#define MEMSIZE_IPM_HARD_CORE_QP s_memsize_ipm_hard_core_qp
+#define OCP_QP s_ocp_qp
+#define OCP_QP_SOL s_ocp_qp_sol
+#define PRINT_E_MAT s_print_e_mat
+#define PRINT_E_STRVEC s_print_e_strvec
+#define PRINT_E_TRAN_STRVEC s_print_e_tran_strvec
+#define PRINT_STRMAT s_print_strmat
+#define PRINT_STRVEC s_print_strvec
+#define PRINT_TRAN_STRVEC s_print_tran_strvec
+#define REAL float
+#define SIZE_STRMAT s_size_strmat
+#define SIZE_STRVEC s_size_strvec
+#define SOLVE_KKT_STEP_HARD_OCP_QP s_solve_kkt_step_hard_ocp_qp
+#define STRMAT s_strmat
+#define STRVEC s_strvec
+#define UPDATE_VAR_HARD_QP s_update_var_hard_qp
+
+
+
+#define MEMSIZE_IPM_HARD_OCP_QP s_memsize_ipm_hard_ocp_qp
+#define CREATE_IPM_HARD_OCP_QP s_create_ipm_hard_ocp_qp
+#define SOLVE_IPM_HARD_OCP_QP s_solve_ipm_hard_ocp_qp
+#define SOLVE_IPM2_HARD_OCP_QP s_solve_ipm2_hard_ocp_qp
+
+
+
+#include "x_ocp_qp_ipm_hard.c"
+
diff --git a/ocp_qp/s_ocp_qp_kkt.c b/ocp_qp/s_ocp_qp_kkt.c
new file mode 100644
index 0000000..69776fb
--- /dev/null
+++ b/ocp_qp/s_ocp_qp_kkt.c
@@ -0,0 +1,102 @@
+/**************************************************************************************************
+* *
+* 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 <math.h>
+
+#include <blasfeo_target.h>
+#include <blasfeo_common.h>
+#include <blasfeo_s_aux.h>
+#include <blasfeo_s_blas.h>
+
+#include "../include/hpipm_s_ocp_qp.h"
+#include "../include/hpipm_s_ocp_qp_sol.h"
+#include "../include/hpipm_s_ocp_qp_ipm_hard.h"
+#include "../include/hpipm_s_core_qp_ipm_hard.h"
+#include "../include/hpipm_s_core_qp_ipm_hard_aux.h"
+
+
+
+#define SINGLE_PRECISION
+
+#define AXPY_LIBSTR saxpy_libstr
+#define COMPUTE_LAM_T_HARD_QP s_compute_lam_t_hard_qp
+#define COMPUTE_QX_HARD_QP s_compute_qx_hard_qp
+#define COMPUTE_QX_QX_HARD_QP s_compute_Qx_qx_hard_qp
+#define DIAAD_SP_LIBSTR sdiaad_sp_libstr
+#define GEAD_LIBSTR sgead_libstr
+#define GECP_LIBSTR sgecp_libstr
+#define GEMM_R_DIAG_LIBSTR sgemm_r_diag_libstr
+#define GEMV_N_LIBSTR sgemv_n_libstr
+#define GEMV_NT_LIBSTR sgemv_nt_libstr
+#define GEMV_T_LIBSTR sgemv_t_libstr
+#define IPM_HARD_CORE_QP_WORKSPACE s_ipm_hard_core_qp_workspace
+#define IPM_HARD_OCP_QP_WORKSPACE s_ipm_hard_ocp_qp_workspace
+#define OCP_QP s_ocp_qp
+#define OCP_QP_SOL s_ocp_qp_sol
+#define POTRF_L_MN_LIBSTR spotrf_l_mn_libstr
+#define PRINT_E_MAT s_print_e_mat
+#define PRINT_E_STRVEC s_print_e_strvec
+#define PRINT_E_TRAN_STRVEC s_print_e_tran_strvec
+#define PRINT_STRMAT s_print_strmat
+#define PRINT_STRVEC s_print_strvec
+#define PRINT_TRAN_STRVEC s_print_tran_strvec
+#define REAL float
+#define ROWAD_SP_LIBSTR srowad_sp_libstr
+#define ROWEX_LIBSTR srowex_libstr
+#define ROWIN_LIBSTR srowin_libstr
+#define STRMAT s_strmat
+#define STRVEC s_strvec
+#define SYMV_L_LIBSTR ssymv_l_libstr
+#define SYRK_POTRF_LN_LIBSTR ssyrk_spotrf_ln_libstr
+#define TRCP_L_LIBSTR strcp_l_libstr
+#define TRMM_RLNN_LIBSTR strmm_rlnn_libstr
+#define TRMV_LNN_LIBSTR strmv_lnn_libstr
+#define TRMV_LTN_LIBSTR strmv_ltn_libstr
+#define TRSV_LNN_LIBSTR strsv_lnn_libstr
+#define TRSV_LTN_LIBSTR strsv_ltn_libstr
+#define TRSV_LNN_MN_LIBSTR strsv_lnn_mn_libstr
+#define TRSV_LTN_MN_LIBSTR strsv_ltn_mn_libstr
+#define VECAD_SP_LIBSTR svecad_sp_libstr
+#define VECCP_LIBSTR sveccp_libstr
+#define VECEX_SP_LIBSTR svecex_sp_libstr
+#define VECMULDOT_LIBSTR svecmuldot_libstr
+#define VECSC_LIBSTR svecsc_libstr
+
+
+
+#define INIT_VAR_HARD_OCP_QP s_init_var_hard_ocp_qp
+#define COMPUTE_RES_HARD_OCP_QP s_compute_res_hard_ocp_qp
+#define FACT_SOLVE_KKT_UNCONSTR_OCP_QP s_fact_solve_kkt_unconstr_ocp_qp
+#define FACT_SOLVE_KKT_STEP_HARD_OCP_QP s_fact_solve_kkt_step_hard_ocp_qp
+#define SOLVE_KKT_STEP_HARD_OCP_QP s_solve_kkt_step_hard_ocp_qp
+
+
+
+#include "x_ocp_qp_kkt.c"
+
diff --git a/ocp_qp/s_ocp_qp_sol.c b/ocp_qp/s_ocp_qp_sol.c
new file mode 100644
index 0000000..0dac619
--- /dev/null
+++ b/ocp_qp/s_ocp_qp_sol.c
@@ -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 *
+* *
+**************************************************************************************************/
+
+
+
+#if defined(RUNTIME_CHECKS)
+#include <stdlib.h>
+#endif
+
+#include <blasfeo_target.h>
+#include <blasfeo_common.h>
+#include <blasfeo_s_aux.h>
+
+#include "../include/hpipm_s_ocp_qp.h"
+#include "../include/hpipm_s_ocp_qp_sol.h"
+
+
+
+#define CREATE_STRVEC s_create_strvec
+#define CVT_STRVEC2VEC s_cvt_strvec2vec
+#define OCP_QP s_ocp_qp
+#define OCP_QP_SOL s_ocp_qp_sol
+#define REAL float
+#define STRVEC s_strvec
+#define SIZE_STRVEC s_size_strvec
+#define VECCP_LIBSTR sveccp_libstr
+
+#define CREATE_OCP_QP_SOL s_create_ocp_qp_sol
+#define MEMSIZE_OCP_QP_SOL s_memsize_ocp_qp_sol
+#define CVT_OCP_QP_SOL_TO_COLMAJ s_cvt_ocp_qp_sol_to_colmaj
+#define CVT_OCP_QP_SOL_TO_ROWMAJ s_cvt_ocp_qp_sol_to_rowmaj
+#define CVT_OCP_QP_SOL_TO_LIBSTR s_cvt_ocp_qp_sol_to_libstr
+
+
+
+#include "x_ocp_qp_sol.c"
+
diff --git a/ocp_qp/x_ocp_qp.c b/ocp_qp/x_ocp_qp.c
new file mode 100644
index 0000000..bc5605a
--- /dev/null
+++ b/ocp_qp/x_ocp_qp.c
@@ -0,0 +1,448 @@
+/**************************************************************************************************
+* *
+* 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 *
+* *
+**************************************************************************************************/
+
+
+
+int MEMSIZE_OCP_QP(int N, int *nx, int *nu, int *nb, int *ng)
+ {
+
+ int ii;
+
+ int size = 0;
+
+ size += 4*(N+1)*sizeof(int); // nx nu nb ng
+ size += (N+1)*sizeof(int *); // idxb
+ size += (1*N+2*(N+1))*sizeof(struct STRMAT); // BAbt ; RSqrq DCt
+ size += (1*N+5*(N+1))*sizeof(struct STRVEC); // b ; rq d_lb d_ub d_lg d_ug
+
+ for(ii=0; ii<N; ii++)
+ {
+ size += nb[ii]*sizeof(int); // idxb
+ size += SIZE_STRMAT(nu[ii]+nx[ii]+1, nx[ii+1]); // BAbt
+ size += SIZE_STRVEC(nx[ii+1]); // b
+ size += SIZE_STRMAT(nu[ii]+nx[ii]+1, nu[ii]+nx[ii]); // RSQrq
+ size += SIZE_STRVEC(nu[ii]+nx[ii]); // rq
+ size += SIZE_STRMAT(nu[ii]+nx[ii], ng[ii]); // DCt
+ size += 2*SIZE_STRVEC(nb[ii]); // d_lb d_ub
+ size += 2*SIZE_STRVEC(ng[ii]); // d_lg d_ug
+ }
+ ii = N;
+ size += nb[ii]*sizeof(int); // idxb
+ size += SIZE_STRMAT(nu[ii]+nx[ii]+1, nu[ii]+nx[ii]); // RSQrq
+ size += SIZE_STRVEC(nu[ii]+nx[ii]); // rq
+ size += SIZE_STRMAT(nu[ii]+nx[ii], ng[ii]); // DCt
+ size += 2*SIZE_STRVEC(nb[ii]); // d_lb d_ub
+ size += 2*SIZE_STRVEC(ng[ii]); // d_lg d_ug
+
+ size = (size+63)/64*64; // make multiple of typical cache line size
+ size += 64; // align to typical cache line size
+
+ return size;
+
+ }
+
+
+
+void CREATE_OCP_QP(int N, int *nx, int *nu, int *nb, int *ng, struct OCP_QP *qp, void *memory)
+ {
+
+ int ii;
+
+
+ // memsize
+ qp->memsize = MEMSIZE_OCP_QP(N, nx, nu, nb, ng);
+
+
+ // horizon length
+ qp->N = N;
+
+
+ // int pointer stuff
+ int **ip_ptr;
+ ip_ptr = (int **) memory;
+
+ // idxb
+ qp->idxb = ip_ptr;
+ ip_ptr += N+1;
+
+
+ // matrix struct stuff
+ struct STRMAT *sm_ptr = (struct STRMAT *) ip_ptr;
+
+ // BAbt
+ qp->BAbt = sm_ptr;
+ sm_ptr += N;
+
+ // RSQrq
+ qp->RSQrq = sm_ptr;
+ sm_ptr += N+1;
+
+ // DCt
+ qp->DCt = sm_ptr;
+ sm_ptr += N+1;
+
+
+ // vector struct stuff
+ struct STRVEC *sv_ptr = (struct STRVEC *) sm_ptr;
+
+ // b
+ qp->b = sv_ptr;
+ sv_ptr += N;
+
+ // rq
+ qp->rq = sv_ptr;
+ sv_ptr += N+1;
+
+ // d_lb
+ qp->d_lb = sv_ptr;
+ sv_ptr += N+1;
+
+ // d_ub
+ qp->d_ub = sv_ptr;
+ sv_ptr += N+1;
+
+ // d_lg
+ qp->d_lg = sv_ptr;
+ sv_ptr += N+1;
+
+ // d_ug
+ qp->d_ug = sv_ptr;
+ sv_ptr += N+1;
+
+
+ // integer stuff
+ int *i_ptr;
+ i_ptr = (int *) sv_ptr;
+
+ // nx
+ qp->nx = i_ptr;
+ for(ii=0; ii<=N; ii++)
+ {
+ i_ptr[ii] = nx[ii];
+ }
+ i_ptr += N+1;
+
+ // nu
+ qp->nu = i_ptr;
+ for(ii=0; ii<=N; ii++)
+ {
+ i_ptr[ii] = nu[ii];
+ }
+ i_ptr += N+1;
+
+ // nb
+ qp->nb = i_ptr;
+ for(ii=0; ii<=N; ii++)
+ {
+ i_ptr[ii] = nb[ii];
+ }
+ i_ptr += N+1;
+
+ // ng
+ qp->ng = i_ptr;
+ for(ii=0; ii<=N; ii++)
+ {
+ i_ptr[ii] = ng[ii];
+ }
+ i_ptr += N+1;
+
+ // idxb
+ for(ii=0; ii<=N; ii++)
+ {
+ (qp->idxb)[ii] = i_ptr;
+ i_ptr += nb[ii];
+ }
+
+
+ // align to typical cache line size
+ long long l_ptr = (long long) i_ptr;
+ l_ptr = (l_ptr+63)/64*64;
+
+
+ // double stuff
+ char *c_ptr;
+ c_ptr = (char *) l_ptr;
+
+ // BAbt
+ for(ii=0; ii<N; ii++)
+ {
+ CREATE_STRMAT(nu[ii]+nx[ii]+1, nx[ii+1], qp->BAbt+ii, c_ptr);
+ c_ptr += (qp->BAbt+ii)->memory_size;
+ }
+
+ // RSQrq
+ for(ii=0; ii<=N; ii++)
+ {
+ CREATE_STRMAT(nu[ii]+nx[ii]+1, nu[ii]+nx[ii], qp->RSQrq+ii, c_ptr);
+ c_ptr += (qp->RSQrq+ii)->memory_size;
+ }
+
+ // DCt
+ for(ii=0; ii<=N; ii++)
+ {
+ CREATE_STRMAT(nu[ii]+nx[ii], ng[ii], qp->DCt+ii, c_ptr);
+ c_ptr += (qp->DCt+ii)->memory_size;
+ }
+
+ // b
+ for(ii=0; ii<N; ii++)
+ {
+ CREATE_STRVEC(nx[ii+1], qp->b+ii, c_ptr);
+ c_ptr += (qp->b+ii)->memory_size;
+ }
+
+ // rq
+ for(ii=0; ii<=N; ii++)
+ {
+ CREATE_STRVEC(nu[ii]+nx[ii], qp->rq+ii, c_ptr);
+ c_ptr += (qp->rq+ii)->memory_size;
+ }
+
+ // d_lb
+ for(ii=0; ii<=N; ii++)
+ {
+ CREATE_STRVEC(nb[ii], qp->d_lb+ii, c_ptr);
+ c_ptr += (qp->d_lb+ii)->memory_size;
+ }
+
+ // d_ub
+ for(ii=0; ii<=N; ii++)
+ {
+ CREATE_STRVEC(nb[ii], qp->d_ub+ii, c_ptr);
+ c_ptr += (qp->d_ub+ii)->memory_size;
+ }
+
+ // d_lg
+ for(ii=0; ii<=N; ii++)
+ {
+ CREATE_STRVEC(ng[ii], qp->d_lg+ii, c_ptr);
+ c_ptr += (qp->d_lg+ii)->memory_size;
+ }
+
+ // d_ug
+ for(ii=0; ii<=N; ii++)
+ {
+ CREATE_STRVEC(ng[ii], qp->d_ug+ii, c_ptr);
+ c_ptr += (qp->d_ug+ii)->memory_size;
+ }
+
+ return;
+
+ }
+
+
+
+void CAST_OCP_QP(int N, int *nx, int *nu, int *nb, int **idxb, int *ng, struct STRMAT *BAbt, struct STRVEC *b, struct STRMAT *RSQrq, struct STRVEC *rq, struct STRMAT *DCt, struct STRVEC *d_lb, struct STRVEC *d_ub, struct STRVEC *d_lg, struct STRVEC *d_ug, struct OCP_QP *qp)
+ {
+
+ qp->N = N;
+ qp->nx = nx;
+ qp->nu = nu;
+ qp->nb = nb;
+ qp->idxb = idxb;
+ qp->ng = ng;
+ qp->BAbt = BAbt;
+ qp->b = b;
+ qp->RSQrq = RSQrq;
+ qp->rq = rq;
+ qp->DCt = DCt;
+ qp->d_lb = d_lb;
+ qp->d_ub = d_ub;
+ qp->d_lg = d_lg;
+ qp->d_ug = d_ug;
+
+ return;
+
+ }
+
+
+
+void CVT_COLMAJ_TO_OCP_QP(REAL **A, REAL **B, REAL **b, REAL **Q, REAL **S, REAL **R, REAL **q, REAL **r, int **idxb, REAL **d_lb, REAL **d_ub, REAL **C, REAL **D, REAL **d_lg, REAL **d_ug, struct OCP_QP *qp)
+ {
+
+ int N = qp->N;
+ int *nx = qp->nx;
+ int *nu = qp->nu;
+ int *nb = qp->nb;
+ int *ng = qp->ng;
+
+ int ii, jj;
+
+ for(ii=0; ii<N; ii++)
+ {
+ CVT_TRAN_MAT2STRMAT(nx[ii+1], nu[ii], B[ii], nx[ii+1], qp->BAbt+ii, 0, 0);
+ CVT_TRAN_MAT2STRMAT(nx[ii+1], nx[ii], A[ii], nx[ii+1], qp->BAbt+ii, nu[ii], 0);
+ CVT_TRAN_MAT2STRMAT(nx[ii+1], 1, b[ii], nx[ii+1], qp->BAbt+ii, nu[ii]+nx[ii], 0);
+ CVT_VEC2STRVEC(nx[ii+1], b[ii], qp->b+ii, 0);
+ }
+
+ for(ii=0; ii<=N; ii++)
+ {
+ CVT_MAT2STRMAT(nu[ii], nu[ii], R[ii], nu[ii], qp->RSQrq+ii, 0, 0);
+ CVT_TRAN_MAT2STRMAT(nu[ii], nx[ii], S[ii], nu[ii], qp->RSQrq+ii, nu[ii], 0);
+ CVT_MAT2STRMAT(nx[ii], nx[ii], Q[ii], nx[ii], qp->RSQrq+ii, nu[ii], nu[ii]);
+ CVT_TRAN_MAT2STRMAT(nu[ii], 1, r[ii], nu[ii], qp->RSQrq+ii, nu[ii]+nx[ii], 0);
+ CVT_TRAN_MAT2STRMAT(nx[ii], 1, q[ii], nx[ii], qp->RSQrq+ii, nu[ii]+nx[ii], nu[ii]);
+ CVT_VEC2STRVEC(nu[ii], r[ii], qp->rq+ii, 0);
+ CVT_VEC2STRVEC(nx[ii], q[ii], qp->rq+ii, nu[ii]);
+ }
+
+ for(ii=0; ii<=N; ii++)
+ {
+ for(jj=0; jj<nb[ii]; jj++)
+ qp->idxb[ii][jj] = idxb[ii][jj];
+ CVT_VEC2STRVEC(nb[ii], d_lb[ii], qp->d_lb+ii, 0);
+ CVT_VEC2STRVEC(nb[ii], d_ub[ii], qp->d_ub+ii, 0);
+ }
+
+ for(ii=0; ii<=N; ii++)
+ {
+ CVT_TRAN_MAT2STRMAT(ng[ii], nu[ii], D[ii], ng[ii], qp->DCt+ii, 0, 0);
+ CVT_TRAN_MAT2STRMAT(ng[ii], nx[ii], C[ii], ng[ii], qp->DCt+ii, nu[ii], 0);
+ CVT_VEC2STRVEC(ng[ii], d_lg[ii], qp->d_lg+ii, 0);
+ CVT_VEC2STRVEC(ng[ii], d_ug[ii], qp->d_ug+ii, 0);
+ }
+
+ return;
+
+ }
+
+
+
+void CVT_ROWMAJ_TO_OCP_QP(REAL **A, REAL **B, REAL **b, REAL **Q, REAL **S, REAL **R, REAL **q, REAL **r, int **idxb, REAL **d_lb, REAL **d_ub, REAL **C, REAL **D, REAL **d_lg, REAL **d_ug, struct OCP_QP *qp)
+ {
+
+ int N = qp->N;
+ int *nx = qp->nx;
+ int *nu = qp->nu;
+ int *nb = qp->nb;
+ int *ng = qp->ng;
+
+ int ii, jj;
+
+ for(ii=0; ii<N; ii++)
+ {
+ CVT_MAT2STRMAT(nu[ii], nx[ii+1], B[ii], nu[ii], qp->BAbt+ii, 0, 0);
+ CVT_MAT2STRMAT(nx[ii], nx[ii+1], A[ii], nx[ii], qp->BAbt+ii, nu[ii], 0);
+ CVT_TRAN_MAT2STRMAT(nx[ii+1], 1, b[ii], nx[ii+1], qp->BAbt+ii, nu[ii]+nx[ii], 0);
+ CVT_VEC2STRVEC(nx[ii+1], b[ii], qp->b+ii, 0);
+ }
+
+ for(ii=0; ii<=N; ii++)
+ {
+ CVT_TRAN_MAT2STRMAT(nu[ii], nu[ii], R[ii], nu[ii], qp->RSQrq+ii, 0, 0);
+ CVT_MAT2STRMAT(nx[ii], nu[ii], S[ii], nx[ii], qp->RSQrq+ii, nu[ii], 0);
+ CVT_TRAN_MAT2STRMAT(nx[ii], nx[ii], Q[ii], nx[ii], qp->RSQrq+ii, nu[ii], nu[ii]);
+ CVT_TRAN_MAT2STRMAT(nu[ii], 1, r[ii], nu[ii], qp->RSQrq+ii, nu[ii]+nx[ii], 0);
+ CVT_TRAN_MAT2STRMAT(nx[ii], 1, q[ii], nx[ii], qp->RSQrq+ii, nu[ii]+nx[ii], nu[ii]);
+ CVT_VEC2STRVEC(nu[ii], r[ii], qp->rq+ii, 0);
+ CVT_VEC2STRVEC(nx[ii], q[ii], qp->rq+ii, nu[ii]);
+ }
+
+ for(ii=0; ii<=N; ii++)
+ {
+ for(jj=0; jj<nb[ii]; jj++)
+ qp->idxb[ii][jj] = idxb[ii][jj];
+ CVT_VEC2STRVEC(nb[ii], d_lb[ii], qp->d_lb+ii, 0);
+ CVT_VEC2STRVEC(nb[ii], d_ub[ii], qp->d_ub+ii, 0);
+ }
+
+ for(ii=0; ii<=N; ii++)
+ {
+ CVT_MAT2STRMAT(nu[ii], ng[ii], D[ii], nu[ii], qp->DCt+ii, 0, 0);
+ CVT_MAT2STRMAT(nx[ii], ng[ii], C[ii], nx[ii], qp->DCt+ii, nu[ii], 0);
+ CVT_VEC2STRVEC(ng[ii], d_lg[ii], qp->d_lg+ii, 0);
+ CVT_VEC2STRVEC(ng[ii], d_ug[ii], qp->d_ug+ii, 0);
+ }
+
+ return;
+
+ }
+
+
+
+void COPY_OCP_QP(struct OCP_QP *qp_in, struct OCP_QP *qp_out)
+ {
+
+ int N = qp_in->N;
+ int *nx = qp_in->nx;
+ int *nu = qp_in->nu;
+ int *nb = qp_in->nb;
+ int *ng = qp_in->ng;
+
+ int ii, jj;
+
+#if defined(RUNTIME_CHECKS)
+ if(qp_out->N != qp_in->N)
+ {
+ printf("\nError : x_copy_ocp_qp : qp_out->N != qp_out->N : %d != %d\n\n", qp_out->N, qp_in->N);
+ exit(1);
+ }
+ for(ii=0; ii<=N; ii++)
+ {
+ if(qp_out->nx[ii] != qp_in->nx[ii])
+ {
+ printf("\nError : x_copy_ocp_qp : qp_out->nx[%d] != qp_out->nx[%d] : %d != %d\n\n", ii, ii, qp_out->nx[ii], qp_in->nx[ii]);
+ exit(1);
+ }
+ if(qp_out->nu[ii] != qp_in->nu[ii])
+ {
+ printf("\nError : x_copy_ocp_qp : qp_out->nu[%d] != qp_out->nu[%d] : %d != %d\n\n", ii, ii, qp_out->nu[ii], qp_in->nu[ii]);
+ exit(1);
+ }
+ if(qp_out->nb[ii] != qp_in->nb[ii])
+ {
+ printf("\nError : x_copy_ocp_qp : qp_out->nb[%d] != qp_out->nb[%d] : %d != %d\n\n", ii, ii, qp_out->nb[ii], qp_in->nb[ii]);
+ exit(1);
+ }
+ if(qp_out->ng[ii] != qp_in->ng[ii])
+ {
+ printf("\nError : x_copy_ocp_qp : qp_out->ng[%d] != qp_out->ng[%d] : %d != %d\n\n", ii, ii, qp_out->ng[ii], qp_in->ng[ii]);
+ exit(1);
+ }
+ }
+#endif
+
+ for(ii=0; ii<N; ii++)
+ {
+ for(jj=0; jj<qp_in->nb[ii]; jj++) qp_out->idxb[ii][jj] = qp_in->idxb[ii][jj];
+ GECP_LIBSTR(nx[ii]+nu[ii]+1, nx[ii+1], qp_in->BAbt+ii, 0, 0, qp_out->BAbt+ii, 0, 0);
+ VECCP_LIBSTR(nx[ii+1], qp_in->b+ii, 0, qp_out->b+ii, 0);
+ GECP_LIBSTR(nx[ii]+nu[ii]+1, nu[ii]+nx[ii], qp_in->RSQrq+ii, 0, 0, qp_out->RSQrq+ii, 0, 0);
+ VECCP_LIBSTR(nu[ii]+nx[ii], qp_in->rq+ii, 0, qp_out->rq+ii, 0);
+ GECP_LIBSTR(nx[ii]+nu[ii], ng[ii], qp_in->DCt+ii, 0, 0, qp_out->DCt+ii, 0, 0);
+ VECCP_LIBSTR(nb[ii], qp_in->d_lb+ii, 0, qp_out->d_lb+ii, 0);
+ VECCP_LIBSTR(nb[ii], qp_in->d_ub+ii, 0, qp_out->d_ub+ii, 0);
+ VECCP_LIBSTR(ng[ii], qp_in->d_lg+ii, 0, qp_out->d_lg+ii, 0);
+ VECCP_LIBSTR(ng[ii], qp_in->d_ug+ii, 0, qp_out->d_ug+ii, 0);
+ }
+
+ return;
+
+ }
+
+
diff --git a/ocp_qp/x_ocp_qp_ipm_hard.c b/ocp_qp/x_ocp_qp_ipm_hard.c
new file mode 100644
index 0000000..df1bacb
--- /dev/null
+++ b/ocp_qp/x_ocp_qp_ipm_hard.c
@@ -0,0 +1,649 @@
+/**************************************************************************************************
+* *
+* 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 *
+* *
+**************************************************************************************************/
+
+
+
+int MEMSIZE_IPM_HARD_OCP_QP(struct OCP_QP *qp, struct IPM_HARD_OCP_QP_ARG *arg)
+ {
+
+ // loop index
+ int ii;
+
+ // extract ocp qp size
+ int N = qp->N;
+ int *nx = qp->nx;
+ int *nu = qp->nu;
+ int *nb = qp->nb;
+ int *ng = qp->ng;
+
+ // compute core qp size and max size
+ int nvt = 0;
+ int net = 0;
+ int nbt = 0;
+ int ngt = 0;
+ int nxM = 0;
+ int nuM = 0;
+ int nbM = 0;
+ int ngM = 0;
+
+ for(ii=0; ii<N; ii++)
+ {
+ nvt += nx[ii]+nu[ii];
+ net += nx[ii+1];
+ nbt += nb[ii];
+ ngt += ng[ii];
+ nxM = nx[ii]>nxM ? nx[ii] : nxM;
+ nuM = nu[ii]>nuM ? nu[ii] : nuM;
+ nbM = nb[ii]>nbM ? nb[ii] : nbM;
+ ngM = ng[ii]>ngM ? ng[ii] : ngM;
+ }
+ ii = N;
+ nvt += nx[ii]+nu[ii];
+ nbt += nb[ii];
+ ngt += ng[ii];
+ nxM = nx[ii]>nxM ? nx[ii] : nxM;
+ nuM = nu[ii]>nuM ? nu[ii] : nuM;
+ nbM = nb[ii]>nbM ? nb[ii] : nbM;
+ ngM = ng[ii]>ngM ? ng[ii] : ngM;
+
+ int size = 0;
+
+ size += (5+(N+1)*19)*sizeof(struct STRVEC); // dux dpi dt_lb dt_lg res_g res_b res_d res_d_lb res_d_ub res_d_lg res_d_ug res_m res_m_lb res_m_ub res_m_lg res_m_ug Qx_lb Qx_lg qx_lb qx_lg Pb tmp_nbM tmp_nxM tmp_ngM
+ size += (1+(N+1)*1)*sizeof(struct STRMAT); // L AL
+
+ size += 1*SIZE_STRVEC(nbM); // tmp_nbM
+ size += 1*SIZE_STRVEC(nxM); // tmp_nxM
+ size += 2*SIZE_STRVEC(nxM); // tmp_ngM
+ for(ii=0; ii<N; ii++) size += 1*SIZE_STRVEC(nx[ii+1]);
+ for(ii=0; ii<=N; ii++) size += 1*SIZE_STRMAT(nu[ii]+nx[ii]+1, nu[ii]+nx[ii]); // L
+ size += 2*SIZE_STRMAT(nuM+nxM+1, nxM+ngM); // AL
+
+ size += 1*sizeof(struct IPM_HARD_CORE_QP_WORKSPACE);
+ size += 1*MEMSIZE_IPM_HARD_CORE_QP(nvt, net, nbt, ngt, arg->iter_max);
+
+ size = (size+63)/64*64; // make multiple of typical cache line size
+ size += 1*64; // align once to typical cache line size
+
+ return size;
+
+ }
+
+
+
+void CREATE_IPM_HARD_OCP_QP(struct OCP_QP *qp, struct IPM_HARD_OCP_QP_ARG *arg, struct IPM_HARD_OCP_QP_WORKSPACE *workspace, void *mem)
+ {
+
+ // loop index
+ int ii;
+
+ // extract ocp qp size
+ int N = qp->N;
+ int *nx = qp->nx;
+ int *nu = qp->nu;
+ int *nb = qp->nb;
+ int *ng = qp->ng;
+
+
+ workspace->memsize = MEMSIZE_IPM_HARD_OCP_QP(qp, arg);
+
+
+ // compute core qp size and max size
+ int nvt = 0;
+ int net = 0;
+ int nbt = 0;
+ int ngt = 0;
+ int nxM = 0;
+ int nuM = 0;
+ int nbM = 0;
+ int ngM = 0;
+
+ for(ii=0; ii<N; ii++)
+ {
+ nvt += nx[ii]+nu[ii];
+ net += nx[ii+1];
+ nbt += nb[ii];
+ ngt += ng[ii];
+ nxM = nx[ii]>nxM ? nx[ii] : nxM;
+ nuM = nu[ii]>nuM ? nu[ii] : nuM;
+ nbM = nb[ii]>nbM ? nb[ii] : nbM;
+ ngM = ng[ii]>ngM ? ng[ii] : ngM;
+ }
+ ii = N;
+ nvt += nx[ii]+nu[ii];
+ nbt += nb[ii];
+ ngt += ng[ii];
+ nxM = nx[ii]>nxM ? nx[ii] : nxM;
+ nuM = nu[ii]>nuM ? nu[ii] : nuM;
+ nbM = nb[ii]>nbM ? nb[ii] : nbM;
+ ngM = ng[ii]>ngM ? ng[ii] : ngM;
+
+
+ // core struct
+ struct IPM_HARD_CORE_QP_WORKSPACE *sr_ptr = mem;
+
+ // core workspace
+ workspace->core_workspace = sr_ptr;
+ sr_ptr += 1;
+ struct IPM_HARD_CORE_QP_WORKSPACE *rwork = workspace->core_workspace;
+
+
+ // matrix struct
+ struct STRMAT *sm_ptr = (struct STRMAT *) sr_ptr;
+
+ workspace->L = sm_ptr;
+ sm_ptr += N+1;
+ workspace->AL = sm_ptr;
+ sm_ptr += 2;
+
+
+ // vector struct
+ struct STRVEC *sv_ptr = (struct STRVEC *) sm_ptr;
+
+ workspace->dux = sv_ptr;
+ sv_ptr += N+1;
+ workspace->dpi = sv_ptr;
+ sv_ptr += N+1;
+ workspace->dt_lb = sv_ptr;
+ sv_ptr += N+1;
+ workspace->dt_lg = sv_ptr;
+ sv_ptr += N+1;
+ workspace->res_g = sv_ptr;
+ sv_ptr += N+1;
+ workspace->res_b = sv_ptr;
+ sv_ptr += N+1;
+ workspace->res_d = sv_ptr;
+ sv_ptr += 1;
+ workspace->res_d_lb = sv_ptr;
+ sv_ptr += N+1;
+ workspace->res_d_ub = sv_ptr;
+ sv_ptr += N+1;
+ workspace->res_d_lg = sv_ptr;
+ sv_ptr += N+1;
+ workspace->res_d_ug = sv_ptr;
+ sv_ptr += N+1;
+ workspace->res_m = sv_ptr;
+ sv_ptr += 1;
+ workspace->res_m_lb = sv_ptr;
+ sv_ptr += N+1;
+ workspace->res_m_ub = sv_ptr;
+ sv_ptr += N+1;
+ workspace->res_m_lg = sv_ptr;
+ sv_ptr += N+1;
+ workspace->res_m_ug = sv_ptr;
+ sv_ptr += N+1;
+ workspace->Qx_lb = sv_ptr;
+ sv_ptr += N+1;
+ workspace->Qx_lg = sv_ptr;
+ sv_ptr += N+1;
+ workspace->qx_lb = sv_ptr;
+ sv_ptr += N+1;
+ workspace->qx_lg = sv_ptr;
+ sv_ptr += N+1;
+ workspace->Pb = sv_ptr;
+ sv_ptr += N+1;
+ workspace->tmp_nbM = sv_ptr;
+ sv_ptr += 1;
+ workspace->tmp_nxM = sv_ptr;
+ sv_ptr += 1;
+ workspace->tmp_ngM = sv_ptr;
+ sv_ptr += 2;
+
+
+ // align to typicl cache line size
+ size_t s_ptr = (size_t) sv_ptr;
+ s_ptr = (s_ptr+63)/64*64;
+
+
+ // void stuf
+ void *v_ptr = (void *) s_ptr;
+
+ for(ii=0; ii<=N; ii++)
+ {
+ CREATE_STRMAT(nu[ii]+nx[ii]+1, nu[ii]+nx[ii], workspace->L+ii, v_ptr);
+ v_ptr += (workspace->L+ii)->memory_size;
+ }
+
+ CREATE_STRMAT(nuM+nxM+1, nxM+ngM, workspace->AL+0, v_ptr);
+ v_ptr += (workspace->AL+0)->memory_size;
+
+ CREATE_STRMAT(nuM+nxM+1, nxM+ngM, workspace->AL+1, v_ptr);
+ v_ptr += (workspace->AL+1)->memory_size;
+
+ for(ii=0; ii<N; ii++)
+ {
+ CREATE_STRVEC(nx[ii+1], workspace->Pb+ii, v_ptr);
+ v_ptr += (workspace->Pb+ii)->memory_size;
+ }
+
+ CREATE_STRVEC(nbM, workspace->tmp_nbM, v_ptr);
+ v_ptr += workspace->tmp_nbM->memory_size;
+
+ CREATE_STRVEC(nxM, workspace->tmp_nxM, v_ptr);
+ v_ptr += workspace->tmp_nxM->memory_size;
+
+ CREATE_STRVEC(ngM, workspace->tmp_ngM+0, v_ptr);
+ v_ptr += (workspace->tmp_ngM+0)->memory_size;
+
+ CREATE_STRVEC(ngM, workspace->tmp_ngM+1, v_ptr);
+ v_ptr += (workspace->tmp_ngM+1)->memory_size;
+
+
+
+ rwork->nv = nvt;
+ rwork->ne = net;
+ rwork->nb = nbt;
+ rwork->ng = ngt;
+ rwork->iter_max = arg->iter_max;
+ CREATE_IPM_HARD_CORE_QP(rwork, v_ptr);
+ v_ptr += workspace->core_workspace->memsize;
+
+ rwork->alpha_min = arg->alpha_min;
+ rwork->mu_max = arg->mu_max;
+ rwork->mu0 = arg->mu0;
+ rwork->nt_inv = 1.0/(2*nbt+2*ngt); // TODO avoid computation if nt=0
+
+
+ // alias members of workspace and core_workspace
+ v_ptr = rwork->dv;
+ for(ii=0; ii<=N; ii++)
+ {
+ CREATE_STRVEC(nu[ii]+nx[ii], workspace->dux+ii, v_ptr);
+ v_ptr += (nu[ii]+nx[ii])*sizeof(REAL);
+ }
+ v_ptr = rwork->dpi;
+ for(ii=0; ii<N; ii++)
+ {
+ CREATE_STRVEC(nx[ii+1], workspace->dpi+ii, v_ptr);
+ v_ptr += (nx[ii+1])*sizeof(REAL);
+ }
+ v_ptr = rwork->dt_lb;
+ for(ii=0; ii<=N; ii++)
+ {
+ CREATE_STRVEC(nb[ii], workspace->dt_lb+ii, v_ptr);
+ v_ptr += (nb[ii])*sizeof(REAL);
+ }
+ v_ptr = rwork->dt_lg;
+ for(ii=0; ii<=N; ii++)
+ {
+ CREATE_STRVEC(ng[ii], workspace->dt_lg+ii, v_ptr);
+ v_ptr += (ng[ii])*sizeof(REAL);
+ }
+ v_ptr = rwork->res_g;
+ for(ii=0; ii<=N; ii++)
+ {
+ CREATE_STRVEC(nu[ii]+nx[ii], workspace->res_g+ii, v_ptr);
+ v_ptr += (nu[ii]+nx[ii])*sizeof(REAL);
+ }
+ v_ptr = rwork->res_b;
+ for(ii=0; ii<N; ii++)
+ {
+ CREATE_STRVEC(nx[ii+1], workspace->res_b+ii, v_ptr);
+ v_ptr += (nx[ii+1])*sizeof(REAL);
+ }
+ v_ptr = rwork->res_d;
+ CREATE_STRVEC(2*nbt+2*ngt, workspace->res_d, v_ptr);
+ v_ptr = rwork->res_d_lb;
+ for(ii=0; ii<=N; ii++)
+ {
+ CREATE_STRVEC(nb[ii], workspace->res_d_lb+ii, v_ptr);
+ v_ptr += (nb[ii])*sizeof(REAL);
+ }
+ v_ptr = rwork->res_d_ub;
+ for(ii=0; ii<=N; ii++)
+ {
+ CREATE_STRVEC(nb[ii], workspace->res_d_ub+ii, v_ptr);
+ v_ptr += (nb[ii])*sizeof(REAL);
+ }
+ v_ptr = rwork->res_d_lg;
+ for(ii=0; ii<=N; ii++)
+ {
+ CREATE_STRVEC(ng[ii], workspace->res_d_lg+ii, v_ptr);
+ v_ptr += (ng[ii])*sizeof(REAL);
+ }
+ v_ptr = rwork->res_d_ug;
+ for(ii=0; ii<=N; ii++)
+ {
+ CREATE_STRVEC(ng[ii], workspace->res_d_ug+ii, v_ptr);
+ v_ptr += (ng[ii])*sizeof(REAL);
+ }
+ v_ptr = rwork->res_m;
+ CREATE_STRVEC(2*nbt+2*ngt, workspace->res_m, v_ptr);
+ v_ptr = rwork->res_m_lb;
+ for(ii=0; ii<=N; ii++)
+ {
+ CREATE_STRVEC(nb[ii], workspace->res_m_lb+ii, v_ptr);
+ v_ptr += (nb[ii])*sizeof(REAL);
+ }
+ v_ptr = rwork->res_m_ub;
+ for(ii=0; ii<=N; ii++)
+ {
+ CREATE_STRVEC(nb[ii], workspace->res_m_ub+ii, v_ptr);
+ v_ptr += (nb[ii])*sizeof(REAL);
+ }
+ v_ptr = rwork->res_m_lg;
+ for(ii=0; ii<=N; ii++)
+ {
+ CREATE_STRVEC(ng[ii], workspace->res_m_lg+ii, v_ptr);
+ v_ptr += (ng[ii])*sizeof(REAL);
+ }
+ v_ptr = rwork->res_m_ug;
+ for(ii=0; ii<=N; ii++)
+ {
+ CREATE_STRVEC(ng[ii], workspace->res_m_ug+ii, v_ptr);
+ v_ptr += (ng[ii])*sizeof(REAL);
+ }
+ v_ptr = rwork->Qx_lb;
+ for(ii=0; ii<=N; ii++)
+ {
+ CREATE_STRVEC(nb[ii], workspace->Qx_lb+ii, v_ptr);
+ v_ptr += (nb[ii])*sizeof(REAL);
+ }
+ v_ptr = rwork->Qx_lg;
+ for(ii=0; ii<=N; ii++)
+ {
+ CREATE_STRVEC(ng[ii], workspace->Qx_lg+ii, v_ptr);
+ v_ptr += (ng[ii])*sizeof(REAL);
+ }
+ v_ptr = rwork->qx_lb;
+ for(ii=0; ii<=N; ii++)
+ {
+ CREATE_STRVEC(nb[ii], workspace->qx_lb+ii, v_ptr);
+ v_ptr += (nb[ii])*sizeof(REAL);
+ }
+ v_ptr = rwork->qx_lg;
+ for(ii=0; ii<=N; ii++)
+ {
+ CREATE_STRVEC(ng[ii], workspace->qx_lg+ii, v_ptr);
+ v_ptr += (ng[ii])*sizeof(REAL);
+ }
+ workspace->stat = rwork->stat;
+
+ return;
+
+ }
+
+
+
+void SOLVE_IPM_HARD_OCP_QP(struct OCP_QP *qp, struct OCP_QP_SOL *qp_sol, struct IPM_HARD_OCP_QP_WORKSPACE *ws)
+ {
+
+ struct IPM_HARD_CORE_QP_WORKSPACE *cws = ws->core_workspace;
+
+ // alias qp vectors into qp
+ cws->d_lb = qp->d_lb->pa;
+ cws->d_ub = qp->d_ub->pa;
+ cws->d_lg = qp->d_lg->pa;
+ cws->d_ug = qp->d_ug->pa;
+
+ // alias qp vectors into qp_sol
+ cws->v = qp_sol->ux->pa;
+ cws->pi = qp_sol->pi->pa;
+ cws->lam = qp_sol->lam_lb->pa;
+ cws->lam_lb = qp_sol->lam_lb->pa;
+ cws->lam_ub = qp_sol->lam_ub->pa;
+ cws->lam_lg = qp_sol->lam_lg->pa;
+ cws->lam_ug = qp_sol->lam_ug->pa;
+ cws->t = qp_sol->t_lb->pa;
+ cws->t_lb = qp_sol->t_lb->pa;
+ cws->t_ub = qp_sol->t_ub->pa;
+ cws->t_lg = qp_sol->t_lg->pa;
+ cws->t_ug = qp_sol->t_ug->pa;
+
+ if(cws->nb+cws->ng==0)
+ {
+ FACT_SOLVE_KKT_UNCONSTR_OCP_QP(qp, qp_sol, ws);
+ COMPUTE_RES_HARD_OCP_QP(qp, qp_sol, ws);
+ cws->mu = ws->res_mu;
+ ws->iter = 0;
+ return;
+ }
+
+ // init solver
+ INIT_VAR_HARD_OCP_QP(qp, qp_sol, ws);
+
+ // compute residuals
+ COMPUTE_RES_HARD_OCP_QP(qp, qp_sol, ws);
+ cws->mu = ws->res_mu;
+
+ int kk;
+ for(kk=0; kk<cws->iter_max & cws->mu>cws->mu_max; kk++)
+ {
+
+ // fact and solve kkt
+ FACT_SOLVE_KKT_STEP_HARD_OCP_QP(qp, ws);
+
+ // alpha
+ COMPUTE_ALPHA_HARD_QP(cws);
+ cws->stat[5*kk+0] = cws->alpha;
+
+ //
+ UPDATE_VAR_HARD_QP(cws);
+
+ // compute residuals
+ COMPUTE_RES_HARD_OCP_QP(qp, qp_sol, ws);
+ cws->mu = ws->res_mu;
+ cws->stat[5*kk+1] = ws->res_mu;
+
+ }
+
+ ws->iter = kk;
+
+ return;
+
+ }
+
+
+
+void SOLVE_IPM2_HARD_OCP_QP(struct OCP_QP *qp, struct OCP_QP_SOL *qp_sol, struct IPM_HARD_OCP_QP_WORKSPACE *ws)
+ {
+
+ struct IPM_HARD_CORE_QP_WORKSPACE *cws = ws->core_workspace;
+
+ // alias qp vectors into qp
+ cws->d_lb = qp->d_lb->pa;
+ cws->d_ub = qp->d_ub->pa;
+ cws->d_lg = qp->d_lg->pa;
+ cws->d_ug = qp->d_ug->pa;
+
+ // alias qp vectors into qp_sol
+ cws->v = qp_sol->ux->pa;
+ cws->pi = qp_sol->pi->pa;
+ cws->lam = qp_sol->lam_lb->pa;
+ cws->lam_lb = qp_sol->lam_lb->pa;
+ cws->lam_ub = qp_sol->lam_ub->pa;
+ cws->lam_lg = qp_sol->lam_lg->pa;
+ cws->lam_ug = qp_sol->lam_ug->pa;
+ cws->t = qp_sol->t_lb->pa;
+ cws->t_lb = qp_sol->t_lb->pa;
+ cws->t_ub = qp_sol->t_ub->pa;
+ cws->t_lg = qp_sol->t_lg->pa;
+ cws->t_ug = qp_sol->t_ug->pa;
+
+ REAL tmp;
+
+ if(cws->nb+cws->ng==0)
+ {
+ FACT_SOLVE_KKT_UNCONSTR_OCP_QP(qp, qp_sol, ws); // TODO tailored routine ???
+ COMPUTE_RES_HARD_OCP_QP(qp, qp_sol, ws);
+ cws->mu = ws->res_mu;
+ ws->iter = 0;
+ return;
+ }
+
+ // init solver
+ INIT_VAR_HARD_OCP_QP(qp, qp_sol, ws);
+
+ // compute residuals
+ COMPUTE_RES_HARD_OCP_QP(qp, qp_sol, ws);
+ cws->mu = ws->res_mu;
+
+#if 0
+ printf("\nres_g\n");
+ for(int ii=0; ii<=qp->N; ii++)
+ {
+ PRINT_E_TRAN_STRVEC(qp->nx[ii]+qp->nu[ii], ws->res_g+ii, 0);
+ }
+ printf("\nres_b\n");
+ for(int ii=0; ii<qp->N; ii++)
+ {
+ PRINT_E_TRAN_STRVEC(qp->nx[ii+1], ws->res_b+ii, 0);
+ }
+ printf("\nres_d_lb\n");
+ for(int ii=0; ii<qp->N; ii++)
+ {
+ PRINT_E_TRAN_STRVEC(qp->nb[ii], ws->res_d_lb+ii, 0);
+ }
+ printf("\nres_d_ub\n");
+ for(int ii=0; ii<qp->N; ii++)
+ {
+ PRINT_E_TRAN_STRVEC(qp->nb[ii], ws->res_d_ub+ii, 0);
+ }
+ printf("\nres_d_lg\n");
+ for(int ii=0; ii<qp->N; ii++)
+ {
+ PRINT_E_TRAN_STRVEC(qp->ng[ii], ws->res_d_lg+ii, 0);
+ }
+ printf("\nres_d_ug\n");
+ for(int ii=0; ii<qp->N; ii++)
+ {
+ PRINT_E_TRAN_STRVEC(qp->ng[ii], ws->res_d_ug+ii, 0);
+ }
+ printf("\nres_m_lb\n");
+ for(int ii=0; ii<qp->N; ii++)
+ {
+ PRINT_E_TRAN_STRVEC(qp->nb[ii], ws->res_m_lb+ii, 0);
+ }
+ printf("\nres_m_ub\n");
+ for(int ii=0; ii<qp->N; ii++)
+ {
+ PRINT_E_TRAN_STRVEC(qp->nb[ii], ws->res_m_ub+ii, 0);
+ }
+ printf("\nres_m_lg\n");
+ for(int ii=0; ii<qp->N; ii++)
+ {
+ PRINT_E_TRAN_STRVEC(qp->ng[ii], ws->res_m_lg+ii, 0);
+ }
+ printf("\nres_m_ug\n");
+ for(int ii=0; ii<qp->N; ii++)
+ {
+ PRINT_E_TRAN_STRVEC(qp->ng[ii], ws->res_m_ug+ii, 0);
+ }
+ exit(1);
+#endif
+
+#if 0
+ int ii;
+ for(ii=0; ii<1; ii++)
+ {
+ cws->sigma = 1.0;
+ cws->stat[5*kk+2] = cws->sigma;
+ COMPUTE_CENTERING_CORRECTION_HARD_QP(cws);
+ FACT_SOLVE_KKT_STEP_HARD_OCP_QP(qp, ws);
+ COMPUTE_ALPHA_HARD_QP(cws);
+ cws->stat[5*kk+3] = cws->alpha;
+ UPDATE_VAR_HARD_QP(cws);
+ COMPUTE_RES_HARD_OCP_QP(qp, qp_sol, ws);
+ cws->mu = ws->res_mu;
+ cws->stat[5*kk+4] = ws->res_mu;
+ kk++;
+ }
+// ws->iter = kk;
+// return;
+#endif
+
+ int kk = 0;
+ for(; kk<cws->iter_max & cws->mu>cws->mu_max; kk++)
+ {
+
+ // fact and solve kkt
+ FACT_SOLVE_KKT_STEP_HARD_OCP_QP(qp, ws);
+
+#if 0
+ printf("\ndux\n");
+ for(int ii=0; ii<=qp->N; ii++)
+ {
+ PRINT_E_TRAN_STRVEC(qp->nx[ii]+qp->nu[ii], ws->dux+ii, 0);
+ }
+ printf("\ndpi\n");
+ for(int ii=0; ii<qp->N; ii++)
+ {
+ PRINT_E_TRAN_STRVEC(qp->nx[ii+1], ws->dpi+ii, 0);
+ }
+ printf("\ndt\n");
+ for(int ii=0; ii<qp->N; ii++)
+ {
+ PRINT_E_TRAN_STRVEC(2*qp->nb[ii]+2*qp->ng[ii], ws->dt_lb+ii, 0);
+ }
+ exit(1);
+#endif
+ // alpha
+ COMPUTE_ALPHA_HARD_QP(cws);
+ cws->stat[5*kk+0] = cws->alpha;
+
+ // mu_aff
+ COMPUTE_MU_AFF_HARD_QP(cws);
+ cws->stat[5*kk+1] = cws->mu_aff;
+
+ tmp = cws->mu_aff/cws->mu;
+ cws->sigma = tmp*tmp*tmp;
+ cws->stat[5*kk+2] = cws->sigma;
+
+ COMPUTE_CENTERING_CORRECTION_HARD_QP(cws);
+
+ // fact and solve kkt
+ SOLVE_KKT_STEP_HARD_OCP_QP(qp, ws);
+
+#if 0
+int ii;
+for(ii=0; ii<=qp->N; ii++)
+ d_print_tran_strvec(qp->nu[ii]+qp->nx[ii], ws->dux+ii, 0);
+for(ii=0; ii<qp->N; ii++)
+ d_print_tran_strvec(qp->nx[ii+1], ws->dpi+ii, 0);
+exit(1);
+#endif
+ // alpha
+ COMPUTE_ALPHA_HARD_QP(cws);
+ cws->stat[5*kk+3] = cws->alpha;
+
+ //
+ UPDATE_VAR_HARD_QP(cws);
+
+ // compute residuals
+ COMPUTE_RES_HARD_OCP_QP(qp, qp_sol, ws);
+ cws->mu = ws->res_mu;
+ cws->stat[5*kk+4] = ws->res_mu;
+
+ }
+
+ ws->iter = kk;
+
+ return;
+
+ }
+
+
+
diff --git a/ocp_qp/x_ocp_qp_kkt.c b/ocp_qp/x_ocp_qp_kkt.c
new file mode 100644
index 0000000..2106fd6
--- /dev/null
+++ b/ocp_qp/x_ocp_qp_kkt.c
@@ -0,0 +1,640 @@
+/**************************************************************************************************
+* *
+* 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 INIT_VAR_HARD_OCP_QP(struct OCP_QP *qp, struct OCP_QP_SOL *qp_sol, struct IPM_HARD_OCP_QP_WORKSPACE *ws)
+ {
+
+ struct IPM_HARD_CORE_QP_WORKSPACE *cws = ws->core_workspace;
+
+ // loop index
+ int ii, jj;
+
+ //
+ int N = qp->N;
+ int *nx = qp->nx;
+ int *nu = qp->nu;
+ int *nb = qp->nb;
+ int *ng = qp->ng;
+
+ REAL mu0 = cws->mu0;
+
+ //
+ REAL *ux, *pi, *d_lb, *d_ub, *d_lg, *d_ug, *lam_lb, *lam_ub, *lam_lg, *lam_ug, *t_lb, *t_ub, *t_lg, *t_ug;
+ int *idxb;
+
+ REAL thr0 = 0.1;
+
+ // warm start TODO
+
+ // cold start
+
+ // ux
+ for(ii=0; ii<=N; ii++)
+ {
+ ux = qp_sol->ux[ii].pa;
+ for(jj=0; jj<nu[ii]+nx[ii]; jj++)
+ {
+ ux[jj] = 0.0;
+ }
+ }
+
+ // pi
+ for(ii=0; ii<N; ii++)
+ {
+ pi = qp_sol->pi[ii].pa;
+ for(jj=0; jj<nx[ii+1]; jj++)
+ {
+ pi[jj] = 0.0;
+ }
+ }
+
+ // box constraints
+ for(ii=0; ii<=N; ii++)
+ {
+ ux = qp_sol->ux[ii].pa;
+ d_lb = qp->d_lb[ii].pa;
+ d_ub = qp->d_ub[ii].pa;
+ lam_lb = qp_sol->lam_lb[ii].pa;
+ lam_ub = qp_sol->lam_ub[ii].pa;
+ t_lb = qp_sol->t_lb[ii].pa;
+ t_ub = qp_sol->t_ub[ii].pa;
+ idxb = qp->idxb[ii];
+ for(jj=0; jj<nb[ii]; jj++)
+ {
+ t_lb[jj] = - d_lb[jj] + ux[idxb[jj]];
+ t_ub[jj] = d_ub[jj] - ux[idxb[jj]];
+ if(t_lb[jj]<thr0)
+ {
+ if(t_ub[jj]<thr0)
+ {
+ ux[idxb[jj]] = 0.5*(d_lb[jj]-d_ub[jj]);
+ t_lb[jj] = thr0;
+ t_ub[jj] = thr0;
+ }
+ else
+ {
+ t_lb[jj] = thr0;
+ ux[idxb[jj]] = d_lb[jj] + thr0;
+ }
+ }
+ else if(t_ub[jj]<thr0)
+ {
+ t_ub[jj] = thr0;
+ ux[idxb[jj]] = d_ub[jj] - thr0;
+ }
+ lam_lb[jj] = mu0/t_lb[jj];
+ lam_ub[jj] = mu0/t_ub[jj];
+ }
+ }
+
+ // general constraints
+ for(ii=0; ii<=N; ii++)
+ {
+ t_lg = qp_sol->t_lg[ii].pa;
+ t_ug = qp_sol->t_ug[ii].pa;
+ lam_lg = qp_sol->lam_lg[ii].pa;
+ lam_ug = qp_sol->lam_ug[ii].pa;
+ d_lg = qp->d_lg[ii].pa;
+ d_ug = qp->d_ug[ii].pa;
+ ux = qp_sol->ux[ii].pa;
+ GEMV_T_LIBSTR(nu[ii]+nx[ii], ng[ii], 1.0, qp->DCt+ii, 0, 0, qp_sol->ux+ii, 0, 0.0, qp_sol->t_lg+ii, 0, qp_sol->t_lg+ii, 0);
+ for(jj=0; jj<ng[ii]; jj++)
+ {
+ t_ug[jj] = - t_lg[jj];
+ t_lg[jj] -= d_lg[jj];
+ t_ug[jj] += d_ug[jj];
+// t_lg[jj] = fmax(thr0, t_lg[jj]);
+// t_ug[jj] = fmax(thr0, t_ug[jj]);
+ t_lg[jj] = thr0>t_lg[jj] ? thr0 : t_lg[jj];
+ t_ug[jj] = thr0>t_ug[jj] ? thr0 : t_ug[jj];
+ lam_lg[jj] = mu0/t_lg[jj];
+ lam_ug[jj] = mu0/t_ug[jj];
+ }
+ }
+
+
+ return;
+
+ }
+
+
+
+void COMPUTE_RES_HARD_OCP_QP(struct OCP_QP *qp, struct OCP_QP_SOL *qp_sol, struct IPM_HARD_OCP_QP_WORKSPACE *ws)
+ {
+
+ struct IPM_HARD_CORE_QP_WORKSPACE *cws = ws->core_workspace;
+
+ // loop index
+ int ii;
+
+ //
+ int N = qp->N;
+ int *nx = qp->nx;
+ int *nu = qp->nu;
+ int *nb = qp->nb;
+ int *ng = qp->ng;
+
+ int nbt = ws->core_workspace->nb;
+ int ngt = ws->core_workspace->ng;
+
+ struct STRMAT *BAbt = qp->BAbt;
+ struct STRMAT *RSQrq = qp->RSQrq;
+ struct STRMAT *DCt = qp->DCt;
+ struct STRVEC *b = qp->b;
+ struct STRVEC *rq = qp->rq;
+ struct STRVEC *d_lb = qp->d_lb;
+ struct STRVEC *d_ub = qp->d_ub;
+ struct STRVEC *d_lg = qp->d_lg;
+ struct STRVEC *d_ug = qp->d_ug;
+ int **idxb = qp->idxb;
+
+ struct STRVEC *ux = qp_sol->ux;
+ struct STRVEC *pi = qp_sol->pi;
+ struct STRVEC *lam_lb = qp_sol->lam_lb;
+ struct STRVEC *lam_ub = qp_sol->lam_ub;
+ struct STRVEC *lam_lg = qp_sol->lam_lg;
+ struct STRVEC *lam_ug = qp_sol->lam_ug;
+ struct STRVEC *t_lb = qp_sol->t_lb;
+ struct STRVEC *t_ub = qp_sol->t_ub;
+ struct STRVEC *t_lg = qp_sol->t_lg;
+ struct STRVEC *t_ug = qp_sol->t_ug;
+
+ struct STRVEC *res_g = ws->res_g;
+ struct STRVEC *res_b = ws->res_b;
+ struct STRVEC *res_d_lb = ws->res_d_lb;
+ struct STRVEC *res_d_ub = ws->res_d_ub;
+ struct STRVEC *res_d_lg = ws->res_d_lg;
+ struct STRVEC *res_d_ug = ws->res_d_ug;
+ struct STRVEC *tmp_ngM = ws->tmp_ngM;
+ struct STRVEC *tmp_nbM = ws->tmp_nbM;
+
+ int nx0, nx1, nu0, nu1, nb0, ng0;
+
+ //
+ REAL mu = 0.0;
+
+ // loop over stages
+ for(ii=0; ii<=N; ii++)
+ {
+
+ nx0 = nx[ii];
+ nu0 = nu[ii];
+ nb0 = nb[ii];
+ ng0 = ng[ii];
+
+ VECCP_LIBSTR(nu0+nx0, rq+ii, 0, res_g+ii, 0);
+
+ if(ii>0)
+ AXPY_LIBSTR(nx0, -1.0, pi+(ii-1), 0, res_g+ii, nu0, res_g+ii, nu0);
+
+ SYMV_L_LIBSTR(nu0+nx0, nu0+nx0, 1.0, RSQrq+ii, 0, 0, ux+ii, 0, 1.0, res_g+ii, 0, res_g+ii, 0);
+
+ if(nb0>0)
+ {
+
+ AXPY_LIBSTR(nb0, -1.0, lam_lb+ii, 0, lam_ub+ii, 0, tmp_nbM, 0);
+ VECAD_SP_LIBSTR(nb0, 1.0, tmp_nbM, 0, idxb[ii], res_g+ii, 0);
+
+ VECEX_SP_LIBSTR(nb0, -1.0, idxb[ii], ux+ii, 0, res_d_lb+ii, 0);
+ VECCP_LIBSTR(nb0, res_d_lb+ii, 0, res_d_ub+ii, 0);
+ AXPY_LIBSTR(nb0, 1.0, d_lb+ii, 0, res_d_lb+ii, 0, res_d_lb+ii, 0);
+ AXPY_LIBSTR(nb0, 1.0, d_ub+ii, 0, res_d_ub+ii, 0, res_d_ub+ii, 0);
+ AXPY_LIBSTR(nb0, 1.0, t_lb+ii, 0, res_d_lb+ii, 0, res_d_lb+ii, 0);
+ AXPY_LIBSTR(nb0, -1.0, t_ub+ii, 0, res_d_ub+ii, 0, res_d_ub+ii, 0);
+
+ }
+
+ if(ng0>0) // TODO merge with bounds as much as possible
+ {
+
+ AXPY_LIBSTR(ng0, -1.0, lam_lg+ii, 0, lam_ug+ii, 0, tmp_ngM+0, 0);
+
+ AXPY_LIBSTR(ng0, 1.0, t_lg+ii, 0, d_lg+ii, 0, res_d_lg+ii, 0);
+ AXPY_LIBSTR(ng0, -1.0, t_ug+ii, 0, d_ug+ii, 0, res_d_ug+ii, 0);
+
+ GEMV_NT_LIBSTR(nu0+nx0, ng0, 1.0, 1.0, DCt+ii, 0, 0, tmp_ngM+0, 0, ux+ii, 0, 1.0, 0.0, res_g+ii, 0, tmp_ngM+1, 0, res_g+ii, 0, tmp_ngM+1, 0);
+
+ AXPY_LIBSTR(ng0, -1.0, tmp_ngM+1, 0, res_d_lg+ii, 0, res_d_lg+ii, 0);
+ AXPY_LIBSTR(ng0, -1.0, tmp_ngM+1, 0, res_d_ug+ii, 0, res_d_ug+ii, 0);
+
+ }
+
+
+ if(ii<N)
+ {
+
+ nu1 = nu[ii+1];
+ nx1 = nx[ii+1];
+
+ AXPY_LIBSTR(nx1, -1.0, ux+(ii+1), nu1, b+ii, 0, res_b+ii, 0);
+
+ GEMV_NT_LIBSTR(nu0+nx0, nx1, 1.0, 1.0, BAbt+ii, 0, 0, pi+ii, 0, ux+ii, 0, 1.0, 1.0, res_g+ii, 0, res_b+ii, 0, res_g+ii, 0, res_b+ii, 0);
+
+ }
+
+ }
+
+ mu += VECMULDOT_LIBSTR(2*nbt+2*ngt, lam_lb, 0, t_lb, 0, ws->res_m, 0);
+
+ if(cws->nb+cws->ng>0)
+ ws->res_mu = mu*cws->nt_inv;
+ else
+ ws->res_mu = 0.0;
+
+ return;
+
+ }
+
+
+
+// backward Riccati recursion
+void FACT_SOLVE_KKT_UNCONSTR_OCP_QP(struct OCP_QP *qp, struct OCP_QP_SOL *qp_sol, struct IPM_HARD_OCP_QP_WORKSPACE *ws)
+ {
+
+ int N = qp->N;
+ int *nx = qp->nx;
+ int *nu = qp->nu;
+ int *nb = qp->nb;
+ int *ng = qp->ng;
+
+ struct STRMAT *BAbt = qp->BAbt;
+ struct STRMAT *RSQrq = qp->RSQrq;
+ struct STRVEC *b = qp->b;
+
+ struct STRVEC *ux = qp_sol->ux;
+ struct STRVEC *pi = qp_sol->pi;
+
+ struct STRMAT *L = ws->L;
+ struct STRMAT *AL = ws->AL;
+ struct STRVEC *tmp_nxM = ws->tmp_nxM;
+
+ //
+ int ii;
+
+ // factorization and backward substitution
+
+ // last stage
+ POTRF_L_MN_LIBSTR(nu[N]+nx[N]+1, nu[N]+nx[N], RSQrq+N, 0, 0, L+N, 0, 0);
+
+ // middle stages
+ for(ii=0; ii<N; ii++)
+ {
+ GECP_LIBSTR(nu[N-ii-1]+nx[N-ii-1]+1, nx[N-ii], BAbt+(N-ii-1), 0, 0, AL, 0, 0);
+ TRMM_RLNN_LIBSTR(nu[N-ii-1]+nx[N-ii-1]+1, nx[N-ii], 1.0, L+(N-ii), nu[N-ii], nu[N-ii], AL, 0, 0, AL, 0, 0);
+ ROWEX_LIBSTR(nx[N-ii], 1.0, AL, nu[N-ii-1]+nx[N-ii-1], 0, tmp_nxM, 0);
+ GEAD_LIBSTR(1, nx[N-ii], 1.0, L+(N-ii), nu[N-ii]+nx[N-ii], nu[N-ii], AL, nu[N-ii-1]+nx[N-ii-1], 0);
+
+ SYRK_POTRF_LN_LIBSTR(nu[N-ii-1]+nx[N-ii-1]+1, nu[N-ii-1]+nx[N-ii-1], nx[N-ii], AL, 0, 0, AL, 0, 0, RSQrq+(N-ii-1), 0, 0, L+(N-ii-1), 0, 0);
+
+// d_print_strmat(nu[N-ii-1]+nx[N-ii-1]+1, nu[N-ii-1]+nx[N-ii-1], L+(N-ii-1), 0, 0);
+ }
+
+ // forward substitution
+
+ // first stage
+ ii = 0;
+ ROWEX_LIBSTR(nu[ii]+nx[ii], -1.0, L+(ii), nu[ii]+nx[ii], 0, ux+ii, 0);
+ TRSV_LTN_LIBSTR(nu[ii]+nx[ii], L+ii, 0, 0, ux+ii, 0, ux+ii, 0);
+ GEMV_T_LIBSTR(nu[ii]+nx[ii], nx[ii+1], 1.0, BAbt+ii, 0, 0, ux+ii, 0, 1.0, b+ii, 0, ux+(ii+1), nu[ii+1]);
+ ROWEX_LIBSTR(nx[ii+1], 1.0, L+(ii+1), nu[ii+1]+nx[ii+1], nu[ii+1], tmp_nxM, 0);
+ TRMV_LTN_LIBSTR(nx[ii+1], nx[ii+1], L+(ii+1), nu[ii+1], nu[ii+1], ux+(ii+1), nu[ii+1], pi+ii, 0);
+ AXPY_LIBSTR(nx[ii+1], 1.0, tmp_nxM, 0, pi+ii, 0, pi+ii, 0);
+ TRMV_LNN_LIBSTR(nx[ii+1], nx[ii+1], L+(ii+1), nu[ii+1], nu[ii+1], pi+ii, 0, pi+ii, 0);
+
+// d_print_tran_strvec(nu[ii]+nx[ii], ux+ii, 0);
+
+ // middle stages
+ for(ii=1; ii<N; ii++)
+ {
+ ROWEX_LIBSTR(nu[ii], -1.0, L+(ii), nu[ii]+nx[ii], 0, ux+ii, 0);
+ TRSV_LTN_MN_LIBSTR(nu[ii]+nx[ii], nu[ii], L+ii, 0, 0, ux+ii, 0, ux+ii, 0);
+ GEMV_T_LIBSTR(nu[ii]+nx[ii], nx[ii+1], 1.0, BAbt+ii, 0, 0, ux+ii, 0, 1.0, b+ii, 0, ux+(ii+1), nu[ii+1]);
+ ROWEX_LIBSTR(nx[ii+1], 1.0, L+(ii+1), nu[ii+1]+nx[ii+1], nu[ii+1], tmp_nxM, 0);
+ TRMV_LTN_LIBSTR(nx[ii+1], nx[ii+1], L+(ii+1), nu[ii+1], nu[ii+1], ux+(ii+1), nu[ii+1], pi+ii, 0);
+ AXPY_LIBSTR(nx[ii+1], 1.0, tmp_nxM, 0, pi+ii, 0, pi+ii, 0);
+ TRMV_LNN_LIBSTR(nx[ii+1], nx[ii+1], L+(ii+1), nu[ii+1], nu[ii+1], pi+ii, 0, pi+ii, 0);
+
+// d_print_tran_strvec(nu[ii]+nx[ii], ux+ii, 0);
+ }
+
+ return;
+
+ }
+
+
+
+// backward Riccati recursion
+void FACT_SOLVE_KKT_STEP_HARD_OCP_QP(struct OCP_QP *qp, struct IPM_HARD_OCP_QP_WORKSPACE *ws)
+ {
+
+ int N = qp->N;
+ int *nx = qp->nx;
+ int *nu = qp->nu;
+ int *nb = qp->nb;
+ int *ng = qp->ng;
+
+ struct STRMAT *BAbt = qp->BAbt;
+ struct STRMAT *RSQrq = qp->RSQrq;
+ struct STRMAT *DCt = qp->DCt;
+ int **idxb = qp->idxb;
+
+ struct STRMAT *L = ws->L;
+ struct STRMAT *AL = ws->AL;
+ struct STRVEC *res_b = ws->res_b;
+ struct STRVEC *res_g = ws->res_g;
+ struct STRVEC *dux = ws->dux;
+ struct STRVEC *dpi = ws->dpi;
+ struct STRVEC *dt_lb = ws->dt_lb;
+ struct STRVEC *dt_lg = ws->dt_lg;
+ struct STRVEC *Qx_lg = ws->Qx_lg;
+ struct STRVEC *Qx_lb = ws->Qx_lb;
+ struct STRVEC *qx_lg = ws->qx_lg;
+ struct STRVEC *qx_lb = ws->qx_lb;
+ struct STRVEC *Pb = ws->Pb;
+ struct STRVEC *tmp_nxM = ws->tmp_nxM;
+
+ //
+ int ii;
+
+ struct IPM_HARD_CORE_QP_WORKSPACE *cws = ws->core_workspace;
+
+// if(nb>0 | ng>0)
+// {
+ COMPUTE_QX_QX_HARD_QP(cws);
+// }
+
+ // factorization and backward substitution
+
+ // last stage
+#if defined(DOUBLE_PRECISION)
+ TRCP_L_LIBSTR(nu[N]+nx[N], RSQrq+N, 0, 0, L+N, 0, 0); // TODO dtrcp_l_libstr with m and n, for m>=n
+#else
+ GECP_LIBSTR(nu[N]+nx[N], nu[N]+nx[N], RSQrq+N, 0, 0, L+N, 0, 0); // TODO dtrcp_l_libstr with m and n, for m>=n
+#endif
+ ROWIN_LIBSTR(nu[N]+nx[N], 1.0, res_g+N, 0, L+N, nu[N]+nx[N], 0);
+ if(nb[N]>0)
+ {
+ DIAAD_SP_LIBSTR(nb[N], 1.0, Qx_lb+N, 0, idxb[N], L+N, 0, 0);
+ ROWAD_SP_LIBSTR(nb[N], 1.0, qx_lb+N, 0, idxb[N], L+N, nu[N]+nx[N], 0);
+ }
+ if(ng[N]>0)
+ {
+ GEMM_R_DIAG_LIBSTR(nu[N]+nx[N], ng[N], 1.0, DCt+N, 0, 0, Qx_lg+N, 0, 0.0, AL+0, 0, 0, AL+0, 0, 0);
+ ROWIN_LIBSTR(ng[N], 1.0, qx_lg+N, 0, AL+0, nu[N]+nx[N], 0);
+ SYRK_POTRF_LN_LIBSTR(nu[N]+nx[N]+1, nu[N]+nx[N], ng[N], AL+0, 0, 0, DCt+N, 0, 0, L+N, 0, 0, L+N, 0, 0);
+ }
+ else
+ {
+ POTRF_L_MN_LIBSTR(nu[N]+nx[N]+1, nu[N]+nx[N], L+N, 0, 0, L+N, 0, 0);
+ }
+
+ // middle stages
+ for(ii=0; ii<N; ii++)
+ {
+ GECP_LIBSTR(nu[N-ii-1]+nx[N-ii-1], nx[N-ii], BAbt+(N-ii-1), 0, 0, AL, 0, 0);
+ ROWIN_LIBSTR(nx[N-ii], 1.0, res_b+(N-ii-1), 0, AL, nu[N-ii-1]+nx[N-ii-1], 0);
+ TRMM_RLNN_LIBSTR(nu[N-ii-1]+nx[N-ii-1]+1, nx[N-ii], 1.0, L+(N-ii), nu[N-ii], nu[N-ii], AL, 0, 0, AL, 0, 0);
+ ROWEX_LIBSTR(nx[N-ii], 1.0, AL, nu[N-ii-1]+nx[N-ii-1], 0, tmp_nxM, 0);
+ TRMV_LNN_LIBSTR(nx[N-ii], nx[N-ii], L+(N-ii), nu[N-ii], nu[N-ii], tmp_nxM, 0, Pb+(N-ii-1), 0);
+ GEAD_LIBSTR(1, nx[N-ii], 1.0, L+(N-ii), nu[N-ii]+nx[N-ii], nu[N-ii], AL, nu[N-ii-1]+nx[N-ii-1], 0);
+
+#if defined(DOUBLE_PRECISION)
+ TRCP_L_LIBSTR(nu[N-ii-1]+nx[N-ii-1], RSQrq+(N-ii-1), 0, 0, L+(N-ii-1), 0, 0);
+#else
+ GECP_LIBSTR(nu[N-ii-1]+nx[N-ii-1], nu[N-ii-1]+nx[N-ii-1], RSQrq+(N-ii-1), 0, 0, L+(N-ii-1), 0, 0);
+#endif
+ ROWIN_LIBSTR(nu[N-ii-1]+nx[N-ii-1], 1.0, res_g+(N-ii-1), 0, L+(N-ii-1), nu[N-ii-1]+nx[N-ii-1], 0);
+
+ if(nb[N-ii-1]>0)
+ {
+ DIAAD_SP_LIBSTR(nb[N-ii-1], 1.0, Qx_lb+(N-ii-1), 0, idxb[N-ii-1], L+(N-ii-1), 0, 0);
+ ROWAD_SP_LIBSTR(nb[N-ii-1], 1.0, qx_lb+(N-ii-1), 0, idxb[N-ii-1], L+(N-ii-1), nu[N-ii-1]+nx[N-ii-1], 0);
+ }
+
+ if(ng[N-ii-1]>0)
+ {
+ GEMM_R_DIAG_LIBSTR(nu[N-ii-1]+nx[N-ii-1], ng[N-ii-1], 1.0, DCt+N-ii-1, 0, 0, Qx_lg+N-ii-1, 0, 0.0, AL+0, 0, nx[N-ii], AL+0, 0, nx[N-ii]);
+ ROWIN_LIBSTR(ng[N-ii-1], 1.0, qx_lg+N-ii-1, 0, AL+0, nu[N-ii-1]+nx[N-ii-1], nx[N-ii]);
+ GECP_LIBSTR(nu[N-ii-1]+nx[N-ii-1], nx[N-ii], AL+0, 0, 0, AL+1, 0, 0);
+ GECP_LIBSTR(nu[N-ii-1]+nx[N-ii-1], ng[N-ii-1], DCt+N-ii-1, 0, 0, AL+1, 0, nx[N-ii]);
+ SYRK_POTRF_LN_LIBSTR(nu[N-ii-1]+nx[N-ii-1]+1, nu[N-ii-1]+nx[N-ii-1], nx[N-ii]+ng[N-ii-1], AL+0, 0, 0, AL+1, 0, 0, L+N-ii-1, 0, 0, L+N-ii-1, 0, 0);
+ }
+ else
+ {
+ SYRK_POTRF_LN_LIBSTR(nu[N-ii-1]+nx[N-ii-1]+1, nu[N-ii-1]+nx[N-ii-1], nx[N-ii], AL, 0, 0, AL, 0, 0, L+(N-ii-1), 0, 0, L+(N-ii-1), 0, 0);
+ }
+
+// d_print_strmat(nu[N-ii-1]+nx[N-ii-1]+1, nu[N-ii-1]+nx[N-ii-1], L+(N-ii-1), 0, 0);
+ }
+
+ // forward substitution
+
+ // first stage
+ ii = 0;
+ ROWEX_LIBSTR(nu[ii]+nx[ii], -1.0, L+(ii), nu[ii]+nx[ii], 0, dux+ii, 0);
+ TRSV_LTN_LIBSTR(nu[ii]+nx[ii], L+ii, 0, 0, dux+ii, 0, dux+ii, 0);
+ GEMV_T_LIBSTR(nu[ii]+nx[ii], nx[ii+1], 1.0, BAbt+ii, 0, 0, dux+ii, 0, 1.0, res_b+ii, 0, dux+(ii+1), nu[ii+1]);
+ ROWEX_LIBSTR(nx[ii+1], 1.0, L+(ii+1), nu[ii+1]+nx[ii+1], nu[ii+1], tmp_nxM, 0);
+ TRMV_LTN_LIBSTR(nx[ii+1], nx[ii+1], L+(ii+1), nu[ii+1], nu[ii+1], dux+(ii+1), nu[ii+1], dpi+ii, 0);
+ AXPY_LIBSTR(nx[ii+1], 1.0, tmp_nxM, 0, dpi+ii, 0, dpi+ii, 0);
+ TRMV_LNN_LIBSTR(nx[ii+1], nx[ii+1], L+(ii+1), nu[ii+1], nu[ii+1], dpi+ii, 0, dpi+ii, 0);
+
+// d_print_tran_strvec(nu[ii]+nx[ii], dux+ii, 0);
+
+ // middle stages
+ for(ii=1; ii<N; ii++)
+ {
+ ROWEX_LIBSTR(nu[ii], -1.0, L+(ii), nu[ii]+nx[ii], 0, dux+ii, 0);
+ TRSV_LTN_MN_LIBSTR(nu[ii]+nx[ii], nu[ii], L+ii, 0, 0, dux+ii, 0, dux+ii, 0);
+ GEMV_T_LIBSTR(nu[ii]+nx[ii], nx[ii+1], 1.0, BAbt+ii, 0, 0, dux+ii, 0, 1.0, res_b+ii, 0, dux+(ii+1), nu[ii+1]);
+ ROWEX_LIBSTR(nx[ii+1], 1.0, L+(ii+1), nu[ii+1]+nx[ii+1], nu[ii+1], tmp_nxM, 0);
+ TRMV_LTN_LIBSTR(nx[ii+1], nx[ii+1], L+(ii+1), nu[ii+1], nu[ii+1], dux+(ii+1), nu[ii+1], dpi+ii, 0);
+ AXPY_LIBSTR(nx[ii+1], 1.0, tmp_nxM, 0, dpi+ii, 0, dpi+ii, 0);
+ TRMV_LNN_LIBSTR(nx[ii+1], nx[ii+1], L+(ii+1), nu[ii+1], nu[ii+1], dpi+ii, 0, dpi+ii, 0);
+
+// d_print_tran_strvec(nu[ii]+nx[ii], dux+ii, 0);
+ }
+
+
+
+// if(nb>0)
+// {
+ for(ii=0; ii<=N; ii++)
+ VECEX_SP_LIBSTR(nb[ii], 1.0, idxb[ii], dux+ii, 0, dt_lb+ii, 0);
+// }
+
+// if(ng>0)
+// {
+ for(ii=0; ii<=N; ii++)
+ GEMV_T_LIBSTR(nu[ii]+nx[ii], ng[ii], 1.0, DCt+ii, 0, 0, dux+ii, 0, 0.0, dt_lg+ii, 0, dt_lg+ii, 0);
+// }
+
+// if(nb>0 | ng>0)
+// {
+ COMPUTE_LAM_T_HARD_QP(cws);
+// }
+
+ return;
+
+ }
+
+
+
+// backward Riccati recursion
+void SOLVE_KKT_STEP_HARD_OCP_QP(struct OCP_QP *qp, struct IPM_HARD_OCP_QP_WORKSPACE *ws)
+ {
+
+ int N = qp->N;
+ int *nx = qp->nx;
+ int *nu = qp->nu;
+ int *nb = qp->nb;
+ int *ng = qp->ng;
+
+ struct STRMAT *BAbt = qp->BAbt;
+ struct STRMAT *RSQrq = qp->RSQrq;
+ struct STRMAT *DCt = qp->DCt;
+ int **idxb = qp->idxb;
+
+ struct STRMAT *L = ws->L;
+ struct STRMAT *AL = ws->AL;
+ struct STRVEC *res_b = ws->res_b;
+ struct STRVEC *res_g = ws->res_g;
+ struct STRVEC *dux = ws->dux;
+ struct STRVEC *dpi = ws->dpi;
+ struct STRVEC *dt_lb = ws->dt_lb;
+ struct STRVEC *dt_lg = ws->dt_lg;
+ struct STRVEC *qx_lg = ws->qx_lg;
+ struct STRVEC *qx_lb = ws->qx_lb;
+ struct STRVEC *Pb = ws->Pb;
+ struct STRVEC *tmp_nxM = ws->tmp_nxM;
+
+ //
+ int ii;
+
+ struct IPM_HARD_CORE_QP_WORKSPACE *cws = ws->core_workspace;
+
+// if(nb>0 | ng>0)
+// {
+ COMPUTE_QX_HARD_QP(cws);
+// }
+
+ // backward substitution
+
+ // last stage
+ VECCP_LIBSTR(nu[N]+nx[N], res_g+N, 0, dux+N, 0);
+ if(nb[N]>0)
+ {
+ VECAD_SP_LIBSTR(nb[N], 1.0, qx_lb+N, 0, idxb[N], dux+N, 0);
+ }
+ // general constraints
+ if(ng[N]>0)
+ {
+ GEMV_N_LIBSTR(nu[N]+nx[N], ng[N], 1.0, DCt+N, 0, 0, qx_lg+N, 0, 1.0, dux+N, 0, dux+N, 0);
+ }
+
+ // middle stages
+ for(ii=0; ii<N-1; ii++)
+ {
+ VECCP_LIBSTR(nu[N-ii-1]+nx[N-ii-1], res_g+N-ii-1, 0, dux+N-ii-1, 0);
+ if(nb[N-ii-1]>0)
+ {
+ VECAD_SP_LIBSTR(nb[N-ii-1], 1.0, qx_lb+N-ii-1, 0, idxb[N-ii-1], dux+N-ii-1, 0);
+ }
+ if(ng[N-ii-1]>0)
+ {
+ GEMV_N_LIBSTR(nu[N-ii-1]+nx[N-ii-1], ng[N-ii-1], 1.0, DCt+N-ii-1, 0, 0, qx_lg+N-ii-1, 0, 1.0, dux+N-ii-1, 0, dux+N-ii-1, 0);
+ }
+ AXPY_LIBSTR(nx[N-ii], 1.0, dux+N-ii, nu[N-ii], Pb+N-ii-1, 0, tmp_nxM, 0);
+ GEMV_N_LIBSTR(nu[N-ii-1]+nx[N-ii-1], nx[N-ii], 1.0, BAbt+N-ii-1, 0, 0, tmp_nxM, 0, 1.0, dux+N-ii-1, 0, dux+N-ii-1, 0);
+ TRSV_LNN_MN_LIBSTR(nu[N-ii-1]+nx[N-ii-1], nu[N-ii-1], L+N-ii-1, 0, 0, dux+N-ii-1, 0, dux+N-ii-1, 0);
+ }
+
+ // first stage
+ ii = N-1;
+ VECCP_LIBSTR(nu[N-ii-1]+nx[N-ii-1], res_g+N-ii-1, 0, dux+N-ii-1, 0);
+ if(nb[N-ii-1]>0)
+ {
+ VECAD_SP_LIBSTR(nb[N-ii-1], 1.0, qx_lb+N-ii-1, 0, idxb[N-ii-1], dux+N-ii-1, 0);
+ }
+ if(ng[N-ii-1]>0)
+ {
+ GEMV_N_LIBSTR(nu[N-ii-1]+nx[N-ii-1], ng[N-ii-1], 1.0, DCt+N-ii-1, 0, 0, qx_lg+N-ii-1, 0, 1.0, dux+N-ii-1, 0, dux+N-ii-1, 0);
+ }
+ AXPY_LIBSTR(nx[N-ii], 1.0, dux+N-ii, nu[N-ii], Pb+N-ii-1, 0, tmp_nxM, 0);
+ GEMV_N_LIBSTR(nu[N-ii-1]+nx[N-ii-1], nx[N-ii], 1.0, BAbt+N-ii-1, 0, 0, tmp_nxM, 0, 1.0, dux+N-ii-1, 0, dux+N-ii-1, 0);
+ TRSV_LNN_LIBSTR(nu[N-ii-1]+nx[N-ii-1], L+N-ii-1, 0, 0, dux+N-ii-1, 0, dux+N-ii-1, 0);
+
+ // first stage
+ ii = 0;
+ VECCP_LIBSTR(nx[ii+1], dux+ii+1, nu[ii+1], dpi+ii, 0);
+ VECSC_LIBSTR(nu[ii]+nx[ii], -1.0, dux+ii, 0);
+ TRSV_LTN_LIBSTR(nu[ii]+nx[ii], L+ii, 0, 0, dux+ii, 0, dux+ii, 0);
+ GEMV_T_LIBSTR(nu[ii]+nx[ii], nx[ii+1], 1.0, BAbt+ii, 0, 0, dux+ii, 0, 1.0, res_b+ii, 0, dux+ii+1, nu[ii+1]);
+ VECCP_LIBSTR(nx[ii+1], dux+ii+1, nu[ii+1], tmp_nxM, 0);
+ TRMV_LTN_LIBSTR(nx[ii+1], nx[ii+1], L+ii+1, nu[ii+1], nu[ii+1], tmp_nxM, 0, tmp_nxM, 0);
+ TRMV_LNN_LIBSTR(nx[ii+1], nx[ii+1], L+ii+1, nu[ii+1], nu[ii+1], tmp_nxM, 0, tmp_nxM, 0);
+ AXPY_LIBSTR(nx[ii+1], 1.0, tmp_nxM, 0, dpi+ii, 0, dpi+ii, 0);
+
+ // middle stages
+ for(ii=1; ii<N; ii++)
+ {
+ VECCP_LIBSTR(nx[ii+1], dux+ii+1, nu[ii+1], dpi+ii, 0);
+ VECSC_LIBSTR(nu[ii], -1.0, dux+ii, 0);
+ TRSV_LTN_MN_LIBSTR(nu[ii]+nx[ii], nu[ii], L+ii, 0, 0, dux+ii, 0, dux+ii, 0);
+ GEMV_T_LIBSTR(nu[ii]+nx[ii], nx[ii+1], 1.0, BAbt+ii, 0, 0, dux+ii, 0, 1.0, res_b+ii, 0, dux+ii+1, nu[ii+1]);
+ VECCP_LIBSTR(nx[ii+1], dux+ii+1, nu[ii+1], tmp_nxM, 0);
+ TRMV_LTN_LIBSTR(nx[ii+1], nx[ii+1], L+ii+1, nu[ii+1], nu[ii+1], tmp_nxM, 0, tmp_nxM, 0);
+ TRMV_LNN_LIBSTR(nx[ii+1], nx[ii+1], L+ii+1, nu[ii+1], nu[ii+1], tmp_nxM, 0, tmp_nxM, 0);
+ AXPY_LIBSTR(nx[ii+1], 1.0, tmp_nxM, 0, dpi+ii, 0, dpi+ii, 0);
+ }
+
+
+
+// if(nb>0)
+// {
+ for(ii=0; ii<=N; ii++)
+ VECEX_SP_LIBSTR(nb[ii], 1.0, idxb[ii], dux+ii, 0, dt_lb+ii, 0);
+// }
+
+// if(ng>0)
+// {
+ for(ii=0; ii<=N; ii++)
+ GEMV_T_LIBSTR(nu[ii]+nx[ii], ng[ii], 1.0, DCt+ii, 0, 0, dux+ii, 0, 0.0, dt_lg+ii, 0, dt_lg+ii, 0);
+// }
+
+// if(nb>0 | ng>0)
+// {
+ COMPUTE_LAM_T_HARD_QP(cws);
+// }
+
+ return;
+
+ }
+
+
diff --git a/ocp_qp/x_ocp_qp_sol.c b/ocp_qp/x_ocp_qp_sol.c
new file mode 100644
index 0000000..274f0f0
--- /dev/null
+++ b/ocp_qp/x_ocp_qp_sol.c
@@ -0,0 +1,304 @@
+/**************************************************************************************************
+* *
+* 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 *
+* *
+**************************************************************************************************/
+
+
+
+int MEMSIZE_OCP_QP_SOL(int N, int *nx, int *nu, int *nb, int *ng)
+ {
+
+ int ii;
+
+ int nvt = 0;
+ int net = 0;
+ int nbt = 0;
+ int ngt = 0;
+ for(ii=0; ii<N; ii++)
+ {
+ nvt += nu[ii]+nx[ii];
+ net += nx[ii+1];
+ nbt += nb[ii];
+ ngt += ng[ii];
+ }
+ nvt += nu[ii]+nx[ii];
+ nbt += nb[ii];
+ ngt += ng[ii];
+
+ int size = 0;
+
+ size += (0+10*(N+1))*sizeof(struct STRVEC); // ux pi lam_lb lam_ub lam_lg lam_ug t_lb t_ub t_lg t_ug
+
+ size += 1*SIZE_STRVEC(nvt); // ux
+ size += 1*SIZE_STRVEC(net); // pi
+ size += 8*SIZE_STRVEC(2*nbt+2*ngt); // lam_lb lam_ub lam_lg lam_ug t_lb t_ub t_lg t_ug
+
+ size = (size+63)/64*64; // make multiple of typical cache line size
+ size += 64; // align to typical cache line size
+
+ return size;
+
+ }
+
+
+
+void CREATE_OCP_QP_SOL(int N, int *nx, int *nu, int *nb, int *ng, struct OCP_QP_SOL *qp_sol, void *memory)
+ {
+
+ int ii;
+
+
+ // memsize
+ qp_sol->memsize = MEMSIZE_OCP_QP_SOL(N, nx, nu, nb, ng);
+
+
+ int nvt = 0;
+ int net = 0;
+ int nbt = 0;
+ int ngt = 0;
+ for(ii=0; ii<N; ii++)
+ {
+ nvt += nu[ii]+nx[ii];
+ net += nx[ii+1];
+ nbt += nb[ii];
+ ngt += ng[ii];
+ }
+ nvt += nu[ii]+nx[ii];
+ nbt += nb[ii];
+ ngt += ng[ii];
+
+
+ // vector struct stuff
+ struct STRVEC *sv_ptr = (struct STRVEC *) memory;
+
+ qp_sol->ux = sv_ptr;
+ sv_ptr += N+1;
+ qp_sol->pi = sv_ptr;
+ sv_ptr += N+1;
+ qp_sol->lam_lb = sv_ptr;
+ sv_ptr += N+1;
+ qp_sol->lam_ub = sv_ptr;
+ sv_ptr += N+1;
+ qp_sol->lam_lg = sv_ptr;
+ sv_ptr += N+1;
+ qp_sol->lam_ug = sv_ptr;
+ sv_ptr += N+1;
+ qp_sol->t_lb = sv_ptr;
+ sv_ptr += N+1;
+ qp_sol->t_ub = sv_ptr;
+ sv_ptr += N+1;
+ qp_sol->t_lg = sv_ptr;
+ sv_ptr += N+1;
+ qp_sol->t_ug = sv_ptr;
+ sv_ptr += N+1;
+
+
+ // align to typical cache line size
+ long long l_ptr = (long long) sv_ptr;
+ l_ptr = (l_ptr+63)/64*64;
+
+
+ // double stuff
+ void *v_ptr;
+ v_ptr = (void *) l_ptr;
+
+ void *tmp_ptr;
+
+ // ux
+ tmp_ptr = v_ptr;
+ v_ptr += SIZE_STRVEC(nvt);
+ for(ii=0; ii<=N; ii++)
+ {
+ CREATE_STRVEC(nu[ii]+nx[ii], qp_sol->ux+ii, tmp_ptr);
+ tmp_ptr += (nu[ii]+nx[ii])*sizeof(REAL);
+ }
+ // pi
+ tmp_ptr = v_ptr;
+ v_ptr += SIZE_STRVEC(net);
+ for(ii=0; ii<N; ii++)
+ {
+ CREATE_STRVEC(nx[ii+1], qp_sol->pi+ii, tmp_ptr);
+ tmp_ptr += (nx[ii+1])*sizeof(REAL);
+ }
+ // lam
+ tmp_ptr = v_ptr;
+ v_ptr += SIZE_STRVEC(2*nbt+2*ngt);
+ // lam_lb
+ for(ii=0; ii<=N; ii++)
+ {
+ CREATE_STRVEC(nb[ii], qp_sol->lam_lb+ii, tmp_ptr);
+ tmp_ptr += (nb[ii])*sizeof(REAL);
+ }
+ // lam_lg
+ for(ii=0; ii<=N; ii++)
+ {
+ CREATE_STRVEC(ng[ii], qp_sol->lam_lg+ii, tmp_ptr);
+ tmp_ptr += (ng[ii])*sizeof(REAL);
+ }
+ // lam_ub
+ for(ii=0; ii<=N; ii++)
+ {
+ CREATE_STRVEC(nb[ii], qp_sol->lam_ub+ii, tmp_ptr);
+ tmp_ptr += (nb[ii])*sizeof(REAL);
+ }
+ // lam_ug
+ for(ii=0; ii<=N; ii++)
+ {
+ CREATE_STRVEC(ng[ii], qp_sol->lam_ug+ii, tmp_ptr);
+ tmp_ptr += (ng[ii])*sizeof(REAL);
+ }
+ // t
+ tmp_ptr = v_ptr;
+ v_ptr += SIZE_STRVEC(2*nbt+2*ngt);
+ // t_lb
+ for(ii=0; ii<=N; ii++)
+ {
+ CREATE_STRVEC(nb[ii], qp_sol->t_lb+ii, tmp_ptr);
+ tmp_ptr += (nb[ii])*sizeof(REAL);
+ }
+ // t_lg
+ for(ii=0; ii<=N; ii++)
+ {
+ CREATE_STRVEC(ng[ii], qp_sol->t_lg+ii, tmp_ptr);
+ tmp_ptr += (ng[ii])*sizeof(REAL);
+ }
+ // t_ub
+ for(ii=0; ii<=N; ii++)
+ {
+ CREATE_STRVEC(nb[ii], qp_sol->t_ub+ii, tmp_ptr);
+ tmp_ptr += (nb[ii])*sizeof(REAL);
+ }
+ // t_ug
+ for(ii=0; ii<=N; ii++)
+ {
+ CREATE_STRVEC(ng[ii], qp_sol->t_ug+ii, tmp_ptr);
+ tmp_ptr += (ng[ii])*sizeof(REAL);
+ }
+
+ return;
+
+ }
+
+
+
+void CVT_OCP_QP_SOL_TO_COLMAJ(struct OCP_QP *qp, struct OCP_QP_SOL *qp_sol, REAL **u, REAL **x, REAL **pi, REAL **lam_lb, REAL **lam_ub, REAL **lam_lg, REAL **lam_ug)
+ {
+
+ int N = qp->N;
+ int *nx = qp->nx;
+ int *nu = qp->nu;
+ int *nb = qp->nb;
+ int *ng = qp->ng;
+
+ int ii;
+
+ for(ii=0; ii<N; ii++)
+ {
+ CVT_STRVEC2VEC(nu[ii], qp_sol->ux+ii, 0, u[ii]);
+ CVT_STRVEC2VEC(nx[ii], qp_sol->ux+ii, nu[ii], x[ii]);
+ CVT_STRVEC2VEC(nx[ii+1], qp_sol->pi+ii, 0, pi[ii]);
+ CVT_STRVEC2VEC(nb[ii], qp_sol->lam_lb+ii, 0, lam_lb[ii]);
+ CVT_STRVEC2VEC(nb[ii], qp_sol->lam_ub+ii, 0, lam_ub[ii]);
+ CVT_STRVEC2VEC(ng[ii], qp_sol->lam_lg+ii, 0, lam_lg[ii]);
+ CVT_STRVEC2VEC(ng[ii], qp_sol->lam_ug+ii, 0, lam_ug[ii]);
+ }
+ CVT_STRVEC2VEC(nu[ii], qp_sol->ux+ii, 0, u[ii]);
+ CVT_STRVEC2VEC(nx[ii], qp_sol->ux+ii, nu[ii], x[ii]);
+ CVT_STRVEC2VEC(nb[ii], qp_sol->lam_lb+ii, 0, lam_lb[ii]);
+ CVT_STRVEC2VEC(nb[ii], qp_sol->lam_ub+ii, 0, lam_ub[ii]);
+ CVT_STRVEC2VEC(ng[ii], qp_sol->lam_lg+ii, 0, lam_lg[ii]);
+ CVT_STRVEC2VEC(ng[ii], qp_sol->lam_ug+ii, 0, lam_ug[ii]);
+
+ return;
+
+ }
+
+
+
+void CVT_OCP_QP_SOL_TO_ROWMAJ(struct OCP_QP *qp, struct OCP_QP_SOL *qp_sol, REAL **u, REAL **x, REAL **pi, REAL **lam_lb, REAL **lam_ub, REAL **lam_lg, REAL **lam_ug)
+ {
+
+ int N = qp->N;
+ int *nx = qp->nx;
+ int *nu = qp->nu;
+ int *nb = qp->nb;
+ int *ng = qp->ng;
+
+ int ii;
+
+ for(ii=0; ii<N; ii++)
+ {
+ CVT_STRVEC2VEC(nu[ii], qp_sol->ux+ii, 0, u[ii]);
+ CVT_STRVEC2VEC(nx[ii], qp_sol->ux+ii, nu[ii], x[ii]);
+ CVT_STRVEC2VEC(nx[ii+1], qp_sol->pi+ii, 0, pi[ii]);
+ CVT_STRVEC2VEC(nb[ii], qp_sol->lam_lb+ii, 0, lam_lb[ii]);
+ CVT_STRVEC2VEC(nb[ii], qp_sol->lam_ub+ii, 0, lam_ub[ii]);
+ CVT_STRVEC2VEC(ng[ii], qp_sol->lam_lg+ii, 0, lam_lg[ii]);
+ CVT_STRVEC2VEC(ng[ii], qp_sol->lam_ug+ii, 0, lam_ug[ii]);
+ }
+ CVT_STRVEC2VEC(nu[ii], qp_sol->ux+ii, 0, u[ii]);
+ CVT_STRVEC2VEC(nx[ii], qp_sol->ux+ii, nu[ii], x[ii]);
+ CVT_STRVEC2VEC(nb[ii], qp_sol->lam_lb+ii, 0, lam_lb[ii]);
+ CVT_STRVEC2VEC(nb[ii], qp_sol->lam_ub+ii, 0, lam_ub[ii]);
+ CVT_STRVEC2VEC(ng[ii], qp_sol->lam_lg+ii, 0, lam_lg[ii]);
+ CVT_STRVEC2VEC(ng[ii], qp_sol->lam_ug+ii, 0, lam_ug[ii]);
+
+ return;
+
+ }
+
+
+
+void CVT_OCP_QP_SOL_TO_LIBSTR(struct OCP_QP *qp, struct OCP_QP_SOL *qp_sol, struct STRVEC *u, struct STRVEC *x, struct STRVEC *pi, struct STRVEC *lam_lb, struct STRVEC *lam_ub, struct STRVEC *lam_lg, struct STRVEC *lam_ug)
+ {
+
+ int N = qp->N;
+ int *nx = qp->nx;
+ int *nu = qp->nu;
+ int *nb = qp->nb;
+ int *ng = qp->ng;
+
+ int ii;
+
+ for(ii=0; ii<N; ii++)
+ {
+ VECCP_LIBSTR(nu[ii], qp_sol->ux+ii, 0, u+ii, 0);
+ VECCP_LIBSTR(nx[ii], qp_sol->ux+ii, nu[ii], x+ii, 0);
+ VECCP_LIBSTR(nx[ii+1], qp_sol->pi+ii, 0, pi+ii, 0);
+ VECCP_LIBSTR(nb[ii], qp_sol->lam_lb+ii, 0, lam_lb+ii, 0);
+ VECCP_LIBSTR(nb[ii], qp_sol->lam_ub+ii, 0, lam_ub+ii, 0);
+ VECCP_LIBSTR(ng[ii], qp_sol->lam_lg+ii, 0, lam_lg+ii, 0);
+ VECCP_LIBSTR(ng[ii], qp_sol->lam_ug+ii, 0, lam_ug+ii, 0);
+ }
+ VECCP_LIBSTR(nu[ii], qp_sol->ux+ii, 0, u+ii, 0);
+ VECCP_LIBSTR(nx[ii], qp_sol->ux+ii, nu[ii], x+ii, 0);
+ VECCP_LIBSTR(nb[ii], qp_sol->lam_lb+ii, 0, lam_lb+ii, 0);
+ VECCP_LIBSTR(nb[ii], qp_sol->lam_ub+ii, 0, lam_ub+ii, 0);
+ VECCP_LIBSTR(ng[ii], qp_sol->lam_lg+ii, 0, lam_lg+ii, 0);
+ VECCP_LIBSTR(ng[ii], qp_sol->lam_ug+ii, 0, lam_ug+ii, 0);
+
+ return;
+
+ }