Squashed 'third_party/hpipm/' content from commit c2c0261
Change-Id: I05e186a6e1e1f075aa629092e7ad86e6116ca711
git-subtree-dir: third_party/hpipm
git-subtree-split: c2c0261e8ded36f636d9c4390a2899bdc4c999cc
diff --git a/.gitignore b/.gitignore
new file mode 100644
index 0000000..0643666
--- /dev/null
+++ b/.gitignore
@@ -0,0 +1,9 @@
+*.o
+*.out
+*.s
+libhpipm.a
+libhpipm.so
+hpipm_target.h
+*.swp
+*.swo
+build/
diff --git a/CMakeLists.txt b/CMakeLists.txt
new file mode 100644
index 0000000..71c9df1
--- /dev/null
+++ b/CMakeLists.txt
@@ -0,0 +1,139 @@
+###################################################################################################
+# #
+# 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 #
+# #
+###################################################################################################
+
+cmake_minimum_required(VERSION 2.8.11)
+project(hpipm)
+
+# Target architecture
+set(TARGET GENERIC)
+
+# BLAS and LAPACK version (for LA=BLAS in BLASFEO)
+set(REF_BLAS 0)
+#set(REF_BLAS OPENBLAS)
+#set(REF_BLAS NETLIB)
+#set(REF_BLAS MKL)
+#set(REF_BLAS BLIS)
+#set(REF_BLAS ATLAS)
+
+# BLASFEO
+if(NOT TARGET blasfeo)
+ # BLASFEO installation directory
+ set(BLASFEO_PATH /opt/blasfeo)
+ add_library(blasfeo STATIC IMPORTED)
+ set_target_properties(blasfeo PROPERTIES IMPORTED_LOCATION ${BLASFEO_PATH}/lib/libblasfeo.a)
+ set(BLASFEO_INCLUDE_DIR ${BLASFEO_PATH}/include)
+else()
+ # TODO: horrible hardcode; needs to go
+ set(BLASFEO_INCLUDE_DIR ${CMAKE_CURRENT_SOURCE_DIR}/../blasfeo/include)
+endif()
+
+# enable runtine checks
+set(RUNTIME_CHECKS 0)
+#set(RUNTIME_CHECKS 0)
+
+# compiler flags
+set(CMAKE_C_FLAGS "")
+
+# optimization flags
+set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -O2 -fPIC")
+#
+if(${REF_BLAS} MATCHES 0)
+ set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ")
+endif(${REF_BLAS} MATCHES 0)
+if(${REF_BLAS} MATCHES OPENBLAS)
+ set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -DREF_BLAS_OPENBLAS -I/opt/openblas/include")
+endif(${REF_BLAS} MATCHES OPENBLAS)
+if(${REF_BLAS} MATCHES BLIS)
+ set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -DREF_BLAS_BLIS -std=c99")
+endif(${REF_BLAS} MATCHES BLIS)
+if(${REF_BLAS} MATCHES NETLIB)
+ set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -DREF_BLAS_NETLIB")
+endif(${REF_BLAS} MATCHES NETLIB)
+if(${REF_BLAS} MATCHES MKL)
+ set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -DREF_BLAS_MKL -m64 -I/opt/intel/mkl/include")
+endif(${REF_BLAS} MATCHES MKL)
+if(${REF_BLAS} MATCHES ATLAS)
+ set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -DREF_BLAS_ATLAS")
+endif(${REF_BLAS} MATCHES ATLAS)
+
+#
+if(${RUNTIME_CHECKS} MATCHES 1)
+ set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -DRUNTIME_CHECKS")
+endif(${RUNTIME_CHECKS} MATCHES 1)
+
+# architecture-specific flags
+if(${TARGET} MATCHES GENERIC)
+ set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -DTARGET_GENERIC")
+endif(${TARGET} MATCHES GENERIC)
+
+# source files
+# dense qp
+file(GLOB DENSE_QP_SRC
+ ${PROJECT_SOURCE_DIR}/dense_qp/d_dense_qp.c
+ ${PROJECT_SOURCE_DIR}/dense_qp/d_dense_qp_sol.c
+ ${PROJECT_SOURCE_DIR}/dense_qp/d_dense_qp_kkt.c
+ ${PROJECT_SOURCE_DIR}/dense_qp/d_dense_qp_ipm_hard.c
+ ${PROJECT_SOURCE_DIR}/dense_qp/s_dense_qp.c
+ ${PROJECT_SOURCE_DIR}/dense_qp/s_dense_qp_sol.c
+ ${PROJECT_SOURCE_DIR}/dense_qp/s_dense_qp_kkt.c
+ ${PROJECT_SOURCE_DIR}/dense_qp/s_dense_qp_ipm_hard.c
+ )
+# ocp qp
+file(GLOB OCP_QP_SRC
+ ${PROJECT_SOURCE_DIR}/ocp_qp/d_ocp_qp.c
+ ${PROJECT_SOURCE_DIR}/ocp_qp/d_ocp_qp_sol.c
+ ${PROJECT_SOURCE_DIR}/ocp_qp/d_ocp_qp_kkt.c
+ ${PROJECT_SOURCE_DIR}/ocp_qp/d_ocp_qp_ipm_hard.c
+ ${PROJECT_SOURCE_DIR}/ocp_qp/s_ocp_qp.c
+ ${PROJECT_SOURCE_DIR}/ocp_qp/s_ocp_qp_sol.c
+ ${PROJECT_SOURCE_DIR}/ocp_qp/s_ocp_qp_kkt.c
+ ${PROJECT_SOURCE_DIR}/ocp_qp/s_ocp_qp_ipm_hard.c
+ ${PROJECT_SOURCE_DIR}/ocp_qp/m_ocp_qp.c
+ ${PROJECT_SOURCE_DIR}/ocp_qp/m_ocp_qp_kkt.c
+ ${PROJECT_SOURCE_DIR}/ocp_qp/m_ocp_qp_ipm_hard.c
+ )
+# core qp
+file(GLOB CORE_QP_SRC
+ ${PROJECT_SOURCE_DIR}/core_qp/d_core_qp_ipm_hard.c
+ ${PROJECT_SOURCE_DIR}/core_qp/d_core_qp_ipm_hard_aux.c
+ ${PROJECT_SOURCE_DIR}/core_qp/s_core_qp_ipm_hard.c
+ ${PROJECT_SOURCE_DIR}/core_qp/s_core_qp_ipm_hard_aux.c
+ )
+# cond
+file(GLOB COND_SRC
+ ${PROJECT_SOURCE_DIR}/cond/d_cond.c
+ ${PROJECT_SOURCE_DIR}/cond/d_cond_aux.c
+ )
+
+set(HPIPM_SRC ${DENSE_QP_SRC} ${OCP_QP_SRC} ${CORE_QP_SRC} ${COND_SRC})
+
+add_library(hpipm ${HPIPM_SRC})
+target_include_directories(hpipm PUBLIC include ${BLASFEO_INCLUDE_DIR})
+target_link_libraries(hpipm blasfeo)
+
+# test problems
+add_subdirectory(test_problems)
diff --git a/LICENSE.txt b/LICENSE.txt
new file mode 100644
index 0000000..5ab7695
--- /dev/null
+++ b/LICENSE.txt
@@ -0,0 +1,504 @@
+ GNU LESSER GENERAL PUBLIC LICENSE
+ Version 2.1, February 1999
+
+ Copyright (C) 1991, 1999 Free Software Foundation, Inc.
+ 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
+ Everyone is permitted to copy and distribute verbatim copies
+ of this license document, but changing it is not allowed.
+
+[This is the first released version of the Lesser GPL. It also counts
+ as the successor of the GNU Library Public License, version 2, hence
+ the version number 2.1.]
+
+ Preamble
+
+ The licenses for most software are designed to take away your
+freedom to share and change it. By contrast, the GNU General Public
+Licenses are intended to guarantee your freedom to share and change
+free software--to make sure the software is free for all its users.
+
+ This license, the Lesser General Public License, applies to some
+specially designated software packages--typically libraries--of the
+Free Software Foundation and other authors who decide to use it. You
+can use it too, but we suggest you first think carefully about whether
+this license or the ordinary General Public License is the better
+strategy to use in any particular case, based on the explanations below.
+
+ When we speak of free software, we are referring to freedom of use,
+not price. Our General Public Licenses are designed to make sure that
+you have the freedom to distribute copies of free software (and charge
+for this service if you wish); that you receive source code or can get
+it if you want it; that you can change the software and use pieces of
+it in new free programs; and that you are informed that you can do
+these things.
+
+ To protect your rights, we need to make restrictions that forbid
+distributors to deny you these rights or to ask you to surrender these
+rights. These restrictions translate to certain responsibilities for
+you if you distribute copies of the library or if you modify it.
+
+ For example, if you distribute copies of the library, whether gratis
+or for a fee, you must give the recipients all the rights that we gave
+you. You must make sure that they, too, receive or can get the source
+code. If you link other code with the library, you must provide
+complete object files to the recipients, so that they can relink them
+with the library after making changes to the library and recompiling
+it. And you must show them these terms so they know their rights.
+
+ We protect your rights with a two-step method: (1) we copyright the
+library, and (2) we offer you this license, which gives you legal
+permission to copy, distribute and/or modify the library.
+
+ To protect each distributor, we want to make it very clear that
+there is no warranty for the free library. Also, if the library is
+modified by someone else and passed on, the recipients should know
+that what they have is not the original version, so that the original
+author's reputation will not be affected by problems that might be
+introduced by others.
+
+ Finally, software patents pose a constant threat to the existence of
+any free program. We wish to make sure that a company cannot
+effectively restrict the users of a free program by obtaining a
+restrictive license from a patent holder. Therefore, we insist that
+any patent license obtained for a version of the library must be
+consistent with the full freedom of use specified in this license.
+
+ Most GNU software, including some libraries, is covered by the
+ordinary GNU General Public License. This license, the GNU Lesser
+General Public License, applies to certain designated libraries, and
+is quite different from the ordinary General Public License. We use
+this license for certain libraries in order to permit linking those
+libraries into non-free programs.
+
+ When a program is linked with a library, whether statically or using
+a shared library, the combination of the two is legally speaking a
+combined work, a derivative of the original library. The ordinary
+General Public License therefore permits such linking only if the
+entire combination fits its criteria of freedom. The Lesser General
+Public License permits more lax criteria for linking other code with
+the library.
+
+ We call this license the "Lesser" General Public License because it
+does Less to protect the user's freedom than the ordinary General
+Public License. It also provides other free software developers Less
+of an advantage over competing non-free programs. These disadvantages
+are the reason we use the ordinary General Public License for many
+libraries. However, the Lesser license provides advantages in certain
+special circumstances.
+
+ For example, on rare occasions, there may be a special need to
+encourage the widest possible use of a certain library, so that it becomes
+a de-facto standard. To achieve this, non-free programs must be
+allowed to use the library. A more frequent case is that a free
+library does the same job as widely used non-free libraries. In this
+case, there is little to gain by limiting the free library to free
+software only, so we use the Lesser General Public License.
+
+ In other cases, permission to use a particular library in non-free
+programs enables a greater number of people to use a large body of
+free software. For example, permission to use the GNU C Library in
+non-free programs enables many more people to use the whole GNU
+operating system, as well as its variant, the GNU/Linux operating
+system.
+
+ Although the Lesser General Public License is Less protective of the
+users' freedom, it does ensure that the user of a program that is
+linked with the Library has the freedom and the wherewithal to run
+that program using a modified version of the Library.
+
+ The precise terms and conditions for copying, distribution and
+modification follow. Pay close attention to the difference between a
+"work based on the library" and a "work that uses the library". The
+former contains code derived from the library, whereas the latter must
+be combined with the library in order to run.
+
+ GNU LESSER GENERAL PUBLIC LICENSE
+ TERMS AND CONDITIONS FOR COPYING, DISTRIBUTION AND MODIFICATION
+
+ 0. This License Agreement applies to any software library or other
+program which contains a notice placed by the copyright holder or
+other authorized party saying it may be distributed under the terms of
+this Lesser General Public License (also called "this License").
+Each licensee is addressed as "you".
+
+ A "library" means a collection of software functions and/or data
+prepared so as to be conveniently linked with application programs
+(which use some of those functions and data) to form executables.
+
+ The "Library", below, refers to any such software library or work
+which has been distributed under these terms. A "work based on the
+Library" means either the Library or any derivative work under
+copyright law: that is to say, a work containing the Library or a
+portion of it, either verbatim or with modifications and/or translated
+straightforwardly into another language. (Hereinafter, translation is
+included without limitation in the term "modification".)
+
+ "Source code" for a work means the preferred form of the work for
+making modifications to it. For a library, complete source code means
+all the source code for all modules it contains, plus any associated
+interface definition files, plus the scripts used to control compilation
+and installation of the library.
+
+ Activities other than copying, distribution and modification are not
+covered by this License; they are outside its scope. The act of
+running a program using the Library is not restricted, and output from
+such a program is covered only if its contents constitute a work based
+on the Library (independent of the use of the Library in a tool for
+writing it). Whether that is true depends on what the Library does
+and what the program that uses the Library does.
+
+ 1. You may copy and distribute verbatim copies of the Library's
+complete source code as you receive it, in any medium, provided that
+you conspicuously and appropriately publish on each copy an
+appropriate copyright notice and disclaimer of warranty; keep intact
+all the notices that refer to this License and to the absence of any
+warranty; and distribute a copy of this License along with the
+Library.
+
+ You may charge a fee for the physical act of transferring a copy,
+and you may at your option offer warranty protection in exchange for a
+fee.
+
+ 2. You may modify your copy or copies of the Library or any portion
+of it, thus forming a work based on the Library, and copy and
+distribute such modifications or work under the terms of Section 1
+above, provided that you also meet all of these conditions:
+
+ a) The modified work must itself be a software library.
+
+ b) You must cause the files modified to carry prominent notices
+ stating that you changed the files and the date of any change.
+
+ c) You must cause the whole of the work to be licensed at no
+ charge to all third parties under the terms of this License.
+
+ d) If a facility in the modified Library refers to a function or a
+ table of data to be supplied by an application program that uses
+ the facility, other than as an argument passed when the facility
+ is invoked, then you must make a good faith effort to ensure that,
+ in the event an application does not supply such function or
+ table, the facility still operates, and performs whatever part of
+ its purpose remains meaningful.
+
+ (For example, a function in a library to compute square roots has
+ a purpose that is entirely well-defined independent of the
+ application. Therefore, Subsection 2d requires that any
+ application-supplied function or table used by this function must
+ be optional: if the application does not supply it, the square
+ root function must still compute square roots.)
+
+These requirements apply to the modified work as a whole. If
+identifiable sections of that work are not derived from the Library,
+and can be reasonably considered independent and separate works in
+themselves, then this License, and its terms, do not apply to those
+sections when you distribute them as separate works. But when you
+distribute the same sections as part of a whole which is a work based
+on the Library, the distribution of the whole must be on the terms of
+this License, whose permissions for other licensees extend to the
+entire whole, and thus to each and every part regardless of who wrote
+it.
+
+Thus, it is not the intent of this section to claim rights or contest
+your rights to work written entirely by you; rather, the intent is to
+exercise the right to control the distribution of derivative or
+collective works based on the Library.
+
+In addition, mere aggregation of another work not based on the Library
+with the Library (or with a work based on the Library) on a volume of
+a storage or distribution medium does not bring the other work under
+the scope of this License.
+
+ 3. You may opt to apply the terms of the ordinary GNU General Public
+License instead of this License to a given copy of the Library. To do
+this, you must alter all the notices that refer to this License, so
+that they refer to the ordinary GNU General Public License, version 2,
+instead of to this License. (If a newer version than version 2 of the
+ordinary GNU General Public License has appeared, then you can specify
+that version instead if you wish.) Do not make any other change in
+these notices.
+
+ Once this change is made in a given copy, it is irreversible for
+that copy, so the ordinary GNU General Public License applies to all
+subsequent copies and derivative works made from that copy.
+
+ This option is useful when you wish to copy part of the code of
+the Library into a program that is not a library.
+
+ 4. You may copy and distribute the Library (or a portion or
+derivative of it, under Section 2) in object code or executable form
+under the terms of Sections 1 and 2 above provided that you accompany
+it with the complete corresponding machine-readable source code, which
+must be distributed under the terms of Sections 1 and 2 above on a
+medium customarily used for software interchange.
+
+ If distribution of object code is made by offering access to copy
+from a designated place, then offering equivalent access to copy the
+source code from the same place satisfies the requirement to
+distribute the source code, even though third parties are not
+compelled to copy the source along with the object code.
+
+ 5. A program that contains no derivative of any portion of the
+Library, but is designed to work with the Library by being compiled or
+linked with it, is called a "work that uses the Library". Such a
+work, in isolation, is not a derivative work of the Library, and
+therefore falls outside the scope of this License.
+
+ However, linking a "work that uses the Library" with the Library
+creates an executable that is a derivative of the Library (because it
+contains portions of the Library), rather than a "work that uses the
+library". The executable is therefore covered by this License.
+Section 6 states terms for distribution of such executables.
+
+ When a "work that uses the Library" uses material from a header file
+that is part of the Library, the object code for the work may be a
+derivative work of the Library even though the source code is not.
+Whether this is true is especially significant if the work can be
+linked without the Library, or if the work is itself a library. The
+threshold for this to be true is not precisely defined by law.
+
+ If such an object file uses only numerical parameters, data
+structure layouts and accessors, and small macros and small inline
+functions (ten lines or less in length), then the use of the object
+file is unrestricted, regardless of whether it is legally a derivative
+work. (Executables containing this object code plus portions of the
+Library will still fall under Section 6.)
+
+ Otherwise, if the work is a derivative of the Library, you may
+distribute the object code for the work under the terms of Section 6.
+Any executables containing that work also fall under Section 6,
+whether or not they are linked directly with the Library itself.
+
+ 6. As an exception to the Sections above, you may also combine or
+link a "work that uses the Library" with the Library to produce a
+work containing portions of the Library, and distribute that work
+under terms of your choice, provided that the terms permit
+modification of the work for the customer's own use and reverse
+engineering for debugging such modifications.
+
+ You must give prominent notice with each copy of the work that the
+Library is used in it and that the Library and its use are covered by
+this License. You must supply a copy of this License. If the work
+during execution displays copyright notices, you must include the
+copyright notice for the Library among them, as well as a reference
+directing the user to the copy of this License. Also, you must do one
+of these things:
+
+ a) Accompany the work with the complete corresponding
+ machine-readable source code for the Library including whatever
+ changes were used in the work (which must be distributed under
+ Sections 1 and 2 above); and, if the work is an executable linked
+ with the Library, with the complete machine-readable "work that
+ uses the Library", as object code and/or source code, so that the
+ user can modify the Library and then relink to produce a modified
+ executable containing the modified Library. (It is understood
+ that the user who changes the contents of definitions files in the
+ Library will not necessarily be able to recompile the application
+ to use the modified definitions.)
+
+ b) Use a suitable shared library mechanism for linking with the
+ Library. A suitable mechanism is one that (1) uses at run time a
+ copy of the library already present on the user's computer system,
+ rather than copying library functions into the executable, and (2)
+ will operate properly with a modified version of the library, if
+ the user installs one, as long as the modified version is
+ interface-compatible with the version that the work was made with.
+
+ c) Accompany the work with a written offer, valid for at
+ least three years, to give the same user the materials
+ specified in Subsection 6a, above, for a charge no more
+ than the cost of performing this distribution.
+
+ d) If distribution of the work is made by offering access to copy
+ from a designated place, offer equivalent access to copy the above
+ specified materials from the same place.
+
+ e) Verify that the user has already received a copy of these
+ materials or that you have already sent this user a copy.
+
+ For an executable, the required form of the "work that uses the
+Library" must include any data and utility programs needed for
+reproducing the executable from it. However, as a special exception,
+the materials to be distributed need not include anything that is
+normally distributed (in either source or binary form) with the major
+components (compiler, kernel, and so on) of the operating system on
+which the executable runs, unless that component itself accompanies
+the executable.
+
+ It may happen that this requirement contradicts the license
+restrictions of other proprietary libraries that do not normally
+accompany the operating system. Such a contradiction means you cannot
+use both them and the Library together in an executable that you
+distribute.
+
+ 7. You may place library facilities that are a work based on the
+Library side-by-side in a single library together with other library
+facilities not covered by this License, and distribute such a combined
+library, provided that the separate distribution of the work based on
+the Library and of the other library facilities is otherwise
+permitted, and provided that you do these two things:
+
+ a) Accompany the combined library with a copy of the same work
+ based on the Library, uncombined with any other library
+ facilities. This must be distributed under the terms of the
+ Sections above.
+
+ b) Give prominent notice with the combined library of the fact
+ that part of it is a work based on the Library, and explaining
+ where to find the accompanying uncombined form of the same work.
+
+ 8. You may not copy, modify, sublicense, link with, or distribute
+the Library except as expressly provided under this License. Any
+attempt otherwise to copy, modify, sublicense, link with, or
+distribute the Library is void, and will automatically terminate your
+rights under this License. However, parties who have received copies,
+or rights, from you under this License will not have their licenses
+terminated so long as such parties remain in full compliance.
+
+ 9. You are not required to accept this License, since you have not
+signed it. However, nothing else grants you permission to modify or
+distribute the Library or its derivative works. These actions are
+prohibited by law if you do not accept this License. Therefore, by
+modifying or distributing the Library (or any work based on the
+Library), you indicate your acceptance of this License to do so, and
+all its terms and conditions for copying, distributing or modifying
+the Library or works based on it.
+
+ 10. Each time you redistribute the Library (or any work based on the
+Library), the recipient automatically receives a license from the
+original licensor to copy, distribute, link with or modify the Library
+subject to these terms and conditions. You may not impose any further
+restrictions on the recipients' exercise of the rights granted herein.
+You are not responsible for enforcing compliance by third parties with
+this License.
+
+ 11. If, as a consequence of a court judgment or allegation of patent
+infringement or for any other reason (not limited to patent issues),
+conditions are imposed on you (whether by court order, agreement or
+otherwise) that contradict the conditions of this License, they do not
+excuse you from the conditions of this License. If you cannot
+distribute so as to satisfy simultaneously your obligations under this
+License and any other pertinent obligations, then as a consequence you
+may not distribute the Library at all. For example, if a patent
+license would not permit royalty-free redistribution of the Library by
+all those who receive copies directly or indirectly through you, then
+the only way you could satisfy both it and this License would be to
+refrain entirely from distribution of the Library.
+
+If any portion of this section is held invalid or unenforceable under any
+particular circumstance, the balance of the section is intended to apply,
+and the section as a whole is intended to apply in other circumstances.
+
+It is not the purpose of this section to induce you to infringe any
+patents or other property right claims or to contest validity of any
+such claims; this section has the sole purpose of protecting the
+integrity of the free software distribution system which is
+implemented by public license practices. Many people have made
+generous contributions to the wide range of software distributed
+through that system in reliance on consistent application of that
+system; it is up to the author/donor to decide if he or she is willing
+to distribute software through any other system and a licensee cannot
+impose that choice.
+
+This section is intended to make thoroughly clear what is believed to
+be a consequence of the rest of this License.
+
+ 12. If the distribution and/or use of the Library is restricted in
+certain countries either by patents or by copyrighted interfaces, the
+original copyright holder who places the Library under this License may add
+an explicit geographical distribution limitation excluding those countries,
+so that distribution is permitted only in or among countries not thus
+excluded. In such case, this License incorporates the limitation as if
+written in the body of this License.
+
+ 13. The Free Software Foundation may publish revised and/or new
+versions of the Lesser General Public License from time to time.
+Such new versions will be similar in spirit to the present version,
+but may differ in detail to address new problems or concerns.
+
+Each version is given a distinguishing version number. If the Library
+specifies a version number of this License which applies to it and
+"any later version", you have the option of following the terms and
+conditions either of that version or of any later version published by
+the Free Software Foundation. If the Library does not specify a
+license version number, you may choose any version ever published by
+the Free Software Foundation.
+
+ 14. If you wish to incorporate parts of the Library into other free
+programs whose distribution conditions are incompatible with these,
+write to the author to ask for permission. For software which is
+copyrighted by the Free Software Foundation, write to the Free
+Software Foundation; we sometimes make exceptions for this. Our
+decision will be guided by the two goals of preserving the free status
+of all derivatives of our free software and of promoting the sharing
+and reuse of software generally.
+
+ NO WARRANTY
+
+ 15. BECAUSE THE LIBRARY IS LICENSED FREE OF CHARGE, THERE IS NO
+WARRANTY FOR THE LIBRARY, TO THE EXTENT PERMITTED BY APPLICABLE LAW.
+EXCEPT WHEN OTHERWISE STATED IN WRITING THE COPYRIGHT HOLDERS AND/OR
+OTHER PARTIES PROVIDE THE LIBRARY "AS IS" WITHOUT WARRANTY OF ANY
+KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING, BUT NOT LIMITED TO, THE
+IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR
+PURPOSE. THE ENTIRE RISK AS TO THE QUALITY AND PERFORMANCE OF THE
+LIBRARY IS WITH YOU. SHOULD THE LIBRARY PROVE DEFECTIVE, YOU ASSUME
+THE COST OF ALL NECESSARY SERVICING, REPAIR OR CORRECTION.
+
+ 16. IN NO EVENT UNLESS REQUIRED BY APPLICABLE LAW OR AGREED TO IN
+WRITING WILL ANY COPYRIGHT HOLDER, OR ANY OTHER PARTY WHO MAY MODIFY
+AND/OR REDISTRIBUTE THE LIBRARY AS PERMITTED ABOVE, BE LIABLE TO YOU
+FOR DAMAGES, INCLUDING ANY GENERAL, SPECIAL, INCIDENTAL OR
+CONSEQUENTIAL DAMAGES ARISING OUT OF THE USE OR INABILITY TO USE THE
+LIBRARY (INCLUDING BUT NOT LIMITED TO LOSS OF DATA OR DATA BEING
+RENDERED INACCURATE OR LOSSES SUSTAINED BY YOU OR THIRD PARTIES OR A
+FAILURE OF THE LIBRARY TO OPERATE WITH ANY OTHER SOFTWARE), EVEN IF
+SUCH HOLDER OR OTHER PARTY HAS BEEN ADVISED OF THE POSSIBILITY OF SUCH
+DAMAGES.
+
+ END OF TERMS AND CONDITIONS
+
+ How to Apply These Terms to Your New Libraries
+
+ If you develop a new library, and you want it to be of the greatest
+possible use to the public, we recommend making it free software that
+everyone can redistribute and change. You can do so by permitting
+redistribution under these terms (or, alternatively, under the terms of the
+ordinary General Public License).
+
+ To apply these terms, attach the following notices to the library. It is
+safest to attach them to the start of each source file to most effectively
+convey the exclusion of warranty; and each file should have at least the
+"copyright" line and a pointer to where the full notice is found.
+
+ <one line to give the library's name and a brief idea of what it does.>
+ Copyright (C) <year> <name of author>
+
+ This library 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.
+
+ This library 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 this library; if not, write to the Free Software
+ Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
+
+Also add information on how to contact you by electronic and paper mail.
+
+You should also get your employer (if you work as a programmer) or your
+school, if any, to sign a "copyright disclaimer" for the library, if
+necessary. Here is a sample; alter the names:
+
+ Yoyodyne, Inc., hereby disclaims all copyright interest in the
+ library `Frob' (a library for tweaking knobs) written by James Random Hacker.
+
+ <signature of Ty Coon>, 1 April 1990
+ Ty Coon, President of Vice
+
+That's all there is to it!
+
+
diff --git a/Makefile b/Makefile
new file mode 100644
index 0000000..5ed32db
--- /dev/null
+++ b/Makefile
@@ -0,0 +1,115 @@
+###################################################################################################
+# #
+# 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
+
+# dense qp
+OBJS += dense_qp/d_dense_qp.o dense_qp/d_dense_qp_sol.o dense_qp/d_dense_qp_kkt.o dense_qp/d_dense_qp_ipm_hard.o
+OBJS += dense_qp/s_dense_qp.o dense_qp/s_dense_qp_sol.o dense_qp/s_dense_qp_kkt.o dense_qp/s_dense_qp_ipm_hard.o
+# ocp qp
+OBJS += ocp_qp/d_ocp_qp.o ocp_qp/d_ocp_qp_sol.o ocp_qp/d_ocp_qp_kkt.o ocp_qp/d_ocp_qp_ipm_hard.o
+OBJS += ocp_qp/s_ocp_qp.o ocp_qp/s_ocp_qp_sol.o ocp_qp/s_ocp_qp_kkt.o ocp_qp/s_ocp_qp_ipm_hard.o
+OBJS += ocp_qp/m_ocp_qp.o ocp_qp/m_ocp_qp_kkt.o ocp_qp/m_ocp_qp_ipm_hard.o
+# core qp
+OBJS += core_qp/d_core_qp_ipm_hard_aux.o core_qp/d_core_qp_ipm_hard.o
+OBJS += core_qp/s_core_qp_ipm_hard_aux.o core_qp/s_core_qp_ipm_hard.o
+# cond
+OBJS += cond/d_cond_aux.o cond/d_cond.o cond/d_part_cond.o
+
+all: clean static_library
+
+static_library: target
+ ( cd cond; $(MAKE) obj)
+ ( cd core_qp; $(MAKE) obj)
+ ( cd dense_qp; $(MAKE) obj)
+ ( cd ocp_qp; $(MAKE) obj)
+ ar rcs libhpipm.a $(OBJS)
+ cp libhpipm.a ./lib/
+ @echo
+ @echo " libhpipm.a static library build complete."
+ @echo
+
+shared_library: target
+ ( cd cond; $(MAKE) obj)
+ ( cd core_qp; $(MAKE) obj)
+ ( cd dense_qp; $(MAKE) obj)
+ ( cd ocp_qp; $(MAKE) obj)
+ gcc -shared -o libhpipm.so $(OBJS)
+ cp libhpipm.so ./lib/
+ @echo
+ @echo " libhpipm.so shared library build complete."
+ @echo
+
+target:
+ touch ./include/hpipm_target.h
+ifeq ($(TARGET), GENERIC)
+ echo "#ifndef TARGET_GENERIC" > ./include/hpipm_target.h
+ echo "#define TARGET_GENERIC" >> ./include/hpipm_target.h
+ echo "#endif" >> ./include/hpipm_target.h
+endif
+
+install_static:
+ mkdir -p $(PREFIX)/hpipm
+ mkdir -p $(PREFIX)/hpipm/lib
+ cp -f libhpipm.a $(PREFIX)/hpipm/lib/
+ mkdir -p $(PREFIX)/hpipm/include
+ cp -f ./include/*.h $(PREFIX)/hpipm/include/
+
+install_shared:
+ mkdir -p $(PREFIX)/hpipm
+ mkdir -p $(PREFIX)/hpipm/lib
+ cp -f libhpipm.so $(PREFIX)/hpipm/lib/
+ mkdir -p $(PREFIX)/hpipm/include
+ cp -f ./include/*.h $(PREFIX)/hpipm/include/
+
+test_problem:
+ cp libhpipm.a ./test_problems/libhpipm.a
+ make -C test_problems obj
+ @echo
+ @echo " Test problem build complete."
+ @echo
+
+run:
+ ./test_problems/test.out
+
+clean:
+ rm -f libhpipm.a
+ rm -f libhpipm.so
+ rm -f ./lib/libhpipm.a
+ rm -f ./lib/libhpipm.so
+ make -C cond clean
+ make -C core_qp clean
+ make -C dense_qp clean
+ make -C ocp_qp clean
+ make -C test_problems clean
+
diff --git a/Makefile.rule b/Makefile.rule
new file mode 100644
index 0000000..681a31d
--- /dev/null
+++ b/Makefile.rule
@@ -0,0 +1,116 @@
+###################################################################################################
+# #
+# 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 #
+# #
+###################################################################################################
+
+# Target architecture
+TARGET = GENERIC
+
+# BLAS and LAPACK version (for LA=BLAS in BLASFEO)
+REF_BLAS = 0
+#REF_BLAS = OPENBLAS
+#REF_BLAS = NETLIB
+#REF_BLAS = MKL
+#REF_BLAS = BLIS
+#REF_BLAS = ATLAS
+
+# operating system
+#UNAME_S := $(shell uname -s)
+#ifeq ($(UNAME_S),Linux)
+# OS = LINUX
+#endif
+#ifeq ($(UNAME_S),Darwin)
+# OS = MAC
+#endif
+#OS = LINUX
+#OS = MAC
+#OS = WINDOWS
+
+# C Compiler
+CC = gcc
+#CC = clang
+#CC = x86_64-w64-mingw32-gcc
+
+# Installation directory
+PREFIX = /opt
+
+# BLASFEO installation directory
+BLASFEO_PATH = /opt/blasfeo
+
+# enable runtine checks
+RUNTIME_CHECKS = 0
+#RUNTIME_CHECKS = 1
+
+# compiler flags
+CFLAGS =
+
+# optimization flags
+CFLAGS += -O2 -fPIC
+
+# debugging flags
+CFLAGS += #-g #-Wall -pedantic -Wfloat-equal #-pg
+
+# search directories
+CFLAGS += -I$(BLASFEO_PATH)/include
+
+# definirions
+#ifeq ($(OS), LINUX)
+#CFLAGS += -DOS_LINUX
+#endif
+#ifeq ($(OS), MAC)
+#CFLAGS += -DOS_MAC
+#endif
+#ifeq ($(OS), WINDOWS)
+#CFLAGS += -DOS_WINDOWS
+#endif
+
+ifeq ($(REF_BLAS), 0)
+CFLAGS +=
+endif
+ifeq ($(REF_BLAS), OPENBLAS)
+CFLAGS += -DREF_BLAS_OPENBLAS -I/opt/openblas/include
+endif
+ifeq ($(REF_BLAS), BLIS)
+CFLAGS += -DREF_BLAS_BLIS -std=c99
+endif
+ifeq ($(REF_BLAS), NETLIB)
+CFLAGS += -DREF_BLAS_NETLIB
+endif
+ifeq ($(REF_BLAS), MKL)
+CFLAGS += -DREF_BLAS_MKL -m64 -I/opt/intel/mkl/include
+endif
+ifeq ($(REF_BLAS), ATLAS)
+CFLAGS += -DREF_BLAS_ATLAS
+endif
+
+ifeq ($(RUNTIME_CHECKS), 1)
+CFLAGS += -DRUNTIME_CHECKS
+endif
+
+# architecture-specific flags
+ifeq ($(TARGET), GENERIC)
+CFLAGS += -DTARGET_GENERIC
+endif
+
diff --git a/README.md b/README.md
new file mode 100644
index 0000000..52c9a2b
--- /dev/null
+++ b/README.md
@@ -0,0 +1 @@
+# HPIPM
diff --git a/cond/Makefile b/cond/Makefile
new file mode 100644
index 0000000..07067ba
--- /dev/null
+++ b/cond/Makefile
@@ -0,0 +1,44 @@
+###################################################################################################
+# #
+# 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_cond_aux.o d_cond.o d_part_cond.o
+OBJS +=
+
+obj: $(OBJS)
+
+clean:
+ rm -f *.o
+ rm -f *.s
+
diff --git a/cond/d_cond.c b/cond/d_cond.c
new file mode 100644
index 0000000..299efa5
--- /dev/null
+++ b/cond/d_cond.c
@@ -0,0 +1,287 @@
+/**************************************************************************************************
+* *
+* 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 <stdlib.h>
+#include <stdio.h>
+
+#include <blasfeo_target.h>
+#include <blasfeo_common.h>
+#include <blasfeo_d_blas.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_dense_qp.h"
+#include "../include/hpipm_d_dense_qp_sol.h"
+#include "../include/hpipm_d_cond.h"
+#include "../include/hpipm_d_cond_aux.h"
+
+
+
+void d_compute_qp_size_ocp2dense(int N, int *nx, int *nu, int *nb, int **idxb, int *ng, int *nvc, int *nec, int *nbc, int *ngc)
+ {
+
+ int ii, jj;
+
+ nvc[0] = 0;
+ nec[0] = 0;
+ nbc[0] = 0;
+ ngc[0] = 0;
+
+ // first stage
+ nvc[0] += nx[0]+nu[0];
+ nbc[0] += nb[0];
+ ngc[0] += ng[0];
+ // remaining stages
+ for(ii=1; ii<=N; ii++)
+ {
+ nvc[0] += nu[ii];
+ for(jj=0; jj<nb[ii]; jj++)
+ {
+ if(idxb[ii][jj]<nu[ii]) // input constraint
+ {
+ nbc[0]++;
+ }
+ else // state constraint
+ {
+ ngc[0]++;
+ }
+ }
+ ngc[0] += ng[ii];
+ }
+
+ return;
+
+ }
+
+
+
+int d_memsize_cond_qp_ocp2dense(struct d_ocp_qp *ocp_qp, struct d_dense_qp *dense_qp) // XXX + args for algorithm type ???
+ {
+
+ int ii;
+
+ int N = ocp_qp->N;
+ int *nx = ocp_qp->nx;
+ int *nu = ocp_qp->nu;
+ int *nb = ocp_qp->nb;
+ int *ng = ocp_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 += (2+2*(N+1))*sizeof(struct d_strmat); // Gamma L Lx AL
+ size += (2+1*(N+1))*sizeof(struct d_strvec); // Gammab tmp_ngM tmp_nuxM
+
+ int nu_tmp = 0;
+ for(ii=0; ii<N; ii++)
+ {
+ nu_tmp += nu[ii];
+ size += d_size_strmat(nu_tmp+nx[0]+1, nx[ii+1]); // Gamma
+ }
+ for(ii=0; ii<=N; ii++)
+ size += d_size_strmat(nu[ii]+nx[ii]+1, nu[ii]+nx[ii]); // L
+ size += d_size_strmat(nxM+1, nxM); // Lx
+ size += d_size_strmat(nuM+nxM+1, nxM); // AL
+ for(ii=0; ii<N; ii++)
+ size += 1*d_size_strvec(nx[ii+1]); // Gammab
+ size += d_size_strvec(ngM); // tmp_ngM
+ size += 1*d_size_strvec(nuM+nxM); // tmp_nuxM
+ size += 1*d_size_strvec(ngM); // tmp_ngM
+
+ 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 d_create_cond_qp_ocp2dense(struct d_ocp_qp *ocp_qp, struct d_dense_qp *dense_qp, struct d_cond_qp_ocp2dense_workspace *cond_ws, void *mem)
+ {
+
+ int ii;
+
+ int N = ocp_qp->N;
+ int *nx = ocp_qp->nx;
+ int *nu = ocp_qp->nu;
+ int *nb = ocp_qp->nb;
+ int *ng = ocp_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;
+
+
+ // matrix struct
+ struct d_strmat *sm_ptr = (struct d_strmat *) mem;
+
+ cond_ws->Gamma = sm_ptr;
+ sm_ptr += N+1;
+ cond_ws->L = sm_ptr;
+ sm_ptr += N+1;
+ cond_ws->Lx = sm_ptr;
+ sm_ptr += 1;
+ cond_ws->AL = sm_ptr;
+ sm_ptr += 1;
+
+
+ // vector struct
+ struct d_strvec *sv_ptr = (struct d_strvec *) sm_ptr;
+
+ cond_ws->Gammab = sv_ptr;
+ sv_ptr += N+1;
+ cond_ws->tmp_ngM = sv_ptr;
+ sv_ptr += 1;
+ cond_ws->tmp_nuxM = sv_ptr;
+ sv_ptr += 1;
+
+
+ // align to typicl cache line size
+ size_t s_ptr = (size_t) sv_ptr;
+ s_ptr = (s_ptr+63)/64*64;
+
+
+ // void stuf
+ char *c_ptr = (char *) s_ptr;
+ char *c_tmp;
+
+ int nu_tmp = 0;
+ for(ii=0; ii<N; ii++)
+ {
+ nu_tmp += nu[ii];
+ d_create_strmat(nu_tmp+nx[0]+1, nx[ii+1], cond_ws->Gamma+ii, c_ptr);
+ c_ptr += (cond_ws->Gamma+ii)->memory_size;
+ }
+ for(ii=0; ii<=N; ii++)
+ {
+ d_create_strmat(nu[ii]+nx[ii]+1, nu[ii]+nx[ii], cond_ws->L+ii, c_ptr);
+ c_ptr += (cond_ws->L+ii)->memory_size;
+ }
+ d_create_strmat(nxM+1, nxM, cond_ws->Lx, c_ptr);
+ c_ptr += cond_ws->Lx->memory_size;
+ d_create_strmat(nuM+nxM+1, nxM, cond_ws->AL, c_ptr);
+ c_ptr += cond_ws->AL->memory_size;
+ for(ii=0; ii<N; ii++)
+ {
+ d_create_strvec(nx[ii+1], cond_ws->Gammab+ii, c_ptr);
+ c_ptr += (cond_ws->Gammab+ii)->memory_size;
+ }
+ d_create_strvec(ngM, cond_ws->tmp_ngM, c_ptr);
+ c_ptr += cond_ws->tmp_ngM->memory_size;
+ c_tmp = c_ptr;
+ d_create_strvec(nuM+nxM, cond_ws->tmp_nuxM, c_ptr);
+ c_ptr += cond_ws->tmp_nuxM->memory_size;
+
+ cond_ws->memsize = d_memsize_cond_qp_ocp2dense(ocp_qp, dense_qp);
+
+ return;
+
+ }
+
+
+
+void d_cond_qp_ocp2dense(struct d_ocp_qp *ocp_qp, struct d_dense_qp *dense_qp, struct d_cond_qp_ocp2dense_workspace *cond_ws)
+ {
+
+ d_compute_Gamma(ocp_qp, cond_ws);
+
+ d_cond_RSQrq_N2nx3(ocp_qp, dense_qp->Hg, dense_qp->g, cond_ws);
+
+ d_cond_DCtd(ocp_qp, dense_qp->idxb, dense_qp->d_lb, dense_qp->d_ub, dense_qp->Ct, dense_qp->d_lg, dense_qp->d_ug, cond_ws);
+
+ return;
+
+ }
+
+
+
+void d_expand_sol_dense2ocp(struct d_ocp_qp *ocp_qp, struct d_dense_qp_sol *dense_qp_sol, struct d_ocp_qp_sol *ocp_qp_sol, struct d_cond_qp_ocp2dense_workspace *cond_ws)
+ {
+
+ d_expand_sol(ocp_qp, dense_qp_sol, ocp_qp_sol->ux, ocp_qp_sol->pi, ocp_qp_sol->lam_lb, ocp_qp_sol->lam_ub, ocp_qp_sol->lam_lg, ocp_qp_sol->lam_ug, ocp_qp_sol->t_lb, ocp_qp_sol->t_ub, ocp_qp_sol->t_lg, ocp_qp_sol->t_ug, cond_ws->tmp_nuxM, cond_ws->tmp_ngM);
+
+ return;
+
+ }
diff --git a/cond/d_cond_aux.c b/cond/d_cond_aux.c
new file mode 100644
index 0000000..8838609
--- /dev/null
+++ b/cond/d_cond_aux.c
@@ -0,0 +1,663 @@
+/**************************************************************************************************
+* *
+* 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 <stdlib.h>
+#include <stdio.h>
+
+#include <blasfeo_target.h>
+#include <blasfeo_common.h>
+#include <blasfeo_d_blas.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_dense_qp.h"
+#include "../include/hpipm_d_dense_qp_sol.h"
+#include "../include/hpipm_d_cond.h"
+
+
+
+
+void d_compute_Gamma(struct d_ocp_qp *ocp_qp, struct d_cond_qp_ocp2dense_workspace *cond_ws)
+ {
+
+ int N = ocp_qp->N;
+
+ // early return
+ if(N<0)
+ return;
+
+ // extract input members
+ int *nx = ocp_qp->nx;
+ int *nu = ocp_qp->nu;
+ struct d_strmat *BAbt = ocp_qp->BAbt;
+
+ // extract memory members
+ struct d_strmat *Gamma = cond_ws->Gamma;
+ struct d_strvec *Gammab = cond_ws->Gammab;
+
+ int ii, jj;
+
+ int nu_tmp;
+
+ nu_tmp = 0;
+ ii = 0;
+ // B & A & b
+ dgecp_libstr(nu[0]+nx[0]+1, nx[1], &BAbt[0], 0, 0, &Gamma[0], 0, 0);
+ // b
+ drowex_libstr(nx[1], 1.0, &Gamma[0], nu[0]+nx[0], 0, &Gammab[0], 0);
+
+
+ nu_tmp += nu[0];
+ ii++;
+
+ for(ii=1; ii<N; ii++)
+ {
+ // TODO check for equal pointers and avoid copy
+
+ // Gamma * A^T
+ dgemm_nn_libstr(nu_tmp+nx[0]+1, nx[ii+1], nx[ii], 1.0, &Gamma[ii-1], 0, 0, &BAbt[ii], nu[ii], 0, 0.0, &Gamma[ii], nu[ii], 0, &Gamma[ii], nu[ii], 0); // Gamma * A^T
+
+ dgecp_libstr(nu[ii], nx[ii+1], &BAbt[ii], 0, 0, &Gamma[ii], 0, 0);
+
+ nu_tmp += nu[ii];
+
+ dgead_libstr(1, nx[ii+1], 1.0, &BAbt[ii], nu[ii]+nx[ii], 0, &Gamma[ii], nu_tmp+nx[0], 0);
+
+ drowex_libstr(nx[ii+1], 1.0, &Gamma[ii], nu_tmp+nx[0], 0, &Gammab[ii], 0);
+ }
+
+ return;
+
+ }
+
+
+
+#if 0
+void d_cond_BAbt(int N, struct d_ocp_qp *ocp_qp, int idx_in, struct d_strmat *BAbt2, struct d_strvec *b2, struct d_cond_qp_ocp2dense_workspace *cond_ws)
+ {
+
+ // early return
+ if(N<0)
+ return;
+
+ // extract input members
+ int *nx = ocp_qp->nx + idx_in;
+ int *nu = ocp_qp->nu + idx_in;
+ struct d_strmat *BAbt = ocp_qp->BAbt + idx_in;
+
+ // extract memory members
+ struct d_strmat *Gamma = cond_ws->Gamma;
+
+ int ii, jj;
+
+ int nu_tmp;
+
+ nu_tmp = 0;
+ ii = 0;
+ // B & A & b
+ dgecp_libstr(nu[0]+nx[0]+1, nx[1], &BAbt[0], 0, 0, &Gamma[0], 0, 0);
+ //
+ nu_tmp += nu[0];
+ ii++;
+
+ for(ii=1; ii<N; ii++)
+ {
+ // TODO check for equal pointers and avoid copy
+
+ // Gamma * A^T
+ dgemm_nn_libstr(nu_tmp+nx[0]+1, nx[ii+1], nx[ii], 1.0, &Gamma[ii-1], 0, 0, &BAbt[ii], nu[ii], 0, 0.0, &Gamma[ii], nu[ii], 0, &Gamma[ii], nu[ii], 0); // Gamma * A^T
+
+ dgecp_libstr(nu[ii], nx[ii+1], &BAbt[ii], 0, 0, &Gamma[ii], 0, 0);
+
+ nu_tmp += nu[ii];
+
+ dgead_libstr(1, nx[ii+1], 1.0, &BAbt[ii], nu[ii]+nx[ii], 0, &Gamma[ii], nu_tmp+nx[0], 0);
+ }
+
+ // B & A & b
+ dgecp_libstr(nu_tmp+nx[0]+1, nx[N], &Gamma[N-1], 0, 0, &BAbt2[0], 0, 0);
+ // b
+ drowex_libstr(nx[N], 1.0, &BAbt2[0], 0, 0, &b2[0], 0);
+
+ return;
+
+ }
+#endif
+
+
+
+void d_cond_RSQrq_N2nx3(struct d_ocp_qp *ocp_qp, struct d_strmat *RSQrq2, struct d_strvec *rq2, struct d_cond_qp_ocp2dense_workspace *cond_ws)
+ {
+
+ int N = ocp_qp->N;
+
+ // early return
+ if(N<0)
+ return;
+
+ // extract input members
+ int *nx = ocp_qp->nx;
+ int *nu = ocp_qp->nu;
+
+ struct d_strmat *BAbt = ocp_qp->BAbt;
+ struct d_strmat *RSQrq = ocp_qp->RSQrq;
+
+ // extract memory members
+ struct d_strmat *Gamma = cond_ws->Gamma;
+ struct d_strmat *L = cond_ws->L;
+ struct d_strmat *Lx = cond_ws->Lx;
+ struct d_strmat *AL = cond_ws->AL;
+
+ // declare workspace matrices XXX use cast on cond_ws matrices
+// struct d_strmat *Lx2;
+// struct d_strmat *AL2;
+
+ // early return
+ if(N==0)
+ {
+ dgecp_libstr(nu[0]+nx[0]+1, nu[0]+nx[0], &RSQrq[0], 0, 0, &RSQrq2[0], 0, 0);
+ return;
+ }
+
+ int nn;
+
+ int nu2 = 0; // sum of all nu
+ for(nn=0; nn<=N; nn++)
+ nu2 += nu[nn];
+
+ int nub = nu2; // backward partial sum
+ int nuf = 0; // forward partial sum
+
+ // final stage
+ nub -= nu[N];
+
+ // TODO g is not part of H !!!!!
+ dgecp_libstr(nu[N]+nx[N]+1, nu[N]+nx[N], &RSQrq[N], 0, 0, &L[N], 0, 0);
+
+ // D
+ dtrcp_l_libstr(nu[N], &L[N], 0, 0, &RSQrq2[0], nuf, nuf);
+
+ dgemm_nn_libstr(nub+nx[0]+1, nu[N], nx[N], 1.0, &Gamma[N-1], 0, 0, &L[N], nu[N], 0, 0.0, &RSQrq2[0], nuf+nu[N], nuf, &RSQrq2[0], nuf+nu[N], nuf);
+
+ // m
+ dgead_libstr(1, nu[N], 1.0, &L[N], nu[N]+nx[N], 0, &RSQrq2[0], nu2+nx[0], nuf);
+
+ nuf += nu[N];
+
+
+
+ // middle stages
+ for(nn=0; nn<N-1; nn++)
+ {
+ nub -= nu[N-nn-1];
+
+// d_create_strmat(nx[N-nn]+1, nx[N-nn], Lx, workspace+0);
+// d_create_strmat(nu[N-nn-1]+nx[N-nn-1]+1, nx[N-nn], AL, workspace+sizes[0]);
+
+#if defined(LA_HIGH_PERFORMANCE)
+ dgecp_libstr(nx[N-nn]+1, nx[N-nn], &L[N-nn], nu[N-nn], nu[N-nn], Lx, 0, 0);
+
+ dpotrf_l_mn_libstr(nx[N-nn]+1, nx[N-nn], Lx, 0, 0, Lx, 0, 0);
+
+ dtrmm_rlnn_libstr(nu[N-nn-1]+nx[N-nn-1]+1, nx[N-nn], 1.0, Lx, 0, 0, &BAbt[N-nn-1], 0, 0, AL, 0, 0);
+#else
+ dpotrf_l_mn_libstr(nx[N-nn]+1, nx[N-nn], &L[N-nn], nu[N-nn], nu[N-nn], Lx, 0, 0);
+
+ dtrmm_rlnn_libstr(nu[N-nn-1]+nx[N-nn-1]+1, nx[N-nn], 1.0, Lx, 0, 0, &BAbt[N-nn-1], 0, 0, AL, 0, 0);
+#endif
+ dgead_libstr(1, nx[N-nn], 1.0, Lx, nx[N-nn], 0, AL, nu[N-nn-1]+nx[N-nn-1], 0);
+
+ dsyrk_ln_mn_libstr(nu[N-nn-1]+nx[N-nn-1]+1, nu[N-nn-1]+nx[N-nn-1], nx[N-nn], 1.0, AL, 0, 0, AL, 0, 0, 1.0, &RSQrq[N-nn-1], 0, 0, &L[N-nn-1], 0, 0);
+
+ // D
+ dtrcp_l_libstr(nu[N-nn-1], &L[N-nn-1], 0, 0, &RSQrq2[0], nuf, nuf);
+
+ dgemm_nn_libstr(nub+nx[0]+1, nu[N-nn-1], nx[N-nn-1], 1.0, &Gamma[N-nn-2], 0, 0, &L[N-nn-1], nu[N-nn-1], 0, 0.0, &RSQrq2[0], nuf+nu[N-nn-1], nuf, &RSQrq2[0], nuf+nu[N-nn-1], nuf);
+
+ // m
+ dgead_libstr(1, nu[N-nn-1], 1.0, &L[N-nn-1], nu[N-nn-1]+nx[N-nn-1], 0, &RSQrq2[0], nu2+nx[0], nuf);
+
+ nuf += nu[N-nn-1];
+
+ }
+
+ // first stage
+ nn = N-1;
+
+// d_create_strmat(nx[N-nn]+1, nx[N-nn], Lx, workspace+0);
+// d_create_strmat(nu[N-nn-1]+nx[N-nn-1]+1, nx[N-nn], AL, workspace+sizes[0]);
+
+#if defined(LA_HIGH_PERFORMANCE)
+ dgecp_libstr(nx[N-nn]+1, nx[N-nn], &L[N-nn], nu[N-nn], nu[N-nn], Lx, 0, 0);
+
+ dpotrf_l_mn_libstr(nx[N-nn]+1, nx[N-nn], Lx, 0, 0, Lx, 0, 0);
+
+ dtrmm_rlnn_libstr(nu[N-nn-1]+nx[N-nn-1]+1, nx[N-nn], 1.0, Lx, 0, 0, &BAbt[N-nn-1], 0, 0, AL, 0, 0);
+#else
+ dpotrf_l_mn_libstr(nx[N-nn]+1, nx[N-nn], &L[N-nn], nu[N-nn], nu[N-nn], Lx, 0, 0);
+
+ dtrmm_rlnn_libstr(nu[N-nn-1]+nx[N-nn-1]+1, nx[N-nn], 1.0, Lx, 0, 0, &BAbt[N-nn-1], 0, 0, AL, 0, 0);
+#endif
+ dgead_libstr(1, nx[N-nn], 1.0, Lx, nx[N-nn], 0, AL, nu[N-nn-1]+nx[N-nn-1], 0);
+
+ dsyrk_ln_mn_libstr(nu[N-nn-1]+nx[N-nn-1]+1, nu[N-nn-1]+nx[N-nn-1], nx[N-nn], 1.0, AL, 0, 0, AL, 0, 0, 1.0, &RSQrq[N-nn-1], 0, 0, &L[N-nn-1], 0, 0);
+
+ // D, M, m, P, p
+// dgecp_libstr(nu[0]+nx[0]+1, nu[0]+nx[0], &L[N-nn-1], 0, 0, &RSQrq2[0], nuf, nuf); // TODO dtrcp for 'rectangular' matrices
+ dtrcp_l_libstr(nu[0]+nx[0], &L[N-nn-1], 0, 0, &RSQrq2[0], nuf, nuf); // TODO dtrcp for 'rectangular' matrices
+ dgecp_libstr(1, nu[0]+nx[0], &L[N-nn-1], nu[0]+nx[0], 0, &RSQrq2[0], nuf+nu[0]+nx[0], nuf); // TODO dtrcp for 'rectangular' matrices
+ // m p
+ drowex_libstr(nu2+nx[0], 1.0, &RSQrq2[0], nu2+nx[0], 0, &rq2[0], 0);
+
+ return;
+
+ }
+
+
+
+#if 0
+int d_cond_DCtd_workspace_size(int N, struct d_ocp_qp *qp_in, int idx_in)
+ {
+
+ // early return
+ if(N<0)
+ return 0;
+
+ // extract input members
+ int *nx = qp_in->nx + idx_in;
+ int *ng = qp_in->ng + idx_in;
+
+ int ii;
+ int tmp;
+
+ int size = 0;
+
+ for(ii=0; ii<=N; ii++)
+ {
+ tmp = d_size_strmat(ng[ii], nx[ii]);
+ tmp += d_size_strvec(nx[ii]);
+ tmp += d_size_strvec(ng[ii]);
+ size = tmp > size ? tmp : size;
+ }
+
+ return size;
+
+ }
+#endif
+
+
+
+void d_cond_DCtd(struct d_ocp_qp *ocp_qp, int *idxb2, struct d_strvec *d_lb2, struct d_strvec *d_ub2, struct d_strmat *DCt2, struct d_strvec *d_lg2, struct d_strvec *d_ug2, struct d_cond_qp_ocp2dense_workspace *cond_ws)
+ {
+
+ int N = ocp_qp->N;
+
+ // early return
+ if(N<0)
+ return;
+
+ // extract input members
+ int *nx = ocp_qp->nx;
+ int *nu = ocp_qp->nu;
+ int *nb = ocp_qp->nb;
+ int *ng = ocp_qp->ng;
+
+ int **idxb = ocp_qp->idxb;
+ struct d_strvec *d_lb = ocp_qp->d_lb;
+ struct d_strvec *d_ub = ocp_qp->d_ub;
+ struct d_strmat *DCt = ocp_qp->DCt;
+ struct d_strvec *d_lg = ocp_qp->d_lg;
+ struct d_strvec *d_ug = ocp_qp->d_ug;
+
+ // extract memory members
+ struct d_strmat *Gamma = cond_ws->Gamma;
+ struct d_strvec *Gammab = cond_ws->Gammab;
+ struct d_strvec *tmp_ngM = cond_ws->tmp_ngM;
+
+
+ double *d_lb3 = d_lb2->pa;
+ double *d_ub3 = d_ub2->pa;
+ double *d_lg3 = d_lg2->pa;
+ double *d_ug3 = d_ug2->pa;
+
+ double *ptr_d_lb;
+ double *ptr_d_ub;
+
+ int nu_tmp, ng_tmp;
+
+ int ii, jj;
+
+ int nu0, nx0, nb0, ng0;
+
+ // problem size
+
+ int nbb = nb[0]; // box that remain box constraints
+ int nbg = 0; // box that becomes general constraints
+ for(ii=1; ii<=N; ii++)
+ for(jj=0; jj<nb[ii]; jj++)
+ if(idxb[ii][jj]<nu[ii])
+ nbb++;
+ else
+ nbg++;
+
+ int nx2 = nx[0];
+ int nu2 = nu[0];
+ int ngg = ng[0];
+ for(ii=1; ii<=N; ii++)
+ {
+ nu2 += nu[ii];
+ ngg += ng[ii];
+ }
+ int ng2 = nbg + ngg;
+ int nb2 = nbb;
+ int nt2 = nb2 + ng2;
+
+ // set constraint matrix to zero (it's 2 lower triangular matrices atm)
+ dgese_libstr(nu2+nx2, ng2, 0.0, &DCt2[0], 0, 0);
+
+ // box constraints
+
+ int idx_gammab = nx[0];
+ for(ii=0; ii<N; ii++)
+ idx_gammab += nu[ii];
+
+ int ib = 0;
+ int ig = 0;
+
+ double tmp;
+ int idx_g;
+
+ // middle stages
+ nu_tmp = 0;
+ for(ii=0; ii<N; ii++)
+ {
+ nu0 = nu[N-ii];
+ nb0 = nb[N-ii];
+ ng0 = ng[N-ii];
+ nu_tmp += nu0;
+ ptr_d_lb = d_lb[N-ii].pa;
+ ptr_d_ub = d_ub[N-ii].pa;
+ for(jj=0; jj<nb0; jj++)
+ {
+ if(idxb[N-ii][jj]<nu0) // input: box constraint
+ {
+ d_lb3[ib] = ptr_d_lb[jj];
+ d_ub3[ib] = ptr_d_ub[jj];
+ idxb2[ib] = nu_tmp - nu0 + idxb[N-ii][jj];
+ ib++;
+ }
+ else // state: general constraint
+ {
+ idx_g = idxb[N-ii][jj]-nu0;
+ tmp = dgeex1_libstr(&Gamma[N-1-ii], idx_gammab, idx_g);
+ d_lg3[ig] = ptr_d_lb[jj] - tmp;
+ d_ug3[ig] = ptr_d_ub[jj] - tmp;
+ dgecp_libstr(idx_gammab, 1, &Gamma[N-ii-1], 0, idx_g, &DCt2[0], nu_tmp, ig);
+ ig++;
+ }
+ }
+ idx_gammab -= nu[N-1-ii];
+ }
+
+ // initial stage: both inputs and states as box constraints
+ nu0 = nu[0];
+ nb0 = nb[0];
+ ng0 = ng[0];
+ nu_tmp += nu0;
+ ptr_d_lb = d_lb[0].pa;
+ ptr_d_ub = d_ub[0].pa;
+ for(jj=0; jj<nb0; jj++)
+ {
+ d_lb3[ib] = ptr_d_lb[jj];
+ d_ub3[ib] = ptr_d_ub[jj];
+ idxb2[ib] = nu_tmp - nu0 + idxb[0][jj];
+ ib++;
+ }
+
+ // XXX for now, just shift after box-to-general constraints
+ // better interleave them, to keep the block lower trianlgular structure !!!
+
+ // general constraints
+
+ char *c_ptr;
+
+ nu_tmp = 0;
+ ng_tmp = 0;
+ for(ii=0; ii<N; ii++)
+ {
+
+ nx0 = nx[N-ii];
+ nu0 = nu[N-ii];
+ ng0 = ng[N-ii];
+
+ if(ng0>0)
+ {
+
+ dgecp_libstr(nu0, ng0, &DCt[N-ii], 0, 0, DCt2, nu_tmp, nbg+ng_tmp);
+
+ nu_tmp += nu0;
+
+ dgemm_nn_libstr(nu2+nx[0]-nu_tmp, ng0, nx0, 1.0, &Gamma[N-1-ii], 0, 0, &DCt[N-ii], nu0, 0, 0.0, DCt2, nu_tmp, nbg+ng_tmp, DCt2, nu_tmp, nbg+ng_tmp);
+
+ dveccp_libstr(ng0, &d_lg[N-ii], 0, d_lg2, nbg+ng_tmp);
+ dveccp_libstr(ng0, &d_ug[N-ii], 0, d_ug2, nbg+ng_tmp);
+
+ dgemv_t_libstr(nx0, ng0, 1.0, &DCt[N-ii], nu0, 0, &Gammab[N-ii-1], 0, 0.0, tmp_ngM, 0, tmp_ngM, 0);
+
+ daxpy_libstr(ng0, -1.0, tmp_ngM, 0, d_lg2, nbg+ng_tmp, d_lg2, nbg+ng_tmp);
+ daxpy_libstr(ng0, -1.0, tmp_ngM, 0, d_ug2, nbg+ng_tmp, d_ug2, nbg+ng_tmp);
+
+ ng_tmp += ng0;
+
+ }
+ else
+ {
+
+ nu_tmp += nu0;
+
+ }
+
+ }
+
+ ii = N;
+
+ nx0 = nx[0];
+ nu0 = nu[0];
+ ng0 = ng[0];
+
+ if(ng0>0)
+ {
+
+ dgecp_libstr(nu0+nx0, ng0, &DCt[0], 0, 0, DCt2, nu_tmp, nbg+ng_tmp);
+
+ dveccp_libstr(ng0, &d_lg[0], 0, d_lg2, nbg+ng_tmp);
+ dveccp_libstr(ng0, &d_ug[0], 0, d_ug2, nbg+ng_tmp);
+
+// ng_tmp += ng[N-ii];
+
+ }
+
+ return;
+
+ }
+
+
+
+void d_expand_sol(struct d_ocp_qp *ocp_qp, struct d_dense_qp_sol *dense_qp_sol, struct d_strvec *ux, struct d_strvec *pi, struct d_strvec *lam_lb, struct d_strvec *lam_ub, struct d_strvec *lam_lg, struct d_strvec *lam_ug, struct d_strvec *t_lb, struct d_strvec *t_ub, struct d_strvec *t_lg, struct d_strvec *t_ug, struct d_strvec *tmp_nuxM, struct d_strvec *tmp_ngM)
+ {
+
+ int N = ocp_qp->N;
+ int *nu = ocp_qp->nu;
+ int *nx = ocp_qp->nx;
+ int *nb = ocp_qp->nb;
+ int *ng = ocp_qp->ng;
+
+ struct d_strmat *BAbt = ocp_qp->BAbt;
+ struct d_strvec *b = ocp_qp->b;
+ int **idxb = ocp_qp->idxb;
+ struct d_strmat *RSQrq = ocp_qp->RSQrq;
+ struct d_strvec *rq = ocp_qp->rq;
+ struct d_strmat *DCt = ocp_qp->DCt;
+
+ struct d_strvec *vc = dense_qp_sol->v;
+ struct d_strvec *pic = dense_qp_sol->pi;
+ struct d_strvec *lam_lbc = dense_qp_sol->lam_lb;
+ struct d_strvec *lam_ubc = dense_qp_sol->lam_ub;
+ struct d_strvec *lam_lgc = dense_qp_sol->lam_lg;
+ struct d_strvec *lam_ugc = dense_qp_sol->lam_ug;
+ struct d_strvec *t_lbc = dense_qp_sol->t_lb;
+ struct d_strvec *t_ubc = dense_qp_sol->t_ub;
+ struct d_strvec *t_lgc = dense_qp_sol->t_lg;
+ struct d_strvec *t_ugc = dense_qp_sol->t_ug;
+
+ int ii, jj;
+
+ // inputs & initial states
+ int nu_tmp = 0;
+ // final stages: copy only input
+ for(ii=0; ii<N; ii++)
+ {
+ dveccp_libstr(nu[N-ii], vc, nu_tmp, ux+(N-ii), 0);
+ nu_tmp += nu[N-ii];
+ }
+ // first stage: copy input and state
+ dveccp_libstr(nu[0]+nx[0], vc, nu_tmp, ux+0, 0);
+
+ // compute missing states by simulation within each block
+ for(ii=0; ii<N; ii++)
+ {
+ dgemv_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]);
+ }
+
+ // slack variables and ineq lagrange multipliers
+ int nbb = 0;
+ int nbg = 0;
+ int ngg = 0;
+ double *ptr_lam_lb;
+ double *ptr_lam_ub;
+ double *ptr_lam_lg;
+ double *ptr_lam_ug;
+ double *ptr_t_lb;
+ double *ptr_t_ub;
+ double *ptr_t_lg;
+ double *ptr_t_ug;
+ double *ptr_lam_lbc = lam_lbc->pa;
+ double *ptr_lam_ubc = lam_ubc->pa;
+ double *ptr_lam_lgc = lam_lgc->pa;
+ double *ptr_lam_ugc = lam_ugc->pa;
+ double *ptr_t_lbc = t_lbc->pa;
+ double *ptr_t_ubc = t_ubc->pa;
+ double *ptr_t_lgc = t_lgc->pa;
+ double *ptr_t_ugc = t_ugc->pa;
+ // final stages
+ for(ii=0; ii<N; ii++)
+ {
+ ptr_lam_lb = (lam_lb+(N-ii))->pa;
+ ptr_lam_ub = (lam_ub+(N-ii))->pa;
+ ptr_t_lb = (t_lb+(N-ii))->pa;
+ ptr_t_ub = (t_ub+(N-ii))->pa;
+ for(jj=0; jj<nb[N-ii]; jj++)
+ {
+ if(idxb[N-ii][jj]<nu[N-ii])
+ {
+ // box as box
+ ptr_lam_lb[jj] = ptr_lam_lbc[nbb];
+ ptr_lam_ub[jj] = ptr_lam_ubc[nbb];
+ ptr_t_lb[jj] = ptr_t_lbc[nbb];
+ ptr_t_ub[jj] = ptr_t_ubc[nbb];
+ nbb++;
+ }
+ else
+ {
+ // box as general XXX change when decide where nbg are placed wrt ng
+ ptr_lam_lb[jj] = ptr_lam_lgc[nbg];
+ ptr_lam_ub[jj] = ptr_lam_ugc[nbg];
+ ptr_t_lb[jj] = ptr_t_lgc[nbg];
+ ptr_t_ub[jj] = ptr_t_ugc[nbg];
+ nbg++;
+ }
+ }
+ }
+ // process as vectors ???
+ for(ii=0; ii<N; ii++)
+ {
+ ptr_lam_lg = (lam_lg+(N-ii))->pa;
+ ptr_lam_ug = (lam_ug+(N-ii))->pa;
+ ptr_t_lg = (t_lg+(N-ii))->pa;
+ ptr_t_ug = (t_ug+(N-ii))->pa;
+ for(jj=0; jj<ng[N-ii]; jj++)
+ {
+ // gnenral as general
+ ptr_lam_lg[jj] = ptr_lam_lgc[nbg+ngg];
+ ptr_lam_ug[jj] = ptr_lam_ugc[nbg+ngg];
+ ptr_t_lg[jj] = ptr_t_lgc[nbg+ngg];
+ ptr_t_ug[jj] = ptr_t_ugc[nbg+ngg];
+ ngg++;
+ }
+ }
+ // first stage
+ // all box as box
+ dveccp_libstr(nb[0], lam_lbc, nbb, lam_lb+0, 0);
+ dveccp_libstr(nb[0], lam_ubc, nbb, lam_ub+0, 0);
+ dveccp_libstr(nb[0], t_lbc, nbb, t_lb+0, 0);
+ dveccp_libstr(nb[0], t_ubc, nbb, t_ub+0, 0);
+ // all general as general
+ dveccp_libstr(ng[0], lam_lgc, ngg, lam_lg+0, 0);
+ dveccp_libstr(ng[0], lam_ugc, ngg, lam_ug+0, 0);
+ dveccp_libstr(ng[0], t_lgc, ngg, t_lg+0, 0);
+ dveccp_libstr(ng[0], t_ugc, ngg, t_ug+0, 0);
+
+ // lagrange multipliers of equality constraints
+ double *ptr_nuxM = tmp_nuxM->pa;
+ double *ptr_ngM = tmp_ngM->pa;
+ // last stage
+ dsymv_l_libstr(nx[N], nx[N], 1.0, RSQrq+N, nu[N], nu[N], ux+N, nu[N], 1.0, rq+N, nu[N], pi+(N-1), 0);
+ // TODO avoid to multiply by R and B (i.e. the u part)
+ for(ii=0; ii<N-1; ii++)
+ {
+ ptr_lam_lb = (lam_lb+N-1-ii)->pa;
+ ptr_lam_ub = (lam_ub+N-1-ii)->pa;
+ ptr_lam_lg = (lam_lg+N-1-ii)->pa;
+ ptr_lam_ug = (lam_ug+N-1-ii)->pa;
+ dveccp_libstr(nu[N-1-ii]+nx[N-1-ii], rq+(N-1-ii), 0, tmp_nuxM, 0);
+ for(jj=0; jj<nb[N-1-ii]; jj++)
+ ptr_nuxM[idxb[N-1-ii][jj]] += ptr_lam_ub[jj] - ptr_lam_lb[jj];
+ dsymv_l_libstr(nu[N-1-ii]+nx[N-1-ii], nu[N-1-ii]+nx[N-1-ii], 1.0, RSQrq+(N-1-ii), 0, 0, ux+(N-1-ii), 0, 1.0, tmp_nuxM, 0, tmp_nuxM, 0);
+ dgemv_n_libstr(nu[N-1-ii]+nx[N-1-ii], nx[N-ii], 1.0, BAbt+(N-1-ii), 0, 0, pi+(N-1-ii), 0, 1.0, tmp_nuxM, 0, tmp_nuxM, 0);
+ for(jj=0; jj<ng[N-1-ii]; jj++)
+ ptr_ngM[jj] = ptr_lam_ug[jj] - ptr_lam_lg[jj];
+ dgemv_n_libstr(nu[N-1-ii]+nx[N-1-ii], ng[N-1-ii], 1.0, DCt+(N-1-ii), 0, 0, tmp_ngM, 0, 1.0, tmp_nuxM, 0, tmp_nuxM, 0);
+
+ dveccp_libstr(nx[N-1-ii], tmp_nuxM, nu[N-1-ii], pi+(N-2-ii), 0);
+ }
+
+
+ return;
+
+ }
diff --git a/cond/d_part_cond.c b/cond/d_part_cond.c
new file mode 100644
index 0000000..e995d15
--- /dev/null
+++ b/cond/d_part_cond.c
@@ -0,0 +1,38 @@
+/**************************************************************************************************
+* *
+* 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 <stdlib.h>
+#include <stdio.h>
+
+#include <blasfeo_target.h>
+#include <blasfeo_common.h>
+#include <blasfeo_d_blas.h>
+#include <blasfeo_d_aux.h>
+
+
+
diff --git a/core_qp/Makefile b/core_qp/Makefile
new file mode 100644
index 0000000..47e8e23
--- /dev/null
+++ b/core_qp/Makefile
@@ -0,0 +1,48 @@
+###################################################################################################
+# #
+# 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_core_qp_ipm_hard_aux.o d_core_qp_ipm_hard.o
+OBJS += s_core_qp_ipm_hard_aux.o s_core_qp_ipm_hard.o
+
+obj: $(OBJS)
+
+clean:
+ rm -f *.o
+ rm -f *.s
+
+d_core_qp_ipm_hard.o: d_core_qp_ipm_hard.c x_core_qp_ipm_hard.c
+s_core_qp_ipm_hard.o: s_core_qp_ipm_hard.c x_core_qp_ipm_hard.c
+d_core_qp_ipm_hard_aux.o: d_core_qp_ipm_hard_aux.c x_core_qp_ipm_hard_aux.c
+s_core_qp_ipm_hard_aux.o: s_core_qp_ipm_hard_aux.c x_core_qp_ipm_hard_aux.c
diff --git a/core_qp/d_core_qp_ipm_hard.c b/core_qp/d_core_qp_ipm_hard.c
new file mode 100644
index 0000000..51a7e4c
--- /dev/null
+++ b/core_qp/d_core_qp_ipm_hard.c
@@ -0,0 +1,43 @@
+/**************************************************************************************************
+* *
+* 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 "../include/hpipm_d_core_qp_ipm_hard.h"
+#include "../include/hpipm_d_core_qp_ipm_hard_aux.h"
+
+
+
+#define IPM_HARD_CORE_QP_WORKSPACE d_ipm_hard_core_qp_workspace
+#define REAL double
+
+#define MEMSIZE_IPM_HARD_CORE_QP d_memsize_ipm_hard_core_qp
+#define CREATE_IPM_HARD_CORE_QP d_create_ipm_hard_core_qp
+
+
+
+#include "x_core_qp_ipm_hard.c"
diff --git a/core_qp/d_core_qp_ipm_hard_aux.c b/core_qp/d_core_qp_ipm_hard_aux.c
new file mode 100644
index 0000000..5a8fba8
--- /dev/null
+++ b/core_qp/d_core_qp_ipm_hard_aux.c
@@ -0,0 +1,52 @@
+/**************************************************************************************************
+* *
+* 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_d_blas.h>
+
+#include "../include/hpipm_d_core_qp_ipm_hard.h"
+
+
+
+#define IPM_HARD_CORE_QP_WORKSPACE d_ipm_hard_core_qp_workspace
+#define REAL double
+
+#define COMPUTE_QX_QX_HARD_QP d_compute_Qx_qx_hard_qp
+#define COMPUTE_LAM_T_HARD_QP d_compute_lam_t_hard_qp
+#define COMPUTE_ALPHA_HARD_QP d_compute_alpha_hard_qp
+#define UPDATE_VAR_HARD_QP d_update_var_hard_qp
+#define COMPUTE_MU_AFF_HARD_QP d_compute_mu_aff_hard_qp
+#define COMPUTE_CENTERING_CORRECTION_HARD_QP d_compute_centering_correction_hard_qp
+#define COMPUTE_QX_HARD_QP d_compute_qx_hard_qp
+
+
+
+#include "x_core_qp_ipm_hard_aux.c"
diff --git a/core_qp/s_core_qp_ipm_hard.c b/core_qp/s_core_qp_ipm_hard.c
new file mode 100644
index 0000000..7aacba0
--- /dev/null
+++ b/core_qp/s_core_qp_ipm_hard.c
@@ -0,0 +1,44 @@
+/**************************************************************************************************
+* *
+* 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 "../include/hpipm_s_core_qp_ipm_hard.h"
+#include "../include/hpipm_s_core_qp_ipm_hard_aux.h"
+
+
+
+#define IPM_HARD_CORE_QP_WORKSPACE s_ipm_hard_core_qp_workspace
+#define REAL float
+
+#define MEMSIZE_IPM_HARD_CORE_QP s_memsize_ipm_hard_core_qp
+#define CREATE_IPM_HARD_CORE_QP s_create_ipm_hard_core_qp
+
+
+
+#include "x_core_qp_ipm_hard.c"
+
diff --git a/core_qp/s_core_qp_ipm_hard_aux.c b/core_qp/s_core_qp_ipm_hard_aux.c
new file mode 100644
index 0000000..6c5c8c1
--- /dev/null
+++ b/core_qp/s_core_qp_ipm_hard_aux.c
@@ -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 <blasfeo_target.h>
+#include <blasfeo_common.h>
+#include <blasfeo_s_aux.h>
+#include <blasfeo_s_blas.h>
+
+#include "../include/hpipm_s_core_qp_ipm_hard.h"
+
+
+
+#define IPM_HARD_CORE_QP_WORKSPACE s_ipm_hard_core_qp_workspace
+#define REAL float
+
+#define COMPUTE_QX_QX_HARD_QP s_compute_Qx_qx_hard_qp
+#define COMPUTE_LAM_T_HARD_QP s_compute_lam_t_hard_qp
+#define COMPUTE_ALPHA_HARD_QP s_compute_alpha_hard_qp
+#define UPDATE_VAR_HARD_QP s_update_var_hard_qp
+#define COMPUTE_MU_AFF_HARD_QP s_compute_mu_aff_hard_qp
+#define COMPUTE_CENTERING_CORRECTION_HARD_QP s_compute_centering_correction_hard_qp
+#define COMPUTE_QX_HARD_QP s_compute_qx_hard_qp
+
+
+
+#include "x_core_qp_ipm_hard_aux.c"
+
diff --git a/core_qp/x_core_qp_ipm_hard.c b/core_qp/x_core_qp_ipm_hard.c
new file mode 100644
index 0000000..95cb007
--- /dev/null
+++ b/core_qp/x_core_qp_ipm_hard.c
@@ -0,0 +1,143 @@
+/**************************************************************************************************
+* *
+* 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_CORE_QP(int nv, int ne, int nb, int ng, int iter_max)
+ {
+
+ int size;
+
+ int nv0 = nv;
+ int ne0 = ne;
+ int nb0 = nb;
+ int ng0 = ng;
+// if target avx
+// nv0 = ...
+
+ size = 0;
+
+ size += 2*nv0*sizeof(REAL); // dv res_g
+ size += 2*ne0*sizeof(REAL); // dpi res_b
+ size += 5*(2*nb0+2*ng0)*sizeof(REAL); // dlam dt res_d res_m t_inv
+ size += 2*(nb0+ng0)*sizeof(REAL); // Qx qx
+ size += 5*iter_max*sizeof(REAL); // stat
+
+ size = (size+63)/64*64; // make multiple of cache line size
+
+ return size;
+
+ }
+
+
+
+void CREATE_IPM_HARD_CORE_QP(struct IPM_HARD_CORE_QP_WORKSPACE *workspace, void *mem)
+ {
+
+ int nv = workspace->nv;
+ int ne = workspace->ne;
+ int nb = workspace->nb;
+ int ng = workspace->ng;
+
+ int nv0 = nv;
+ int ne0 = ne;
+ int nb0 = nb;
+ int ng0 = ng;
+// if target avx NO!!!!
+// nv0 = ...
+
+ workspace->memsize = MEMSIZE_IPM_HARD_CORE_QP(nv, ne, nb, ng, workspace->iter_max);
+
+ REAL *d_ptr = (REAL *) mem;
+
+ workspace->t_inv = d_ptr; // t_inv
+ workspace->t_inv_lb = d_ptr;
+ workspace->t_inv_lg = d_ptr+nb0;
+ workspace->t_inv_ub = d_ptr+nb0+ng0;
+ workspace->t_inv_ug = d_ptr+2*nb0+ng0;
+ d_ptr += 2*nb0+2*ng0;
+
+ workspace->dv = d_ptr; // dv
+ d_ptr += nv0;
+
+ workspace->dpi = d_ptr; // dpi
+ d_ptr += ne0;
+
+ workspace->dlam = d_ptr; // dlam
+ workspace->dlam_lb = d_ptr;
+ workspace->dlam_lg = d_ptr+nb0;
+ workspace->dlam_ub = d_ptr+nb0+ng0;
+ workspace->dlam_ug = d_ptr+2*nb0+ng0;
+ d_ptr += 2*nb0+2*ng0;
+
+ workspace->dt = d_ptr; // dt
+ workspace->dt_lb = d_ptr;
+ workspace->dt_lg = d_ptr+nb0;
+ workspace->dt_ub = d_ptr+nb0+ng0;
+ workspace->dt_ug = d_ptr+2*nb0+ng0;
+ d_ptr += 2*nb0+2*ng0;
+
+ workspace->res_g = d_ptr; // res_g
+ d_ptr += nv0;
+
+ workspace->res_b = d_ptr; // res_b
+ d_ptr += ne0;
+
+ workspace->res_d = d_ptr; // res_d
+ workspace->res_d_lb = d_ptr;
+ workspace->res_d_lg = d_ptr+nb0;
+ workspace->res_d_ub = d_ptr+nb0+ng0;
+ workspace->res_d_ug = d_ptr+2*nb0+ng0;
+ d_ptr += 2*nb0+2*ng0;
+
+ workspace->res_m = d_ptr; // res_m
+ workspace->res_m_lb = d_ptr;
+ workspace->res_m_lg = d_ptr+nb0;
+ workspace->res_m_ub = d_ptr+nb0+ng0;
+ workspace->res_m_ug = d_ptr+2*nb0+ng0;
+ d_ptr += 2*nb0+2*ng0;
+
+ workspace->Qx = d_ptr; // Qx
+ workspace->Qx_lb = d_ptr;
+ workspace->Qx_lg = d_ptr+nb0;
+ d_ptr += nb0+ng0;
+
+ workspace->qx = d_ptr; // qx
+ workspace->qx_lb = d_ptr;
+ workspace->qx_lg = d_ptr+nb0;
+ d_ptr += nb0+ng0;
+
+ workspace->stat = d_ptr; // stat
+ d_ptr += 5*workspace->iter_max;
+
+ int *i_ptr = (int *) d_ptr;
+
+ return;
+
+ }
+
+
diff --git a/core_qp/x_core_qp_ipm_hard_aux.c b/core_qp/x_core_qp_ipm_hard_aux.c
new file mode 100644
index 0000000..d1763e4
--- /dev/null
+++ b/core_qp/x_core_qp_ipm_hard_aux.c
@@ -0,0 +1,308 @@
+/**************************************************************************************************
+* *
+* 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 COMPUTE_QX_QX_HARD_QP(struct IPM_HARD_CORE_QP_WORKSPACE *rws)
+ {
+
+ int nb = rws->nb;
+ int ng = rws->ng;
+
+ REAL *lam_lb = rws->lam_lb;
+ REAL *lam_ub = rws->lam_ub;
+ REAL *t_lb = rws->t_lb;
+ REAL *t_ub = rws->t_ub;
+ REAL *res_m_lb = rws->res_m_lb;
+ REAL *res_m_ub = rws->res_m_ub;
+ REAL *res_d_lb = rws->res_d_lb;
+ REAL *res_d_ub = rws->res_d_ub;
+ REAL *t_inv_lb = rws->t_inv_lb;
+ REAL *t_inv_ub = rws->t_inv_ub;
+ REAL *Qx = rws->Qx;
+ REAL *qx = rws->qx;
+
+ // local variables
+ int nt = nb+ng;
+ int ii;
+
+ for(ii=0; ii<nt; ii++)
+ {
+
+ t_inv_lb[ii] = 1.0/t_lb[ii];
+ t_inv_ub[ii] = 1.0/t_ub[ii];
+ // TODO mask out unconstrained components for one-sided
+ Qx[ii] = t_inv_lb[ii]*lam_lb[ii] \
+ + t_inv_ub[ii]*lam_ub[ii];
+ qx[ii] = t_inv_lb[ii]*(res_m_lb[ii]-lam_lb[ii]*res_d_lb[ii]) \
+ - t_inv_ub[ii]*(res_m_ub[ii]+lam_ub[ii]*res_d_ub[ii]);
+
+ }
+
+ return;
+
+ }
+
+
+
+void COMPUTE_LAM_T_HARD_QP(struct IPM_HARD_CORE_QP_WORKSPACE *rws)
+ {
+
+ int nb = rws->nb;
+ int ng = rws->ng;
+
+ REAL *lam_lb = rws->lam_lb;
+ REAL *lam_ub = rws->lam_ub;
+ REAL *dlam_lb = rws->dlam_lb;
+ REAL *dlam_ub = rws->dlam_ub;
+ REAL *dt_lb = rws->dt_lb;
+ REAL *dt_ub = rws->dt_ub;
+ REAL *res_d_lb = rws->res_d_lb;
+ REAL *res_d_ub = rws->res_d_ub;
+ REAL *res_m_lb = rws->res_m_lb;
+ REAL *res_m_ub = rws->res_m_ub;
+ REAL *t_inv_lb = rws->t_inv_lb;
+ REAL *t_inv_ub = rws->t_inv_ub;
+
+ // local variables
+ int ii;
+ int nt = nb+ng;
+
+ for(ii=0; ii<nt; ii++)
+ {
+
+ dt_ub[ii] = - dt_lb[ii];
+
+ dt_lb[ii] -= res_d_lb[ii];
+ dt_ub[ii] += res_d_ub[ii];
+
+ // TODO compute lamda alone ???
+ dlam_lb[ii] = - t_inv_lb[ii] * (lam_lb[ii]*dt_lb[ii] + res_m_lb[ii]);
+ dlam_ub[ii] = - t_inv_ub[ii] * (lam_ub[ii]*dt_ub[ii] + res_m_ub[ii]);
+
+ }
+
+ return;
+
+ }
+
+
+
+void COMPUTE_ALPHA_HARD_QP(struct IPM_HARD_CORE_QP_WORKSPACE *rws)
+ {
+
+ // extract workspace members
+ int nb = rws->nb;
+ int ng = rws->ng;
+
+ REAL *lam_lb = rws->lam_lb;
+ REAL *t_lb = rws->t_lb;
+ REAL *dlam_lb = rws->dlam_lb;
+ REAL *dt_lb = rws->dt_lb;
+
+ REAL alpha = - 1.0;
+
+ // local variables
+ int nt = nb+ng;
+ int ii;
+
+ for(ii=0; ii<2*nt; ii++)
+ {
+
+ if( alpha*dlam_lb[ii+0]>lam_lb[ii+0] )
+ {
+ alpha = lam_lb[ii+0] / dlam_lb[ii+0];
+ }
+ if( alpha*dt_lb[ii+0]>t_lb[ii+0] )
+ {
+ alpha = t_lb[ii+0] / dt_lb[ii+0];
+ }
+
+ }
+
+ // store alpha
+ rws->alpha = - alpha;
+
+ return;
+
+ }
+
+
+
+void UPDATE_VAR_HARD_QP(struct IPM_HARD_CORE_QP_WORKSPACE *rws)
+ {
+
+ // extract workspace members
+ int nv = rws->nv;
+ int ne = rws->ne;
+ int nb = rws->nb;
+ int ng = rws->ng;
+
+ REAL *v = rws->v;
+ REAL *pi = rws->pi;
+ REAL *lam = rws->lam;
+ REAL *t = rws->t;
+ REAL *dv = rws->dv;
+ REAL *dpi = rws->dpi;
+ REAL *dlam = rws->dlam;
+ REAL *dt = rws->dt;
+ REAL alpha = rws->alpha;
+#if 0
+ if(alpha<1.0)
+ alpha *= 0.995;
+#else
+ alpha = alpha * ((1.0-alpha)*0.99 + alpha*0.9999999);
+#endif
+
+ // local variables
+ int nt = nb+ng;
+ int ii;
+
+ // update v
+ for(ii=0; ii<nv; ii++)
+ {
+ v[ii] += alpha * dv[ii];
+ }
+
+ // update pi
+ for(ii=0; ii<ne; ii++)
+ {
+ pi[ii] += alpha * dpi[ii];
+ }
+
+ // update lam
+ for(ii=0; ii<2*nt; ii++)
+ {
+ lam[ii] += alpha * dlam[ii];
+ }
+
+ // update t
+ for(ii=0; ii<2*nt; ii++)
+ {
+ t[ii] += alpha * dt[ii];
+ }
+
+ return;
+
+ }
+
+
+
+void COMPUTE_MU_AFF_HARD_QP(struct IPM_HARD_CORE_QP_WORKSPACE *rws)
+ {
+
+ int ii;
+
+ // extract workspace members
+ int nb = rws->nb;
+ int ng = rws->ng;
+ int nt = nb+ng;
+
+ REAL *ptr_lam = rws->lam_lb;
+ REAL *ptr_t = rws->t_lb;
+ REAL *ptr_dlam = rws->dlam_lb;
+ REAL *ptr_dt = rws->dt_lb;
+ REAL alpha = rws->alpha;
+ // this affects the minimum value of signa !!!
+// alpha *= 0.99;
+
+ REAL mu = 0;
+
+ for(ii=0; ii<2*nt; ii++)
+ {
+ mu += (ptr_lam[ii+0] + alpha*ptr_dlam[ii+0]) * (ptr_t[ii+0] + alpha*ptr_dt[ii+0]);
+ }
+
+ rws->mu_aff = mu*rws->nt_inv;
+
+ return;
+
+ }
+
+
+
+void COMPUTE_CENTERING_CORRECTION_HARD_QP(struct IPM_HARD_CORE_QP_WORKSPACE *rws)
+ {
+
+ int ii;
+
+ // extract workspace members
+ int nb = rws->nb;
+ int ng = rws->ng;
+ int nt = nb+ng;
+
+ REAL *ptr_dlam = rws->dlam_lb;
+ REAL *ptr_dt = rws->dt_lb;
+ REAL *ptr_res_m = rws->res_m_lb;
+
+ REAL sigma_mu = rws->sigma*rws->mu;
+
+ for(ii=0; ii<2*nt; ii++)
+ {
+ ptr_res_m[ii+0] += ptr_dt[ii+0] * ptr_dlam[ii+0] - sigma_mu;
+ }
+
+ return;
+
+ }
+
+
+
+void COMPUTE_QX_HARD_QP(struct IPM_HARD_CORE_QP_WORKSPACE *rws)
+ {
+
+ int nb = rws->nb;
+ int ng = rws->ng;
+
+ REAL *lam_lb = rws->lam_lb;
+ REAL *lam_ub = rws->lam_ub;
+ REAL *res_m_lb = rws->res_m_lb;
+ REAL *res_m_ub = rws->res_m_ub;
+ REAL *res_d_lb = rws->res_d_lb;
+ REAL *res_d_ub = rws->res_d_ub;
+ REAL *t_inv_lb = rws->t_inv_lb;
+ REAL *t_inv_ub = rws->t_inv_ub;
+ REAL *qx = rws->qx;
+
+ // local variables
+ int nt = nb+ng;
+ int ii;
+
+ for(ii=0; ii<nt; ii++)
+ {
+
+ // TODO mask out unconstrained components for one-sided
+ qx[ii] = t_inv_lb[ii]*(res_m_lb[ii]-lam_lb[ii]*res_d_lb[ii]) \
+ - t_inv_ub[ii]*(res_m_ub[ii]+lam_ub[ii]*res_d_ub[ii]);
+
+ }
+
+ return;
+
+ }
+
+
diff --git a/dense_qp/Makefile b/dense_qp/Makefile
new file mode 100644
index 0000000..3e629ea
--- /dev/null
+++ b/dense_qp/Makefile
@@ -0,0 +1,52 @@
+###################################################################################################
+# #
+# 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_dense_qp.o d_dense_qp_sol.o d_dense_qp_kkt.o d_dense_qp_ipm_hard.o
+OBJS += s_dense_qp.o s_dense_qp_sol.o s_dense_qp_kkt.o s_dense_qp_ipm_hard.o
+
+obj: $(OBJS)
+
+clean:
+ rm -f *.o
+ rm -f *.s
+
+d_dense_qp.o: d_dense_qp.c x_dense_qp.c
+s_dense_qp.o: s_dense_qp.c x_dense_qp.c
+d_dense_qp_sol.o: d_dense_qp_sol.c x_dense_qp_sol.c
+s_dense_qp_sol.o: s_dense_qp_sol.c x_dense_qp_sol.c
+d_dense_qp_kkt.o: d_dense_qp_kkt.c x_dense_qp_kkt.c
+s_dense_qp_kkt.o: s_dense_qp_kkt.c x_dense_qp_kkt.c
+d_dense_qp_ipm_hard.o: d_dense_qp_ipm_hard.c x_dense_qp_ipm_hard.c
+s_dense_qp_ipm_hard.o: s_dense_qp_ipm_hard.c x_dense_qp_ipm_hard.c
diff --git a/dense_qp/d_dense_qp.c b/dense_qp/d_dense_qp.c
new file mode 100644
index 0000000..42b4681
--- /dev/null
+++ b/dense_qp/d_dense_qp.c
@@ -0,0 +1,77 @@
+/**************************************************************************************************
+* *
+* 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_dense_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_TRAN_STRMAT2MAT d_cvt_tran_strmat2mat
+#define CVT_VEC2STRVEC d_cvt_vec2strvec
+#define CVT_STRMAT2MAT d_cvt_strmat2mat
+#define CVT_STRVEC2VEC d_cvt_strvec2vec
+#define DENSE_QP_DIM d_dense_qp_dim
+#define DENSE_QP_VEC d_dense_qp_vec
+#define DENSE_QP_MAT d_dense_qp_mat
+#define DENSE_QP d_dense_qp
+#define GECP_LIBSTR dgecp_libstr
+#define GETR_LIBSTR dgetr_libstr
+#define REAL double
+#define ROWIN_LIBSTR drowin_libstr
+#define SIZE_STRMAT d_size_strmat
+#define SIZE_STRVEC d_size_strvec
+#define STRMAT d_strmat
+#define STRVEC d_strvec
+#define VECCP_LIBSTR dveccp_libstr
+
+#define MEMSIZE_DENSE_QP d_memsize_dense_qp
+#define CREATE_DENSE_QP d_create_dense_qp
+#define CVT_COLMAJ_TO_DENSE_QP d_cvt_colmaj_to_dense_qp
+#define CVT_DENSE_QP_TO_COLMAJ d_cvt_dense_qp_to_colmaj
+#define CVT_ROWMAJ_TO_DENSE_QP d_cvt_rowmaj_to_dense_qp
+#define CVT_DENSE_QP_TO_ROWMAJ d_cvt_dense_qp_to_rowmaj
+#define CVT_LIBSTR_TO_DENSE_QP d_cvt_libstr_to_dense_qp
+#define CVT_DENSE_QP_TO_LIBSTR d_cvt_dense_qp_to_libstr
+#define CAST_DENSE_QP_DIM d_cast_dense_qp_dim
+//#define CREATE_DENSE_QP d_create_dense_qp
+//#define COPY_DENSE_QP d_copy_dense_qp
+
+
+
+#include "x_dense_qp.c"
diff --git a/dense_qp/d_dense_qp_ipm_hard.c b/dense_qp/d_dense_qp_ipm_hard.c
new file mode 100644
index 0000000..8a70f6f
--- /dev/null
+++ b/dense_qp/d_dense_qp_ipm_hard.c
@@ -0,0 +1,76 @@
+/**************************************************************************************************
+* *
+* 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_dense_qp.h"
+#include "../include/hpipm_d_dense_qp_sol.h"
+#include "../include/hpipm_d_dense_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_dense_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_DENSE_QP d_compute_res_hard_dense_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 DENSE_QP d_dense_qp
+#define DENSE_QP_SOL d_dense_qp_sol
+#define FACT_SOLVE_KKT_STEP_HARD_DENSE_QP d_fact_solve_kkt_step_hard_dense_qp
+#define FACT_SOLVE_KKT_UNCONSTR_DENSE_QP d_fact_solve_kkt_unconstr_dense_qp
+#define INIT_VAR_HARD_DENSE_QP d_init_var_hard_dense_qp
+#define IPM_HARD_CORE_QP_WORKSPACE d_ipm_hard_core_qp_workspace
+#define IPM_HARD_DENSE_QP_ARG d_ipm_hard_dense_qp_arg
+#define IPM_HARD_DENSE_QP_WORKSPACE d_ipm_hard_dense_qp_workspace
+#define MEMSIZE_IPM_HARD_CORE_QP d_memsize_ipm_hard_core_qp
+#define REAL double
+#define SIZE_STRMAT d_size_strmat
+#define SIZE_STRVEC d_size_strvec
+#define SOLVE_KKT_STEP_HARD_DENSE_QP d_solve_kkt_step_hard_dense_qp
+#define STRMAT d_strmat
+#define STRVEC d_strvec
+#define UPDATE_VAR_HARD_QP d_update_var_hard_qp
+
+
+
+#define MEMSIZE_IPM_HARD_DENSE_QP d_memsize_ipm_hard_dense_qp
+#define CREATE_IPM_HARD_DENSE_QP d_create_ipm_hard_dense_qp
+#define SOLVE_IPM_HARD_DENSE_QP d_solve_ipm_hard_dense_qp
+#define SOLVE_IPM2_HARD_DENSE_QP d_solve_ipm2_hard_dense_qp
+
+
+
+#include "x_dense_qp_ipm_hard.c"
diff --git a/dense_qp/d_dense_qp_kkt.c b/dense_qp/d_dense_qp_kkt.c
new file mode 100644
index 0000000..2d3968c
--- /dev/null
+++ b/dense_qp/d_dense_qp_kkt.c
@@ -0,0 +1,88 @@
+/**************************************************************************************************
+* *
+* 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_dense_qp.h"
+#include "../include/hpipm_d_dense_qp_sol.h"
+#include "../include/hpipm_d_dense_qp_ipm_hard.h"
+#include "../include/hpipm_d_core_qp_ipm_hard.h"
+#include "../include/hpipm_d_core_qp_ipm_hard_aux.h"
+
+
+
+#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 DENSE_QP d_dense_qp
+#define DENSE_QP_SOL d_dense_qp_sol
+#define DIAAD_SP_LIBSTR ddiaad_sp_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 GESE_LIBSTR dgese_libstr
+#define IPM_HARD_CORE_QP_WORKSPACE d_ipm_hard_core_qp_workspace
+#define IPM_HARD_DENSE_QP_WORKSPACE d_ipm_hard_dense_qp_workspace
+#define POTRF_L_LIBSTR dpotrf_l_libstr
+#define POTRF_L_MN_LIBSTR dpotrf_l_mn_libstr
+#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 TRSM_RLTN_LIBSTR dtrsm_rltn_libstr
+#define TRSV_LNN_LIBSTR dtrsv_lnn_libstr
+#define TRSV_LTN_LIBSTR dtrsv_ltn_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_DENSE_QP d_init_var_hard_dense_qp
+#define COMPUTE_RES_HARD_DENSE_QP d_compute_res_hard_dense_qp
+#define FACT_SOLVE_KKT_UNCONSTR_DENSE_QP d_fact_solve_kkt_unconstr_dense_qp
+#define FACT_SOLVE_KKT_STEP_HARD_DENSE_QP d_fact_solve_kkt_step_hard_dense_qp
+#define SOLVE_KKT_STEP_HARD_DENSE_QP d_solve_kkt_step_hard_dense_qp
+
+
+
+#include "x_dense_qp_kkt.c"
diff --git a/dense_qp/d_dense_qp_sol.c b/dense_qp/d_dense_qp_sol.c
new file mode 100644
index 0000000..2d59048
--- /dev/null
+++ b/dense_qp/d_dense_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_d_aux.h>
+
+#include "../include/hpipm_d_dense_qp.h"
+#include "../include/hpipm_d_dense_qp_sol.h"
+
+
+
+#define CREATE_STRVEC d_create_strvec
+#define CVT_STRVEC2VEC d_cvt_strvec2vec
+#define DENSE_QP d_dense_qp
+#define DENSE_QP_SOL d_dense_qp_sol
+#define REAL double
+#define STRVEC d_strvec
+#define SIZE_STRVEC d_size_strvec
+#define VECCP_LIBSTR dveccp_libstr
+
+#define CREATE_DENSE_QP_SOL d_create_dense_qp_sol
+#define MEMSIZE_DENSE_QP_SOL d_memsize_dense_qp_sol
+#define CVT_DENSE_QP_SOL_TO_COLMAJ d_cvt_dense_qp_sol_to_colmaj
+#define CVT_DENSE_QP_SOL_TO_ROWMAJ d_cvt_dense_qp_sol_to_rowmaj
+#define CVT_DENSE_QP_SOL_TO_LIBSTR d_cvt_dense_qp_sol_to_libstr
+
+
+
+#include "x_dense_qp_sol.c"
+
diff --git a/dense_qp/s_dense_qp.c b/dense_qp/s_dense_qp.c
new file mode 100644
index 0000000..dfd2329
--- /dev/null
+++ b/dense_qp/s_dense_qp.c
@@ -0,0 +1,79 @@
+/**************************************************************************************************
+* *
+* 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_dense_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_TRAN_STRMAT2MAT s_cvt_tran_strmat2mat
+#define CVT_VEC2STRVEC s_cvt_vec2strvec
+#define CVT_STRMAT2MAT s_cvt_strmat2mat
+#define CVT_STRVEC2VEC s_cvt_strvec2vec
+#define DENSE_QP_DIM s_dense_qp_dim
+#define DENSE_QP_VEC s_dense_qp_vec
+#define DENSE_QP_MAT s_dense_qp_mat
+#define DENSE_QP s_dense_qp
+#define GECP_LIBSTR sgecp_libstr
+#define GETR_LIBSTR sgetr_libstr
+#define REAL float
+#define ROWIN_LIBSTR srowin_libstr
+#define SIZE_STRMAT s_size_strmat
+#define SIZE_STRVEC s_size_strvec
+#define STRMAT s_strmat
+#define STRVEC s_strvec
+#define VECCP_LIBSTR sveccp_libstr
+
+#define MEMSIZE_DENSE_QP s_memsize_dense_qp
+#define CREATE_DENSE_QP s_create_dense_qp
+#define CVT_COLMAJ_TO_DENSE_QP s_cvt_colmaj_to_dense_qp
+#define CVT_DENSE_QP_TO_COLMAJ s_cvt_dense_qp_to_colmaj
+#define CVT_ROWMAJ_TO_DENSE_QP s_cvt_rowmaj_to_dense_qp
+#define CVT_DENSE_QP_TO_ROWMAJ s_cvt_dense_qp_to_rowmaj
+#define CVT_LIBSTR_TO_DENSE_QP s_cvt_libstr_to_dense_qp
+#define CVT_DENSE_QP_TO_LIBSTR s_cvt_dense_qp_to_libstr
+#define CAST_DENSE_QP_DIM s_cast_dense_qp_dim
+//#define CREATE_DENSE_QP s_create_dense_qp
+//#define COPY_DENSE_QP s_copy_dense_qp
+
+
+
+#include "x_dense_qp.c"
+
diff --git a/dense_qp/s_dense_qp_ipm_hard.c b/dense_qp/s_dense_qp_ipm_hard.c
new file mode 100644
index 0000000..e415f5b
--- /dev/null
+++ b/dense_qp/s_dense_qp_ipm_hard.c
@@ -0,0 +1,77 @@
+/**************************************************************************************************
+* *
+* 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_dense_qp.h"
+#include "../include/hpipm_s_dense_qp_sol.h"
+#include "../include/hpipm_s_dense_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_dense_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_DENSE_QP s_compute_res_hard_dense_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 DENSE_QP s_dense_qp
+#define DENSE_QP_SOL s_dense_qp_sol
+#define FACT_SOLVE_KKT_STEP_HARD_DENSE_QP s_fact_solve_kkt_step_hard_dense_qp
+#define FACT_SOLVE_KKT_UNCONSTR_DENSE_QP s_fact_solve_kkt_unconstr_dense_qp
+#define INIT_VAR_HARD_DENSE_QP s_init_var_hard_dense_qp
+#define IPM_HARD_CORE_QP_WORKSPACE s_ipm_hard_core_qp_workspace
+#define IPM_HARD_DENSE_QP_ARG s_ipm_hard_dense_qp_arg
+#define IPM_HARD_DENSE_QP_WORKSPACE s_ipm_hard_dense_qp_workspace
+#define MEMSIZE_IPM_HARD_CORE_QP s_memsize_ipm_hard_core_qp
+#define REAL float
+#define SIZE_STRMAT s_size_strmat
+#define SIZE_STRVEC s_size_strvec
+#define SOLVE_KKT_STEP_HARD_DENSE_QP s_solve_kkt_step_hard_dense_qp
+#define STRMAT s_strmat
+#define STRVEC s_strvec
+#define UPDATE_VAR_HARD_QP s_update_var_hard_qp
+
+
+
+#define MEMSIZE_IPM_HARD_DENSE_QP s_memsize_ipm_hard_dense_qp
+#define CREATE_IPM_HARD_DENSE_QP s_create_ipm_hard_dense_qp
+#define SOLVE_IPM_HARD_DENSE_QP s_solve_ipm_hard_dense_qp
+#define SOLVE_IPM2_HARD_DENSE_QP s_solve_ipm2_hard_dense_qp
+
+
+
+#include "x_dense_qp_ipm_hard.c"
+
diff --git a/dense_qp/s_dense_qp_kkt.c b/dense_qp/s_dense_qp_kkt.c
new file mode 100644
index 0000000..f57b8ff
--- /dev/null
+++ b/dense_qp/s_dense_qp_kkt.c
@@ -0,0 +1,89 @@
+/**************************************************************************************************
+* *
+* This file is part of HPIPM. *
+* *
+* HPIPM -- High Performance Interior Point Method. *
+* Copyright (C) 2017 by Gianluca Frison. *
+* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. *
+* All rights reserved. *
+* *
+* HPMPC is free software; you can redistribute it and/or *
+* modify it under the terms of the GNU Lesser General Public *
+* License as published by the Free Software Foundation; either *
+* version 2.1 of the License, or (at your option) any later version. *
+* *
+* HPMPC is distributed in the hope that it will be useful, *
+* but WITHOUT ANY WARRANTY; without even the implied warranty of *
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. *
+* See the GNU Lesser General Public License for more details. *
+* *
+* You should have received a copy of the GNU Lesser General Public *
+* License along with HPMPC; if not, write to the Free Software *
+* Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA *
+* *
+* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de *
+* *
+**************************************************************************************************/
+
+
+
+#include <math.h>
+
+#include <blasfeo_target.h>
+#include <blasfeo_common.h>
+#include <blasfeo_s_aux.h>
+#include <blasfeo_s_blas.h>
+
+#include "../include/hpipm_s_dense_qp.h"
+#include "../include/hpipm_s_dense_qp_sol.h"
+#include "../include/hpipm_s_dense_qp_ipm_hard.h"
+#include "../include/hpipm_s_core_qp_ipm_hard.h"
+#include "../include/hpipm_s_core_qp_ipm_hard_aux.h"
+
+
+
+#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 DENSE_QP s_dense_qp
+#define DENSE_QP_SOL s_dense_qp_sol
+#define DIAAD_SP_LIBSTR sdiaad_sp_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 GESE_LIBSTR sgese_libstr
+#define IPM_HARD_CORE_QP_WORKSPACE s_ipm_hard_core_qp_workspace
+#define IPM_HARD_DENSE_QP_WORKSPACE s_ipm_hard_dense_qp_workspace
+#define POTRF_L_LIBSTR spotrf_l_libstr
+#define POTRF_L_MN_LIBSTR spotrf_l_mn_libstr
+#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 TRSM_RLTN_LIBSTR strsm_rltn_libstr
+#define TRSV_LNN_LIBSTR strsv_lnn_libstr
+#define TRSV_LTN_LIBSTR strsv_ltn_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_DENSE_QP s_init_var_hard_dense_qp
+#define COMPUTE_RES_HARD_DENSE_QP s_compute_res_hard_dense_qp
+#define FACT_SOLVE_KKT_UNCONSTR_DENSE_QP s_fact_solve_kkt_unconstr_dense_qp
+#define FACT_SOLVE_KKT_STEP_HARD_DENSE_QP s_fact_solve_kkt_step_hard_dense_qp
+#define SOLVE_KKT_STEP_HARD_DENSE_QP s_solve_kkt_step_hard_dense_qp
+
+
+
+#include "x_dense_qp_kkt.c"
+
diff --git a/dense_qp/s_dense_qp_sol.c b/dense_qp/s_dense_qp_sol.c
new file mode 100644
index 0000000..2a61ccc
--- /dev/null
+++ b/dense_qp/s_dense_qp_sol.c
@@ -0,0 +1,62 @@
+/**************************************************************************************************
+* *
+* 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_dense_qp.h"
+#include "../include/hpipm_s_dense_qp_sol.h"
+
+
+
+#define CREATE_STRVEC s_create_strvec
+#define CVT_STRVEC2VEC s_cvt_strvec2vec
+#define DENSE_QP s_dense_qp
+#define DENSE_QP_SOL s_dense_qp_sol
+#define REAL float
+#define STRVEC s_strvec
+#define SIZE_STRVEC s_size_strvec
+#define VECCP_LIBSTR sveccp_libstr
+
+#define CREATE_DENSE_QP_SOL s_create_dense_qp_sol
+#define MEMSIZE_DENSE_QP_SOL s_memsize_dense_qp_sol
+#define CVT_DENSE_QP_SOL_TO_COLMAJ s_cvt_dense_qp_sol_to_colmaj
+#define CVT_DENSE_QP_SOL_TO_ROWMAJ s_cvt_dense_qp_sol_to_rowmaj
+#define CVT_DENSE_QP_SOL_TO_LIBSTR s_cvt_dense_qp_sol_to_libstr
+
+
+
+#include "x_dense_qp_sol.c"
+
+
diff --git a/dense_qp/x_dense_qp.c b/dense_qp/x_dense_qp.c
new file mode 100644
index 0000000..34ad1bb
--- /dev/null
+++ b/dense_qp/x_dense_qp.c
@@ -0,0 +1,363 @@
+/**************************************************************************************************
+* *
+* 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_DENSE_QP(int nv, int ne, int nb, int ng)
+ {
+
+ int size = 0;
+
+ size += 7*sizeof(struct STRVEC); // g b d d_lb d_ub d_lg d_ug
+ size += 3*sizeof(struct STRMAT); // Hg A Ct
+
+ size += 1*SIZE_STRVEC(nv); // g
+ size += 1*SIZE_STRVEC(ne); // b
+ size += 1*SIZE_STRVEC(2*nb+2*ng); // d
+ size += 1*nb*sizeof(int); // idxb
+
+ size += 1*SIZE_STRMAT(nv+1, nv); // Hg
+ size += 1*SIZE_STRMAT(ne, nv); // A
+ size += 1*SIZE_STRMAT(nv, ng); // Ct
+
+ 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_DENSE_QP(int nv, int ne, int nb, int ng, struct DENSE_QP *qp, void *memory)
+ {
+
+ qp->memsize = MEMSIZE_DENSE_QP(nv, ne, nb, ng);
+
+
+ // problem size
+ qp->nv = nv;
+ qp->ne = ne;
+ qp->nb = nb;
+ qp->ng = ng;
+
+
+ // matrix struct stuff
+ struct STRMAT *sm_ptr = (struct STRMAT *) memory;
+
+ qp->Hg = sm_ptr;
+ sm_ptr += 1;
+
+ qp->A = sm_ptr;
+ sm_ptr += 1;
+
+ qp->Ct = sm_ptr;
+ sm_ptr += 1;
+
+
+ // vector struct stuff
+ struct STRVEC *sv_ptr = (struct STRVEC *) sm_ptr;
+
+ qp->g = sv_ptr;
+ sv_ptr += 1;
+
+ qp->b = sv_ptr;
+ sv_ptr += 1;
+
+ qp->d = sv_ptr;
+ sv_ptr += 1;
+
+ qp->d_lb = sv_ptr;
+ sv_ptr += 1;
+
+ qp->d_ub = sv_ptr;
+ sv_ptr += 1;
+
+ qp->d_lg = sv_ptr;
+ sv_ptr += 1;
+
+ qp->d_ug = sv_ptr;
+ sv_ptr += 1;
+
+
+ // int stuff
+ int *i_ptr;
+ i_ptr = (int *) sv_ptr;
+
+ // idxb
+ qp->idxb = i_ptr;
+ i_ptr += nb;
+
+
+ // align to typical cache line size
+ size_t s_ptr = (size_t) i_ptr;
+ s_ptr = (s_ptr+63)/64*64;
+
+
+ // stuff
+ char *c_ptr;
+ c_ptr = (char *) s_ptr;
+
+ CREATE_STRMAT(nv+1, nv, qp->Hg, c_ptr);
+ c_ptr += qp->Hg->memory_size;
+
+ CREATE_STRMAT(ne, nv, qp->A, c_ptr);
+ c_ptr += qp->A->memory_size;
+
+ CREATE_STRMAT(nv, ng, qp->Ct, c_ptr);
+ c_ptr += qp->Ct->memory_size;
+
+ CREATE_STRVEC(nv, qp->g, c_ptr);
+ c_ptr += qp->g->memory_size;
+
+ CREATE_STRVEC(ne, qp->b, c_ptr);
+ c_ptr += qp->b->memory_size;
+
+ CREATE_STRVEC(2*nb+2*ng, qp->d, c_ptr);
+ CREATE_STRVEC(nb, qp->d_lb, c_ptr+0*sizeof(REAL));
+ CREATE_STRVEC(ng, qp->d_lg, c_ptr+(nb)*sizeof(REAL));
+ CREATE_STRVEC(nb, qp->d_ub, c_ptr+(nb+ng)*sizeof(REAL));
+ CREATE_STRVEC(ng, qp->d_ug, c_ptr+(2*nb+ng)*sizeof(REAL));
+ c_ptr += qp->d->memory_size;
+
+ return;
+
+ }
+
+
+
+void CVT_COLMAJ_TO_DENSE_QP(REAL *H, REAL *g, REAL *A, REAL *b, int *idxb, REAL *d_lb, REAL *d_ub, REAL *C, REAL *d_lg, REAL *d_ug, struct DENSE_QP *qp)
+ {
+
+ int ii;
+
+ int nv = qp->nv;
+ int ne = qp->ne;
+ int nb = qp->nb;
+ int ng = qp->ng;
+
+ CVT_MAT2STRMAT(nv, nv, H, nv, qp->Hg, 0, 0);
+ CVT_TRAN_MAT2STRMAT(nv, 1, g, nv, qp->Hg, nv, 0);
+ CVT_MAT2STRMAT(ne, nv, A, ne, qp->A, 0, 0);
+ CVT_TRAN_MAT2STRMAT(ng, nv, C, ng, qp->Ct, 0, 0);
+ CVT_VEC2STRVEC(nv, g, qp->g, 0);
+ CVT_VEC2STRVEC(ne, b, qp->b, 0);
+ CVT_VEC2STRVEC(nb, d_lb, qp->d_lb, 0);
+ CVT_VEC2STRVEC(nb, d_ub, qp->d_ub, 0);
+ CVT_VEC2STRVEC(ng, d_lg, qp->d_lg, 0);
+ CVT_VEC2STRVEC(ng, d_ug, qp->d_ug, 0);
+ for(ii=0; ii<nb; ii++) qp->idxb[ii] = idxb[ii];
+
+ return;
+
+ }
+
+
+
+void CVT_DENSE_QP_TO_COLMAJ(struct DENSE_QP *qp, REAL *H, REAL *g, REAL *A, REAL *b, int *idxb, REAL *d_lb, REAL *d_ub, REAL *C, REAL *d_lg, REAL *d_ug)
+ {
+
+ int ii;
+
+ int nv = qp->nv;
+ int ne = qp->ne;
+ int nb = qp->nb;
+ int ng = qp->ng;
+
+ CVT_STRMAT2MAT(nv, nv, qp->Hg, 0, 0, H, nv);
+ CVT_STRMAT2MAT(ne, nv, qp->A, 0, 0, A, ne);
+ CVT_TRAN_STRMAT2MAT(nv, ng, qp->Ct, 0, 0, C, ng);
+ CVT_STRVEC2VEC(nv, qp->g, 0, g);
+ CVT_STRVEC2VEC(ne, qp->b, 0, b);
+ CVT_STRVEC2VEC(nb, qp->d_lb, 0, d_lb);
+ CVT_STRVEC2VEC(nb, qp->d_ub, 0, d_ub);
+ CVT_STRVEC2VEC(ng, qp->d_lg, 0, d_lg);
+ CVT_STRVEC2VEC(ng, qp->d_ug, 0, d_ug);
+ for(ii=0; ii<nb; ii++) idxb[ii] = qp->idxb[ii];
+
+ return;
+
+ }
+
+
+
+void CVT_ROWMAJ_TO_DENSE_QP(REAL *H, REAL *g, REAL *A, REAL *b, int *idxb, REAL *d_lb, REAL *d_ub, REAL *C, REAL *d_lg, REAL *d_ug, struct DENSE_QP *qp)
+ {
+
+ int ii;
+
+ int nv = qp->nv;
+ int ne = qp->ne;
+ int nb = qp->nb;
+ int ng = qp->ng;
+
+ CVT_TRAN_MAT2STRMAT(nv, nv, H, nv, qp->Hg, 0, 0);
+ CVT_TRAN_MAT2STRMAT(nv, 1, g, nv, qp->Hg, nv, 0);
+ CVT_TRAN_MAT2STRMAT(nv, ne, A, nv, qp->A, 0, 0);
+ CVT_MAT2STRMAT(nv, ng, C, nv, qp->Ct, 0, 0);
+ CVT_VEC2STRVEC(nv, g, qp->g, 0);
+ CVT_VEC2STRVEC(ne, b, qp->b, 0);
+ CVT_VEC2STRVEC(nb, d_lb, qp->d_lb, 0);
+ CVT_VEC2STRVEC(nb, d_ub, qp->d_ub, 0);
+ CVT_VEC2STRVEC(ng, d_lg, qp->d_lg, 0);
+ CVT_VEC2STRVEC(ng, d_ug, qp->d_ug, 0);
+ for(ii=0; ii<nb; ii++) qp->idxb[ii] = idxb[ii];
+
+ return;
+
+ }
+
+
+
+void CVT_DENSE_QP_TO_ROWMAJ(struct DENSE_QP *qp, REAL *H, REAL *g, REAL *A, REAL *b, int *idxb, REAL *d_lb, REAL *d_ub, REAL *C, REAL *d_lg, REAL *d_ug)
+ {
+
+ int ii;
+
+ int nv = qp->nv;
+ int ne = qp->ne;
+ int nb = qp->nb;
+ int ng = qp->ng;
+
+ CVT_TRAN_STRMAT2MAT(nv, nv, qp->Hg, 0, 0, H, nv);
+ CVT_TRAN_STRMAT2MAT(ne, nv, qp->A, 0, 0, A, nv);
+ CVT_STRMAT2MAT(nv, ng, qp->Ct, 0, 0, C, nv);
+ CVT_STRVEC2VEC(nv, qp->g, 0, g);
+ CVT_STRVEC2VEC(ne, qp->b, 0, b);
+ CVT_STRVEC2VEC(nb, qp->d_lb, 0, d_lb);
+ CVT_STRVEC2VEC(nb, qp->d_ub, 0, d_ub);
+ CVT_STRVEC2VEC(ng, qp->d_lg, 0, d_lg);
+ CVT_STRVEC2VEC(ng, qp->d_ug, 0, d_ug);
+ for(ii=0; ii<nb; ii++) idxb[ii] = qp->idxb[ii];
+
+ return;
+
+ }
+
+
+
+void CVT_LIBSTR_TO_DENSE_QP(struct STRMAT *H, struct STRMAT *A, struct STRMAT *C, struct STRVEC *g, struct STRVEC *b, struct STRVEC *d_lb, struct STRVEC *d_ub, struct STRVEC *d_lg, struct STRVEC *d_ug, int *idxb, struct DENSE_QP *qp)
+ {
+
+ int ii;
+
+ int nv = qp->nv;
+ int ne = qp->ne;
+ int nb = qp->nb;
+ int ng = qp->ng;
+
+ GECP_LIBSTR(nv, nv, H, 0, 0, qp->Hg, 0, 0);
+ ROWIN_LIBSTR(nv, 1.0, g, 0, qp->Hg, nv, 0);
+ GECP_LIBSTR(ne, nv, A, 0, 0, qp->A, 0, 0);
+ GETR_LIBSTR(ng, nv, C, 0, 0, qp->Ct, 0, 0);
+ VECCP_LIBSTR(nv, g, 0, qp->g, 0);
+ VECCP_LIBSTR(ne, b, 0, qp->b, 0);
+ VECCP_LIBSTR(nb, d_lb, 0, qp->d_lb, 0);
+ VECCP_LIBSTR(nb, d_ub, 0, qp->d_ub, 0);
+ VECCP_LIBSTR(ng, d_lg, 0, qp->d_lg, 0);
+ VECCP_LIBSTR(ng, d_ug, 0, qp->d_ug, 0);
+ for(ii=0; ii<nb; ii++) qp->idxb[ii] = idxb[ii];
+
+ return;
+
+ }
+
+
+
+void CVT_DENSE_QP_TO_LIBSTR(struct DENSE_QP *qp, struct STRMAT *H, struct STRMAT *A, struct STRMAT *C, struct STRVEC *g, struct STRVEC *b, struct STRVEC *d_lb, struct STRVEC *d_ub, struct STRVEC *d_lg, struct STRVEC *d_ug, int *idxb)
+ {
+
+ int ii;
+
+ int nv = qp->nv;
+ int ne = qp->ne;
+ int nb = qp->nb;
+ int ng = qp->ng;
+
+ GECP_LIBSTR(nv, nv, qp->Hg, 0, 0, H, 0, 0);
+ GECP_LIBSTR(ne, nv, qp->A, 0, 0, A, 0, 0);
+ GETR_LIBSTR(nv, ng, qp->Ct, 0, 0, C, 0, 0);
+ VECCP_LIBSTR(nv, qp->g, 0, g, 0);
+ VECCP_LIBSTR(ne, qp->b, 0, b, 0);
+ VECCP_LIBSTR(nb, qp->d_lb, 0, d_lb, 0);
+ VECCP_LIBSTR(nb, qp->d_ub, 0, d_ub, 0);
+ VECCP_LIBSTR(ng, qp->d_lg, 0, d_lg, 0);
+ VECCP_LIBSTR(ng, qp->d_ug, 0, d_ug, 0);
+ for(ii=0; ii<nb; ii++) idxb[ii] = qp->idxb[ii];
+
+ return;
+
+ }
+
+
+
+#if 0
+void COPY_DENSE_QP(struct DENSE_QP *str_in, struct DENSE_QP *str_out)
+ {
+
+ int ii;
+
+#if defined(RUNTIME_CHECKS)
+ if(str_out->nv != str_in->nv)
+ {
+ printf("\nError : d_copy_dense_qp : str_out->nv != str_out->nv : %d != %d\n\n", str_out->nv, str_in->nv);
+ exit(1);
+ }
+ if(str_out->ne != str_in->ne)
+ {
+ printf("\nError : d_copy_dense_qp : str_out->ne != str_out->ne : %d != %d\n\n", str_out->ne, str_in->ne);
+ exit(1);
+ }
+ if(str_out->nb != str_in->nb)
+ {
+ printf("\nError : d_copy_dense_qp : str_out->nb != str_out->nb : %d != %d\n\n", str_out->nb, str_in->nb);
+ exit(1);
+ }
+ if(str_out->ng != str_in->ng)
+ {
+ printf("\nError : d_copy_dense_qp : str_out->ng != str_out->ng : %d != %d\n\n", str_out->ng, str_in->ng);
+ exit(1);
+ }
+#endif
+
+ for(ii=0; ii<str_in->nb; ii++) str_out->idxb[ii] = str_in->idxb[ii];
+ GECP_LIBSTR(str_in->nv, str_in->nv, &(str_out->sQ), 0, 0, &(str_in->sQ), 0, 0);
+ VECCP_LIBSTR(str_in->nv, &(str_out->sq), 0, &(str_in->sq), 0);
+ GECP_LIBSTR(str_in->ne, str_in->nv, &(str_out->sA), 0, 0, &(str_in->sA), 0, 0);
+ VECCP_LIBSTR(str_in->ne, &(str_out->sb), 0, &(str_in->sb), 0);
+ GECP_LIBSTR(str_in->nv, str_in->ng, &(str_out->sCt), 0, 0, &(str_in->sCt), 0, 0);
+ VECCP_LIBSTR(str_in->nb, &(str_out->slb), 0, &(str_in->slb), 0);
+ VECCP_LIBSTR(str_in->nb, &(str_out->sub), 0, &(str_in->sub), 0);
+ VECCP_LIBSTR(str_in->ng, &(str_out->slg), 0, &(str_in->slg), 0);
+ VECCP_LIBSTR(str_in->ng, &(str_out->sug), 0, &(str_in->sug), 0);
+
+ return;
+
+ }
+#endif
+
+
diff --git a/dense_qp/x_dense_qp_ipm_hard.c b/dense_qp/x_dense_qp_ipm_hard.c
new file mode 100644
index 0000000..beae7a3
--- /dev/null
+++ b/dense_qp/x_dense_qp_ipm_hard.c
@@ -0,0 +1,377 @@
+/**************************************************************************************************
+* *
+* 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_DENSE_QP(struct DENSE_QP *qp, struct IPM_HARD_DENSE_QP_ARG *arg)
+ {
+
+ int nv = qp->nv;
+ int ne = qp->ne;
+ int nb = qp->nb;
+ int ng = qp->ng;
+
+ int size = 0;
+
+ size += 22*sizeof(struct STRVEC); // dv dpi dlam dt dt_lb dt_ub dt_lg dt_ug res_g res_b res_d res_d_lb res_d_ub res_d_lg res_d_ug res_m Qx qx lv tmp_nb tmp_ng0 tmp_ng1
+ size += 4*sizeof(struct STRMAT); // Lv AL Le Ctx
+
+ size += 1*SIZE_STRVEC(nb); // tmp_nb
+ size += 2*SIZE_STRVEC(ng); // tmp_ng0 tmp_ng1
+ size += 1*SIZE_STRVEC(nv); // lv
+ size += 1*SIZE_STRMAT(nv+1, nv); // Lv
+ size += 1*SIZE_STRMAT(ne, nv); // AL
+ size += 1*SIZE_STRMAT(ne, ne); // Le
+ size += 1*SIZE_STRMAT(nv+1, ng); // Ctx
+
+ size += 1*sizeof(struct IPM_HARD_CORE_QP_WORKSPACE);
+ size += 1*MEMSIZE_IPM_HARD_CORE_QP(qp->nv, qp->ne, qp->nb, qp->ng, 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_DENSE_QP(struct DENSE_QP *qp, struct IPM_HARD_DENSE_QP_ARG *arg, struct IPM_HARD_DENSE_QP_WORKSPACE *workspace, void *mem)
+ {
+
+ workspace->memsize = MEMSIZE_IPM_HARD_DENSE_QP(qp, arg);
+
+
+ int nv = qp->nv;
+ int ne = qp->ne;
+ int nb = qp->nb;
+ int ng = qp->ng;
+
+
+ // 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->Lv = sm_ptr;
+ sm_ptr += 1;
+ workspace->AL = sm_ptr;
+ sm_ptr += 1;
+ workspace->Le = sm_ptr;
+ sm_ptr += 1;
+ workspace->Ctx = sm_ptr;
+ sm_ptr += 1;
+
+
+ // vector struct
+ struct STRVEC *sv_ptr = (struct STRVEC *) sm_ptr;
+
+ workspace->dv = sv_ptr;
+ sv_ptr += 1;
+ workspace->dpi = sv_ptr;
+ sv_ptr += 1;
+ workspace->dlam = sv_ptr;
+ sv_ptr += 1;
+ workspace->dt = sv_ptr;
+ sv_ptr += 1;
+ workspace->dt_lb = sv_ptr;
+ sv_ptr += 1;
+ workspace->dt_ub = sv_ptr;
+ sv_ptr += 1;
+ workspace->dt_lg = sv_ptr;
+ sv_ptr += 1;
+ workspace->dt_ug = sv_ptr;
+ sv_ptr += 1;
+ workspace->res_g = sv_ptr;
+ sv_ptr += 1;
+ workspace->res_b = sv_ptr;
+ sv_ptr += 1;
+ workspace->res_d = sv_ptr;
+ sv_ptr += 1;
+ workspace->res_d_lb = sv_ptr;
+ sv_ptr += 1;
+ workspace->res_d_ub = sv_ptr;
+ sv_ptr += 1;
+ workspace->res_d_lg = sv_ptr;
+ sv_ptr += 1;
+ workspace->res_d_ug = sv_ptr;
+ sv_ptr += 1;
+ workspace->res_m = sv_ptr;
+ sv_ptr += 1;
+ workspace->Qx = sv_ptr;
+ sv_ptr += 1;
+ workspace->qx = sv_ptr;
+ sv_ptr += 1;
+ workspace->lv = sv_ptr;
+ sv_ptr += 1;
+ workspace->tmp_nb = sv_ptr;
+ sv_ptr += 1;
+ workspace->tmp_ng0 = sv_ptr;
+ sv_ptr += 1;
+ workspace->tmp_ng1 = sv_ptr;
+ sv_ptr += 1;
+
+
+ // 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;
+
+ CREATE_STRMAT(nv+1, nv, workspace->Lv, v_ptr);
+ v_ptr += workspace->Lv->memory_size;
+
+ CREATE_STRMAT(ne, nv, workspace->AL, v_ptr);
+ v_ptr += workspace->AL->memory_size;
+
+ CREATE_STRMAT(ne, ne, workspace->Le, v_ptr);
+ v_ptr += workspace->Le->memory_size;
+
+ CREATE_STRMAT(nv+1, ng, workspace->Ctx, v_ptr);
+ v_ptr += workspace->Ctx->memory_size;
+
+ CREATE_STRVEC(nv, workspace->lv, v_ptr);
+ v_ptr += workspace->lv->memory_size;
+
+ CREATE_STRVEC(nb, workspace->tmp_nb, v_ptr);
+ v_ptr += workspace->tmp_nb->memory_size;
+
+ CREATE_STRVEC(ng, workspace->tmp_ng0, v_ptr);
+ v_ptr += workspace->tmp_ng0->memory_size;
+
+ CREATE_STRVEC(ng, workspace->tmp_ng1, v_ptr);
+ v_ptr += workspace->tmp_ng1->memory_size;
+
+ rwork->nv = nv;
+ rwork->ne = ne;
+ rwork->nb = nb;
+ rwork->ng = ng;
+ 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*nb+2*ng);
+
+
+ // alias members of workspace and core_workspace
+ CREATE_STRVEC(nv, workspace->dv, rwork->dv);
+ CREATE_STRVEC(ne, workspace->dpi, rwork->dpi);
+ CREATE_STRVEC(2*nb+2*ng, workspace->dlam, rwork->dlam);
+ CREATE_STRVEC(2*nb+2*ng, workspace->dt, rwork->dt);
+ CREATE_STRVEC(nb, workspace->dt_lb, rwork->dt_lb);
+ CREATE_STRVEC(nb, workspace->dt_ub, rwork->dt_ub);
+ CREATE_STRVEC(ng, workspace->dt_lg, rwork->dt_lg);
+ CREATE_STRVEC(ng, workspace->dt_ug, rwork->dt_ug);
+ CREATE_STRVEC(nv, workspace->res_g, rwork->res_g);
+ CREATE_STRVEC(ne, workspace->res_b, rwork->res_b);
+ CREATE_STRVEC(2*nb+2*ng, workspace->res_d, rwork->res_d);
+ CREATE_STRVEC(nb, workspace->res_d_lb, rwork->res_d_lb);
+ CREATE_STRVEC(nb, workspace->res_d_ub, rwork->res_d_ub);
+ CREATE_STRVEC(ng, workspace->res_d_lg, rwork->res_d_lg);
+ CREATE_STRVEC(ng, workspace->res_d_ug, rwork->res_d_ug);
+ CREATE_STRVEC(2*nb+2*ng, workspace->res_m, rwork->res_m);
+ CREATE_STRVEC(nb+ng, workspace->Qx, rwork->Qx);
+ CREATE_STRVEC(nb+ng, workspace->qx, rwork->qx);
+ workspace->stat = rwork->stat;
+
+ return;
+
+ }
+
+
+
+void SOLVE_IPM_HARD_DENSE_QP(struct DENSE_QP *qp, struct DENSE_QP_SOL *qp_sol, struct IPM_HARD_DENSE_QP_WORKSPACE *ws)
+ {
+
+ struct IPM_HARD_CORE_QP_WORKSPACE *cws = ws->core_workspace;
+
+ // alias qp vectors into qp
+ cws->d = qp->d->pa; // TODO REMOVE
+ 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->v->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_DENSE_QP(qp, qp_sol, ws);
+ COMPUTE_RES_HARD_DENSE_QP(qp, qp_sol, ws);
+ cws->mu = ws->res_mu;
+ ws->iter = 0;
+ return;
+ }
+
+ // init solver
+ INIT_VAR_HARD_DENSE_QP(qp, qp_sol, ws);
+
+ // compute residuals
+ COMPUTE_RES_HARD_DENSE_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_DENSE_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_DENSE_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_DENSE_QP(struct DENSE_QP *qp, struct DENSE_QP_SOL *qp_sol, struct IPM_HARD_DENSE_QP_WORKSPACE *ws)
+ {
+
+ struct IPM_HARD_CORE_QP_WORKSPACE *cws = ws->core_workspace;
+
+ // alias qp vectors into qp
+ cws->d = qp->d->pa; // TODO REMOVE
+ 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->v->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_DENSE_QP(qp, qp_sol, ws);
+ COMPUTE_RES_HARD_DENSE_QP(qp, qp_sol, ws);
+ cws->mu = ws->res_mu;
+ ws->iter = 0;
+ return;
+ }
+
+ // init solver
+ INIT_VAR_HARD_DENSE_QP(qp, qp_sol, ws);
+
+ // compute residuals
+ COMPUTE_RES_HARD_DENSE_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_DENSE_QP(qp, ws);
+
+ // 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_DENSE_QP(qp, ws);
+
+ // alpha
+ COMPUTE_ALPHA_HARD_QP(cws);
+ cws->stat[5*kk+3] = cws->alpha;
+
+ //
+ UPDATE_VAR_HARD_QP(cws);
+
+ // compute residuals
+ COMPUTE_RES_HARD_DENSE_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/dense_qp/x_dense_qp_kkt.c b/dense_qp/x_dense_qp_kkt.c
new file mode 100644
index 0000000..994532d
--- /dev/null
+++ b/dense_qp/x_dense_qp_kkt.c
@@ -0,0 +1,523 @@
+/**************************************************************************************************
+* *
+* 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_DENSE_QP(struct DENSE_QP *qp, struct DENSE_QP_SOL *qp_sol, struct IPM_HARD_DENSE_QP_WORKSPACE *ws)
+ {
+
+ struct IPM_HARD_CORE_QP_WORKSPACE *rws = ws->core_workspace;
+
+ // extract rws members
+ int nv = qp->nv;
+ int ne = qp->ne;
+ int nb = qp->nb;
+ int ng = qp->ng;
+
+ REAL *d_lb = qp->d_lb->pa;
+ REAL *d_ub = qp->d_ub->pa;
+ REAL *d_lg = qp->d_lg->pa;
+ REAL *d_ug = qp->d_ug->pa;
+ int *idxb = qp->idxb;
+
+ REAL *v = qp_sol->v->pa;
+ REAL *pi = qp_sol->pi->pa;
+ REAL *lam_lb = qp_sol->lam_lb->pa;
+ REAL *lam_ub = qp_sol->lam_ub->pa;
+ REAL *lam_lg = qp_sol->lam_lg->pa;
+ REAL *lam_ug = qp_sol->lam_ug->pa;
+ REAL *t_lb = qp_sol->t_lb->pa;
+ REAL *t_ub = qp_sol->t_ub->pa;
+ REAL *t_lg = qp_sol->t_lg->pa;
+ REAL *t_ug = qp_sol->t_ug->pa;
+
+ REAL mu0 = rws->mu0;
+
+ // local variables
+ int ii;
+ int idxb0;
+ REAL thr0 = 0.1;
+
+ // warm start TODO
+
+
+ // cold start
+
+ // primal variables
+ for(ii=0; ii<nv; ii++)
+ {
+ v[ii] = 0.0;
+ }
+
+ // equality constraints
+ for(ii=0; ii<ne; ii++)
+ {
+ pi[ii] = 0.0;
+ }
+
+ // box constraints
+ for(ii=0; ii<nb; ii++)
+ {
+ idxb0 = idxb[ii];
+ t_lb[ii] = - d_lb[ii] + v[idxb0];
+ t_ub[ii] = d_ub[ii] - v[idxb0];
+ if(t_lb[ii]<thr0)
+ {
+ if(t_ub[ii]<thr0)
+ {
+ v[idxb0] = 0.5*(d_lb[ii] - d_ub[ii]);
+ t_lb[ii] = thr0;
+ t_ub[ii] = thr0;
+ }
+ else
+ {
+ t_lb[ii] = thr0;
+ v[idxb0] = d_lb[ii] + thr0;
+ }
+ }
+ else if(t_ub[ii]<thr0)
+ {
+ t_ub[ii] = thr0;
+ v[idxb0] = d_ub[ii] - thr0;
+ }
+ lam_lb[ii] = mu0/t_lb[ii];
+ lam_ub[ii] = mu0/t_ub[ii];
+ }
+
+ // inequality constraints
+ GEMV_T_LIBSTR(nv, ng, 1.0, qp->Ct, 0, 0, qp_sol->v, 0, 0.0, qp_sol->t_lg, 0, qp_sol->t_lg, 0);
+ for(ii=0; ii<ng; ii++)
+ {
+ t_ug[ii] = t_lg[ii];
+ t_lg[ii] -= d_lg[ii];
+ t_ug[ii] += d_ug[ii];
+ t_lg[ii] = fmax( thr0, t_lg[ii] );
+ t_ug[ii] = fmax( thr0, t_ug[ii] );
+// t_lg[ii] = thr0>t_lg[ii] ? thr0 : t_lg[ii];
+// t_ug[ii] = thr0>t_ug[ii] ? thr0 : t_ug[ii];
+ lam_lg[ii] = mu0/t_lg[ii];
+ lam_ug[ii] = mu0/t_ug[ii];
+ }
+
+ return;
+
+ }
+
+
+
+void COMPUTE_RES_HARD_DENSE_QP(struct DENSE_QP *qp, struct DENSE_QP_SOL *qp_sol, struct IPM_HARD_DENSE_QP_WORKSPACE *ws)
+ {
+
+ struct IPM_HARD_CORE_QP_WORKSPACE *cws = ws->core_workspace;
+
+ int nv = qp->nv;
+ int ne = qp->ne;
+ int nb = qp->nb;
+ int ng = qp->ng;
+
+ // TODO extract qp arguments !!!!!
+
+ struct STRVEC *v = qp_sol->v;
+ 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;
+
+ REAL mu;
+
+ // res g
+ SYMV_L_LIBSTR(nv, nv, 1.0, qp->Hg, 0, 0, v, 0, 1.0, qp->g, 0, ws->res_g, 0);
+
+ if(nb>0)
+ {
+ // res_g
+ AXPY_LIBSTR(nb, -1.0, lam_lb, 0, lam_ub, 0, ws->tmp_nb, 0);
+ VECAD_SP_LIBSTR(nb, 1.0, ws->tmp_nb, 0, qp->idxb, ws->res_g, 0);
+ // res_d
+ VECEX_SP_LIBSTR(nb, -1.0, qp->idxb, v, 0, ws->res_d_lb, 0);
+ VECCP_LIBSTR(nb, ws->res_d_lb, 0, ws->res_d_ub, 0);
+ AXPY_LIBSTR(nb, 1.0, qp->d_lb, 0, ws->res_d_lb, 0, ws->res_d_lb, 0);
+ AXPY_LIBSTR(nb, 1.0, qp->d_ub, 0, ws->res_d_ub, 0, ws->res_d_ub, 0);
+ AXPY_LIBSTR(nb, 1.0, t_lb, 0, ws->res_d_lb, 0, ws->res_d_lb, 0);
+ AXPY_LIBSTR(nb, -1.0, t_ub, 0, ws->res_d_ub, 0, ws->res_d_ub, 0);
+ }
+
+ if(ng>0)
+ {
+ AXPY_LIBSTR(ng, -1.0, lam_lg, 0, lam_ug, 0, ws->tmp_ng0, 0);
+ AXPY_LIBSTR(ng, 1.0, t_lg, 0, qp->d_lg, 0, ws->res_d_lg, 0);
+ AXPY_LIBSTR(ng, -1.0, t_ug, 0, qp->d_ug, 0, ws->res_d_ug, 0);
+ GEMV_NT_LIBSTR(nv, ng, 1.0, 1.0, qp->Ct, 0, 0, ws->tmp_ng0, 0, v, 0, 1.0, 0.0, ws->res_g, 0, ws->tmp_ng1, 0, ws->res_g, 0, ws->tmp_ng1, 0);
+ AXPY_LIBSTR(ng, -1.0, ws->tmp_ng1, 0, ws->res_d_lg, 0, ws->res_d_lg, 0);
+ AXPY_LIBSTR(ng, -1.0, ws->tmp_ng1, 0, ws->res_d_ug, 0, ws->res_d_ug, 0);
+ }
+
+ // res b, res g
+ GEMV_NT_LIBSTR(ne, nv, -1.0, -1.0, qp->A, 0, 0, v, 0, pi, 0, 1.0, 1.0, qp->b, 0, ws->res_g, 0, ws->res_b, 0, ws->res_g, 0);
+
+ // res_mu
+ mu = VECMULDOT_LIBSTR(2*nb+2*ng, 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;
+
+ }
+
+
+// range-space (Schur complement) method
+void FACT_SOLVE_KKT_UNCONSTR_DENSE_QP(struct DENSE_QP *qp, struct DENSE_QP_SOL *qp_sol, struct IPM_HARD_DENSE_QP_WORKSPACE *ws)
+ {
+
+ int nv = qp->nv;
+ int ne = qp->ne;
+ int nb = qp->nb;
+ int ng = qp->ng;
+ struct STRMAT *Hg = qp->Hg;
+ struct STRMAT *A = qp->A;
+ struct STRVEC *g = qp->g;
+ struct STRVEC *b = qp->b;
+
+ struct STRVEC *v = qp_sol->v;
+ struct STRVEC *pi = qp_sol->pi;
+
+ struct STRMAT *Lv = ws->Lv;
+ struct STRMAT *Le = ws->Le;
+ struct STRMAT *Ctx = ws->Ctx;
+ struct STRMAT *AL = ws->AL;
+ struct STRVEC *lv = ws->lv;
+
+ if(ne>0)
+ {
+ POTRF_L_LIBSTR(nv, Hg, 0, 0, Lv, 0, 0);
+
+ GECP_LIBSTR(ne, nv, A, 0, 0, AL, 0, 0);
+ TRSM_RLTN_LIBSTR(ne, nv, 1.0, Lv, 0, 0, A, 0, 0, AL, 0, 0);
+
+ GESE_LIBSTR(ne, ne, 0.0, Le, 0, 0);
+ SYRK_POTRF_LN_LIBSTR(ne, ne, nv, AL, 0, 0, AL, 0, 0, Le, 0, 0, Le, 0, 0);
+
+ TRSV_LNN_LIBSTR(nv, Lv, 0, 0, g, 0, lv, 0);
+
+ GEMV_N_LIBSTR(ne, nv, 1.0, AL, 0, 0, lv, 0, 1.0, b, 0, pi, 0);
+
+ TRSV_LNN_LIBSTR(ne, Le, 0, 0, pi, 0, pi, 0);
+ TRSV_LTN_LIBSTR(ne, Le, 0, 0, pi, 0, pi, 0);
+
+ GEMV_T_LIBSTR(ne, nv, 1.0, A, 0, 0, pi, 0, -1.0, g, 0, v, 0);
+
+ TRSV_LNN_LIBSTR(nv, Lv, 0, 0, v, 0, v, 0);
+ TRSV_LTN_LIBSTR(nv, Lv, 0, 0, v, 0, v, 0);
+ }
+ else
+ {
+#if 0
+ POTRF_L_LIBSTR(nv, Hg, 0, 0, Lv, 0, 0);
+
+ VECCP_LIBSTR(nv, g, 0, v, 0);
+ VECSC_LIBSTR(nv, -1.0, v, 0);
+
+ TRSV_LNN_LIBSTR(nv, Lv, 0, 0, v, 0, v, 0);
+ TRSV_LTN_LIBSTR(nv, Lv, 0, 0, v, 0, v, 0);
+#else
+ POTRF_L_MN_LIBSTR(nv+1, nv, Hg, 0, 0, Lv, 0, 0);
+
+ ROWEX_LIBSTR(nv, -1.0, Lv, nv, 0, v, 0);
+ TRSV_LTN_LIBSTR(nv, Lv, 0, 0, v, 0, v, 0);
+#endif
+ }
+
+ return;
+
+ }
+
+
+
+// range-space (Schur complement) method
+void FACT_SOLVE_KKT_STEP_HARD_DENSE_QP(struct DENSE_QP *qp, struct IPM_HARD_DENSE_QP_WORKSPACE *ws)
+ {
+
+ int nv = qp->nv;
+ int ne = qp->ne;
+ int nb = qp->nb;
+ int ng = qp->ng;
+ struct STRMAT *Hg = qp->Hg;
+ struct STRMAT *A = qp->A;
+ struct STRMAT *Ct = qp->Ct;
+ int *idxb = qp->idxb;
+
+ struct STRMAT *Lv = ws->Lv;
+ struct STRMAT *Le = ws->Le;
+ struct STRMAT *Ctx = ws->Ctx;
+ struct STRMAT *AL = ws->AL;
+ struct STRVEC *lv = ws->lv;
+ struct STRVEC *dv = ws->dv;
+ struct STRVEC *dpi = ws->dpi;
+ struct STRVEC *dt_lb = ws->dt_lb;
+ struct STRVEC *dt_lg = ws->dt_lg;
+ struct STRVEC *res_g = ws->res_g;
+ struct STRVEC *res_b = ws->res_b;
+ struct STRVEC *Qx = ws->Qx;
+ struct STRVEC *qx = ws->qx;
+
+ struct IPM_HARD_CORE_QP_WORKSPACE *rws = ws->core_workspace;
+
+ if(nb>0 | ng>0)
+ {
+ COMPUTE_QX_QX_HARD_QP(rws);
+ }
+
+ if(ne>0)
+ {
+// TRCP_L_LIBSTR(nv, Hg, 0, 0, Lv, 0, 0);
+ GECP_LIBSTR(nv, nv, Hg, 0, 0, Lv, 0, 0);
+
+ VECCP_LIBSTR(nv, res_g, 0, lv, 0);
+
+ if(nb>0)
+ {
+ DIAAD_SP_LIBSTR(nb, 1.0, Qx, 0, idxb, Lv, 0, 0);
+ VECAD_SP_LIBSTR(nb, 1.0, qx, 0, idxb, lv, 0);
+ }
+
+ if(ng>0)
+ {
+ GEMV_N_LIBSTR(nv, ng, 1.0, Ct, 0, 0, qx, nb, 1.0, lv, 0, lv, 0);
+ GEMM_R_DIAG_LIBSTR(nv, ng, 1.0, Ct, 0, 0, Qx, nb, 0.0, Ctx, 0, 0, Ctx, 0, 0);
+ SYRK_POTRF_LN_LIBSTR(nv, nv, ng, Ctx, 0, 0, Ct, 0, 0, Lv, 0, 0, Lv, 0, 0);
+ }
+ else
+ {
+ POTRF_L_LIBSTR(nv, Lv, 0, 0, Lv, 0, 0);
+ }
+
+ VECCP_LIBSTR(nv, lv, 0, dv, 0);
+
+ GECP_LIBSTR(ne, nv, A, 0, 0, AL, 0, 0);
+ TRSM_RLTN_LIBSTR(ne, nv, 1.0, Lv, 0, 0, A, 0, 0, AL, 0, 0);
+
+ GESE_LIBSTR(ne, ne, 0.0, Le, 0, 0);
+ SYRK_POTRF_LN_LIBSTR(ne, ne, nv, AL, 0, 0, AL, 0, 0, Le, 0, 0, Le, 0, 0);
+
+ TRSV_LNN_LIBSTR(nv, Lv, 0, 0, lv, 0, lv, 0);
+
+ GEMV_N_LIBSTR(ne, nv, 1.0, AL, 0, 0, lv, 0, 1.0, res_b, 0, dpi, 0);
+
+ TRSV_LNN_LIBSTR(ne, Le, 0, 0, dpi, 0, dpi, 0);
+ TRSV_LTN_LIBSTR(ne, Le, 0, 0, dpi, 0, dpi, 0);
+
+ GEMV_T_LIBSTR(ne, nv, 1.0, A, 0, 0, dpi, 0, -1.0, dv, 0, dv, 0);
+
+ TRSV_LNN_LIBSTR(nv, Lv, 0, 0, dv, 0, dv, 0);
+ TRSV_LTN_LIBSTR(nv, Lv, 0, 0, dv, 0, dv, 0);
+ }
+ else
+ {
+#if 0
+ TRCP_L_LIBSTR(nv, Hg, 0, 0, Lv, 0, 0);
+ VECCP_LIBSTR(nv, res_g, 0, lv, 0);
+
+ if(nb>0)
+ {
+ DIAAD_SP_LIBSTR(nb, 1.0, Qx, 0, idxb, Lv, 0, 0);
+ VECAD_SP_LIBSTR(nb, 1.0, qx, 0, idxb, lv, 0);
+ }
+
+ if(ng>0)
+ {
+ GEMM_R_DIAG_LIBSTR(nv, ng, 1.0, Ct, 0, 0, Qx, nb, 0.0, Ctx, 0, 0, Ctx, 0, 0);
+ GEMV_N_LIBSTR(nv, ng, 1.0, Ct, 0, 0, qx, nb, 1.0, lv, 0, lv, 0);
+ SYRK_POTRF_LN_LIBSTR(nv, nv, ng, Ctx, 0, 0, Ct, 0, 0, Lv, 0, 0, Lv, 0, 0); // TODO _mn_ routine in BLASFEO !!!
+ }
+ else
+ {
+ POTRF_L_LIBSTR(nv, Lv, 0, 0, Lv, 0, 0);
+ }
+
+ VECCP_LIBSTR(nv, lv, 0, dv, 0);
+ VECSC_LIBSTR(nv, -1.0, dv, 0);
+
+ TRSV_LNN_LIBSTR(nv, Lv, 0, 0, dv, 0, dv, 0);
+ TRSV_LTN_LIBSTR(nv, Lv, 0, 0, dv, 0, dv, 0);
+#else
+// TRCP_L_LIBSTR(nv, Hg, 0, 0, Lv, 0, 0);
+ GECP_LIBSTR(nv, nv, Hg, 0, 0, Lv, 0, 0);
+ ROWIN_LIBSTR(nv, 1.0, res_g, 0, Lv, nv, 0);
+
+ if(nb>0)
+ {
+ DIAAD_SP_LIBSTR(nb, 1.0, Qx, 0, idxb, Lv, 0, 0);
+ ROWAD_SP_LIBSTR(nb, 1.0, qx, 0, idxb, Lv, nv, 0);
+ }
+
+ if(ng>0)
+ {
+ GEMM_R_DIAG_LIBSTR(nv, ng, 1.0, Ct, 0, 0, Qx, nb, 0.0, Ctx, 0, 0, Ctx, 0, 0);
+ ROWIN_LIBSTR(ng, 1.0, qx, nb, Ctx, nv, 0);
+ SYRK_POTRF_LN_LIBSTR(nv+1, nv, ng, Ctx, 0, 0, Ct, 0, 0, Lv, 0, 0, Lv, 0, 0); // TODO _mn_ routine in BLASFEO !!!
+ }
+ else
+ {
+ POTRF_L_MN_LIBSTR(nv+1, nv, Lv, 0, 0, Lv, 0, 0);
+ }
+
+ ROWEX_LIBSTR(nv, -1.0, Lv, nv, 0, dv, 0);
+ TRSV_LTN_LIBSTR(nv, Lv, 0, 0, dv, 0, dv, 0);
+#endif
+ }
+
+ if(nb>0)
+ {
+ VECEX_SP_LIBSTR(nb, 1.0, idxb, dv, 0, dt_lb, 0);
+ }
+
+ if(ng>0)
+ {
+ GEMV_T_LIBSTR(nv, ng, 1.0, Ct, 0, 0, dv, 0, 0.0, dt_lg, 0, dt_lg, 0);
+ }
+
+ if(nb>0 | ng>0)
+ {
+ COMPUTE_LAM_T_HARD_QP(rws);
+ }
+
+ return;
+
+ }
+
+
+
+// range-space (Schur complement) method
+void SOLVE_KKT_STEP_HARD_DENSE_QP(struct DENSE_QP *qp, struct IPM_HARD_DENSE_QP_WORKSPACE *ws)
+ {
+
+ int nv = qp->nv;
+ int ne = qp->ne;
+ int nb = qp->nb;
+ int ng = qp->ng;
+ struct STRMAT *A = qp->A;
+ struct STRMAT *Ct = qp->Ct;
+ int *idxb = qp->idxb;
+
+ struct STRMAT *Lv = ws->Lv;
+ struct STRMAT *Le = ws->Le;
+ struct STRMAT *Ctx = ws->Ctx;
+ struct STRMAT *AL = ws->AL;
+ struct STRVEC *lv = ws->lv;
+ struct STRVEC *dv = ws->dv;
+ struct STRVEC *dpi = ws->dpi;
+ struct STRVEC *dt_lb = ws->dt_lb;
+ struct STRVEC *dt_lg = ws->dt_lg;
+ struct STRVEC *res_g = ws->res_g;
+ struct STRVEC *res_b = ws->res_b;
+ struct STRVEC *qx = ws->qx;
+
+ struct IPM_HARD_CORE_QP_WORKSPACE *rws = ws->core_workspace;
+
+ if(nb>0 | ng>0)
+ {
+ COMPUTE_QX_HARD_QP(rws);
+ }
+
+ if(ne>0)
+ {
+ VECCP_LIBSTR(nv, res_g, 0, lv, 0);
+
+ if(nb>0)
+ {
+ VECAD_SP_LIBSTR(nb, 1.0, qx, 0, idxb, lv, 0);
+ }
+
+ if(ng>0)
+ {
+ GEMV_N_LIBSTR(nv, ng, 1.0, Ct, 0, 0, qx, nb, 1.0, lv, 0, lv, 0);
+ }
+
+ VECCP_LIBSTR(nv, lv, 0, dv, 0);
+
+ TRSV_LNN_LIBSTR(nv, Lv, 0, 0, lv, 0, lv, 0);
+
+ GEMV_N_LIBSTR(ne, nv, 1.0, AL, 0, 0, lv, 0, 1.0, res_b, 0, dpi, 0);
+
+ TRSV_LNN_LIBSTR(ne, Le, 0, 0, dpi, 0, dpi, 0);
+ TRSV_LTN_LIBSTR(ne, Le, 0, 0, dpi, 0, dpi, 0);
+
+ GEMV_T_LIBSTR(ne, nv, 1.0, A, 0, 0, dpi, 0, -1.0, dv, 0, dv, 0);
+
+ TRSV_LNN_LIBSTR(nv, Lv, 0, 0, dv, 0, dv, 0);
+ TRSV_LTN_LIBSTR(nv, Lv, 0, 0, dv, 0, dv, 0);
+ }
+ else
+ {
+ VECCP_LIBSTR(nv, res_g, 0, lv, 0);
+
+ if(nb>0)
+ {
+ VECAD_SP_LIBSTR(nb, 1.0, qx, 0, idxb, lv, 0);
+ }
+
+ if(ng>0)
+ {
+ GEMV_N_LIBSTR(nv, ng, 1.0, Ct, 0, 0, qx, nb, 1.0, lv, 0, lv, 0);
+ }
+
+ VECCP_LIBSTR(nv, lv, 0, dv, 0);
+ VECSC_LIBSTR(nv, -1.0, dv, 0);
+
+ TRSV_LNN_LIBSTR(nv, Lv, 0, 0, dv, 0, dv, 0);
+ TRSV_LTN_LIBSTR(nv, Lv, 0, 0, dv, 0, dv, 0);
+ }
+
+ if(nb>0)
+ {
+ VECEX_SP_LIBSTR(nb, 1.0, idxb, dv, 0, dt_lb, 0);
+ }
+
+ if(ng>0)
+ {
+ GEMV_T_LIBSTR(nv, ng, 1.0, Ct, 0, 0, dv, 0, 0.0, dt_lg, 0, dt_lg, 0);
+ }
+
+ if(nb>0 | ng>0)
+ {
+ COMPUTE_LAM_T_HARD_QP(rws);
+ }
+
+ return;
+
+ }
+
+
diff --git a/dense_qp/x_dense_qp_sol.c b/dense_qp/x_dense_qp_sol.c
new file mode 100644
index 0000000..9529729
--- /dev/null
+++ b/dense_qp/x_dense_qp_sol.c
@@ -0,0 +1,191 @@
+/**************************************************************************************************
+* *
+* 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_DENSE_QP_SOL(int nv, int ne, int nb, int ng)
+ {
+
+ int size = 0;
+
+ size += (10)*sizeof(struct STRVEC); // v pi lam_lb lam_ub lam_lg lam_ug t_lb t_ub t_lg t_ug
+
+ size += 1*SIZE_STRVEC(nv); // ux
+ size += 1*SIZE_STRVEC(ne); // pi
+ size += 8*SIZE_STRVEC(2*nb+2*ng); // 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_DENSE_QP_SOL(int nv, int ne, int nb, int ng, struct DENSE_QP_SOL *qp_sol, void *memory)
+ {
+
+ qp_sol->memsize = MEMSIZE_DENSE_QP_SOL(nv, ne, nb, ng);
+
+
+ // vector struct stuff
+ struct STRVEC *sv_ptr = (struct STRVEC *) memory;
+
+ qp_sol->v = sv_ptr;
+ sv_ptr += 1;
+ qp_sol->pi = sv_ptr;
+ sv_ptr += 1;
+ qp_sol->lam_lb = sv_ptr;
+ sv_ptr += 1;
+ qp_sol->lam_ub = sv_ptr;
+ sv_ptr += 1;
+ qp_sol->lam_lg = sv_ptr;
+ sv_ptr += 1;
+ qp_sol->lam_ug = sv_ptr;
+ sv_ptr += 1;
+ qp_sol->t_lb = sv_ptr;
+ sv_ptr += 1;
+ qp_sol->t_ub = sv_ptr;
+ sv_ptr += 1;
+ qp_sol->t_lg = sv_ptr;
+ sv_ptr += 1;
+ qp_sol->t_ug = sv_ptr;
+ sv_ptr += 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;
+
+ // v
+ CREATE_STRVEC(nv, qp_sol->v, v_ptr);
+ v_ptr += qp_sol->v->memory_size;
+ // pi
+ CREATE_STRVEC(ne, qp_sol->pi, v_ptr);
+ v_ptr += qp_sol->pi->memory_size;
+ // lam
+ tmp_ptr = v_ptr;
+ v_ptr += SIZE_STRVEC(2*nb+2*ng);
+ // lam_lb
+ CREATE_STRVEC(nb, qp_sol->lam_lb, tmp_ptr);
+ tmp_ptr += (nb)*sizeof(REAL);
+ // lam_lg
+ CREATE_STRVEC(ng, qp_sol->lam_lg, tmp_ptr);
+ tmp_ptr += (ng)*sizeof(REAL);
+ // lam_ub
+ CREATE_STRVEC(nb, qp_sol->lam_ub, tmp_ptr);
+ tmp_ptr += (nb)*sizeof(REAL);
+ // lam_ug
+ CREATE_STRVEC(ng, qp_sol->lam_ug, tmp_ptr);
+ tmp_ptr += (ng)*sizeof(REAL);
+ // t_lb
+ CREATE_STRVEC(nb, qp_sol->t_lb, tmp_ptr);
+ tmp_ptr += (nb)*sizeof(REAL);
+ // t_lg
+ CREATE_STRVEC(ng, qp_sol->t_lg, tmp_ptr);
+ tmp_ptr += (ng)*sizeof(REAL);
+ // t_ub
+ CREATE_STRVEC(nb, qp_sol->t_ub, tmp_ptr);
+ tmp_ptr += (nb)*sizeof(REAL);
+ // t_ug
+ CREATE_STRVEC(ng, qp_sol->t_ug, tmp_ptr);
+ tmp_ptr += (ng)*sizeof(REAL);
+
+ return;
+
+ }
+
+
+
+void CVT_DENSE_QP_SOL_TO_COLMAJ(struct DENSE_QP *qp, struct DENSE_QP_SOL *qp_sol, REAL *v, REAL *pi, REAL *lam_lb, REAL *lam_ub, REAL *lam_lg, REAL *lam_ug)
+ {
+
+ int nv = qp->nv;
+ int ne = qp->ne;
+ int nb = qp->nb;
+ int ng = qp->ng;
+
+ CVT_STRVEC2VEC(nv, qp_sol->v, 0, v);
+ CVT_STRVEC2VEC(ne, qp_sol->pi, 0, pi);
+ CVT_STRVEC2VEC(nb, qp_sol->lam_lb, 0, lam_lb);
+ CVT_STRVEC2VEC(nb, qp_sol->lam_ub, 0, lam_ub);
+ CVT_STRVEC2VEC(ng, qp_sol->lam_lg, 0, lam_lg);
+ CVT_STRVEC2VEC(ng, qp_sol->lam_ug, 0, lam_ug);
+
+ return;
+
+ }
+
+
+
+void CVT_DENSE_QP_SOL_TO_ROWMAJ(struct DENSE_QP *qp, struct DENSE_QP_SOL *qp_sol, REAL *v, REAL *pi, REAL *lam_lb, REAL *lam_ub, REAL *lam_lg, REAL *lam_ug)
+ {
+
+ int nv = qp->nv;
+ int ne = qp->ne;
+ int nb = qp->nb;
+ int ng = qp->ng;
+
+ CVT_STRVEC2VEC(nv, qp_sol->v, 0, v);
+ CVT_STRVEC2VEC(ne, qp_sol->pi, 0, pi);
+ CVT_STRVEC2VEC(nb, qp_sol->lam_lb, 0, lam_lb);
+ CVT_STRVEC2VEC(nb, qp_sol->lam_ub, 0, lam_ub);
+ CVT_STRVEC2VEC(ng, qp_sol->lam_lg, 0, lam_lg);
+ CVT_STRVEC2VEC(ng, qp_sol->lam_ug, 0, lam_ug);
+
+ return;
+
+ }
+
+
+
+void CVT_DENSE_QP_SOL_TO_LIBSTR(struct DENSE_QP *qp, struct DENSE_QP_SOL *qp_sol, struct STRVEC *v, struct STRVEC *pi, struct STRVEC *lam_lb, struct STRVEC *lam_ub, struct STRVEC *lam_lg, struct STRVEC *lam_ug)
+ {
+
+ int nv = qp->nv;
+ int ne = qp->ne;
+ int nb = qp->nb;
+ int ng = qp->ng;
+
+ VECCP_LIBSTR(nv, qp_sol->v, 0, v, 0);
+ VECCP_LIBSTR(ne, qp_sol->pi, 0, pi, 0);
+ VECCP_LIBSTR(nb, qp_sol->lam_lb, 0, lam_lb, 0);
+ VECCP_LIBSTR(nb, qp_sol->lam_ub, 0, lam_ub, 0);
+ VECCP_LIBSTR(ng, qp_sol->lam_lg, 0, lam_lg, 0);
+ VECCP_LIBSTR(ng, qp_sol->lam_ug, 0, lam_ug, 0);
+
+ return;
+
+ }
diff --git a/include/hpipm_d_cond.h b/include/hpipm_d_cond.h
new file mode 100644
index 0000000..c5bdbb8
--- /dev/null
+++ b/include/hpipm_d_cond.h
@@ -0,0 +1,58 @@
+/**************************************************************************************************
+* *
+* This file is part of HPIPM. *
+* *
+* HPIPM -- High Performance Interior Point Method. *
+* Copyright (C) 2017 by Gianluca Frison. *
+* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. *
+* All rights reserved. *
+* *
+* HPMPC is free software; you can redistribute it and/or *
+* modify it under the terms of the GNU Lesser General Public *
+* License as published by the Free Software Foundation; either *
+* version 2.1 of the License, or (at your option) any later version. *
+* *
+* HPMPC is distributed in the hope that it will be useful, *
+* but WITHOUT ANY WARRANTY; without even the implied warranty of *
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. *
+* See the GNU Lesser General Public License for more details. *
+* *
+* You should have received a copy of the GNU Lesser General Public *
+* License along with HPMPC; if not, write to the Free Software *
+* Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA *
+* *
+* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de *
+* *
+**************************************************************************************************/
+
+
+
+#include <blasfeo_target.h>
+#include <blasfeo_common.h>
+
+
+
+struct d_cond_qp_ocp2dense_workspace
+ {
+ struct d_strmat *Gamma;
+ struct d_strmat *L;
+ struct d_strmat *Lx;
+ struct d_strmat *AL;
+ struct d_strvec *Gammab;
+ struct d_strvec *tmp_ngM;
+ struct d_strvec *tmp_nuxM;
+ int memsize;
+ };
+
+
+
+//
+void d_compute_qp_size_ocp2dense(int N, int *nx, int *nu, int *nb, int **idxb, int *ng, int *nvc, int *nec, int *nbc, int *ngc);
+//
+int d_memsize_cond_qp_ocp2dense(struct d_ocp_qp *ocp_qp, struct d_dense_qp *dense_qp); // XXX + args for algorithm type ???
+//
+void d_create_cond_qp_ocp2dense(struct d_ocp_qp *ocp_qp, struct d_dense_qp *dense_qp, struct d_cond_qp_ocp2dense_workspace *cond_ws, void *mem);
+//
+void d_cond_qp_ocp2dense(struct d_ocp_qp *ocp_qp, struct d_dense_qp *dense_qp, struct d_cond_qp_ocp2dense_workspace *cond_ws);
+//
+void d_expand_sol_dense2ocp(struct d_ocp_qp *ocp_qp, struct d_dense_qp_sol *dense_qp_sol, struct d_ocp_qp_sol *ocp_qp_sol, struct d_cond_qp_ocp2dense_workspace *cond_ws);
diff --git a/include/hpipm_d_cond_aux.h b/include/hpipm_d_cond_aux.h
new file mode 100644
index 0000000..88548b7
--- /dev/null
+++ b/include/hpipm_d_cond_aux.h
@@ -0,0 +1,42 @@
+/**************************************************************************************************
+* *
+* This file is part of HPIPM. *
+* *
+* HPIPM -- High Performance Interior Point Method. *
+* Copyright (C) 2017 by Gianluca Frison. *
+* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. *
+* All rights reserved. *
+* *
+* HPMPC is free software; you can redistribute it and/or *
+* modify it under the terms of the GNU Lesser General Public *
+* License as published by the Free Software Foundation; either *
+* version 2.1 of the License, or (at your option) any later version. *
+* *
+* HPMPC is distributed in the hope that it will be useful, *
+* but WITHOUT ANY WARRANTY; without even the implied warranty of *
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. *
+* See the GNU Lesser General Public License for more details. *
+* *
+* You should have received a copy of the GNU Lesser General Public *
+* License along with HPMPC; if not, write to the Free Software *
+* Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA *
+* *
+* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de *
+* *
+**************************************************************************************************/
+
+
+
+#include <blasfeo_target.h>
+#include <blasfeo_common.h>
+
+
+
+//
+void d_compute_Gamma(struct d_ocp_qp *ocp_qp, struct d_cond_qp_ocp2dense_workspace *cond_ws);
+//
+void d_cond_RSQrq_N2nx3(struct d_ocp_qp *ocp_qp, struct d_strmat *RSQrq2, struct d_strvec *rq2, struct d_cond_qp_ocp2dense_workspace *cond_ws);
+//
+void d_cond_DCtd(struct d_ocp_qp *ocp_qp, int *idxb2, struct d_strvec *d_lb2, struct d_strvec *d_ub2, struct d_strmat *DCt2, struct d_strvec *d_lg2, struct d_strvec *d_ug2, struct d_cond_qp_ocp2dense_workspace *cond_ws);
+//
+void d_expand_sol(struct d_ocp_qp *ocp_qp, struct d_dense_qp_sol *dense_qp_sol, struct d_strvec *ux, struct d_strvec *pi, struct d_strvec *lam_lb, struct d_strvec *lam_ub, struct d_strvec *lam_lg, struct d_strvec *lam_ug, struct d_strvec *t_lb, struct d_strvec *t_ub, struct d_strvec *t_lg, struct d_strvec *t_ug, struct d_strvec *tmp_nuxM, struct d_strvec *tmp_ngM);
diff --git a/include/hpipm_d_core_qp_ipm_hard.h b/include/hpipm_d_core_qp_ipm_hard.h
new file mode 100644
index 0000000..ce200ca
--- /dev/null
+++ b/include/hpipm_d_core_qp_ipm_hard.h
@@ -0,0 +1,109 @@
+/**************************************************************************************************
+* *
+* This file is part of HPIPM. *
+* *
+* HPIPM -- High Performance Interior Point Method. *
+* Copyright (C) 2017 by Gianluca Frison. *
+* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. *
+* All rights reserved. *
+* *
+* HPMPC is free software; you can redistribute it and/or *
+* modify it under the terms of the GNU Lesser General Public *
+* License as published by the Free Software Foundation; either *
+* version 2.1 of the License, or (at your option) any later version. *
+* *
+* HPMPC is distributed in the hope that it will be useful, *
+* but WITHOUT ANY WARRANTY; without even the implied warranty of *
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. *
+* See the GNU Lesser General Public License for more details. *
+* *
+* You should have received a copy of the GNU Lesser General Public *
+* License along with HPMPC; if not, write to the Free Software *
+* Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA *
+* *
+* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de *
+* *
+**************************************************************************************************/
+
+
+
+struct d_ipm_hard_core_qp_workspace
+ {
+ double *d; // constraints
+ double *d_lb; // lower box constraints
+ double *d_ub; // upper box constraints
+ double *d_lg; // lower general constraints
+ double *d_ug; // upper general constraints
+ double *v; // primal variables
+ double *pi; // equality constraints multipliers
+ double *lam; // inequality constraints multipliers
+ double *lam_lb; // lower bounds multipliers
+ double *lam_lg; // lower general constraints multipliers
+ double *lam_ub; // upper bounds multipliers
+ double *lam_ug; // upper general constraints multipliers
+ double *t; // inequality constraints slacks
+ double *t_lb; // lower box constraints slacks
+ double *t_lg; // lower general constraints slacks
+ double *t_ub; // upper box constraints slacks
+ double *t_ug; // upper general constraints slacks
+ double *t_inv; // inverse of t
+ double *t_inv_lb; // inverse of t
+ double *t_inv_ub; // inverse of t
+ double *t_inv_lg; // inverse of t
+ double *t_inv_ug; // inverse of t
+ double *dv; // step in v
+ double *dpi; // step in pi
+ double *dlam; // step in lam
+ double *dlam_lb; //
+ double *dlam_lg; //
+ double *dlam_ub; //
+ double *dlam_ug; //
+ double *dt; // step in t
+ double *dt_lb; // step in t_lb
+ double *dt_ub; // step in t_ub
+ double *dt_lg; // step in t_lg
+ double *dt_ug; // step in t_ug
+ double *res_g; // q-residuals
+ double *res_b; // b-residuals
+ double *res_d; // d-residuals
+ double *res_d_lb; // d-residuals
+ double *res_d_ub; // d-residuals
+ double *res_d_lg; // d-residuals
+ double *res_d_ug; // d-residuals
+ double *res_m; // m-residuals
+ double *res_m_lb; // m-residuals
+ double *res_m_ub; // m-residuals
+ double *res_m_lg; // m-residuals
+ double *res_m_ug; // m-residuals
+ double *Qx; // Hessian update
+ double *Qx_lb; // Hessian update
+ double *Qx_lg; // Hessian update
+ double *qx; // gradient update
+ double *qx_lb; // gradient update
+ double *qx_lg; // gradient update
+ double *stat; // convergence statistics
+ double alpha; // step length
+ double alpha_min; // exit cond on step lenght
+ double sigma; // centering XXX
+ double mu; // duality measuere
+ double mu_aff; // affine duality measuere
+ double mu0; // initial duality measuere
+ double mu_max; // exit cond on mu
+ double nt_inv; // 1.0/nt, where nt is the total number of constraints
+ int nv; // number of primal variables
+ int ne; // number of equality constraints
+ int nb; // number of two-sized bounds
+ int ng; // number of two-sized constraints
+ int iter_max; // exit cond on iter mumber
+ int memsize; // memory size (in bytes) of workspace
+ };
+
+
+
+//
+int d_memsize_ipm_hard_core_qp(int nv, int ne, int nb, int ng, int iter_max);
+//
+
+void d_create_ipm_hard_core_qp(struct d_ipm_hard_core_qp_workspace *workspace, void *mem);
+//
+void d_ipm_hard_core_qp(struct d_ipm_hard_core_qp_workspace *workspace);
diff --git a/include/hpipm_d_core_qp_ipm_hard_aux.h b/include/hpipm_d_core_qp_ipm_hard_aux.h
new file mode 100644
index 0000000..9d59737
--- /dev/null
+++ b/include/hpipm_d_core_qp_ipm_hard_aux.h
@@ -0,0 +1,41 @@
+/**************************************************************************************************
+* *
+* This file is part of HPIPM. *
+* *
+* HPIPM -- High Performance Interior Point Method. *
+* Copyright (C) 2017 by Gianluca Frison. *
+* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. *
+* All rights reserved. *
+* *
+* HPMPC is free software; you can redistribute it and/or *
+* modify it under the terms of the GNU Lesser General Public *
+* License as published by the Free Software Foundation; either *
+* version 2.1 of the License, or (at your option) any later version. *
+* *
+* HPMPC is distributed in the hope that it will be useful, *
+* but WITHOUT ANY WARRANTY; without even the implied warranty of *
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. *
+* See the GNU Lesser General Public License for more details. *
+* *
+* You should have received a copy of the GNU Lesser General Public *
+* License along with HPMPC; if not, write to the Free Software *
+* Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA *
+* *
+* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de *
+* *
+**************************************************************************************************/
+
+//
+void d_compute_Qx_qx_hard_qp(struct d_ipm_hard_core_qp_workspace *rws);
+//
+void d_compute_lam_t_hard_qp(struct d_ipm_hard_core_qp_workspace *rws);
+//
+void d_compute_alpha_hard_qp(struct d_ipm_hard_core_qp_workspace *rws);
+//
+void d_update_var_hard_qp(struct d_ipm_hard_core_qp_workspace *rws);
+//
+void d_compute_mu_aff_hard_qp(struct d_ipm_hard_core_qp_workspace *rws);
+//
+void d_compute_centering_correction_hard_qp(struct d_ipm_hard_core_qp_workspace *rws);
+//
+void d_compute_qx_hard_qp(struct d_ipm_hard_core_qp_workspace *rws);
diff --git a/include/hpipm_d_dense_qp.h b/include/hpipm_d_dense_qp.h
new file mode 100644
index 0000000..ba60ab4
--- /dev/null
+++ b/include/hpipm_d_dense_qp.h
@@ -0,0 +1,73 @@
+/**************************************************************************************************
+* *
+* This file is part of HPIPM. *
+* *
+* HPIPM -- High Performance Interior Point Method. *
+* Copyright (C) 2017 by Gianluca Frison. *
+* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. *
+* All rights reserved. *
+* *
+* HPMPC is free software; you can redistribute it and/or *
+* modify it under the terms of the GNU Lesser General Public *
+* License as published by the Free Software Foundation; either *
+* version 2.1 of the License, or (at your option) any later version. *
+* *
+* HPMPC is distributed in the hope that it will be useful, *
+* but WITHOUT ANY WARRANTY; without even the implied warranty of *
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. *
+* See the GNU Lesser General Public License for more details. *
+* *
+* You should have received a copy of the GNU Lesser General Public *
+* License along with HPMPC; if not, write to the Free Software *
+* Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA *
+* *
+* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de *
+* *
+**************************************************************************************************/
+
+
+
+#include <blasfeo_target.h>
+#include <blasfeo_common.h>
+
+
+
+struct d_dense_qp
+ {
+ struct d_strmat *Hg; // hessian & gradient
+ struct d_strmat *A; // dynamics matrix
+ struct d_strmat *Ct; // constraints matrix
+ struct d_strvec *g; // gradient
+ struct d_strvec *b; // dynamics vector
+ struct d_strvec *d; // constraint
+ struct d_strvec *d_lb; // lower box constraint
+ struct d_strvec *d_ub; // upper box constraint
+ struct d_strvec *d_lg; // lower general constraint
+ struct d_strvec *d_ug; // upper general constraint
+ int *idxb; // index of box constraints
+ int nv; // number of variables
+ int ne; // number of equality constraints
+ int nb; // number of box constraints
+ int ng; // number of general constraints
+ int memsize; // memory size in bytes
+ };
+
+
+
+//
+int d_memsize_dense_qp(int nv, int ne, int nb, int ng);
+//
+void d_create_dense_qp(int nv, int ne, int nb, int ng, struct d_dense_qp *qp, void *memory);
+//
+void d_cvt_colmaj_to_dense_qp(double *H, double *g, double *A, double *b, int *idxb, double *d_lb, double *d_ub, double *C, double *d_lg, double *d_ug, struct d_dense_qp *qp);
+//
+void d_cvt_dense_qp_to_colmaj(struct d_dense_qp *qp, double *H, double *g, double *A, double *b, int *idxb, double *d_lb, double *d_ub, double *C, double *d_lg, double *d_ug);
+//
+void d_cvt_rowmaj_to_dense_qp(double *H, double *g, double *A, double *b, int *idxb, double *d_lb, double *d_ub, double *C, double *d_lg, double *d_ug, struct d_dense_qp *qp);
+//
+void d_cvt_dense_qp_to_rowmaj(struct d_dense_qp *qp, double *H, double *g, double *A, double *b, int *idxb, double *d_lb, double *d_ub, double *C, double *d_lg, double *d_ug);
+//
+void d_cvt_libstr_to_dense_qp(struct d_strmat *H, struct d_strmat *A, struct d_strmat *C, struct d_strvec *g, struct d_strvec *b, struct d_strvec *d_lb, struct d_strvec *d_ub, struct d_strvec *d_lg, struct d_strvec *d_ug, int *idxb, struct d_dense_qp *qp);
+//
+void d_cvt_dense_qp_to_libstr(struct d_dense_qp *qp, struct d_strmat *H, struct d_strmat *A, struct d_strmat *C, struct d_strvec *g, struct d_strvec *b, struct d_strvec *d_lb, struct d_strvec *d_ub, struct d_strvec *d_lg, struct d_strvec *d_ug, int *idxb);
+
diff --git a/include/hpipm_d_dense_qp_ipm_hard.h b/include/hpipm_d_dense_qp_ipm_hard.h
new file mode 100644
index 0000000..e5ef65b
--- /dev/null
+++ b/include/hpipm_d_dense_qp_ipm_hard.h
@@ -0,0 +1,89 @@
+/**************************************************************************************************
+* *
+* This file is part of HPIPM. *
+* *
+* HPIPM -- High Performance Interior Point Method. *
+* Copyright (C) 2017 by Gianluca Frison. *
+* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. *
+* All rights reserved. *
+* *
+* HPMPC is free software; you can redistribute it and/or *
+* modify it under the terms of the GNU Lesser General Public *
+* License as published by the Free Software Foundation; either *
+* version 2.1 of the License, or (at your option) any later version. *
+* *
+* HPMPC is distributed in the hope that it will be useful, *
+* but WITHOUT ANY WARRANTY; without even the implied warranty of *
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. *
+* See the GNU Lesser General Public License for more details. *
+* *
+* You should have received a copy of the GNU Lesser General Public *
+* License along with HPMPC; if not, write to the Free Software *
+* Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA *
+* *
+* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de *
+* *
+**************************************************************************************************/
+
+
+
+#include <blasfeo_target.h>
+#include <blasfeo_common.h>
+
+
+
+struct d_ipm_hard_dense_qp_workspace
+ {
+ struct d_ipm_hard_core_qp_workspace *core_workspace;
+ struct d_strvec *dv; // step in v
+ struct d_strvec *dpi; // step in pi
+ struct d_strvec *dlam; // step in lam XXX needed ???
+ struct d_strvec *dt; // step in t XXX needed ???
+ struct d_strvec *dt_lb; //
+ struct d_strvec *dt_ub; // XXX needed ???
+ struct d_strvec *dt_lg; //
+ struct d_strvec *dt_ug; // XXX needed ???
+ struct d_strvec *res_g; // q-residuals
+ struct d_strvec *res_b; // b-residuals
+ struct d_strvec *res_d; // d-residuals
+ struct d_strvec *res_d_lb; // d-residuals
+ struct d_strvec *res_d_ub; // d-residuals
+ struct d_strvec *res_d_lg; // d-residuals
+ struct d_strvec *res_d_ug; // d-residuals
+ struct d_strvec *res_m; // m-residuals
+ struct d_strvec *Qx; // hessian update
+ struct d_strvec *qx; // gradient update
+ struct d_strmat *Lv; //
+ struct d_strmat *AL; //
+ struct d_strmat *Le; //
+ struct d_strmat *Ctx; //
+ struct d_strvec *lv; //
+ struct d_strvec *tmp_nb; // work space of size nb
+ struct d_strvec *tmp_ng0; // work space of size nb
+ struct d_strvec *tmp_ng1; // work space of size nb
+ double *stat; // convergence statistics
+ double res_mu; // mu-residual
+ int iter; // iteration number
+ int memsize; // memory size (in bytes) of workspace
+ };
+
+
+
+struct d_ipm_hard_dense_qp_arg
+ {
+ double alpha_min; // exit cond on step length
+ double mu_max; // exit cond on duality measure
+ double mu0; // initial value for duality measure
+ int iter_max; // exit cond in iter number
+ };
+
+
+
+//
+int d_memsize_ipm_hard_dense_qp(struct d_dense_qp *qp, struct d_ipm_hard_dense_qp_arg *arg);
+//
+void d_create_ipm_hard_dense_qp(struct d_dense_qp *qp, struct d_ipm_hard_dense_qp_arg *arg, struct d_ipm_hard_dense_qp_workspace *ws, void *mem);
+//
+void d_solve_ipm_hard_dense_qp(struct d_dense_qp *qp, struct d_dense_qp_sol *qp_sol, struct d_ipm_hard_dense_qp_workspace *ws);
+//
+void d_solve_ipm2_hard_dense_qp(struct d_dense_qp *qp, struct d_dense_qp_sol *qp_sol, struct d_ipm_hard_dense_qp_workspace *ws);
diff --git a/include/hpipm_d_dense_qp_kkt.h b/include/hpipm_d_dense_qp_kkt.h
new file mode 100644
index 0000000..4721657
--- /dev/null
+++ b/include/hpipm_d_dense_qp_kkt.h
@@ -0,0 +1,39 @@
+/**************************************************************************************************
+* *
+* This file is part of HPIPM. *
+* *
+* HPIPM -- High Performance Interior Point Method. *
+* Copyright (C) 2017 by Gianluca Frison. *
+* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. *
+* All rights reserved. *
+* *
+* HPMPC is free software; you can redistribute it and/or *
+* modify it under the terms of the GNU Lesser General Public *
+* License as published by the Free Software Foundation; either *
+* version 2.1 of the License, or (at your option) any later version. *
+* *
+* HPMPC is distributed in the hope that it will be useful, *
+* but WITHOUT ANY WARRANTY; without even the implied warranty of *
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. *
+* See the GNU Lesser General Public License for more details. *
+* *
+* You should have received a copy of the GNU Lesser General Public *
+* License along with HPMPC; if not, write to the Free Software *
+* Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA *
+* *
+* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de *
+* *
+**************************************************************************************************/
+
+
+
+//
+void d_init_var_hard_dense_qp(struct d_dense_qp *qp, struct d_dense_qp_sol *qp_sol, struct d_ipm_hard_dense_qp_workspace *ws);
+//
+void d_compute_res_hard_dense_qp(struct d_dense_qp *qp, struct d_dense_qp_sol *qp_sol, struct d_ipm_hard_dense_qp_workspace *ws);
+//
+void d_fact_solve_kkt_unconstr_dense_qp(struct d_dense_qp *qp, struct d_dense_qp_sol *qp_sol, struct d_ipm_hard_dense_qp_workspace *ws);
+//
+void d_fact_solve_kkt_step_hard_dense_qp(struct d_dense_qp *qp, struct d_ipm_hard_dense_qp_workspace *ws);
+//
+void d_solve_kkt_step_hard_dense_qp(struct d_dense_qp *qp, struct d_ipm_hard_dense_qp_workspace *ws);
diff --git a/include/hpipm_d_dense_qp_sol.h b/include/hpipm_d_dense_qp_sol.h
new file mode 100644
index 0000000..0348e10
--- /dev/null
+++ b/include/hpipm_d_dense_qp_sol.h
@@ -0,0 +1,61 @@
+/**************************************************************************************************
+* *
+* This file is part of HPIPM. *
+* *
+* HPIPM -- High Performance Interior Point Method. *
+* Copyright (C) 2017 by Gianluca Frison. *
+* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. *
+* All rights reserved. *
+* *
+* HPMPC is free software; you can redistribute it and/or *
+* modify it under the terms of the GNU Lesser General Public *
+* License as published by the Free Software Foundation; either *
+* version 2.1 of the License, or (at your option) any later version. *
+* *
+* HPMPC is distributed in the hope that it will be useful, *
+* but WITHOUT ANY WARRANTY; without even the implied warranty of *
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. *
+* See the GNU Lesser General Public License for more details. *
+* *
+* You should have received a copy of the GNU Lesser General Public *
+* License along with HPMPC; if not, write to the Free Software *
+* Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA *
+* *
+* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de *
+* *
+**************************************************************************************************/
+
+
+
+#include <blasfeo_target.h>
+#include <blasfeo_common.h>
+
+
+
+struct d_dense_qp_sol
+ {
+ struct d_strvec *v;
+ struct d_strvec *pi;
+ struct d_strvec *lam_lb;
+ struct d_strvec *lam_ub;
+ struct d_strvec *lam_lg;
+ struct d_strvec *lam_ug;
+ struct d_strvec *t_lb;
+ struct d_strvec *t_ub;
+ struct d_strvec *t_lg;
+ struct d_strvec *t_ug;
+ int memsize;
+ };
+
+
+
+//
+int d_memsize_dense_qp_sol(int nv, int ne, int nb, int ng);
+//
+void d_create_dense_qp_sol(int nv, int ne, int nb, int ng, struct d_dense_qp_sol *qp_sol, void *memory);
+//
+void d_cvt_dense_qp_sol_to_colmaj(struct d_dense_qp *qp, struct d_dense_qp_sol *qp_sol, double *v, double *pi, double *lam_lb, double *lam_ub, double *lam_lg, double *lam_ug);
+//
+void d_cvt_dense_qp_sol_to_rowmaj(struct d_dense_qp *qp, struct d_dense_qp_sol *qp_sol, double *v, double *pi, double *lam_lb, double *lam_ub, double *lam_lg, double *lam_ug);
+//
+void d_cvt_dense_qp_sol_to_libstr(struct d_dense_qp *qp, struct d_dense_qp_sol *qp_sol, struct d_strvec *v, struct d_strvec *pi, struct d_strvec *lam_lb, struct d_strvec *lam_ub, struct d_strvec *lam_lg, struct d_strvec *lam_ug);
diff --git a/include/hpipm_d_ocp_qp.h b/include/hpipm_d_ocp_qp.h
new file mode 100644
index 0000000..ec4f767
--- /dev/null
+++ b/include/hpipm_d_ocp_qp.h
@@ -0,0 +1,68 @@
+/**************************************************************************************************
+* *
+* This file is part of HPIPM. *
+* *
+* HPIPM -- High Performance Interior Point Method. *
+* Copyright (C) 2017 by Gianluca Frison. *
+* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. *
+* All rights reserved. *
+* *
+* HPMPC is free software; you can redistribute it and/or *
+* modify it under the terms of the GNU Lesser General Public *
+* License as published by the Free Software Foundation; either *
+* version 2.1 of the License, or (at your option) any later version. *
+* *
+* HPMPC is distributed in the hope that it will be useful, *
+* but WITHOUT ANY WARRANTY; without even the implied warranty of *
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. *
+* See the GNU Lesser General Public License for more details. *
+* *
+* You should have received a copy of the GNU Lesser General Public *
+* License along with HPMPC; if not, write to the Free Software *
+* Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA *
+* *
+* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de *
+* *
+**************************************************************************************************/
+
+
+
+#include <blasfeo_target.h>
+#include <blasfeo_common.h>
+
+
+
+struct d_ocp_qp
+ {
+ struct d_strmat *BAbt;
+ struct d_strvec *b;
+ struct d_strmat *RSQrq;
+ struct d_strvec *rq;
+ struct d_strmat *DCt;
+ struct d_strvec *d_lb;
+ struct d_strvec *d_ub;
+ struct d_strvec *d_lg;
+ struct d_strvec *d_ug;
+ int *nx; // number of states
+ int *nu; // number of inputs
+ int *nb; // number of box constraints
+ int **idxb; // index of box constraints
+ int *ng; // number of general constraints
+ int N; // hotizon lenght
+ int memsize; // memory size in bytes
+ };
+
+
+
+//
+int d_memsize_ocp_qp(int N, int *nx, int *nu, int *nb, int *ng);
+//
+void d_create_ocp_qp(int N, int *nx, int *nu, int *nb, int *ng, struct d_ocp_qp *qp, void *memory);
+//
+void d_cvt_colmaj_to_ocp_qp(double **A, double **B, double **b, double **Q, double **S, double **R, double **q, double **r, int **idxb, double **lb, double **ub, double **C, double **D, double **lg, double **ug, struct d_ocp_qp *qp);
+//
+void d_cvt_rowmaj_to_ocp_qp(double **A, double **B, double **b, double **Q, double **S, double **R, double **q, double **r, int **idxb, double **lb, double **ub, double **C, double **D, double **lg, double **ug, struct d_ocp_qp *qp);
+//
+//void d_cast_ocp_qp(int N, int *nx, int *nu, int *nb, int **idxb, int *ng, struct d_strmat *sBAbt, struct d_strvec *sb, struct d_strmat *sRSQrq, struct d_strvec *srq, struct d_strmat *sDCt, struct d_strvec *slb, struct d_strvec *sub, struct d_strvec *slg, struct d_strvec *sug, struct d_ocp_qp *str_out);
+//
+void d_copy_ocp_qp(struct d_ocp_qp *qp_in, struct d_ocp_qp *qp_out);
diff --git a/include/hpipm_d_ocp_qp_ipm_hard.h b/include/hpipm_d_ocp_qp_ipm_hard.h
new file mode 100644
index 0000000..e3660d6
--- /dev/null
+++ b/include/hpipm_d_ocp_qp_ipm_hard.h
@@ -0,0 +1,91 @@
+/**************************************************************************************************
+* *
+* This file is part of HPIPM. *
+* *
+* HPIPM -- High Performance Interior Point Method. *
+* Copyright (C) 2017 by Gianluca Frison. *
+* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. *
+* All rights reserved. *
+* *
+* HPMPC is free software; you can redistribute it and/or *
+* modify it under the terms of the GNU Lesser General Public *
+* License as published by the Free Software Foundation; either *
+* version 2.1 of the License, or (at your option) any later version. *
+* *
+* HPMPC is distributed in the hope that it will be useful, *
+* but WITHOUT ANY WARRANTY; without even the implied warranty of *
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. *
+* See the GNU Lesser General Public License for more details. *
+* *
+* You should have received a copy of the GNU Lesser General Public *
+* License along with HPMPC; if not, write to the Free Software *
+* Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA *
+* *
+* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de *
+* *
+**************************************************************************************************/
+
+
+
+#include <blasfeo_target.h>
+#include <blasfeo_common.h>
+
+
+
+
+struct d_ipm_hard_ocp_qp_workspace
+ {
+ struct d_ipm_hard_core_qp_workspace *core_workspace;
+ struct d_strvec *dux;
+ struct d_strvec *dpi;
+ struct d_strvec *dt_lb;
+ struct d_strvec *dt_lg;
+ struct d_strvec *res_g; // q-residuals
+ struct d_strvec *res_b; // b-residuals
+ struct d_strvec *res_d; // d-residuals XXX remove ???
+ struct d_strvec *res_d_lb; // d-residuals
+ struct d_strvec *res_d_ub; // d-residuals
+ struct d_strvec *res_d_lg; // d-residuals
+ struct d_strvec *res_d_ug; // d-residuals
+ struct d_strvec *res_m; // m-residuals
+ struct d_strvec *res_m_lb; // m-residuals
+ struct d_strvec *res_m_ub; // m-residuals
+ struct d_strvec *res_m_lg; // m-residuals
+ struct d_strvec *res_m_ug; // m-residuals
+ struct d_strvec *Qx_lb; // hessian update
+ struct d_strvec *Qx_lg; // hessian update
+ struct d_strvec *qx_lb; // gradient update
+ struct d_strvec *qx_lg; // gradient update
+ struct d_strvec *tmp_nbM; // work space of size nbM
+ struct d_strvec *tmp_nxM; // work space of size nxM
+ struct d_strvec *tmp_ngM; // work space of size ngM
+ struct d_strvec *Pb; // Pb
+ struct d_strmat *L;
+ struct d_strmat *AL;
+ double *stat; // convergence statistics
+ double res_mu; // mu-residual
+ int iter; // iteration number
+ int memsize;
+ };
+
+
+
+struct d_ipm_hard_ocp_qp_arg
+ {
+ double alpha_min; // exit cond on step length
+ double mu_max; // exit cond on duality measure
+ double mu0; // initial value for duality measure
+ int iter_max; // exit cond in iter number
+ };
+
+
+
+//
+int d_memsize_ipm_hard_ocp_qp(struct d_ocp_qp *qp, struct d_ipm_hard_ocp_qp_arg *arg);
+//
+void d_create_ipm_hard_ocp_qp(struct d_ocp_qp *qp, struct d_ipm_hard_ocp_qp_arg *arg, struct d_ipm_hard_ocp_qp_workspace *ws, void *mem);
+//
+void d_solve_ipm_hard_ocp_qp(struct d_ocp_qp *qp, struct d_ocp_qp_sol *qp_sol, struct d_ipm_hard_ocp_qp_workspace *ws);
+//
+void d_solve_ipm2_hard_ocp_qp(struct d_ocp_qp *qp, struct d_ocp_qp_sol *qp_sol, struct d_ipm_hard_ocp_qp_workspace *ws);
+
diff --git a/include/hpipm_d_ocp_qp_kkt.h b/include/hpipm_d_ocp_qp_kkt.h
new file mode 100644
index 0000000..eb41bd4
--- /dev/null
+++ b/include/hpipm_d_ocp_qp_kkt.h
@@ -0,0 +1,40 @@
+/**************************************************************************************************
+* *
+* This file is part of HPIPM. *
+* *
+* HPIPM -- High Performance Interior Point Method. *
+* Copyright (C) 2017 by Gianluca Frison. *
+* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. *
+* All rights reserved. *
+* *
+* HPMPC is free software; you can redistribute it and/or *
+* modify it under the terms of the GNU Lesser General Public *
+* License as published by the Free Software Foundation; either *
+* version 2.1 of the License, or (at your option) any later version. *
+* *
+* HPMPC is distributed in the hope that it will be useful, *
+* but WITHOUT ANY WARRANTY; without even the implied warranty of *
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. *
+* See the GNU Lesser General Public License for more details. *
+* *
+* You should have received a copy of the GNU Lesser General Public *
+* License along with HPMPC; if not, write to the Free Software *
+* Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA *
+* *
+* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de *
+* *
+**************************************************************************************************/
+
+
+
+
+//
+void d_init_var_hard_ocp_qp(struct d_ocp_qp *qp, struct d_ocp_qp_sol *qp_sol, struct d_ipm_hard_ocp_qp_workspace *ws);
+//
+void d_compute_res_hard_ocp_qp(struct d_ocp_qp *qp, struct d_ocp_qp_sol *qp_sol, struct d_ipm_hard_ocp_qp_workspace *ws);
+//
+void d_fact_solve_kkt_unconstr_ocp_qp(struct d_ocp_qp *qp, struct d_ocp_qp_sol *qp_sol, struct d_ipm_hard_ocp_qp_workspace *ws);
+//
+void d_fact_solve_kkt_step_hard_ocp_qp(struct d_ocp_qp *qp, struct d_ipm_hard_ocp_qp_workspace *ws);
+//
+void d_solve_kkt_step_hard_ocp_qp(struct d_ocp_qp *qp, struct d_ipm_hard_ocp_qp_workspace *ws);
diff --git a/include/hpipm_d_ocp_qp_sol.h b/include/hpipm_d_ocp_qp_sol.h
new file mode 100644
index 0000000..73519ca
--- /dev/null
+++ b/include/hpipm_d_ocp_qp_sol.h
@@ -0,0 +1,61 @@
+/**************************************************************************************************
+* *
+* This file is part of HPIPM. *
+* *
+* HPIPM -- High Performance Interior Point Method. *
+* Copyright (C) 2017 by Gianluca Frison. *
+* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. *
+* All rights reserved. *
+* *
+* HPMPC is free software; you can redistribute it and/or *
+* modify it under the terms of the GNU Lesser General Public *
+* License as published by the Free Software Foundation; either *
+* version 2.1 of the License, or (at your option) any later version. *
+* *
+* HPMPC is distributed in the hope that it will be useful, *
+* but WITHOUT ANY WARRANTY; without even the implied warranty of *
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. *
+* See the GNU Lesser General Public License for more details. *
+* *
+* You should have received a copy of the GNU Lesser General Public *
+* License along with HPMPC; if not, write to the Free Software *
+* Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA *
+* *
+* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de *
+* *
+**************************************************************************************************/
+
+
+
+#include <blasfeo_target.h>
+#include <blasfeo_common.h>
+
+
+
+struct d_ocp_qp_sol
+ {
+ struct d_strvec *ux;
+ struct d_strvec *pi;
+ struct d_strvec *lam_lb;
+ struct d_strvec *lam_ub;
+ struct d_strvec *lam_lg;
+ struct d_strvec *lam_ug;
+ struct d_strvec *t_lb;
+ struct d_strvec *t_ub;
+ struct d_strvec *t_lg;
+ struct d_strvec *t_ug;
+ int memsize; // memory size in bytes
+ };
+
+
+
+//
+int d_memsize_ocp_qp_sol(int N, int *nx, int *nu, int *nb, int *ng);
+//
+void d_create_ocp_qp_sol(int N, int *nx, int *nu, int *nb, int *ng, struct d_ocp_qp_sol *qp_sol, void *memory);
+//
+void d_cvt_ocp_qp_sol_to_colmaj(struct d_ocp_qp *qp, struct d_ocp_qp_sol *qp_sol, double **u, double **x, double **pi, double **lam_lb, double **lam_ub, double **lam_lg, double **lam_ug);
+//
+void d_cvt_ocp_qp_sol_to_rowmaj(struct d_ocp_qp *qp, struct d_ocp_qp_sol *qp_sol, double **u, double **x, double **pi, double **lam_lb, double **lam_ub, double **lam_lg, double **lam_ug);
+//
+void d_cvt_ocp_qp_sol_to_libstr(struct d_ocp_qp *qp, struct d_ocp_qp_sol *qp_sol, struct d_strvec *u, struct d_strvec *x, struct d_strvec *pi, struct d_strvec *lam_lb, struct d_strvec *lam_ub, struct d_strvec *lam_lg, struct d_strvec *lam_ug);
diff --git a/include/hpipm_m_ocp_qp.h b/include/hpipm_m_ocp_qp.h
new file mode 100644
index 0000000..bd9090b
--- /dev/null
+++ b/include/hpipm_m_ocp_qp.h
@@ -0,0 +1,36 @@
+/**************************************************************************************************
+* *
+* This file is part of HPIPM. *
+* *
+* HPIPM -- High Performance Interior Point Method. *
+* Copyright (C) 2017 by Gianluca Frison. *
+* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. *
+* All rights reserved. *
+* *
+* HPMPC is free software; you can redistribute it and/or *
+* modify it under the terms of the GNU Lesser General Public *
+* License as published by the Free Software Foundation; either *
+* version 2.1 of the License, or (at your option) any later version. *
+* *
+* HPMPC is distributed in the hope that it will be useful, *
+* but WITHOUT ANY WARRANTY; without even the implied warranty of *
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. *
+* See the GNU Lesser General Public License for more details. *
+* *
+* You should have received a copy of the GNU Lesser General Public *
+* License along with HPMPC; if not, write to the Free Software *
+* Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA *
+* *
+* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de *
+* *
+**************************************************************************************************/
+
+
+
+
+#include <blasfeo_target.h>
+#include <blasfeo_common.h>
+
+
+
+void m_cvt_d_ocp_qp_to_s_ocp_qp(struct d_ocp_qp *d_qp, struct s_ocp_qp *s_qp);
diff --git a/include/hpipm_m_ocp_qp_ipm_hard.h b/include/hpipm_m_ocp_qp_ipm_hard.h
new file mode 100644
index 0000000..29a6124
--- /dev/null
+++ b/include/hpipm_m_ocp_qp_ipm_hard.h
@@ -0,0 +1,99 @@
+/**************************************************************************************************
+* *
+* This file is part of HPIPM. *
+* *
+* HPIPM -- High Performance Interior Point Method. *
+* Copyright (C) 2017 by Gianluca Frison. *
+* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. *
+* All rights reserved. *
+* *
+* HPMPC is free software; you can redistribute it and/or *
+* modify it under the terms of the GNU Lesser General Public *
+* License as published by the Free Software Foundation; either *
+* version 2.1 of the License, or (at your option) any later version. *
+* *
+* HPMPC is distributed in the hope that it will be useful, *
+* but WITHOUT ANY WARRANTY; without even the implied warranty of *
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. *
+* See the GNU Lesser General Public License for more details. *
+* *
+* You should have received a copy of the GNU Lesser General Public *
+* License along with HPMPC; if not, write to the Free Software *
+* Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA *
+* *
+* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de *
+* *
+**************************************************************************************************/
+
+
+
+#include <blasfeo_target.h>
+#include <blasfeo_common.h>
+
+
+
+
+struct m_ipm_hard_ocp_qp_workspace
+ {
+ struct d_ipm_hard_core_qp_workspace *core_workspace;
+ struct d_strvec *dux;
+ struct d_strvec *dpi;
+ struct d_strvec *dt_lb;
+ struct d_strvec *dt_lg;
+ struct s_strvec *sdux; // XXX
+ struct s_strvec *sdpi; // XXX
+ struct d_strvec *res_g; // q-residuals
+ struct d_strvec *res_b; // b-residuals
+ struct d_strvec *res_d; // d-residuals XXX remove ???
+ struct d_strvec *res_d_lb; // d-residuals
+ struct d_strvec *res_d_ub; // d-residuals
+ struct d_strvec *res_d_lg; // d-residuals
+ struct d_strvec *res_d_ug; // d-residuals
+ struct d_strvec *res_m; // m-residuals
+ struct d_strvec *res_m_lb; // m-residuals
+ struct d_strvec *res_m_ub; // m-residuals
+ struct d_strvec *res_m_lg; // m-residuals
+ struct d_strvec *res_m_ug; // m-residuals
+ struct s_strvec *sres_g; // q-residuals // XXX
+ struct s_strvec *sres_b; // b-residuals // XXX
+ struct d_strvec *Qx_lb; // hessian update
+ struct d_strvec *Qx_lg; // hessian update
+ struct d_strvec *qx_lb; // gradient update
+ struct d_strvec *qx_lg; // gradient update
+ struct s_strvec *sQx_lb; // hessian update // XXX
+ struct s_strvec *sQx_lg; // hessian update // XXX
+ struct s_strvec *sqx_lb; // gradient update // XXX
+ struct s_strvec *sqx_lg; // gradient update // XXX
+ struct d_strvec *tmp_nbM; // work space of size nbM
+ struct s_strvec *tmp_nxM; // work space of size nxM // XXX
+ struct d_strvec *tmp_ngM; // work space of size ngM
+ struct s_strvec *Pb; // Pb // XXX
+ struct s_strmat *L; // XXX
+ struct s_strmat *AL; // XXX
+ double *stat; // convergence statistics
+ double res_mu; // mu-residual
+ int iter; // iteration number
+ int compute_Pb;
+ };
+
+
+
+struct m_ipm_hard_ocp_qp_arg
+ {
+ double alpha_min; // exit cond on step length
+ double mu_max; // exit cond on duality measure
+ double mu0; // initial value for duality measure
+ int iter_max; // exit cond in iter number
+ };
+
+
+
+//
+int m_memsize_ipm_hard_ocp_qp(struct d_ocp_qp *d_qp, struct s_ocp_qp *s_qp, struct m_ipm_hard_ocp_qp_arg *arg);
+//
+void m_create_ipm_hard_ocp_qp(struct d_ocp_qp *d_qp, struct s_ocp_qp *s_qp, struct m_ipm_hard_ocp_qp_arg *arg, struct m_ipm_hard_ocp_qp_workspace *ws, void *mem);
+//
+void m_solve_ipm_hard_ocp_qp(struct d_ocp_qp *d_qp, struct s_ocp_qp *s_qp, struct d_ocp_qp_sol *qp_sol, struct m_ipm_hard_ocp_qp_workspace *ws);
+//
+void m_solve_ipm2_hard_ocp_qp(struct d_ocp_qp *d_qp, struct s_ocp_qp *s_qp, struct d_ocp_qp_sol *qp_sol, struct m_ipm_hard_ocp_qp_workspace *ws);
+
diff --git a/include/hpipm_m_ocp_qp_kkt.h b/include/hpipm_m_ocp_qp_kkt.h
new file mode 100644
index 0000000..d335cfe
--- /dev/null
+++ b/include/hpipm_m_ocp_qp_kkt.h
@@ -0,0 +1,29 @@
+/**************************************************************************************************
+* *
+* This file is part of HPIPM. *
+* *
+* HPIPM -- High Performance Interior Point Method. *
+* Copyright (C) 2017 by Gianluca Frison. *
+* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. *
+* All rights reserved. *
+* *
+* HPMPC is free software; you can redistribute it and/or *
+* modify it under the terms of the GNU Lesser General Public *
+* License as published by the Free Software Foundation; either *
+* version 2.1 of the License, or (at your option) any later version. *
+* *
+* HPMPC is distributed in the hope that it will be useful, *
+* but WITHOUT ANY WARRANTY; without even the implied warranty of *
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. *
+* See the GNU Lesser General Public License for more details. *
+* *
+* You should have received a copy of the GNU Lesser General Public *
+* License along with HPMPC; if not, write to the Free Software *
+* Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA *
+* *
+* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de *
+* *
+**************************************************************************************************/
+
+void m_fact_solve_kkt_step_hard_ocp_qp(struct d_ocp_qp *d_qp, struct s_ocp_qp *s_qp, struct m_ipm_hard_ocp_qp_workspace *ws);
+void m_solve_kkt_step_hard_ocp_qp(struct d_ocp_qp *d_qp, struct s_ocp_qp *s_qp, struct m_ipm_hard_ocp_qp_workspace *ws);
diff --git a/include/hpipm_s_core_qp_ipm_hard.h b/include/hpipm_s_core_qp_ipm_hard.h
new file mode 100644
index 0000000..4c1fa39
--- /dev/null
+++ b/include/hpipm_s_core_qp_ipm_hard.h
@@ -0,0 +1,110 @@
+/**************************************************************************************************
+* *
+* This file is part of HPIPM. *
+* *
+* HPIPM -- High Performance Interior Point Method. *
+* Copyright (C) 2017 by Gianluca Frison. *
+* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. *
+* All rights reserved. *
+* *
+* HPMPC is free software; you can redistribute it and/or *
+* modify it under the terms of the GNU Lesser General Public *
+* License as published by the Free Software Foundation; either *
+* version 2.1 of the License, or (at your option) any later version. *
+* *
+* HPMPC is distributed in the hope that it will be useful, *
+* but WITHOUT ANY WARRANTY; without even the implied warranty of *
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. *
+* See the GNU Lesser General Public License for more details. *
+* *
+* You should have received a copy of the GNU Lesser General Public *
+* License along with HPMPC; if not, write to the Free Software *
+* Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA *
+* *
+* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de *
+* *
+**************************************************************************************************/
+
+
+
+struct s_ipm_hard_core_qp_workspace
+ {
+ float *d; // constraints
+ float *d_lb; // lower box constraints
+ float *d_ub; // upper box constraints
+ float *d_lg; // lower general constraints
+ float *d_ug; // upper general constraints
+ float *v; // primal variables
+ float *pi; // equality constraints multipliers
+ float *lam; // inequality constraints multipliers
+ float *lam_lb; // lower bounds multipliers
+ float *lam_lg; // lower general constraints multipliers
+ float *lam_ub; // upper bounds multipliers
+ float *lam_ug; // upper general constraints multipliers
+ float *t; // inequality constraints slacks
+ float *t_lb; // lower box constraints slacks
+ float *t_lg; // lower general constraints slacks
+ float *t_ub; // upper box constraints slacks
+ float *t_ug; // upper general constraints slacks
+ float *t_inv; // inverse of t
+ float *t_inv_lb; // inverse of t
+ float *t_inv_ub; // inverse of t
+ float *t_inv_lg; // inverse of t
+ float *t_inv_ug; // inverse of t
+ float *dv; // step in v
+ float *dpi; // step in pi
+ float *dlam; // step in lam
+ float *dlam_lb; //
+ float *dlam_lg; //
+ float *dlam_ub; //
+ float *dlam_ug; //
+ float *dt; // step in t
+ float *dt_lb; // step in t_lb
+ float *dt_ub; // step in t_ub
+ float *dt_lg; // step in t_lg
+ float *dt_ug; // step in t_ug
+ float *res_g; // q-residuals
+ float *res_b; // b-residuals
+ float *res_d; // d-residuals
+ float *res_d_lb; // d-residuals
+ float *res_d_ub; // d-residuals
+ float *res_d_lg; // d-residuals
+ float *res_d_ug; // d-residuals
+ float *res_m; // m-residuals
+ float *res_m_lb; // m-residuals
+ float *res_m_ub; // m-residuals
+ float *res_m_lg; // m-residuals
+ float *res_m_ug; // m-residuals
+ float *Qx; // Hessian update
+ float *Qx_lb; // Hessian update
+ float *Qx_lg; // Hessian update
+ float *qx; // gradient update
+ float *qx_lb; // gradient update
+ float *qx_lg; // gradient update
+ float *stat; // convergence statistics
+ float alpha; // step length
+ float alpha_min; // exit cond on step lenght
+ float sigma; // centering XXX
+ float mu; // duality measuere
+ float mu_aff; // affine duality measuere
+ float mu0; // initial duality measuere
+ float mu_max; // exit cond on mu
+ float nt_inv; // 1.0/nt, where nt is the total number of constraints
+ int nv; // number of primal variables
+ int ne; // number of equality constraints
+ int nb; // number of two-sized bounds
+ int ng; // number of two-sized constraints
+ int memsize; // memory size (in bytes) of workspace
+ int iter_max; // exit cond on iter mumber
+ };
+
+
+
+//
+int s_memsize_ipm_hard_core_qp(int nv, int ne, int nb, int ng, int iter_max);
+//
+
+void s_create_ipm_hard_core_qp(struct s_ipm_hard_core_qp_workspace *workspace, void *mem);
+//
+void s_ipm_hard_core_qp(struct s_ipm_hard_core_qp_workspace *workspace);
+
diff --git a/include/hpipm_s_core_qp_ipm_hard_aux.h b/include/hpipm_s_core_qp_ipm_hard_aux.h
new file mode 100644
index 0000000..fb79bf8
--- /dev/null
+++ b/include/hpipm_s_core_qp_ipm_hard_aux.h
@@ -0,0 +1,42 @@
+/**************************************************************************************************
+* *
+* This file is part of HPIPM. *
+* *
+* HPIPM -- High Performance Interior Point Method. *
+* Copyright (C) 2017 by Gianluca Frison. *
+* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. *
+* All rights reserved. *
+* *
+* HPMPC is free software; you can redistribute it and/or *
+* modify it under the terms of the GNU Lesser General Public *
+* License as published by the Free Software Foundation; either *
+* version 2.1 of the License, or (at your option) any later version. *
+* *
+* HPMPC is distributed in the hope that it will be useful, *
+* but WITHOUT ANY WARRANTY; without even the implied warranty of *
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. *
+* See the GNU Lesser General Public License for more details. *
+* *
+* You should have received a copy of the GNU Lesser General Public *
+* License along with HPMPC; if not, write to the Free Software *
+* Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA *
+* *
+* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de *
+* *
+**************************************************************************************************/
+
+//
+void s_compute_Qx_qx_hard_qp(struct s_ipm_hard_core_qp_workspace *rws);
+//
+void s_compute_lam_t_hard_qp(struct s_ipm_hard_core_qp_workspace *rws);
+//
+void s_compute_alpha_hard_qp(struct s_ipm_hard_core_qp_workspace *rws);
+//
+void s_update_var_hard_qp(struct s_ipm_hard_core_qp_workspace *rws);
+//
+void s_compute_mu_aff_hard_qp(struct s_ipm_hard_core_qp_workspace *rws);
+//
+void s_compute_centering_correction_hard_qp(struct s_ipm_hard_core_qp_workspace *rws);
+//
+void s_compute_qx_hard_qp(struct s_ipm_hard_core_qp_workspace *rws);
+
diff --git a/include/hpipm_s_dense_qp.h b/include/hpipm_s_dense_qp.h
new file mode 100644
index 0000000..857017f
--- /dev/null
+++ b/include/hpipm_s_dense_qp.h
@@ -0,0 +1,75 @@
+
+/**************************************************************************************************
+* *
+* This file is part of HPIPM. *
+* *
+* HPIPM -- High Performance Interior Point Method. *
+* Copyright (C) 2017 by Gianluca Frison. *
+* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. *
+* All rights reserved. *
+* *
+* HPMPC is free software; you can redistribute it and/or *
+* modify it under the terms of the GNU Lesser General Public *
+* License as published by the Free Software Foundation; either *
+* version 2.1 of the License, or (at your option) any later version. *
+* *
+* HPMPC is distributed in the hope that it will be useful, *
+* but WITHOUT ANY WARRANTY; without even the implied warranty of *
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. *
+* See the GNU Lesser General Public License for more details. *
+* *
+* You should have received a copy of the GNU Lesser General Public *
+* License along with HPMPC; if not, write to the Free Software *
+* Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA *
+* *
+* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de *
+* *
+**************************************************************************************************/
+
+
+
+#include <blasfeo_target.h>
+#include <blasfeo_common.h>
+
+
+
+struct s_dense_qp
+ {
+ struct s_strmat *Hg; // hessian & gradient
+ struct s_strmat *A; // dynamics matrix
+ struct s_strmat *Ct; // constraints matrix
+ struct s_strvec *g; // gradient
+ struct s_strvec *b; // dynamics vector
+ struct s_strvec *d; // constraint
+ struct s_strvec *d_lb; // lower box constraint
+ struct s_strvec *d_ub; // upper box constraint
+ struct s_strvec *d_lg; // lower general constraint
+ struct s_strvec *d_ug; // upper general constraint
+ int *idxb; // index of box constraints
+ int nv; // number of variables
+ int ne; // number of equality constraints
+ int nb; // number of box constraints
+ int ng; // number of general constraints
+ int memsize; // memory size in bytes
+ };
+
+
+
+//
+int s_memsize_dense_qp(int nv, int ne, int nb, int ng);
+//
+void s_create_dense_qp(int nv, int ne, int nb, int ng, struct s_dense_qp *qp, void *memory);
+//
+void s_cvt_colmaj_to_dense_qp(float *H, float *g, float *A, float *b, int *idxb, float *d_lb, float *d_ub, float *C, float *d_lg, float *d_ug, struct s_dense_qp *qp);
+//
+void s_cvt_dense_qp_to_colmaj(struct s_dense_qp *qp, float *H, float *g, float *A, float *b, int *idxb, float *d_lb, float *d_ub, float *C, float *d_lg, float *d_ug);
+//
+void s_cvt_rowmaj_to_dense_qp(float *H, float *g, float *A, float *b, int *idxb, float *d_lb, float *d_ub, float *C, float *d_lg, float *d_ug, struct s_dense_qp *qp);
+//
+void s_cvt_dense_qp_to_rowmaj(struct s_dense_qp *qp, float *H, float *g, float *A, float *b, int *idxb, float *d_lb, float *d_ub, float *C, float *d_lg, float *d_ug);
+//
+void s_cvt_libstr_to_dense_qp(struct s_strmat *H, struct s_strmat *A, struct s_strmat *C, struct s_strvec *g, struct s_strvec *b, struct s_strvec *d_lb, struct s_strvec *d_ub, struct s_strvec *d_lg, struct s_strvec *d_ug, int *idxb, struct s_dense_qp *qp);
+//
+void s_cvt_dense_qp_to_libstr(struct s_dense_qp *qp, struct s_strmat *H, struct s_strmat *A, struct s_strmat *C, struct s_strvec *g, struct s_strvec *b, struct s_strvec *d_lb, struct s_strvec *d_ub, struct s_strvec *d_lg, struct s_strvec *d_ug, int *idxb);
+
+
diff --git a/include/hpipm_s_dense_qp_ipm_hard.h b/include/hpipm_s_dense_qp_ipm_hard.h
new file mode 100644
index 0000000..519b0e8
--- /dev/null
+++ b/include/hpipm_s_dense_qp_ipm_hard.h
@@ -0,0 +1,90 @@
+/**************************************************************************************************
+* *
+* This file is part of HPIPM. *
+* *
+* HPIPM -- High Performance Interior Point Method. *
+* Copyright (C) 2017 by Gianluca Frison. *
+* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. *
+* All rights reserved. *
+* *
+* HPMPC is free software; you can redistribute it and/or *
+* modify it under the terms of the GNU Lesser General Public *
+* License as published by the Free Software Foundation; either *
+* version 2.1 of the License, or (at your option) any later version. *
+* *
+* HPMPC is distributed in the hope that it will be useful, *
+* but WITHOUT ANY WARRANTY; without even the implied warranty of *
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. *
+* See the GNU Lesser General Public License for more details. *
+* *
+* You should have received a copy of the GNU Lesser General Public *
+* License along with HPMPC; if not, write to the Free Software *
+* Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA *
+* *
+* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de *
+* *
+**************************************************************************************************/
+
+
+
+#include <blasfeo_target.h>
+#include <blasfeo_common.h>
+
+
+
+struct s_ipm_hard_dense_qp_workspace
+ {
+ struct s_ipm_hard_core_qp_workspace *core_workspace;
+ struct s_strvec *dv; // step in v
+ struct s_strvec *dpi; // step in pi
+ struct s_strvec *dlam; // step in lam XXX needed ???
+ struct s_strvec *dt; // step in t XXX needed ???
+ struct s_strvec *dt_lb; //
+ struct s_strvec *dt_ub; // XXX needed ???
+ struct s_strvec *dt_lg; //
+ struct s_strvec *dt_ug; // XXX needed ???
+ struct s_strvec *res_g; // q-residuals
+ struct s_strvec *res_b; // b-residuals
+ struct s_strvec *res_d; // d-residuals
+ struct s_strvec *res_d_lb; // d-residuals
+ struct s_strvec *res_d_ub; // d-residuals
+ struct s_strvec *res_d_lg; // d-residuals
+ struct s_strvec *res_d_ug; // d-residuals
+ struct s_strvec *res_m; // m-residuals
+ struct s_strvec *Qx; // hessian update
+ struct s_strvec *qx; // gradient update
+ struct s_strmat *Lv; //
+ struct s_strmat *AL; //
+ struct s_strmat *Le; //
+ struct s_strmat *Ctx; //
+ struct s_strvec *lv; //
+ struct s_strvec *tmp_nb; // work space of size nb
+ struct s_strvec *tmp_ng0; // work space of size nb
+ struct s_strvec *tmp_ng1; // work space of size nb
+ float *stat; // convergence statistics
+ float res_mu; // mu-residual
+ int iter; // iteration number
+ int memsize; // memory size (in bytes) of workspace
+ };
+
+
+
+struct s_ipm_hard_dense_qp_arg
+ {
+ float alpha_min; // exit cond on step length
+ float mu_max; // exit cond on duality measure
+ float mu0; // initial value for duality measure
+ int iter_max; // exit cond in iter number
+ };
+
+
+
+//
+int s_memsize_ipm_hard_dense_qp(struct s_dense_qp *qp, struct s_ipm_hard_dense_qp_arg *arg);
+//
+void s_create_ipm_hard_dense_qp(struct s_dense_qp *qp, struct s_ipm_hard_dense_qp_arg *arg, struct s_ipm_hard_dense_qp_workspace *ws, void *mem);
+//
+void s_solve_ipm_hard_dense_qp(struct s_dense_qp *qp, struct s_dense_qp_sol *qp_sol, struct s_ipm_hard_dense_qp_workspace *ws);
+//
+void s_solve_ipm2_hard_dense_qp(struct s_dense_qp *qp, struct s_dense_qp_sol *qp_sol, struct s_ipm_hard_dense_qp_workspace *ws);
+
diff --git a/include/hpipm_s_dense_qp_kkt.h b/include/hpipm_s_dense_qp_kkt.h
new file mode 100644
index 0000000..b3d5a75
--- /dev/null
+++ b/include/hpipm_s_dense_qp_kkt.h
@@ -0,0 +1,40 @@
+/**************************************************************************************************
+* *
+* This file is part of HPIPM. *
+* *
+* HPIPM -- High Performance Interior Point Method. *
+* Copyright (C) 2017 by Gianluca Frison. *
+* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. *
+* All rights reserved. *
+* *
+* HPMPC is free software; you can redistribute it and/or *
+* modify it under the terms of the GNU Lesser General Public *
+* License as published by the Free Software Foundation; either *
+* version 2.1 of the License, or (at your option) any later version. *
+* *
+* HPMPC is distributed in the hope that it will be useful, *
+* but WITHOUT ANY WARRANTY; without even the implied warranty of *
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. *
+* See the GNU Lesser General Public License for more details. *
+* *
+* You should have received a copy of the GNU Lesser General Public *
+* License along with HPMPC; if not, write to the Free Software *
+* Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA *
+* *
+* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de *
+* *
+**************************************************************************************************/
+
+
+
+//
+void s_init_var_hard_dense_qp(struct s_dense_qp *qp, struct s_dense_qp_sol *qp_sol, struct s_ipm_hard_dense_qp_workspace *ws);
+//
+void s_compute_res_hard_dense_qp(struct s_dense_qp *qp, struct s_dense_qp_sol *qp_sol, struct s_ipm_hard_dense_qp_workspace *ws);
+//
+void s_fact_solve_kkt_unconstr_dense_qp(struct s_dense_qp *qp, struct s_dense_qp_sol *qp_sol, struct s_ipm_hard_dense_qp_workspace *ws);
+//
+void s_fact_solve_kkt_step_hard_dense_qp(struct s_dense_qp *qp, struct s_ipm_hard_dense_qp_workspace *ws);
+//
+void s_solve_kkt_step_hard_dense_qp(struct s_dense_qp *qp, struct s_ipm_hard_dense_qp_workspace *ws);
+
diff --git a/include/hpipm_s_dense_qp_sol.h b/include/hpipm_s_dense_qp_sol.h
new file mode 100644
index 0000000..1d37341
--- /dev/null
+++ b/include/hpipm_s_dense_qp_sol.h
@@ -0,0 +1,61 @@
+/**************************************************************************************************
+* *
+* This file is part of HPIPM. *
+* *
+* HPIPM -- High Performance Interior Point Method. *
+* Copyright (C) 2017 by Gianluca Frison. *
+* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. *
+* All rights reserved. *
+* *
+* HPMPC is free software; you can redistribute it and/or *
+* modify it under the terms of the GNU Lesser General Public *
+* License as published by the Free Software Foundation; either *
+* version 2.1 of the License, or (at your option) any later version. *
+* *
+* HPMPC is distributed in the hope that it will be useful, *
+* but WITHOUT ANY WARRANTY; without even the implied warranty of *
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. *
+* See the GNU Lesser General Public License for more details. *
+* *
+* You should have received a copy of the GNU Lesser General Public *
+* License along with HPMPC; if not, write to the Free Software *
+* Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA *
+* *
+* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de *
+* *
+**************************************************************************************************/
+
+
+
+#include <blasfeo_target.h>
+#include <blasfeo_common.h>
+
+
+
+struct s_dense_qp_sol
+ {
+ struct s_strvec *v;
+ struct s_strvec *pi;
+ struct s_strvec *lam_lb;
+ struct s_strvec *lam_ub;
+ struct s_strvec *lam_lg;
+ struct s_strvec *lam_ug;
+ struct s_strvec *t_lb;
+ struct s_strvec *t_ub;
+ struct s_strvec *t_lg;
+ struct s_strvec *t_ug;
+ int memsize;
+ };
+
+
+
+//
+int s_memsize_dense_qp_sol(int nv, int ne, int nb, int ng);
+//
+void s_create_dense_qp_sol(int nv, int ne, int nb, int ng, struct s_dense_qp_sol *qp_sol, void *memory);
+//
+void s_cvt_dense_qp_sol_to_colmaj(struct s_dense_qp *qp, struct s_dense_qp_sol *qp_sol, float *v, float *pi, float *lam_lb, float *lam_ub, float *lam_lg, float *lam_ug);
+//
+void s_cvt_dense_qp_sol_to_rowmaj(struct s_dense_qp *qp, struct s_dense_qp_sol *qp_sol, float *v, float *pi, float *lam_lb, float *lam_ub, float *lam_lg, float *lam_ug);
+//
+void s_cvt_dense_qp_sol_to_libstr(struct s_dense_qp *qp, struct s_dense_qp_sol *qp_sol, struct s_strvec *v, struct s_strvec *pi, struct s_strvec *lam_lb, struct s_strvec *lam_ub, struct s_strvec *lam_lg, struct s_strvec *lam_ug);
diff --git a/include/hpipm_s_ocp_qp.h b/include/hpipm_s_ocp_qp.h
new file mode 100644
index 0000000..618726e
--- /dev/null
+++ b/include/hpipm_s_ocp_qp.h
@@ -0,0 +1,68 @@
+/**************************************************************************************************
+* *
+* This file is part of HPIPM. *
+* *
+* HPIPM -- High Performance Interior Point Method. *
+* Copyright (C) 2017 by Gianluca Frison. *
+* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. *
+* All rights reserved. *
+* *
+* HPMPC is free software; you can redistribute it and/or *
+* modify it under the terms of the GNU Lesser General Public *
+* License as published by the Free Software Foundation; either *
+* version 2.1 of the License, or (at your option) any later version. *
+* *
+* HPMPC is distributed in the hope that it will be useful, *
+* but WITHOUT ANY WARRANTY; without even the implied warranty of *
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. *
+* See the GNU Lesser General Public License for more details. *
+* *
+* You should have received a copy of the GNU Lesser General Public *
+* License along with HPMPC; if not, write to the Free Software *
+* Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA *
+* *
+* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de *
+* *
+**************************************************************************************************/
+
+
+
+#include <blasfeo_target.h>
+#include <blasfeo_common.h>
+
+
+
+struct s_ocp_qp
+ {
+ struct s_strmat *BAbt;
+ struct s_strvec *b;
+ struct s_strmat *RSQrq;
+ struct s_strvec *rq;
+ struct s_strmat *DCt;
+ struct s_strvec *d_lb;
+ struct s_strvec *d_ub;
+ struct s_strvec *d_lg;
+ struct s_strvec *d_ug;
+ int *nx; // number of states
+ int *nu; // number of inputs
+ int *nb; // number of box constraints
+ int **idxb; // index of box constraints
+ int *ng; // number of general constraints
+ int N; // hotizon lenght
+ int memsize; // memory size in bytes
+ };
+
+
+
+//
+int s_memsize_ocp_qp(int N, int *nx, int *nu, int *nb, int *ng);
+//
+void s_create_ocp_qp(int N, int *nx, int *nu, int *nb, int *ng, struct s_ocp_qp *qp, void *memory);
+//
+void s_cvt_colmaj_to_ocp_qp(float **A, float **B, float **b, float **Q, float **S, float **R, float **q, float **r, int **idxb, float **lb, float **ub, float **C, float **D, float **lg, float **ug, struct s_ocp_qp *qp);
+//
+void s_cvt_rowmaj_to_ocp_qp(float **A, float **B, float **b, float **Q, float **S, float **R, float **q, float **r, int **idxb, float **lb, float **ub, float **C, float **D, float **lg, float **ug, struct s_ocp_qp *qp);
+//
+//void s_cast_ocp_qp(int N, int *nx, int *nu, int *nb, int **idxb, int *ng, struct s_strmat *sBAbt, struct s_strvec *sb, struct s_strmat *sRSQrq, struct s_strvec *srq, struct s_strmat *sDCt, struct s_strvec *slb, struct s_strvec *sub, struct s_strvec *slg, struct s_strvec *sug, struct s_ocp_qp *str_out);
+//
+void s_copy_ocp_qp(struct s_ocp_qp *qp_in, struct s_ocp_qp *qp_out);
diff --git a/include/hpipm_s_ocp_qp_ipm_hard.h b/include/hpipm_s_ocp_qp_ipm_hard.h
new file mode 100644
index 0000000..a8e3ca3
--- /dev/null
+++ b/include/hpipm_s_ocp_qp_ipm_hard.h
@@ -0,0 +1,92 @@
+/**************************************************************************************************
+* *
+* This file is part of HPIPM. *
+* *
+* HPIPM -- High Performance Interior Point Method. *
+* Copyright (C) 2017 by Gianluca Frison. *
+* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. *
+* All rights reserved. *
+* *
+* HPMPC is free software; you can redistribute it and/or *
+* modify it under the terms of the GNU Lesser General Public *
+* License as published by the Free Software Foundation; either *
+* version 2.1 of the License, or (at your option) any later version. *
+* *
+* HPMPC is distributed in the hope that it will be useful, *
+* but WITHOUT ANY WARRANTY; without even the implied warranty of *
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. *
+* See the GNU Lesser General Public License for more details. *
+* *
+* You should have received a copy of the GNU Lesser General Public *
+* License along with HPMPC; if not, write to the Free Software *
+* Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA *
+* *
+* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de *
+* *
+**************************************************************************************************/
+
+
+
+#include <blasfeo_target.h>
+#include <blasfeo_common.h>
+
+
+
+
+struct s_ipm_hard_ocp_qp_workspace
+ {
+ struct s_ipm_hard_core_qp_workspace *core_workspace;
+ struct s_strvec *dux;
+ struct s_strvec *dpi;
+ struct s_strvec *dt_lb;
+ struct s_strvec *dt_lg;
+ struct s_strvec *res_g; // q-residuals
+ struct s_strvec *res_b; // b-residuals
+ struct s_strvec *res_d; // d-residuals XXX remove ???
+ struct s_strvec *res_d_lb; // d-residuals
+ struct s_strvec *res_d_ub; // d-residuals
+ struct s_strvec *res_d_lg; // d-residuals
+ struct s_strvec *res_d_ug; // d-residuals
+ struct s_strvec *res_m; // m-residuals
+ struct s_strvec *res_m_lb; // m-residuals
+ struct s_strvec *res_m_ub; // m-residuals
+ struct s_strvec *res_m_lg; // m-residuals
+ struct s_strvec *res_m_ug; // m-residuals
+ struct s_strvec *Qx_lb; // hessian update
+ struct s_strvec *Qx_lg; // hessian update
+ struct s_strvec *qx_lb; // gradient update
+ struct s_strvec *qx_lg; // gradient update
+ struct s_strvec *tmp_nbM; // work space of size nbM
+ struct s_strvec *tmp_nxM; // work space of size nxM
+ struct s_strvec *tmp_ngM; // work space of size ngM
+ struct s_strvec *Pb; // Pb
+ struct s_strmat *L;
+ struct s_strmat *AL;
+ float *stat; // convergence statistics
+ float res_mu; // mu-residual
+ int iter; // iteration number
+ int memsize;
+ };
+
+
+
+struct s_ipm_hard_ocp_qp_arg
+ {
+ float alpha_min; // exit cond on step length
+ float mu_max; // exit cond on duality measure
+ float mu0; // initial value for duality measure
+ int iter_max; // exit cond in iter number
+ };
+
+
+
+//
+int s_memsize_ipm_hard_ocp_qp(struct s_ocp_qp *qp, struct s_ipm_hard_ocp_qp_arg *arg);
+//
+void s_create_ipm_hard_ocp_qp(struct s_ocp_qp *qp, struct s_ipm_hard_ocp_qp_arg *arg, struct s_ipm_hard_ocp_qp_workspace *ws, void *mem);
+//
+void s_solve_ipm_hard_ocp_qp(struct s_ocp_qp *qp, struct s_ocp_qp_sol *qp_sol, struct s_ipm_hard_ocp_qp_workspace *ws);
+//
+void s_solve_ipm2_hard_ocp_qp(struct s_ocp_qp *qp, struct s_ocp_qp_sol *qp_sol, struct s_ipm_hard_ocp_qp_workspace *ws);
+
+
diff --git a/include/hpipm_s_ocp_qp_kkt.h b/include/hpipm_s_ocp_qp_kkt.h
new file mode 100644
index 0000000..214d9cc
--- /dev/null
+++ b/include/hpipm_s_ocp_qp_kkt.h
@@ -0,0 +1,41 @@
+/**************************************************************************************************
+* *
+* This file is part of HPIPM. *
+* *
+* HPIPM -- High Performance Interior Point Method. *
+* Copyright (C) 2017 by Gianluca Frison. *
+* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. *
+* All rights reserved. *
+* *
+* HPMPC is free software; you can redistribute it and/or *
+* modify it under the terms of the GNU Lesser General Public *
+* License as published by the Free Software Foundation; either *
+* version 2.1 of the License, or (at your option) any later version. *
+* *
+* HPMPC is distributed in the hope that it will be useful, *
+* but WITHOUT ANY WARRANTY; without even the implied warranty of *
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. *
+* See the GNU Lesser General Public License for more details. *
+* *
+* You should have received a copy of the GNU Lesser General Public *
+* License along with HPMPC; if not, write to the Free Software *
+* Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA *
+* *
+* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de *
+* *
+**************************************************************************************************/
+
+
+
+
+//
+void s_init_var_hard_ocp_qp(struct s_ocp_qp *qp, struct s_ocp_qp_sol *qp_sol, struct s_ipm_hard_ocp_qp_workspace *ws);
+//
+void s_compute_res_hard_ocp_qp(struct s_ocp_qp *qp, struct s_ocp_qp_sol *qp_sol, struct s_ipm_hard_ocp_qp_workspace *ws);
+//
+void s_fact_solve_kkt_unconstr_ocp_qp(struct s_ocp_qp *qp, struct s_ocp_qp_sol *qp_sol, struct s_ipm_hard_ocp_qp_workspace *ws);
+//
+void s_fact_solve_kkt_step_hard_ocp_qp(struct s_ocp_qp *qp, struct s_ipm_hard_ocp_qp_workspace *ws);
+//
+void s_solve_kkt_step_hard_ocp_qp(struct s_ocp_qp *qp, struct s_ipm_hard_ocp_qp_workspace *ws);
+
diff --git a/include/hpipm_s_ocp_qp_sol.h b/include/hpipm_s_ocp_qp_sol.h
new file mode 100644
index 0000000..39759dc
--- /dev/null
+++ b/include/hpipm_s_ocp_qp_sol.h
@@ -0,0 +1,61 @@
+/**************************************************************************************************
+* *
+* This file is part of HPIPM. *
+* *
+* HPIPM -- High Performance Interior Point Method. *
+* Copyright (C) 2017 by Gianluca Frison. *
+* Developed at IMTEK (University of Freiburg) under the supervision of Moritz Diehl. *
+* All rights reserved. *
+* *
+* HPMPC is free software; you can redistribute it and/or *
+* modify it under the terms of the GNU Lesser General Public *
+* License as published by the Free Software Foundation; either *
+* version 2.1 of the License, or (at your option) any later version. *
+* *
+* HPMPC is distributed in the hope that it will be useful, *
+* but WITHOUT ANY WARRANTY; without even the implied warranty of *
+* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. *
+* See the GNU Lesser General Public License for more details. *
+* *
+* You should have received a copy of the GNU Lesser General Public *
+* License along with HPMPC; if not, write to the Free Software *
+* Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA *
+* *
+* Author: Gianluca Frison, gianluca.frison (at) imtek.uni-freiburg.de *
+* *
+**************************************************************************************************/
+
+
+
+#include <blasfeo_target.h>
+#include <blasfeo_common.h>
+
+
+
+struct s_ocp_qp_sol
+ {
+ struct s_strvec *ux;
+ struct s_strvec *pi;
+ struct s_strvec *lam_lb;
+ struct s_strvec *lam_ub;
+ struct s_strvec *lam_lg;
+ struct s_strvec *lam_ug;
+ struct s_strvec *t_lb;
+ struct s_strvec *t_ub;
+ struct s_strvec *t_lg;
+ struct s_strvec *t_ug;
+ int memsize; // memory size in bytes
+ };
+
+
+
+//
+int s_memsize_ocp_qp_sol(int N, int *nx, int *nu, int *nb, int *ng);
+//
+void s_create_ocp_qp_sol(int N, int *nx, int *nu, int *nb, int *ng, struct s_ocp_qp_sol *qp_sol, void *memory);
+//
+void s_cvt_ocp_qp_sol_to_colmaj(struct s_ocp_qp *qp, struct s_ocp_qp_sol *qp_sol, float **u, float **x, float **pi, float **lam_lb, float **lam_ub, float **lam_lg, float **lam_ug);
+//
+void s_cvt_ocp_qp_sol_to_rowmaj(struct s_ocp_qp *qp, struct s_ocp_qp_sol *qp_sol, float **u, float **x, float **pi, float **lam_lb, float **lam_ub, float **lam_lg, float **lam_ug);
+//
+void s_cvt_ocp_qp_sol_to_libstr(struct s_ocp_qp *qp, struct s_ocp_qp_sol *qp_sol, struct s_strvec *u, struct s_strvec *x, struct s_strvec *pi, struct s_strvec *lam_lb, struct s_strvec *lam_ub, struct s_strvec *lam_lg, struct s_strvec *lam_ug);
diff --git a/lib/dummy.txt b/lib/dummy.txt
new file mode 100644
index 0000000..e69de29
--- /dev/null
+++ b/lib/dummy.txt
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;
+
+ }
diff --git a/test_problems/CMakeLists.txt b/test_problems/CMakeLists.txt
new file mode 100644
index 0000000..4e98a5e
--- /dev/null
+++ b/test_problems/CMakeLists.txt
@@ -0,0 +1,44 @@
+###################################################################################################
+# #
+# 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 #
+# #
+###################################################################################################
+
+add_executable(d_dense_qp test_d_dense.c)
+target_link_libraries(d_dense_qp hpipm blasfeo m)
+
+add_executable(s_dense_qp test_s_dense.c)
+target_link_libraries(s_dense_qp hpipm blasfeo m)
+
+add_executable(d_ocp_qp test_d_ocp.c d_tools.c)
+target_link_libraries(d_ocp_qp hpipm blasfeo m)
+
+add_executable(s_ocp_qp test_s_ocp.c s_tools.c)
+target_link_libraries(s_ocp_qp hpipm blasfeo m)
+
+add_executable(m_ocp_qp test_m_ocp.c d_tools.c)
+target_link_libraries(m_ocp_qp hpipm blasfeo m)
+
+add_executable(d_cond test_d_cond.c d_tools.c)
+target_link_libraries(d_cond hpipm blasfeo m)
diff --git a/test_problems/Makefile b/test_problems/Makefile
new file mode 100644
index 0000000..1c5113b
--- /dev/null
+++ b/test_problems/Makefile
@@ -0,0 +1,67 @@
+###################################################################################################
+# #
+# 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
+
+LIBS =
+
+LIBS += $(BLASFEO_PATH)/lib/libblasfeo.a
+
+ifeq ($(REF_BLAS), 0)
+LIBS += -lm
+endif
+ifeq ($(REF_BLAS), OPENBLAS)
+LIBS += /opt/openblas/lib/libopenblas.a -pthread -lgfortran -lm
+endif
+ifeq ($(REF_BLAS), BLIS)
+LIBS += /opt/netlib/liblapack.a /opt/blis/lib/libblis.a -lgfortran -lm -fopenmp
+endif
+ifeq ($(REF_BLAS), NETLIB)
+LIBS += /opt/netlib/liblapack.a /opt/netlib/libblas.a -lgfortran -lm
+endif
+ifeq ($(REF_BLAS), MKL)
+LIBS += -Wl,--start-group /opt/intel/mkl/lib/intel64/libmkl_gf_lp64.a /opt/intel/mkl/lib/intel64/libmkl_core.a /opt/intel/mkl/lib/intel64/libmkl_sequential.a -Wl,--end-group -ldl -lpthread -lm
+endif
+ifeq ($(REF_BLAS), ATLAS)
+LIBS += /opt/atlas/lib/liblapack.a /opt/atlas/lib/libcblas.a /opt/atlas/lib/libf77blas.a /opt/atlas/lib/libatlas.a -lgfortran -lm
+endif
+
+#OBJS_TEST = d_tools.o test_d_cond.o
+#OBJS_TEST = test_d_dense.o
+#OBJS_TEST = test_s_dense.o
+OBJS_TEST = d_tools.o test_d_ocp.o
+#OBJS_TEST = s_tools.o test_s_ocp.o
+#OBJS_TEST = d_tools.o test_m_ocp.o
+
+obj: $(OBJS_TEST)
+ $(CC) -o test.out $(OBJS_TEST) -L. libhpipm.a $(LIBS) #-pg
+
+clean:
+ rm -f *.o
+ rm -f test.out
+ rm -f libhpipm.a
+
diff --git a/test_problems/d_tools.c b/test_problems/d_tools.c
new file mode 100644
index 0000000..8aa5c90
--- /dev/null
+++ b/test_problems/d_tools.c
@@ -0,0 +1,715 @@
+/**************************************************************************************************
+* *
+* This file is part of HPMPC. *
+* *
+* HPMPC -- Library for High-Performance implementation of solvers for MPC. *
+* Copyright (C) 2014-2015 by Technical University of Denmark. 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, giaf (at) dtu.dk *
+* *
+**************************************************************************************************/
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <math.h>
+
+#include <blasfeo_d_aux_ext_dep.h>
+
+
+
+// matrix-vector multiplication
+void dgemv_n_3l(int m, int n, double *A, int lda , double *x, double *z)
+ {
+
+ int ii, jj;
+
+ for(ii=0; ii<m; ii++)
+ {
+ z[ii] = 0.0;
+ for(jj=0; jj<n; jj++)
+ {
+ z[ii] += A[ii+lda*jj] * x[jj];
+ }
+ }
+
+ return;
+
+ }
+
+
+// matrix-matrix multiplication
+void dgemm_nn_3l(int m, int n, int k, double *A, int lda , double *B, int ldb, double *C, int ldc)
+ {
+
+ int ii, jj, kk;
+
+ for(jj=0; jj<n; jj++)
+ {
+ for(ii=0; ii<m; ii++)
+ {
+ C[ii+ldc*jj] = 0;
+ for(kk=0; kk<k; kk++)
+ {
+ C[ii+ldc*jj] += A[ii+lda*kk] * B[kk+ldb*jj];
+ }
+ }
+ }
+
+ return;
+
+ }
+
+
+void daxpy_3l(int n, double da, double *dx, double *dy)
+ {
+ int i;
+ for(i=0; i<n; i++)
+ {
+ dy[i] += da*dx[i];
+ }
+ }
+
+
+
+void dscal_3l(int n, double da, double *dx)
+ {
+ int i;
+ for(i=0; i<n; i++)
+ {
+ dx[i] *= da;
+ }
+ }
+
+
+
+/************************************************
+ Routine that copies a matrix
+************************************************/
+void dmcopy(int row, int col, double *A, int lda, double *B, int ldb)
+ {
+ int i, j;
+ for(j=0; j<col; j++)
+ {
+ for(i=0; i<row; i++)
+ {
+ B[i+j*ldb] = A[i+j*lda];
+ }
+ }
+ }
+
+
+
+int idamax_3l(int n, double *x)
+ {
+
+ if(n<=0)
+ return 0;
+ if(n==1)
+ return 0;
+
+ double dabs;
+ double dmax = (x[0]>0 ? x[0] : -x[0]);
+ int idmax = 0;
+ int jj;
+ for(jj=1; jj<n; jj++)
+ {
+ dabs = (x[jj]>0 ? x[jj] : -x[jj]);
+ if(dabs>dmax)
+ {
+ dmax = dabs;
+ idmax = jj;
+ }
+ }
+
+ return idmax;
+
+ }
+
+
+
+void dswap_3l(int n, double *x, int incx, double *y, int incy)
+ {
+
+ if(n<=0)
+ return;
+
+ double temp;
+ int jj;
+ for(jj=0; jj<n; jj++)
+ {
+ temp = x[0];
+ x[0] = y[0];
+ y[0] = temp;
+ x += incx;
+ y += incy;
+ }
+
+ }
+
+
+
+void dger_3l(int m, int n, double alpha, double *x, int incx, double *y, int incy, double *A, int lda)
+ {
+
+ if(m==0 || n==0 || alpha==0.0)
+ return;
+
+ int i, j;
+ double *px, *py, temp;
+
+ py = y;
+ for(j=0; j<n; j++)
+ {
+ temp = alpha * py[0];
+ px = x;
+ for(i=0; i<m; i++)
+ {
+ A[i+lda*j] += px[0] * temp;
+ px += incx;
+ }
+ py += incy;
+ }
+
+ return;
+
+ }
+
+
+
+void dgetf2_3l(int m, int n, double *A, int lda, int *ipiv, int *info)
+ {
+
+ if(m<=0 || n<=0)
+ return;
+
+ int i, j, jp;
+
+ double Ajj;
+
+ int size_min = ( m<n ? m : n );
+
+ for(j=0; j<size_min; j++)
+ // find the pivot and test for singularity
+ {
+ jp = j + idamax_3l(m-j, &A[j+lda*j]);
+ ipiv[j] = jp;
+ if( A[jp+lda*j]!=0)
+ {
+ // apply the interchange to columns 0:n-1
+ if(jp!=j)
+ {
+ dswap_3l(n, &A[j], lda, &A[jp], lda);
+ }
+ // compute elements j+1:m-1 of j-th column
+ if(j<m-1)
+ {
+ Ajj = A[j+lda*j];
+ if( ( Ajj>0 ? Ajj : -Ajj ) >= 2.22e-16 )
+ {
+ dscal_3l(m-j-1, 1.0/Ajj, &A[j+1+lda*j]);
+ }
+ else
+ {
+ for(i=j+1; i<m; i++)
+ {
+ A[i+lda*j] /= Ajj;
+ }
+ }
+ }
+ }
+ else if(*info==0)
+ {
+ *info = j+1;
+ }
+
+ if( j < size_min )
+ {
+ // update trailing submatrix
+ dger_3l(m-j-1, n-j-1, -1.0, &A[j+1+lda*j], 1, &A[j+lda*(j+1)], lda, &A[j+1+lda*(j+1)], lda);
+ }
+
+ }
+
+ return;
+
+ }
+
+
+
+void dlaswp_3l(int n, double *A, int lda, int k1, int k2, int *ipiv)
+ {
+
+ int i, j, k, ix, ix0, i1, i2, n32, ip;
+ double temp;
+
+ ix0 = k1;
+ i1 = k1;
+ i2 = k2;
+
+ n32 = (n/32)*32;
+ if(n32!=0)
+ {
+ for(j=0; j<n32; j+=32)
+ {
+ ix = ix0;
+ for(i=i1; i<i2; i++)
+ {
+ ip = ipiv[ix];
+ if(ip!=i)
+ {
+ for(k=j; k<j+32; k++)
+ {
+ temp = A[i+lda*k];
+ A[i+lda*k] = A[ip+lda*k];
+ A[ip+lda*k] = temp;
+ }
+ }
+ ix++;
+ }
+ }
+ }
+ if(n32!=n)
+ {
+ ix = ix0;
+ for(i=i1; i<i2; i++)
+ {
+ ip = ipiv[ix];
+ if(ip!=i)
+ {
+ for(k=n32; k<n; k++)
+ {
+ temp = A[i+lda*k];
+ A[i+lda*k] = A[ip+lda*k];
+ A[ip+lda*k] = temp;
+ }
+ }
+ ix++;
+ }
+ }
+
+ return;
+
+ }
+
+
+
+// left lower no-transp unit
+void dtrsm_l_l_n_u_3l(int m, int n, double *A, int lda, double *B, int ldb)
+ {
+
+ if(m==0 || n==0)
+ return;
+
+ int i, j, k;
+
+ for(j=0; j<n; j++)
+ {
+ for(k=0; k<m; k++)
+ {
+ for(i=k+1; i<m; i++)
+ {
+ B[i+ldb*j] -= B[k+ldb*j] * A[i+lda*k];
+ }
+ }
+ }
+
+ return;
+
+ }
+
+
+
+// left upper no-transp non-unit
+void dtrsm_l_u_n_n_3l(int m, int n, double *A, int lda, double *B, int ldb)
+ {
+
+ if(m==0 || n==0)
+ return;
+
+ int i, j, k;
+
+ for(j=0; j<n; j++)
+ {
+ for(k=m-1; k>=0; k--)
+ {
+ B[k+ldb*j] /= A[k+lda*k];
+ for(i=0; i<k; i++)
+ {
+ B[i+ldb*j] -= B[k+ldb*j] * A[i+lda*k];
+ }
+ }
+ }
+
+ return;
+
+ }
+
+
+
+void dgetrs_3l(int n, int nrhs, double *A, int lda, int *ipiv, double *B, int ldb, int *info)
+ {
+
+ if(n==0 || nrhs==0)
+ return;
+
+ // solve A * X = B
+
+ // apply row interchanges to the rhs
+ dlaswp_3l(nrhs, B, ldb, 0, n, ipiv);
+
+ // solve L*X = B, overwriting B with X
+ dtrsm_l_l_n_u_3l(n, nrhs, A, lda, B, ldb);
+
+ // solve U*X = B, overwriting B with X
+ dtrsm_l_u_n_n_3l(n, nrhs, A, lda, B, ldb);
+
+ return;
+
+ }
+
+
+
+void dgesv_3l(int n, int nrhs, double *A, int lda, int *ipiv, double *B, int ldb, int *info)
+ {
+
+ // compute the LU factorization of A
+ dgetf2_3l(n, n, A, lda, ipiv, info);
+
+ if(*info==0)
+ {
+ // solve the system A*X = B, overwriting B with X
+ dgetrs_3l(n, nrhs, A, lda, ipiv, B, ldb, info);
+ }
+
+ return;
+
+ }
+
+
+
+/* one norm of a matrix */
+double onenorm(int row, int col, double *ptrA)
+ {
+ double max, temp;
+ int i, j;
+ temp = 0;
+ for(j=0; j<col; j++)
+ {
+ temp = fabs(*(ptrA+j*row));
+ for(i=1; i<row; i++)
+ {
+ temp += fabs(*(ptrA+j*row+i));
+ }
+ if(j==0) max = temp;
+ else if(max>temp) temp = max;
+ }
+ return temp;
+ }
+
+
+
+/* computes the Pade approximation of degree m of the matrix A */
+void padeapprox(int m, int row, double *A)
+ {
+ int row2 = row*row;
+/* int i1 = 1;*/
+/* double d0 = 0;*/
+/* double d1 = 1;*/
+/* double dm1 = -1;*/
+
+ int ii;
+
+ double *U = malloc(row*row*sizeof(double));
+ double *V = malloc(row*row*sizeof(double));
+
+ if(m==3)
+ {
+ double c[] = {120, 60, 12, 1};
+ double *A0 = malloc(row*row*sizeof(double)); for(ii=0; ii<row2; ii++) A0[ii] = 0.0; for(ii=0; ii<row; ii++) A0[ii*(row+1)] = 1.0;
+ double *A2 = malloc(row*row*sizeof(double));
+// char ta = 'n'; double alpha = 1; double beta = 0;
+// dgemm_(&ta, &ta, &row, &row, &row, &alpha, A, &row, A, &row, &beta, A2, &row);
+ dgemm_nn_3l(row, row, row, A, row, A, row, A2, row);
+ double *temp = malloc(row*row*sizeof(double));
+// dscal_(&row2, &d0, temp, &i1);
+ dscal_3l(row2, 0, temp);
+// daxpy_(&row2, &c[3], A2, &i1, temp, &i1);
+ daxpy_3l(row2, c[3], A2, temp);
+// daxpy_(&row2, &c[1], A0, &i1, temp, &i1);
+ daxpy_3l(row2, c[1], A0, temp);
+// dgemm_(&ta, &ta, &row, &row, &row, &alpha, A, &row, temp, &row, &beta, U, &row);
+ dgemm_nn_3l(row, row, row, A, row, temp, row, U, row);
+// dscal_(&row2, &d0, V, &i1);
+ dscal_3l(row2, 0, V);
+// daxpy_(&row2, &c[2], A2, &i1, V, &i1);
+ daxpy_3l(row2, c[2], A2, V);
+// daxpy_(&row2, &c[0], A0, &i1, V, &i1);
+ daxpy_3l(row2, c[0], A0, V);
+ free(A0);
+ free(A2);
+ free(temp);
+ }
+ else if(m==5)
+ {
+ double c[] = {30240, 15120, 3360, 420, 30, 1};
+ double *A0 = malloc(row*row*sizeof(double)); for(ii=0; ii<row2; ii++) A0[ii] = 0.0; for(ii=0; ii<row; ii++) A0[ii*(row+1)] = 1.0;
+ double *A2 = malloc(row*row*sizeof(double));
+ double *A4 = malloc(row*row*sizeof(double));
+// char ta = 'n'; double alpha = 1; double beta = 0;
+// dgemm_(&ta, &ta, &row, &row, &row, &alpha, A, &row, A, &row, &beta, A2, &row);
+ dgemm_nn_3l(row, row, row, A, row, A, row, A2, row);
+// dgemm_(&ta, &ta, &row, &row, &row, &alpha, A2, &row, A2, &row, &beta, A4, &row);
+ dgemm_nn_3l(row, row, row, A2, row, A2, row, A4, row);
+ dmcopy(row, row, A4, row, V, row);
+ double *temp = malloc(row*row*sizeof(double));
+ dmcopy(row, row, A4, row, temp, row);
+// daxpy_(&row2, &c[3], A2, &i1, temp, &i1);
+ daxpy_3l(row2, c[3], A2, temp);
+// daxpy_(&row2, &c[1], A0, &i1, temp, &i1);
+ daxpy_3l(row2, c[1], A0, temp);
+// dgemm_(&ta, &ta, &row, &row, &row, &alpha, A, &row, temp, &row, &beta, U, &row);
+ dgemm_nn_3l(row, row, row, A, row, temp, row, U, row);
+// dscal_(&row2, &c[4], V, &i1);
+ dscal_3l(row2, c[4], V);
+// daxpy_(&row2, &c[2], A2, &i1, V, &i1);
+ daxpy_3l(row2, c[2], A2, V);
+// daxpy_(&row2, &c[0], A0, &i1, V, &i1);
+ daxpy_3l(row2, c[0], A0, V);
+ free(A0);
+ free(A2);
+ free(A4);
+ free(temp);
+ }
+ else if(m==7)
+ {
+ double c[] = {17297280, 8648640, 1995840, 277200, 25200, 1512, 56, 1};
+ double *A0 = malloc(row*row*sizeof(double)); for(ii=0; ii<row2; ii++) A0[ii] = 0.0; for(ii=0; ii<row; ii++) A0[ii*(row+1)] = 1.0;
+ double *A2 = malloc(row*row*sizeof(double));
+ double *A4 = malloc(row*row*sizeof(double));
+ double *A6 = malloc(row*row*sizeof(double));
+// char ta = 'n'; double alpha = 1; double beta = 1;
+// dgemm_(&ta, &ta, &row, &row, &row, &alpha, A, &row, A, &row, &beta, A2, &row);
+ dgemm_nn_3l(row, row, row, A, row, A, row, A2, row);
+// dgemm_(&ta, &ta, &row, &row, &row, &alpha, A2, &row, A2, &row, &beta, A4, &row);
+ dgemm_nn_3l(row, row, row, A2, row, A2, row, A4, row);
+// dgemm_(&ta, &ta, &row, &row, &row, &alpha, A4, &row, A2, &row, &beta, A6, &row);
+ dgemm_nn_3l(row, row, row, A4, row, A2, row, A6, row);
+ double *temp = malloc(row*row*sizeof(double));
+// dscal_(&row2, &d0, temp, &i1);
+ dscal_3l(row2, 0, temp);
+// daxpy_(&row2, &c[3], A2, &i1, temp, &i1);
+ daxpy_3l(row2, c[3], A2, temp);
+// daxpy_(&row2, &c[1], A0, &i1, temp, &i1);
+ daxpy_3l(row2, c[1], A0, temp);
+// daxpy_(&row2, &c[5], A4, &i1, temp, &i1);
+ daxpy_3l(row2, c[5], A4, temp);
+// daxpy_(&row2, &c[7], A6, &i1, temp, &i1);
+ daxpy_3l(row2, c[7], A6, temp);
+// dgemm_(&ta, &ta, &row, &row, &row, &alpha, A, &row, temp, &row, &beta, U, &row);
+ dgemm_nn_3l(row, row, row, A, row, temp, row, U, row);
+// dscal_(&row2, &d0, V, &i1);
+ dscal_3l(row2, 0, V);
+// daxpy_(&row2, &c[2], A2, &i1, V, &i1);
+ daxpy_3l(row2, c[2], A2, V);
+// daxpy_(&row2, &c[0], A0, &i1, V, &i1);
+ daxpy_3l(row2, c[0], A0, V);
+// daxpy_(&row2, &c[4], A4, &i1, V, &i1);
+ daxpy_3l(row2, c[4], A4, V);
+// daxpy_(&row2, &c[6], A6, &i1, V, &i1);
+ daxpy_3l(row2, c[6], A6, V);
+ free(A0);
+ free(A2);
+ free(A4);
+ free(A6);
+ free(temp);
+ }
+ else if(m==9)
+ {
+ double c[] = {17643225600, 8821612800, 2075673600, 302702400, 30270240, 2162160, 110880, 3960, 90, 1};
+ double *A0 = malloc(row*row*sizeof(double)); for(ii=0; ii<row2; ii++) A0[ii] = 0.0; for(ii=0; ii<row; ii++) A0[ii*(row+1)] = 1.0;
+ double *A2 = malloc(row*row*sizeof(double));
+ double *A4 = malloc(row*row*sizeof(double));
+ double *A6 = malloc(row*row*sizeof(double));
+ double *A8 = malloc(row*row*sizeof(double));
+// char ta = 'n'; double alpha = 1; double beta = 0;
+// dgemm_(&ta, &ta, &row, &row, &row, &alpha, A, &row, A, &row, &beta, A2, &row);
+ dgemm_nn_3l(row, row, row, A, row, A, row, A2, row);
+// dgemm_(&ta, &ta, &row, &row, &row, &alpha, A2, &row, A2, &row, &beta, A4, &row);
+ dgemm_nn_3l(row, row, row, A2, row, A2, row, A4, row);
+// dgemm_(&ta, &ta, &row, &row, &row, &alpha, A4, &row, A2, &row, &beta, A6, &row);
+ dgemm_nn_3l(row, row, row, A4, row, A2, row, A6, row);
+// dgemm_(&ta, &ta, &row, &row, &row, &alpha, A6, &row, A2, &row, &beta, A8, &row);
+ dgemm_nn_3l(row, row, row, A6, row, A2, row, A8, row);
+ dmcopy(row, row, A8, row, V, row);
+ double *temp = malloc(row*row*sizeof(double));
+ dmcopy(row, row, A8, row, temp, row);
+// daxpy_(&row2, &c[3], A2, &i1, temp, &i1);
+ daxpy_3l(row2, c[3], A2, temp);
+// daxpy_(&row2, &c[1], A0, &i1, temp, &i1);
+ daxpy_3l(row2, c[1], A0, temp);
+// daxpy_(&row2, &c[5], A4, &i1, temp, &i1);
+ daxpy_3l(row2, c[5], A4, temp);
+// daxpy_(&row2, &c[7], A6, &i1, temp, &i1);
+ daxpy_3l(row2, c[7], A6, temp);
+// dgemm_(&ta, &ta, &row, &row, &row, &alpha, A, &row, temp, &row, &beta, U, &row);
+ dgemm_nn_3l(row, row, row, A, row, temp, row, U, row);
+// dscal_(&row2, &c[8], V, &i1);
+ dscal_3l(row2, c[8], V);
+// daxpy_(&row2, &c[2], A2, &i1, V, &i1);
+ daxpy_3l(row2, c[2], A2, V);
+// daxpy_(&row2, &c[0], A0, &i1, V, &i1);
+ daxpy_3l(row2, c[0], A0, V);
+// daxpy_(&row2, &c[4], A4, &i1, V, &i1);
+ daxpy_3l(row2, c[4], A4, V);
+// daxpy_(&row2, &c[6], A6, &i1, V, &i1);
+ daxpy_3l(row2, c[6], A6, V);
+ free(A0);
+ free(A2);
+ free(A4);
+ free(A6);
+ free(A8);
+ free(temp);
+ }
+ else if(m==13) // tested
+ {
+ double c[] = {64764752532480000, 32382376266240000, 7771770303897600, 1187353796428800, 129060195264000, 10559470521600, 670442572800, 33522128640, 1323241920, 40840800, 960960, 16380, 182, 1};
+ double *A0 = malloc(row*row*sizeof(double)); for(ii=0; ii<row2; ii++) A0[ii] = 0.0; for(ii=0; ii<row; ii++) A0[ii*(row+1)] = 1.0;
+ double *A2 = malloc(row*row*sizeof(double));
+ double *A4 = malloc(row*row*sizeof(double));
+ double *A6 = malloc(row*row*sizeof(double));
+// char ta = 'n'; double alpha = 1; double beta = 0;
+// dgemm_(&ta, &ta, &row, &row, &row, &alpha, A, &row, A, &row, &beta, A2, &row);
+ dgemm_nn_3l(row, row, row, A, row, A, row, A2, row);
+// dgemm_(&ta, &ta, &row, &row, &row, &alpha, A2, &row, A2, &row, &beta, A4, &row);
+ dgemm_nn_3l(row, row, row, A2, row, A2, row, A4, row);
+// dgemm_(&ta, &ta, &row, &row, &row, &alpha, A4, &row, A2, &row, &beta, A6, &row);
+ dgemm_nn_3l(row, row, row, A4, row, A2, row, A6, row);
+ dmcopy(row, row, A2, row, U, row);
+ double *temp = malloc(row*row*sizeof(double));
+// dscal_(&row2, &c[9], U, &i1);
+ dscal_3l(row2, c[9], U);
+// daxpy_(&row2, &c[11], A4, &i1, U, &i1);
+ daxpy_3l(row2, c[11], A4, U);
+// daxpy_(&row2, &c[13], A6, &i1, U, &i1);
+ daxpy_3l(row2, c[13], A6, U);
+// dgemm_(&ta, &ta, &row, &row, &row, &alpha, A6, &row, U, &row, &beta, temp, &row);
+ dgemm_nn_3l(row, row, row, A6, row, U, row, temp, row);
+// daxpy_(&row2, &c[7], A6, &i1, temp, &i1);
+ daxpy_3l(row2, c[7], A6, temp);
+// daxpy_(&row2, &c[5], A4, &i1, temp, &i1);
+ daxpy_3l(row2, c[5], A4, temp);
+// daxpy_(&row2, &c[3], A2, &i1, temp, &i1);
+ daxpy_3l(row2, c[3], A2, temp);
+// daxpy_(&row2, &c[1], A0, &i1, temp, &i1);
+ daxpy_3l(row2, c[1], A0, temp);
+// dgemm_(&ta, &ta, &row, &row, &row, &alpha, A, &row, temp, &row, &beta, U, &row);
+ dgemm_nn_3l(row, row, row, A, row, temp, row, U, row);
+ dmcopy(row, row, A2, row, temp, row);
+// dscal_(&row2, &c[8], V, &i1);
+ dscal_3l(row2, c[8], V);
+// daxpy_(&row2, &c[12], A6, &i1, temp, &i1);
+ daxpy_3l(row2, c[12], A6, temp);
+// daxpy_(&row2, &c[10], A4, &i1, temp, &i1);
+ daxpy_3l(row2, c[10], A4, temp);
+// dgemm_(&ta, &ta, &row, &row, &row, &alpha, A6, &row, temp, &row, &beta, V, &row);
+ dgemm_nn_3l(row, row, row, A6, row, temp, row, V, row);
+// daxpy_(&row2, &c[6], A6, &i1, V, &i1);
+ daxpy_3l(row2, c[6], A6, V);
+// daxpy_(&row2, &c[4], A4, &i1, V, &i1);
+ daxpy_3l(row2, c[4], A4, V);
+// daxpy_(&row2, &c[2], A2, &i1, V, &i1);
+ daxpy_3l(row2, c[2], A2, V);
+// daxpy_(&row2, &c[0], A0, &i1, V, &i1);
+ daxpy_3l(row2, c[0], A0, V);
+ free(A0);
+ free(A2);
+ free(A4);
+ free(A6);
+ free(temp);
+ }
+ else
+ {
+ printf("%s\n", "Wrong Pade approximatin degree");
+ exit(1);
+ }
+ double *D = malloc(row*row*sizeof(double));
+// dcopy_(&row2, V, &i1, A, &i1);
+ dmcopy(row, row, V, row, A, row);
+// daxpy_(&row2, &d1, U, &i1, A, &i1);
+ daxpy_3l(row2, 1.0, U, A);
+// dcopy_(&row2, V, &i1, D, &i1);
+ dmcopy(row, row, V, row, D, row);
+// daxpy_(&row2, &dm1, U, &i1, D, &i1);
+ daxpy_3l(row2, -1.0, U, D);
+ int *ipiv = (int *) malloc(row*sizeof(int));
+ int info = 0;
+// dgesv_(&row, &row, D, &row, ipiv, A, &row, &info);
+ dgesv_3l(row, row, D, row, ipiv, A, row, &info);
+ free(ipiv);
+ free(D);
+ free(U);
+ free(V);
+ }
+
+
+
+void expm(int row, double *A)
+ {
+
+ int i;
+
+ int m_vals[] = {3, 5, 7, 9, 13};
+ double theta[] = {0.01495585217958292, 0.2539398330063230, 0.9504178996162932, 2.097847961257068, 5.371920351148152};
+ int lentheta = 5;
+
+ double normA = onenorm(row, row, A);
+
+ if(normA<=theta[4])
+ {
+ for(i=0; i<lentheta; i++)
+ {
+ if(normA<=theta[i])
+ {
+ padeapprox(m_vals[i], row, A);
+ break;
+ }
+ }
+ }
+ else
+ {
+ int s;
+ double t = frexp(normA/(theta[4]), &s);
+ s = s - (t==0.5);
+ t = pow(2,-s);
+ int row2 = row*row;
+/* int i1 = 1;*/
+// dscal_(&row2, &t, A, &i1);
+ dscal_3l(row2, t, A);
+ padeapprox(m_vals[4], row, A);
+ double *temp = malloc(row*row*sizeof(double));
+// char ta = 'n'; double alpha = 1; double beta = 0;
+ for(i=0; i<s; i++)
+ {
+// dgemm_(&ta, &ta, &row, &row, &row, &alpha, A, &row, A, &row, &beta, temp, &row);
+ dgemm_nn_3l(row, row, row, A, row, A, row, temp, row);
+ dmcopy(row, row, temp, row, A, row);
+ }
+ free(temp);
+ }
+ }
+
+
diff --git a/test_problems/d_tools.h b/test_problems/d_tools.h
new file mode 100644
index 0000000..137b9ad
--- /dev/null
+++ b/test_problems/d_tools.h
@@ -0,0 +1,38 @@
+/**************************************************************************************************
+* *
+* This file is part of HPMPC. *
+* *
+* HPMPC -- Library for High-Performance implementation of solvers for MPC. *
+* Copyright (C) 2014-2015 by Technical University of Denmark. 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, giaf (at) dtu.dk *
+* *
+**************************************************************************************************/
+
+void dgemv_n_3l(int m, int n, double *A, int lda , double *x, double *z);
+void dgemm_nn_3l(int m, int n, int k, double *A, int lda , double *B, int ldb, double *C, int ldc);
+void daxpy_3l(int n, double da, double *dx, double *dy);
+void dscal_3l(int n, double da, double *dx);
+
+/* copies a matrix into another matrix */
+void dmcopy(int row, int col, double *ptrA, int lda, double *ptrB, int ldb);
+
+/* solution of a system of linear equations */
+void dgesv_3l(int n, int nrhs, double *A, int lda, int *ipiv, double *B, int ldb, int *info);
+
+/* matrix exponential */
+void expm(int row, double *A);
diff --git a/test_problems/s_tools.c b/test_problems/s_tools.c
new file mode 100644
index 0000000..36a655f
--- /dev/null
+++ b/test_problems/s_tools.c
@@ -0,0 +1,715 @@
+/**************************************************************************************************
+* *
+* This file is part of HPMPC. *
+* *
+* HPMPC -- Library for High-Performance implementation of solvers for MPC. *
+* Copyright (C) 2014-2015 by Technical University of Denmark. 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, giaf (at) dtu.dk *
+* *
+**************************************************************************************************/
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <math.h>
+
+#include <blasfeo_s_aux_ext_dep.h>
+
+
+
+// matrix-vector multiplication
+void sgemv_n_3l(int m, int n, float *A, int lda , float *x, float *z)
+ {
+
+ int ii, jj;
+
+ for(ii=0; ii<m; ii++)
+ {
+ z[ii] = 0.0;
+ for(jj=0; jj<n; jj++)
+ {
+ z[ii] += A[ii+lda*jj] * x[jj];
+ }
+ }
+
+ return;
+
+ }
+
+
+// matrix-matrix multiplication
+void sgemm_nn_3l(int m, int n, int k, float *A, int lda , float *B, int ldb, float *C, int ldc)
+ {
+
+ int ii, jj, kk;
+
+ for(jj=0; jj<n; jj++)
+ {
+ for(ii=0; ii<m; ii++)
+ {
+ C[ii+ldc*jj] = 0;
+ for(kk=0; kk<k; kk++)
+ {
+ C[ii+ldc*jj] += A[ii+lda*kk] * B[kk+ldb*jj];
+ }
+ }
+ }
+
+ return;
+
+ }
+
+
+void saxpy_3l(int n, float da, float *dx, float *dy)
+ {
+ int i;
+ for(i=0; i<n; i++)
+ {
+ dy[i] += da*dx[i];
+ }
+ }
+
+
+
+void sscal_3l(int n, float da, float *dx)
+ {
+ int i;
+ for(i=0; i<n; i++)
+ {
+ dx[i] *= da;
+ }
+ }
+
+
+
+/************************************************
+ Routine that copies a matrix
+************************************************/
+void smcopy(int row, int col, float *A, int lda, float *B, int ldb)
+ {
+ int i, j;
+ for(j=0; j<col; j++)
+ {
+ for(i=0; i<row; i++)
+ {
+ B[i+j*ldb] = A[i+j*lda];
+ }
+ }
+ }
+
+
+
+int idamax_3l(int n, float *x)
+ {
+
+ if(n<=0)
+ return 0;
+ if(n==1)
+ return 0;
+
+ float dabs;
+ float dmax = (x[0]>0 ? x[0] : -x[0]);
+ int idmax = 0;
+ int jj;
+ for(jj=1; jj<n; jj++)
+ {
+ dabs = (x[jj]>0 ? x[jj] : -x[jj]);
+ if(dabs>dmax)
+ {
+ dmax = dabs;
+ idmax = jj;
+ }
+ }
+
+ return idmax;
+
+ }
+
+
+
+void sswap_3l(int n, float *x, int incx, float *y, int incy)
+ {
+
+ if(n<=0)
+ return;
+
+ float temp;
+ int jj;
+ for(jj=0; jj<n; jj++)
+ {
+ temp = x[0];
+ x[0] = y[0];
+ y[0] = temp;
+ x += incx;
+ y += incy;
+ }
+
+ }
+
+
+
+void sger_3l(int m, int n, float alpha, float *x, int incx, float *y, int incy, float *A, int lda)
+ {
+
+ if(m==0 || n==0 || alpha==0.0)
+ return;
+
+ int i, j;
+ float *px, *py, temp;
+
+ py = y;
+ for(j=0; j<n; j++)
+ {
+ temp = alpha * py[0];
+ px = x;
+ for(i=0; i<m; i++)
+ {
+ A[i+lda*j] += px[0] * temp;
+ px += incx;
+ }
+ py += incy;
+ }
+
+ return;
+
+ }
+
+
+
+void sgetf2_3l(int m, int n, float *A, int lda, int *ipiv, int *info)
+ {
+
+ if(m<=0 || n<=0)
+ return;
+
+ int i, j, jp;
+
+ float Ajj;
+
+ int size_min = ( m<n ? m : n );
+
+ for(j=0; j<size_min; j++)
+ // find the pivot and test for singularity
+ {
+ jp = j + idamax_3l(m-j, &A[j+lda*j]);
+ ipiv[j] = jp;
+ if( A[jp+lda*j]!=0)
+ {
+ // apply the interchange to columns 0:n-1
+ if(jp!=j)
+ {
+ sswap_3l(n, &A[j], lda, &A[jp], lda);
+ }
+ // compute elements j+1:m-1 of j-th column
+ if(j<m-1)
+ {
+ Ajj = A[j+lda*j];
+ if( ( Ajj>0 ? Ajj : -Ajj ) >= 2.22e-16 )
+ {
+ sscal_3l(m-j-1, 1.0/Ajj, &A[j+1+lda*j]);
+ }
+ else
+ {
+ for(i=j+1; i<m; i++)
+ {
+ A[i+lda*j] /= Ajj;
+ }
+ }
+ }
+ }
+ else if(*info==0)
+ {
+ *info = j+1;
+ }
+
+ if( j < size_min )
+ {
+ // update trailing submatrix
+ sger_3l(m-j-1, n-j-1, -1.0, &A[j+1+lda*j], 1, &A[j+lda*(j+1)], lda, &A[j+1+lda*(j+1)], lda);
+ }
+
+ }
+
+ return;
+
+ }
+
+
+
+void slaswp_3l(int n, float *A, int lda, int k1, int k2, int *ipiv)
+ {
+
+ int i, j, k, ix, ix0, i1, i2, n32, ip;
+ float temp;
+
+ ix0 = k1;
+ i1 = k1;
+ i2 = k2;
+
+ n32 = (n/32)*32;
+ if(n32!=0)
+ {
+ for(j=0; j<n32; j+=32)
+ {
+ ix = ix0;
+ for(i=i1; i<i2; i++)
+ {
+ ip = ipiv[ix];
+ if(ip!=i)
+ {
+ for(k=j; k<j+32; k++)
+ {
+ temp = A[i+lda*k];
+ A[i+lda*k] = A[ip+lda*k];
+ A[ip+lda*k] = temp;
+ }
+ }
+ ix++;
+ }
+ }
+ }
+ if(n32!=n)
+ {
+ ix = ix0;
+ for(i=i1; i<i2; i++)
+ {
+ ip = ipiv[ix];
+ if(ip!=i)
+ {
+ for(k=n32; k<n; k++)
+ {
+ temp = A[i+lda*k];
+ A[i+lda*k] = A[ip+lda*k];
+ A[ip+lda*k] = temp;
+ }
+ }
+ ix++;
+ }
+ }
+
+ return;
+
+ }
+
+
+
+// left lower no-transp unit
+void strsm_l_l_n_u_3l(int m, int n, float *A, int lda, float *B, int ldb)
+ {
+
+ if(m==0 || n==0)
+ return;
+
+ int i, j, k;
+
+ for(j=0; j<n; j++)
+ {
+ for(k=0; k<m; k++)
+ {
+ for(i=k+1; i<m; i++)
+ {
+ B[i+ldb*j] -= B[k+ldb*j] * A[i+lda*k];
+ }
+ }
+ }
+
+ return;
+
+ }
+
+
+
+// left upper no-transp non-unit
+void strsm_l_u_n_n_3l(int m, int n, float *A, int lda, float *B, int ldb)
+ {
+
+ if(m==0 || n==0)
+ return;
+
+ int i, j, k;
+
+ for(j=0; j<n; j++)
+ {
+ for(k=m-1; k>=0; k--)
+ {
+ B[k+ldb*j] /= A[k+lda*k];
+ for(i=0; i<k; i++)
+ {
+ B[i+ldb*j] -= B[k+ldb*j] * A[i+lda*k];
+ }
+ }
+ }
+
+ return;
+
+ }
+
+
+
+void sgetrs_3l(int n, int nrhs, float *A, int lda, int *ipiv, float *B, int ldb, int *info)
+ {
+
+ if(n==0 || nrhs==0)
+ return;
+
+ // solve A * X = B
+
+ // apply row interchanges to the rhs
+ slaswp_3l(nrhs, B, ldb, 0, n, ipiv);
+
+ // solve L*X = B, overwriting B with X
+ strsm_l_l_n_u_3l(n, nrhs, A, lda, B, ldb);
+
+ // solve U*X = B, overwriting B with X
+ strsm_l_u_n_n_3l(n, nrhs, A, lda, B, ldb);
+
+ return;
+
+ }
+
+
+
+void sgesv_3l(int n, int nrhs, float *A, int lda, int *ipiv, float *B, int ldb, int *info)
+ {
+
+ // compute the LU factorization of A
+ sgetf2_3l(n, n, A, lda, ipiv, info);
+
+ if(*info==0)
+ {
+ // solve the system A*X = B, overwriting B with X
+ sgetrs_3l(n, nrhs, A, lda, ipiv, B, ldb, info);
+ }
+
+ return;
+
+ }
+
+
+
+/* one norm of a matrix */
+float onenorm(int row, int col, float *ptrA)
+ {
+ float max, temp;
+ int i, j;
+ temp = 0;
+ for(j=0; j<col; j++)
+ {
+ temp = fabs(*(ptrA+j*row));
+ for(i=1; i<row; i++)
+ {
+ temp += fabs(*(ptrA+j*row+i));
+ }
+ if(j==0) max = temp;
+ else if(max>temp) temp = max;
+ }
+ return temp;
+ }
+
+
+
+/* computes the Pade approximation of degree m of the matrix A */
+void padeapprox(int m, int row, float *A)
+ {
+ int row2 = row*row;
+/* int i1 = 1;*/
+/* float d0 = 0;*/
+/* float d1 = 1;*/
+/* float dm1 = -1;*/
+
+ int ii;
+
+ float *U = malloc(row*row*sizeof(float));
+ float *V = malloc(row*row*sizeof(float));
+
+ if(m==3)
+ {
+ float c[] = {120, 60, 12, 1};
+ float *A0 = malloc(row*row*sizeof(float)); for(ii=0; ii<row2; ii++) A0[ii] = 0.0; for(ii=0; ii<row; ii++) A0[ii*(row+1)] = 1.0;
+ float *A2 = malloc(row*row*sizeof(float));
+// char ta = 'n'; float alpha = 1; float beta = 0;
+// sgemm_(&ta, &ta, &row, &row, &row, &alpha, A, &row, A, &row, &beta, A2, &row);
+ sgemm_nn_3l(row, row, row, A, row, A, row, A2, row);
+ float *temp = malloc(row*row*sizeof(float));
+// sscal_(&row2, &d0, temp, &i1);
+ sscal_3l(row2, 0, temp);
+// saxpy_(&row2, &c[3], A2, &i1, temp, &i1);
+ saxpy_3l(row2, c[3], A2, temp);
+// saxpy_(&row2, &c[1], A0, &i1, temp, &i1);
+ saxpy_3l(row2, c[1], A0, temp);
+// sgemm_(&ta, &ta, &row, &row, &row, &alpha, A, &row, temp, &row, &beta, U, &row);
+ sgemm_nn_3l(row, row, row, A, row, temp, row, U, row);
+// sscal_(&row2, &d0, V, &i1);
+ sscal_3l(row2, 0, V);
+// saxpy_(&row2, &c[2], A2, &i1, V, &i1);
+ saxpy_3l(row2, c[2], A2, V);
+// saxpy_(&row2, &c[0], A0, &i1, V, &i1);
+ saxpy_3l(row2, c[0], A0, V);
+ free(A0);
+ free(A2);
+ free(temp);
+ }
+ else if(m==5)
+ {
+ float c[] = {30240, 15120, 3360, 420, 30, 1};
+ float *A0 = malloc(row*row*sizeof(float)); for(ii=0; ii<row2; ii++) A0[ii] = 0.0; for(ii=0; ii<row; ii++) A0[ii*(row+1)] = 1.0;
+ float *A2 = malloc(row*row*sizeof(float));
+ float *A4 = malloc(row*row*sizeof(float));
+// char ta = 'n'; float alpha = 1; float beta = 0;
+// sgemm_(&ta, &ta, &row, &row, &row, &alpha, A, &row, A, &row, &beta, A2, &row);
+ sgemm_nn_3l(row, row, row, A, row, A, row, A2, row);
+// sgemm_(&ta, &ta, &row, &row, &row, &alpha, A2, &row, A2, &row, &beta, A4, &row);
+ sgemm_nn_3l(row, row, row, A2, row, A2, row, A4, row);
+ smcopy(row, row, A4, row, V, row);
+ float *temp = malloc(row*row*sizeof(float));
+ smcopy(row, row, A4, row, temp, row);
+// saxpy_(&row2, &c[3], A2, &i1, temp, &i1);
+ saxpy_3l(row2, c[3], A2, temp);
+// saxpy_(&row2, &c[1], A0, &i1, temp, &i1);
+ saxpy_3l(row2, c[1], A0, temp);
+// sgemm_(&ta, &ta, &row, &row, &row, &alpha, A, &row, temp, &row, &beta, U, &row);
+ sgemm_nn_3l(row, row, row, A, row, temp, row, U, row);
+// sscal_(&row2, &c[4], V, &i1);
+ sscal_3l(row2, c[4], V);
+// saxpy_(&row2, &c[2], A2, &i1, V, &i1);
+ saxpy_3l(row2, c[2], A2, V);
+// saxpy_(&row2, &c[0], A0, &i1, V, &i1);
+ saxpy_3l(row2, c[0], A0, V);
+ free(A0);
+ free(A2);
+ free(A4);
+ free(temp);
+ }
+ else if(m==7)
+ {
+ float c[] = {17297280, 8648640, 1995840, 277200, 25200, 1512, 56, 1};
+ float *A0 = malloc(row*row*sizeof(float)); for(ii=0; ii<row2; ii++) A0[ii] = 0.0; for(ii=0; ii<row; ii++) A0[ii*(row+1)] = 1.0;
+ float *A2 = malloc(row*row*sizeof(float));
+ float *A4 = malloc(row*row*sizeof(float));
+ float *A6 = malloc(row*row*sizeof(float));
+// char ta = 'n'; float alpha = 1; float beta = 1;
+// sgemm_(&ta, &ta, &row, &row, &row, &alpha, A, &row, A, &row, &beta, A2, &row);
+ sgemm_nn_3l(row, row, row, A, row, A, row, A2, row);
+// sgemm_(&ta, &ta, &row, &row, &row, &alpha, A2, &row, A2, &row, &beta, A4, &row);
+ sgemm_nn_3l(row, row, row, A2, row, A2, row, A4, row);
+// sgemm_(&ta, &ta, &row, &row, &row, &alpha, A4, &row, A2, &row, &beta, A6, &row);
+ sgemm_nn_3l(row, row, row, A4, row, A2, row, A6, row);
+ float *temp = malloc(row*row*sizeof(float));
+// sscal_(&row2, &d0, temp, &i1);
+ sscal_3l(row2, 0, temp);
+// saxpy_(&row2, &c[3], A2, &i1, temp, &i1);
+ saxpy_3l(row2, c[3], A2, temp);
+// saxpy_(&row2, &c[1], A0, &i1, temp, &i1);
+ saxpy_3l(row2, c[1], A0, temp);
+// saxpy_(&row2, &c[5], A4, &i1, temp, &i1);
+ saxpy_3l(row2, c[5], A4, temp);
+// saxpy_(&row2, &c[7], A6, &i1, temp, &i1);
+ saxpy_3l(row2, c[7], A6, temp);
+// sgemm_(&ta, &ta, &row, &row, &row, &alpha, A, &row, temp, &row, &beta, U, &row);
+ sgemm_nn_3l(row, row, row, A, row, temp, row, U, row);
+// sscal_(&row2, &d0, V, &i1);
+ sscal_3l(row2, 0, V);
+// saxpy_(&row2, &c[2], A2, &i1, V, &i1);
+ saxpy_3l(row2, c[2], A2, V);
+// saxpy_(&row2, &c[0], A0, &i1, V, &i1);
+ saxpy_3l(row2, c[0], A0, V);
+// saxpy_(&row2, &c[4], A4, &i1, V, &i1);
+ saxpy_3l(row2, c[4], A4, V);
+// saxpy_(&row2, &c[6], A6, &i1, V, &i1);
+ saxpy_3l(row2, c[6], A6, V);
+ free(A0);
+ free(A2);
+ free(A4);
+ free(A6);
+ free(temp);
+ }
+ else if(m==9)
+ {
+ float c[] = {17643225600, 8821612800, 2075673600, 302702400, 30270240, 2162160, 110880, 3960, 90, 1};
+ float *A0 = malloc(row*row*sizeof(float)); for(ii=0; ii<row2; ii++) A0[ii] = 0.0; for(ii=0; ii<row; ii++) A0[ii*(row+1)] = 1.0;
+ float *A2 = malloc(row*row*sizeof(float));
+ float *A4 = malloc(row*row*sizeof(float));
+ float *A6 = malloc(row*row*sizeof(float));
+ float *A8 = malloc(row*row*sizeof(float));
+// char ta = 'n'; float alpha = 1; float beta = 0;
+// sgemm_(&ta, &ta, &row, &row, &row, &alpha, A, &row, A, &row, &beta, A2, &row);
+ sgemm_nn_3l(row, row, row, A, row, A, row, A2, row);
+// sgemm_(&ta, &ta, &row, &row, &row, &alpha, A2, &row, A2, &row, &beta, A4, &row);
+ sgemm_nn_3l(row, row, row, A2, row, A2, row, A4, row);
+// sgemm_(&ta, &ta, &row, &row, &row, &alpha, A4, &row, A2, &row, &beta, A6, &row);
+ sgemm_nn_3l(row, row, row, A4, row, A2, row, A6, row);
+// sgemm_(&ta, &ta, &row, &row, &row, &alpha, A6, &row, A2, &row, &beta, A8, &row);
+ sgemm_nn_3l(row, row, row, A6, row, A2, row, A8, row);
+ smcopy(row, row, A8, row, V, row);
+ float *temp = malloc(row*row*sizeof(float));
+ smcopy(row, row, A8, row, temp, row);
+// saxpy_(&row2, &c[3], A2, &i1, temp, &i1);
+ saxpy_3l(row2, c[3], A2, temp);
+// saxpy_(&row2, &c[1], A0, &i1, temp, &i1);
+ saxpy_3l(row2, c[1], A0, temp);
+// saxpy_(&row2, &c[5], A4, &i1, temp, &i1);
+ saxpy_3l(row2, c[5], A4, temp);
+// saxpy_(&row2, &c[7], A6, &i1, temp, &i1);
+ saxpy_3l(row2, c[7], A6, temp);
+// sgemm_(&ta, &ta, &row, &row, &row, &alpha, A, &row, temp, &row, &beta, U, &row);
+ sgemm_nn_3l(row, row, row, A, row, temp, row, U, row);
+// sscal_(&row2, &c[8], V, &i1);
+ sscal_3l(row2, c[8], V);
+// saxpy_(&row2, &c[2], A2, &i1, V, &i1);
+ saxpy_3l(row2, c[2], A2, V);
+// saxpy_(&row2, &c[0], A0, &i1, V, &i1);
+ saxpy_3l(row2, c[0], A0, V);
+// saxpy_(&row2, &c[4], A4, &i1, V, &i1);
+ saxpy_3l(row2, c[4], A4, V);
+// saxpy_(&row2, &c[6], A6, &i1, V, &i1);
+ saxpy_3l(row2, c[6], A6, V);
+ free(A0);
+ free(A2);
+ free(A4);
+ free(A6);
+ free(A8);
+ free(temp);
+ }
+ else if(m==13) // tested
+ {
+ float c[] = {64764752532480000, 32382376266240000, 7771770303897600, 1187353796428800, 129060195264000, 10559470521600, 670442572800, 33522128640, 1323241920, 40840800, 960960, 16380, 182, 1};
+ float *A0 = malloc(row*row*sizeof(float)); for(ii=0; ii<row2; ii++) A0[ii] = 0.0; for(ii=0; ii<row; ii++) A0[ii*(row+1)] = 1.0;
+ float *A2 = malloc(row*row*sizeof(float));
+ float *A4 = malloc(row*row*sizeof(float));
+ float *A6 = malloc(row*row*sizeof(float));
+// char ta = 'n'; float alpha = 1; float beta = 0;
+// sgemm_(&ta, &ta, &row, &row, &row, &alpha, A, &row, A, &row, &beta, A2, &row);
+ sgemm_nn_3l(row, row, row, A, row, A, row, A2, row);
+// sgemm_(&ta, &ta, &row, &row, &row, &alpha, A2, &row, A2, &row, &beta, A4, &row);
+ sgemm_nn_3l(row, row, row, A2, row, A2, row, A4, row);
+// sgemm_(&ta, &ta, &row, &row, &row, &alpha, A4, &row, A2, &row, &beta, A6, &row);
+ sgemm_nn_3l(row, row, row, A4, row, A2, row, A6, row);
+ smcopy(row, row, A2, row, U, row);
+ float *temp = malloc(row*row*sizeof(float));
+// sscal_(&row2, &c[9], U, &i1);
+ sscal_3l(row2, c[9], U);
+// saxpy_(&row2, &c[11], A4, &i1, U, &i1);
+ saxpy_3l(row2, c[11], A4, U);
+// saxpy_(&row2, &c[13], A6, &i1, U, &i1);
+ saxpy_3l(row2, c[13], A6, U);
+// sgemm_(&ta, &ta, &row, &row, &row, &alpha, A6, &row, U, &row, &beta, temp, &row);
+ sgemm_nn_3l(row, row, row, A6, row, U, row, temp, row);
+// saxpy_(&row2, &c[7], A6, &i1, temp, &i1);
+ saxpy_3l(row2, c[7], A6, temp);
+// saxpy_(&row2, &c[5], A4, &i1, temp, &i1);
+ saxpy_3l(row2, c[5], A4, temp);
+// saxpy_(&row2, &c[3], A2, &i1, temp, &i1);
+ saxpy_3l(row2, c[3], A2, temp);
+// saxpy_(&row2, &c[1], A0, &i1, temp, &i1);
+ saxpy_3l(row2, c[1], A0, temp);
+// sgemm_(&ta, &ta, &row, &row, &row, &alpha, A, &row, temp, &row, &beta, U, &row);
+ sgemm_nn_3l(row, row, row, A, row, temp, row, U, row);
+ smcopy(row, row, A2, row, temp, row);
+// sscal_(&row2, &c[8], V, &i1);
+ sscal_3l(row2, c[8], V);
+// saxpy_(&row2, &c[12], A6, &i1, temp, &i1);
+ saxpy_3l(row2, c[12], A6, temp);
+// saxpy_(&row2, &c[10], A4, &i1, temp, &i1);
+ saxpy_3l(row2, c[10], A4, temp);
+// sgemm_(&ta, &ta, &row, &row, &row, &alpha, A6, &row, temp, &row, &beta, V, &row);
+ sgemm_nn_3l(row, row, row, A6, row, temp, row, V, row);
+// saxpy_(&row2, &c[6], A6, &i1, V, &i1);
+ saxpy_3l(row2, c[6], A6, V);
+// saxpy_(&row2, &c[4], A4, &i1, V, &i1);
+ saxpy_3l(row2, c[4], A4, V);
+// saxpy_(&row2, &c[2], A2, &i1, V, &i1);
+ saxpy_3l(row2, c[2], A2, V);
+// saxpy_(&row2, &c[0], A0, &i1, V, &i1);
+ saxpy_3l(row2, c[0], A0, V);
+ free(A0);
+ free(A2);
+ free(A4);
+ free(A6);
+ free(temp);
+ }
+ else
+ {
+ printf("%s\n", "Wrong Pade approximatin degree");
+ exit(1);
+ }
+ float *D = malloc(row*row*sizeof(float));
+// dcopy_(&row2, V, &i1, A, &i1);
+ smcopy(row, row, V, row, A, row);
+// saxpy_(&row2, &d1, U, &i1, A, &i1);
+ saxpy_3l(row2, 1.0, U, A);
+// dcopy_(&row2, V, &i1, D, &i1);
+ smcopy(row, row, V, row, D, row);
+// saxpy_(&row2, &dm1, U, &i1, D, &i1);
+ saxpy_3l(row2, -1.0, U, D);
+ int *ipiv = (int *) malloc(row*sizeof(int));
+ int info = 0;
+// dgesv_(&row, &row, D, &row, ipiv, A, &row, &info);
+ sgesv_3l(row, row, D, row, ipiv, A, row, &info);
+ free(ipiv);
+ free(D);
+ free(U);
+ free(V);
+ }
+
+
+
+void expm(int row, float *A)
+ {
+
+ int i;
+
+ int m_vals[] = {3, 5, 7, 9, 13};
+ float theta[] = {0.01495585217958292, 0.2539398330063230, 0.9504178996162932, 2.097847961257068, 5.371920351148152};
+ int lentheta = 5;
+
+ float normA = onenorm(row, row, A);
+
+ if(normA<=theta[4])
+ {
+ for(i=0; i<lentheta; i++)
+ {
+ if(normA<=theta[i])
+ {
+ padeapprox(m_vals[i], row, A);
+ break;
+ }
+ }
+ }
+ else
+ {
+ int s;
+ float t = frexp(normA/(theta[4]), &s);
+ s = s - (t==0.5);
+ t = pow(2,-s);
+ int row2 = row*row;
+/* int i1 = 1;*/
+// sscal_(&row2, &t, A, &i1);
+ sscal_3l(row2, t, A);
+ padeapprox(m_vals[4], row, A);
+ float *temp = malloc(row*row*sizeof(float));
+// char ta = 'n'; float alpha = 1; float beta = 0;
+ for(i=0; i<s; i++)
+ {
+// sgemm_(&ta, &ta, &row, &row, &row, &alpha, A, &row, A, &row, &beta, temp, &row);
+ sgemm_nn_3l(row, row, row, A, row, A, row, temp, row);
+ smcopy(row, row, temp, row, A, row);
+ }
+ free(temp);
+ }
+ }
+
+
diff --git a/test_problems/s_tools.h b/test_problems/s_tools.h
new file mode 100644
index 0000000..8a8ee87
--- /dev/null
+++ b/test_problems/s_tools.h
@@ -0,0 +1,38 @@
+/**************************************************************************************************
+* *
+* This file is part of HPMPC. *
+* *
+* HPMPC -- Library for High-Performance implementation of solvers for MPC. *
+* Copyright (C) 2014-2015 by Technical University of Denmark. 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, giaf (at) dtu.dk *
+* *
+**************************************************************************************************/
+
+void sgemv_n_3l(int m, int n, float *A, int lda , float *x, float *z);
+void sgemm_nn_3l(int m, int n, int k, float *A, int lda , float *B, int ldb, float *C, int ldc);
+void saxpy_3l(int n, float da, float *dx, float *dy);
+void sscal_3l(int n, float da, float *dx);
+
+/* copies a matrix into another matrix */
+void smcopy(int row, int col, float *ptrA, int lda, float *ptrB, int ldb);
+
+/* solution of a system of linear equations */
+void sgesv_3l(int n, int nrhs, float *A, int lda, int *ipiv, float *B, int ldb, int *info);
+
+/* matrix exponential */
+void expm(int row, float *A);
diff --git a/test_problems/test_d_cond.c b/test_problems/test_d_cond.c
new file mode 100644
index 0000000..61f3264
--- /dev/null
+++ b/test_problems/test_d_cond.c
@@ -0,0 +1,778 @@
+/**************************************************************************************************
+* *
+* 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 <stdlib.h>
+#include <stdio.h>
+#include <math.h>
+#include <sys/time.h>
+
+#include <blasfeo_target.h>
+#include <blasfeo_common.h>
+#include <blasfeo_v_aux_ext_dep.h>
+#include <blasfeo_d_aux_ext_dep.h>
+#include <blasfeo_i_aux_ext_dep.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_dense_qp.h"
+#include "../include/hpipm_d_dense_qp_sol.h"
+#include "../include/hpipm_d_dense_qp_ipm_hard.h"
+#include "../include/hpipm_d_cond.h"
+
+#include "d_tools.h"
+
+
+
+#define KEEP_X0 0
+
+#define PRINT 1
+
+
+
+/************************************************
+Mass-spring system: nx/2 masses connected each other with springs (in a row), and the first and the last one to walls. nu (<=nx) controls act on the first nu masses. The system is sampled with sampling time Ts.
+************************************************/
+void mass_spring_system(double Ts, int nx, int nu, int N, double *A, double *B, double *b, double *x0)
+ {
+
+ int nx2 = nx*nx;
+
+ int info = 0;
+
+ int pp = nx/2; // number of masses
+
+/************************************************
+* build the continuous time system
+************************************************/
+
+ double *T; d_zeros(&T, pp, pp);
+ int ii;
+ for(ii=0; ii<pp; ii++) T[ii*(pp+1)] = -2;
+ for(ii=0; ii<pp-1; ii++) T[ii*(pp+1)+1] = 1;
+ for(ii=1; ii<pp; ii++) T[ii*(pp+1)-1] = 1;
+
+ double *Z; d_zeros(&Z, pp, pp);
+ double *I; d_zeros(&I, pp, pp); for(ii=0; ii<pp; ii++) I[ii*(pp+1)]=1.0; // = eye(pp);
+ double *Ac; d_zeros(&Ac, nx, nx);
+ dmcopy(pp, pp, Z, pp, Ac, nx);
+ dmcopy(pp, pp, T, pp, Ac+pp, nx);
+ dmcopy(pp, pp, I, pp, Ac+pp*nx, nx);
+ dmcopy(pp, pp, Z, pp, Ac+pp*(nx+1), nx);
+ free(T);
+ free(Z);
+ free(I);
+
+ d_zeros(&I, nu, nu); for(ii=0; ii<nu; ii++) I[ii*(nu+1)]=1.0; //I = eye(nu);
+ double *Bc; d_zeros(&Bc, nx, nu);
+ dmcopy(nu, nu, I, nu, Bc+pp, nx);
+ free(I);
+
+/************************************************
+* compute the discrete time system
+************************************************/
+
+ double *bb; d_zeros(&bb, nx, 1);
+ dmcopy(nx, 1, bb, nx, b, nx);
+
+ dmcopy(nx, nx, Ac, nx, A, nx);
+ dscal_3l(nx2, Ts, A);
+ expm(nx, A);
+
+ d_zeros(&T, nx, nx);
+ d_zeros(&I, nx, nx); for(ii=0; ii<nx; ii++) I[ii*(nx+1)]=1.0; //I = eye(nx);
+ dmcopy(nx, nx, A, nx, T, nx);
+ daxpy_3l(nx2, -1.0, I, T);
+ dgemm_nn_3l(nx, nu, nx, T, nx, Bc, nx, B, nx);
+ free(T);
+ free(I);
+
+ int *ipiv = (int *) malloc(nx*sizeof(int));
+ dgesv_3l(nx, nu, Ac, nx, ipiv, B, nx, &info);
+ free(ipiv);
+
+ free(Ac);
+ free(Bc);
+ free(bb);
+
+
+/************************************************
+* initial state
+************************************************/
+
+ if(nx==4)
+ {
+ x0[0] = 5;
+ x0[1] = 10;
+ x0[2] = 15;
+ x0[3] = 20;
+ }
+ else
+ {
+ int jj;
+ for(jj=0; jj<nx; jj++)
+ x0[jj] = 1;
+ }
+
+ }
+
+
+
+int main()
+ {
+
+
+ // local variables
+
+ int ii, jj;
+
+ int rep, nrep=1000;
+
+ struct timeval tv0, tv1;
+
+
+
+ // problem size
+
+ int nx_ = 8; // number of states (it has to be even for the mass-spring system test problem)
+ int nu_ = 3; // number of inputs (controllers) (it has to be at least 1 and at most nx/2 for the mass-spring system test problem)
+ int N = 5; // horizon lenght
+
+
+
+ // stage-wise variant size
+
+ int nx[N+1];
+#if KEEP_X0
+ nx[0] = nx_;
+#else
+ nx[0] = 0;
+#endif
+ for(ii=1; ii<=N; ii++)
+ nx[ii] = nx_;
+// nx[N] = 0;
+
+ int nu[N+1];
+ for(ii=0; ii<N; ii++)
+ nu[ii] = nu_;
+ nu[N] = 0;
+
+#if 1
+ int nb[N+1];
+#if KEEP_X0
+ nb[0] = nu[0]+nx[0]/2;
+#else
+ nb[0] = nu[0];
+#endif
+ for(ii=1; ii<N; ii++)
+ nb[ii] = nu[1]+nx[1]/2;
+ nb[N] = nx[N]/2;
+
+ int ng[N+1];
+ ng[0] = 0;
+ for(ii=1; ii<N; ii++)
+ ng[ii] = 0;
+ ng[N] = 0;
+#elif 0
+ int nb[N+1];
+ nb[0] = 0;
+ for(ii=1; ii<N; ii++)
+ nb[ii] = 0;
+ nb[N] = 0;
+
+ int ng[N+1];
+#if KEEP_X0
+ ng[0] = nu[0]+nx[0]/2;
+#else
+ ng[0] = nu[0];
+#endif
+ for(ii=1; ii<N; ii++)
+ ng[ii] = nu[1]+nx[1]/2;
+ ng[N] = nx[N]/2;
+#else
+ int nb[N+1];
+ nb[0] = nu[0] + nx[0]/2;
+ for(ii=1; ii<N; ii++)
+ nb[ii] = nu[ii] + nx[ii]/2;
+ nb[N] = nu[N] + nx[N]/2;
+
+ int ng[N+1];
+#if KEEP_X0
+ ng[0] = nx[0]/2;
+#else
+ ng[0] = 0;
+#endif
+ for(ii=1; ii<N; ii++)
+ ng[ii] = nx[1]/2;
+ ng[N] = nx[N]/2;
+#endif
+
+/************************************************
+* dynamical system
+************************************************/
+
+ double *A; d_zeros(&A, nx_, nx_); // states update matrix
+
+ double *B; d_zeros(&B, nx_, nu_); // inputs matrix
+
+ double *b; d_zeros(&b, nx_, 1); // states offset
+ double *x0; d_zeros(&x0, nx_, 1); // initial state
+
+ double Ts = 0.5; // sampling time
+ mass_spring_system(Ts, nx_, nu_, N, A, B, b, x0);
+
+ for(jj=0; jj<nx_; jj++)
+ b[jj] = 0.1;
+
+ for(jj=0; jj<nx_; jj++)
+ x0[jj] = 0;
+ x0[0] = 2.5;
+ x0[1] = 2.5;
+
+ double *b0; d_zeros(&b0, nx_, 1);
+ dgemv_n_3l(nx_, nx_, A, nx_, x0, b0);
+ daxpy_3l(nx_, 1.0, b, b0);
+
+#if PRINT
+ d_print_mat(nx_, nx_, A, nx_);
+ d_print_mat(nx_, nu_, B, nu_);
+ d_print_mat(1, nx_, b, 1);
+ d_print_mat(1, nx_, x0, 1);
+ d_print_mat(1, nx_, b0, 1);
+#endif
+
+/************************************************
+* cost function
+************************************************/
+
+ double *Q; d_zeros(&Q, nx_, nx_);
+ for(ii=0; ii<nx_; ii++) Q[ii*(nx_+1)] = 1.0;
+
+ double *R; d_zeros(&R, nu_, nu_);
+ for(ii=0; ii<nu_; ii++) R[ii*(nu_+1)] = 2.0;
+
+ double *S; d_zeros(&S, nu_, nx_);
+
+ double *q; d_zeros(&q, nx_, 1);
+ for(ii=0; ii<nx_; ii++) q[ii] = 0.1;
+
+ double *r; d_zeros(&r, nu_, 1);
+ for(ii=0; ii<nu_; ii++) r[ii] = 0.2;
+
+ double *r0; d_zeros(&r0, nu_, 1);
+ dgemv_n_3l(nu_, nx_, S, nu_, x0, r0);
+ daxpy_3l(nu_, 1.0, r, r0);
+
+#if PRINT
+ d_print_mat(nx_, nx_, Q, nx_);
+ d_print_mat(nu_, nu_, R, nu_);
+ d_print_mat(nu_, nx_, S, nu_);
+ d_print_mat(1, nx_, q, 1);
+ d_print_mat(1, nu_, r, 1);
+ d_print_mat(1, nu_, r0, 1);
+#endif
+
+ // maximum element in cost functions
+ double mu0 = 2.0;
+
+/************************************************
+* box & general constraints
+************************************************/
+
+ int *idxb0; int_zeros(&idxb0, nb[0], 1);
+ double *d_lb0; d_zeros(&d_lb0, nb[0], 1);
+ double *d_ub0; d_zeros(&d_ub0, nb[0], 1);
+ double *d_lg0; d_zeros(&d_lg0, ng[0], 1);
+ double *d_ug0; d_zeros(&d_ug0, ng[0], 1);
+ for(ii=0; ii<nb[0]; ii++)
+ {
+ if(ii<nu[0]) // input
+ {
+ d_lb0[ii] = - 0.5; // umin
+ d_ub0[ii] = 0.5; // umax
+ }
+ else // state
+ {
+ d_lb0[ii] = - 4.0; // xmin
+ d_ub0[ii] = 4.0; // xmax
+ }
+ idxb0[ii] = ii;
+ }
+ for(ii=0; ii<ng[0]; ii++)
+ {
+ if(ii<nu[0]-nb[0]) // input
+ {
+ d_lg0[ii] = - 0.5; // umin
+ d_ug0[ii] = 0.5; // umax
+ }
+ else // state
+ {
+ d_lg0[ii] = - 4.0; // xmin
+ d_ug0[ii] = 4.0; // xmax
+ }
+ }
+
+ int *idxb1; int_zeros(&idxb1, nb[1], 1);
+ double *d_lb1; d_zeros(&d_lb1, nb[1], 1);
+ double *d_ub1; d_zeros(&d_ub1, nb[1], 1);
+ double *d_lg1; d_zeros(&d_lg1, ng[1], 1);
+ double *d_ug1; d_zeros(&d_ug1, ng[1], 1);
+ for(ii=0; ii<nb[1]; ii++)
+ {
+ if(ii<nu[1]) // input
+ {
+ d_lb1[ii] = - 0.5; // umin
+ d_ub1[ii] = 0.5; // umax
+ }
+ else // state
+ {
+ d_lb1[ii] = - 4.0; // xmin
+ d_ub1[ii] = 4.0; // xmax
+ }
+ idxb1[ii] = ii;
+ }
+ for(ii=0; ii<ng[1]; ii++)
+ {
+ if(ii<nu[1]-nb[1]) // input
+ {
+ d_lg1[ii] = - 0.5; // umin
+ d_ug1[ii] = 0.5; // umax
+ }
+ else // state
+ {
+ d_lg1[ii] = - 4.0; // xmin
+ d_ug1[ii] = 4.0; // xmax
+ }
+ }
+
+
+ int *idxbN; int_zeros(&idxbN, nb[N], 1);
+ double *d_lbN; d_zeros(&d_lbN, nb[N], 1);
+ double *d_ubN; d_zeros(&d_ubN, nb[N], 1);
+ double *d_lgN; d_zeros(&d_lgN, ng[N], 1);
+ double *d_ugN; d_zeros(&d_ugN, ng[N], 1);
+ for(ii=0; ii<nb[N]; ii++)
+ {
+ d_lbN[ii] = - 4.0; // xmin
+ d_ubN[ii] = 4.0; // xmax
+ idxbN[ii] = ii;
+ }
+ for(ii=0; ii<ng[N]; ii++)
+ {
+ d_lgN[ii] = - 4.0; // dmin
+ d_ugN[ii] = 4.0; // dmax
+ }
+
+ double *C0; d_zeros(&C0, ng[0], nx[0]);
+ double *D0; d_zeros(&D0, ng[0], nu[0]);
+ for(ii=0; ii<nu[0]-nb[0] & ii<ng[0]; ii++)
+ D0[ii+(nb[0]+ii)*ng[0]] = 1.0;
+ for(; ii<ng[0]; ii++)
+ C0[ii+(nb[0]+ii-nu[0])*ng[0]] = 1.0;
+
+ double *C1; d_zeros(&C1, ng[1], nx[1]);
+ double *D1; d_zeros(&D1, ng[1], nu[1]);
+ for(ii=0; ii<nu[1]-nb[1] & ii<ng[1]; ii++)
+ D1[ii+(nb[1]+ii)*ng[1]] = 1.0;
+ for(; ii<ng[1]; ii++)
+ C1[ii+(nb[1]+ii-nu[1])*ng[1]] = 1.0;
+
+ double *CN; d_zeros(&CN, ng[N], nx[N]);
+ double *DN; d_zeros(&DN, ng[N], nu[N]);
+ for(ii=0; ii<nu[N]-nb[N] & ii<ng[N]; ii++)
+ DN[ii+(nb[N]+ii)*ng[N]] = 1.0;
+ for(; ii<ng[N]; ii++)
+ CN[ii+(nb[N]+ii-nu[N])*ng[N]] = 1.0;
+
+#if PRINT
+ // box constraints
+ int_print_mat(1, nb[0], idxb0, 1);
+ d_print_mat(1, nb[0], d_lb0, 1);
+ d_print_mat(1, nb[0], d_ub0, 1);
+ int_print_mat(1, nb[1], idxb1, 1);
+ d_print_mat(1, nb[1], d_lb1, 1);
+ d_print_mat(1, nb[1], d_ub1, 1);
+ int_print_mat(1, nb[N], idxbN, 1);
+ d_print_mat(1, nb[N], d_lbN, 1);
+ d_print_mat(1, nb[N], d_ubN, 1);
+ // general constraints
+ d_print_mat(1, ng[0], d_lg0, 1);
+ d_print_mat(1, ng[0], d_ug0, 1);
+ d_print_mat(ng[0], nu[0], D0, ng[0]);
+ d_print_mat(ng[0], nx[0], C0, ng[0]);
+ d_print_mat(1, ng[1], d_lg1, 1);
+ d_print_mat(1, ng[1], d_ug1, 1);
+ d_print_mat(ng[1], nu[1], D1, ng[1]);
+ d_print_mat(ng[1], nx[1], C1, ng[1]);
+ d_print_mat(1, ng[N], d_lgN, 1);
+ d_print_mat(1, ng[N], d_ugN, 1);
+ d_print_mat(ng[N], nu[N], DN, ng[N]);
+ d_print_mat(ng[N], nx[N], CN, ng[N]);
+#endif
+
+/************************************************
+* array of matrices
+************************************************/
+
+ double *hA[N];
+ double *hB[N];
+ double *hb[N];
+ double *hQ[N+1];
+ double *hS[N+1];
+ double *hR[N+1];
+ double *hq[N+1];
+ double *hr[N+1];
+ double *hd_lb[N+1];
+ double *hd_ub[N+1];
+ double *hd_lg[N+1];
+ double *hd_ug[N+1];
+ double *hC[N+1];
+ double *hD[N+1];
+ int *hidxb[N+1];
+
+ hA[0] = A;
+ hB[0] = B;
+ hb[0] = b0;
+ hQ[0] = Q;
+ hS[0] = S;
+ hR[0] = R;
+ hq[0] = q;
+ hr[0] = r0;
+ hidxb[0] = idxb0;
+ hd_lb[0] = d_lb0;
+ hd_ub[0] = d_ub0;
+ hd_lg[0] = d_lg0;
+ hd_ug[0] = d_ug0;
+ hC[0] = C0;
+ hD[0] = D0;
+ for(ii=1; ii<N; ii++)
+ {
+ hA[ii] = A;
+ hB[ii] = B;
+ hb[ii] = b;
+ hQ[ii] = Q;
+ hS[ii] = S;
+ hR[ii] = R;
+ hq[ii] = q;
+ hr[ii] = r;
+ hidxb[ii] = idxb1;
+ hd_lb[ii] = d_lb1;
+ hd_ub[ii] = d_ub1;
+ hd_lg[ii] = d_lg1;
+ hd_ug[ii] = d_ug1;
+ hC[ii] = C1;
+ hD[ii] = D1;
+ }
+ hQ[N] = Q;
+ hS[N] = S;
+ hR[N] = R;
+ hq[N] = q;
+ hr[N] = r;
+ hidxb[N] = idxbN;
+ hd_lb[N] = d_lbN;
+ hd_ub[N] = d_ubN;
+ hd_lg[N] = d_lgN;
+ hd_ug[N] = d_ugN;
+ hC[N] = CN;
+ hD[N] = DN;
+
+/************************************************
+* ocp qp
+************************************************/
+
+ int ocp_qp_size = d_memsize_ocp_qp(N, nx, nu, nb, ng);
+ printf("\nqp size = %d\n", ocp_qp_size);
+ void *ocp_qp_mem = malloc(ocp_qp_size);
+
+ struct d_ocp_qp ocp_qp;
+ d_create_ocp_qp(N, nx, nu, nb, ng, &ocp_qp, ocp_qp_mem);
+ d_cvt_colmaj_to_ocp_qp(hA, hB, hb, hQ, hS, hR, hq, hr, hidxb, hd_lb, hd_ub, hC, hD, hd_lg, hd_ug, &ocp_qp);
+
+#if 1
+ printf("\nN = %d\n", ocp_qp.N);
+ for(ii=0; ii<N; ii++)
+ d_print_strmat(ocp_qp.nu[ii]+ocp_qp.nx[ii]+1, ocp_qp.nx[ii+1], ocp_qp.BAbt+ii, 0, 0);
+ for(ii=0; ii<N; ii++)
+ d_print_tran_strvec(ocp_qp.nx[ii+1], ocp_qp.b+ii, 0);
+ for(ii=0; ii<=N; ii++)
+ d_print_strmat(ocp_qp.nu[ii]+ocp_qp.nx[ii]+1, ocp_qp.nu[ii]+ocp_qp.nx[ii], ocp_qp.RSQrq+ii, 0, 0);
+ for(ii=0; ii<=N; ii++)
+ d_print_tran_strvec(ocp_qp.nu[ii]+ocp_qp.nx[ii], ocp_qp.rq+ii, 0);
+ for(ii=0; ii<=N; ii++)
+ int_print_mat(1, nb[ii], ocp_qp.idxb[ii], 1);
+ for(ii=0; ii<=N; ii++)
+ d_print_tran_strvec(ocp_qp.nb[ii], ocp_qp.d_lb+ii, 0);
+ for(ii=0; ii<=N; ii++)
+ d_print_tran_strvec(ocp_qp.nb[ii], ocp_qp.d_ub+ii, 0);
+ for(ii=0; ii<=N; ii++)
+ d_print_strmat(ocp_qp.nu[ii]+ocp_qp.nx[ii], ocp_qp.ng[ii], ocp_qp.DCt+ii, 0, 0);
+ for(ii=0; ii<=N; ii++)
+ d_print_tran_strvec(ocp_qp.ng[ii], ocp_qp.d_lg+ii, 0);
+ for(ii=0; ii<=N; ii++)
+ d_print_tran_strvec(ocp_qp.ng[ii], ocp_qp.d_ug+ii, 0);
+#endif
+
+/************************************************
+* dense qp
+************************************************/
+
+ int nvc = 0;
+ int nec = 0;
+ int nbc = 0;
+ int ngc = 0;
+
+ d_compute_qp_size_ocp2dense(N, nx, nu, nb, hidxb, ng, &nvc, &nec, &nbc, &ngc);
+ printf("\nnv = %d, ne = %d, nb = %d, ng = %d\n\n", nvc, nec, nbc, ngc);
+
+ int dense_qp_size = d_memsize_dense_qp(nvc, nec, nbc, ngc);
+ printf("\nqp size = %d\n", dense_qp_size);
+ void *dense_qp_mem = malloc(dense_qp_size);
+
+ struct d_dense_qp dense_qp;
+ d_create_dense_qp(nvc, nec, nbc, ngc, &dense_qp, dense_qp_mem);
+
+ int cond_size = d_memsize_cond_qp_ocp2dense(&ocp_qp, &dense_qp);
+ printf("\ncond size = %d\n", cond_size);
+ void *cond_mem = malloc(cond_size);
+
+ struct d_cond_qp_ocp2dense_workspace cond_ws;
+ d_create_cond_qp_ocp2dense(&ocp_qp, &dense_qp, &cond_ws, cond_mem);
+
+ gettimeofday(&tv0, NULL); // start
+
+ for(rep=0; rep<nrep; rep++)
+ {
+ d_cond_qp_ocp2dense(&ocp_qp, &dense_qp, &cond_ws);
+ }
+
+ gettimeofday(&tv1, NULL); // stop
+
+ double time_cond = (tv1.tv_sec-tv0.tv_sec)/(nrep+0.0)+(tv1.tv_usec-tv0.tv_usec)/(nrep*1e6);
+
+#if 1
+ d_print_strmat(nvc+1, nvc, dense_qp.Hg, 0, 0);
+ d_print_strmat(nec, nvc, dense_qp.A, 0, 0);
+ d_print_strmat(nvc, ngc, dense_qp.Ct, 0, 0);
+ d_print_tran_strvec(nvc, dense_qp.g, 0);
+ d_print_tran_strvec(nec, dense_qp.b, 0);
+ d_print_tran_strvec(2*nbc+2*ngc, dense_qp.d, 0);
+ d_print_tran_strvec(nbc, dense_qp.d_lb, 0);
+ d_print_tran_strvec(nbc, dense_qp.d_ub, 0);
+ d_print_tran_strvec(ngc, dense_qp.d_lg, 0);
+ d_print_tran_strvec(ngc, dense_qp.d_ug, 0);
+#endif
+
+#if 0
+ int nu_tmp = 0;
+ for(ii=0; ii<N; ii++)
+ {
+ nu_tmp += nu[ii];
+ d_print_strmat(nu_tmp+nx[0]+1, nx[ii+1], cond_ws.Gamma+ii, 0, 0);
+ }
+#endif
+
+/************************************************
+* dense qp sol
+************************************************/
+
+ int dense_qp_sol_size = d_memsize_dense_qp_sol(nvc, nec, nbc, ngc);
+ printf("\ndense qp sol size = %d\n", dense_qp_sol_size);
+ void *dense_qp_sol_mem = malloc(dense_qp_sol_size);
+
+ struct d_dense_qp_sol dense_qp_sol;
+ d_create_dense_qp_sol(nvc, nec, nbc, ngc, &dense_qp_sol, dense_qp_sol_mem);
+
+/************************************************
+* ipm
+************************************************/
+
+ struct d_ipm_hard_dense_qp_arg dense_arg;
+ dense_arg.alpha_min = 1e-8;
+ dense_arg.mu_max = 1e-12;
+ dense_arg.iter_max = 20;
+ dense_arg.mu0 = 1.0;
+
+ int dense_ipm_size = d_memsize_ipm_hard_dense_qp(&dense_qp, &dense_arg);
+ printf("\ndense ipm size = %d\n", dense_ipm_size);
+ void *dense_ipm_mem = malloc(dense_ipm_size);
+
+ struct d_ipm_hard_dense_qp_workspace dense_workspace;
+ d_create_ipm_hard_dense_qp(&dense_qp, &dense_arg, &dense_workspace, dense_ipm_mem);
+
+ gettimeofday(&tv0, NULL); // start
+
+ for(rep=0; rep<nrep; rep++)
+ {
+// d_solve_ipm_hard_dense_qp(&dense_qp, &dense_qp_sol, &dense_workspace);
+ d_solve_ipm2_hard_dense_qp(&dense_qp, &dense_qp_sol, &dense_workspace);
+ }
+
+ gettimeofday(&tv1, NULL); // stop
+
+ double time_dense_ipm = (tv1.tv_sec-tv0.tv_sec)/(nrep+0.0)+(tv1.tv_usec-tv0.tv_usec)/(nrep*1e6);
+
+
+ printf("\nsolution\n\n");
+ printf("\nv\n");
+ d_print_tran_strvec(nvc, dense_qp_sol.v, 0);
+ printf("\npi\n");
+ d_print_tran_strvec(nec, dense_qp_sol.pi, 0);
+ printf("\nlam_lb\n");
+ d_print_tran_strvec(nbc, dense_qp_sol.lam_lb, 0);
+ printf("\nlam_ub\n");
+ d_print_tran_strvec(nbc, dense_qp_sol.lam_ub, 0);
+ printf("\nlam_lg\n");
+ d_print_tran_strvec(ngc, dense_qp_sol.lam_lg, 0);
+ printf("\nlam_ug\n");
+ d_print_tran_strvec(ngc, dense_qp_sol.lam_ug, 0);
+ printf("\nt_lb\n");
+ d_print_tran_strvec(nbc, dense_qp_sol.t_lb, 0);
+ printf("\nt_ub\n");
+ d_print_tran_strvec(nbc, dense_qp_sol.t_ub, 0);
+ printf("\nt_lg\n");
+ d_print_tran_strvec(ngc, dense_qp_sol.t_lg, 0);
+ printf("\nt_ug\n");
+ d_print_tran_strvec(ngc, dense_qp_sol.t_ug, 0);
+
+ printf("\nresiduals\n\n");
+ printf("\nres_g\n");
+ d_print_e_tran_strvec(nvc, dense_workspace.res_g, 0);
+ printf("\nres_b\n");
+ d_print_e_tran_strvec(nec, dense_workspace.res_b, 0);
+ printf("\nres_d\n");
+ d_print_e_tran_strvec(2*nbc+2*ngc, dense_workspace.res_d, 0);
+ printf("\nres_m\n");
+ d_print_e_tran_strvec(2*nbc+2*ngc, dense_workspace.res_m, 0);
+ printf("\nres_mu\n");
+ printf("\n%e\n\n", dense_workspace.res_mu);
+
+ printf("\nipm iter = %d\n", dense_workspace.iter);
+ printf("\nalpha_aff\tmu_aff\t\tsigma\t\talpha\t\tmu\n");
+ d_print_e_tran_mat(5, dense_workspace.iter, dense_workspace.stat, 5);
+
+ printf("\ncond time = %e [s], dense ipm time = %e [s]\n\n", time_cond, time_dense_ipm);
+
+/************************************************
+* ocp qp sol
+************************************************/
+
+ int ocp_qp_sol_size = d_memsize_ocp_qp_sol(N, nx, nu, nb, ng);
+ printf("\nocp qp sol size = %d\n", ocp_qp_sol_size);
+ void *ocp_qp_sol_mem = malloc(ocp_qp_sol_size);
+
+ struct d_ocp_qp_sol ocp_qp_sol;
+ d_create_ocp_qp_sol(N, nx, nu, nb, ng, &ocp_qp_sol, ocp_qp_sol_mem);
+
+/************************************************
+* expand solution
+************************************************/
+
+ d_expand_sol_dense2ocp(&ocp_qp, &dense_qp_sol, &ocp_qp_sol, &cond_ws);
+
+ printf("\nfull space solution\n\n");
+ printf("\nux\n");
+ for(ii=0; ii<=N; ii++)
+ d_print_tran_strvec(nu[ii]+nx[ii], ocp_qp_sol.ux+ii, 0);
+ printf("\npi\n");
+ for(ii=0; ii<N; ii++)
+ d_print_tran_strvec(nx[ii+1], ocp_qp_sol.pi+ii, 0);
+ printf("\nlam_lb\n");
+ for(ii=0; ii<=N; ii++)
+ d_print_tran_strvec(nb[ii], ocp_qp_sol.lam_lb+ii, 0);
+ printf("\nlam_ub\n");
+ for(ii=0; ii<=N; ii++)
+ d_print_tran_strvec(nb[ii], ocp_qp_sol.lam_ub+ii, 0);
+ printf("\nlam_lg\n");
+ for(ii=0; ii<=N; ii++)
+ d_print_tran_strvec(ng[ii], ocp_qp_sol.lam_lg+ii, 0);
+ printf("\nlam_ug\n");
+ for(ii=0; ii<=N; ii++)
+ d_print_tran_strvec(ng[ii], ocp_qp_sol.lam_ug+ii, 0);
+ printf("\nt_lb\n");
+ for(ii=0; ii<=N; ii++)
+ d_print_tran_strvec(nb[ii], ocp_qp_sol.t_lb+ii, 0);
+ printf("\nt_ub\n");
+ for(ii=0; ii<=N; ii++)
+ d_print_tran_strvec(nb[ii], ocp_qp_sol.t_ub+ii, 0);
+ printf("\nt_lg\n");
+ for(ii=0; ii<=N; ii++)
+ d_print_tran_strvec(ng[ii], ocp_qp_sol.t_lg+ii, 0);
+ printf("\nt_ug\n");
+ for(ii=0; ii<=N; ii++)
+ d_print_tran_strvec(ng[ii], ocp_qp_sol.t_ug+ii, 0);
+
+/************************************************
+* free memory
+************************************************/
+
+ d_free(A);
+ d_free(B);
+ d_free(b);
+ d_free(x0);
+ d_free(Q);
+ d_free(R);
+ d_free(S);
+ d_free(q);
+ d_free(r);
+ d_free(r0);
+ int_free(idxb0);
+ d_free(d_lb0);
+ d_free(d_ub0);
+ int_free(idxb1);
+ d_free(d_lb1);
+ d_free(d_ub1);
+ int_free(idxbN);
+ d_free(d_lbN);
+ d_free(d_ubN);
+ d_free(C0);
+ d_free(D0);
+ d_free(d_lg0);
+ d_free(d_ug0);
+ d_free(C1);
+ d_free(D1);
+ d_free(d_lg1);
+ d_free(d_ug1);
+ d_free(CN);
+ d_free(DN);
+ d_free(d_lgN);
+ d_free(d_ugN);
+
+ free(ocp_qp_mem);
+ free(ocp_qp_sol_mem);
+ free(dense_qp_mem);
+ free(dense_qp_sol_mem);
+ free(cond_mem);
+ free(dense_ipm_mem);
+
+/************************************************
+* return
+************************************************/
+
+ return 0;
+
+ }
diff --git a/test_problems/test_d_dense.c b/test_problems/test_d_dense.c
new file mode 100644
index 0000000..c70415a
--- /dev/null
+++ b/test_problems/test_d_dense.c
@@ -0,0 +1,264 @@
+/**************************************************************************************************
+* *
+* 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 <stdlib.h>
+#include <stdio.h>
+#include <math.h>
+#include <sys/time.h>
+
+#include <blasfeo_target.h>
+#include <blasfeo_common.h>
+#include <blasfeo_v_aux_ext_dep.h>
+#include <blasfeo_d_aux_ext_dep.h>
+#include <blasfeo_i_aux_ext_dep.h>
+#include <blasfeo_d_aux.h>
+#include <blasfeo_d_blas.h>
+
+#include "../include/hpipm_d_dense_qp.h"
+#include "../include/hpipm_d_dense_qp_sol.h"
+#include "../include/hpipm_d_dense_qp_ipm_hard.h"
+
+
+
+#define PRINT 1
+
+
+
+#if ! defined(EXT_DEP)
+/* prints a matrix in column-major format */
+void d_print_mat(int m, int n, double *A, int lda)
+ {
+ int i, j;
+ for(i=0; i<m; i++)
+ {
+ for(j=0; j<n; j++)
+ {
+ printf("%9.5f ", A[i+lda*j]);
+ }
+ printf("\n");
+ }
+ printf("\n");
+ }
+/* prints the transposed of a matrix in column-major format */
+void d_print_tran_mat(int row, int col, double *A, int lda)
+ {
+ int i, j;
+ for(j=0; j<col; j++)
+ {
+ for(i=0; i<row; i++)
+ {
+ printf("%9.5f ", A[i+lda*j]);
+ }
+ printf("\n");
+ }
+ printf("\n");
+ }
+/* prints a matrix in column-major format (exponential notation) */
+void d_print_e_mat(int m, int n, double *A, int lda)
+ {
+ int i, j;
+ for(i=0; i<m; i++)
+ {
+ for(j=0; j<n; j++)
+ {
+ printf("%e\t", A[i+lda*j]);
+ }
+ printf("\n");
+ }
+ printf("\n");
+ }
+/* prints the transposed of a matrix in column-major format (exponential notation) */
+void d_print_e_tran_mat(int row, int col, double *A, int lda)
+ {
+ int i, j;
+ for(j=0; j<col; j++)
+ {
+ for(i=0; i<row; i++)
+ {
+ printf("%e\t", A[i+lda*j]);
+ }
+ printf("\n");
+ }
+ printf("\n");
+ }
+#endif
+
+
+
+int main()
+ {
+
+ int ii;
+
+/************************************************
+* qp dimension and data
+************************************************/
+
+ int nv = 2;
+ int ne = 1;
+ int nb = 2;
+ int ng = 0;
+
+ double H[] = {4.0, 1.0, 1.0, 2.0};
+ double g[] = {1.0, 1.0};
+ double A[] = {1.0, 1.0};
+ double b[] = {1.0};
+// double d_lb[] = {0.0, 0.0};
+// double d_ub[] = {INFINITY, INFINITY};
+ double d_lb[] = {-1.0, -1.0};
+ double d_ub[] = {1.5, 0.5};
+ int idxb[] = {0, 1};
+ double C[] = {1.0, 0.0, 0.0, 1.0};
+ double d_lg[] = {-1.0, -1.0};
+ double d_ug[] = {1.5, 0.5};
+
+/************************************************
+* dense qp
+************************************************/
+
+ int qp_size = d_memsize_dense_qp(nv, ne, nb, ng);
+ printf("\nqp size = %d\n", qp_size);
+ void *qp_mem = malloc(qp_size);
+
+ struct d_dense_qp qp;
+ d_create_dense_qp(nv, ne, nb, ng, &qp, qp_mem);
+ d_cvt_colmaj_to_dense_qp(H, g, A, b, idxb, d_lb, d_ub, C, d_lg, d_ug, &qp);
+
+#if 0
+ d_print_strmat(nv+1, nv, qp.Hg, 0, 0);
+ d_print_strmat(ne, nv, qp.A, 0, 0);
+ d_print_strmat(nv, ng, qp.Ct, 0, 0);
+ d_print_strvec(nv, qp.g, 0);
+ d_print_strvec(ne, qp.b, 0);
+ d_print_strvec(2*nb+2*ng, qp.d, 0);
+ d_print_strvec(nb, qp.d_lb, 0);
+ d_print_strvec(nb, qp.d_ub, 0);
+ d_print_strvec(ng, qp.d_lg, 0);
+ d_print_strvec(ng, qp.d_ug, 0);
+#endif
+
+/************************************************
+* dense qp sol
+************************************************/
+
+ int qp_sol_size = d_memsize_dense_qp_sol(nv, ne, nb, ng);
+ printf("\nqp sol size = %d\n", qp_sol_size);
+ void *qp_sol_mem = malloc(qp_sol_size);
+
+ struct d_dense_qp_sol qp_sol;
+ d_create_dense_qp_sol(nv, ne, nb, ng, &qp_sol, qp_sol_mem);
+
+/************************************************
+* ipm
+************************************************/
+
+ struct d_ipm_hard_dense_qp_arg arg;
+ arg.alpha_min = 1e-8;
+ arg.mu_max = 1e-12;
+ arg.iter_max = 20;
+ arg.mu0 = 1.0;
+
+ int ipm_size = d_memsize_ipm_hard_dense_qp(&qp, &arg);
+ printf("\nipm size = %d\n", ipm_size);
+ void *ipm_mem = malloc(ipm_size);
+
+ struct d_ipm_hard_dense_qp_workspace workspace;
+ d_create_ipm_hard_dense_qp(&qp, &arg, &workspace, ipm_mem);
+
+ int rep, nrep=1000;
+
+ struct timeval tv0, tv1;
+
+ gettimeofday(&tv0, NULL); // start
+
+ for(rep=0; rep<nrep; rep++)
+ {
+// d_solve_ipm_hard_dense_qp(&qp, &qp_sol, &workspace);
+ d_solve_ipm2_hard_dense_qp(&qp, &qp_sol, &workspace);
+ }
+
+ gettimeofday(&tv1, NULL); // stop
+
+ double time_dense_ipm = (tv1.tv_sec-tv0.tv_sec)/(nrep+0.0)+(tv1.tv_usec-tv0.tv_usec)/(nrep*1e6);
+
+ printf("\nsolution\n\n");
+ printf("\nv\n");
+ d_print_mat(1, nv, qp_sol.v->pa, 1);
+ printf("\npi\n");
+ d_print_mat(1, ne, qp_sol.pi->pa, 1);
+ printf("\nlam_lb\n");
+ d_print_mat(1, nb, qp_sol.lam_lb->pa, 1);
+ printf("\nlam_ub\n");
+ d_print_mat(1, nb, qp_sol.lam_ub->pa, 1);
+ printf("\nlam_lg\n");
+ d_print_mat(1, ng, qp_sol.lam_lg->pa, 1);
+ printf("\nlam_ug\n");
+ d_print_mat(1, ng, qp_sol.lam_ug->pa, 1);
+ printf("\nt_lb\n");
+ d_print_mat(1, nb, qp_sol.t_lb->pa, 1);
+ printf("\nt_ub\n");
+ d_print_mat(1, nb, qp_sol.t_ub->pa, 1);
+ printf("\nt_lg\n");
+ d_print_mat(1, ng, qp_sol.t_lg->pa, 1);
+ printf("\nt_ug\n");
+ d_print_mat(1, ng, qp_sol.t_ug->pa, 1);
+
+ printf("\nresiduals\n\n");
+ printf("\nres_g\n");
+ d_print_e_mat(1, nv, workspace.res_g->pa, 1);
+ printf("\nres_b\n");
+ d_print_e_mat(1, ne, workspace.res_b->pa, 1);
+ printf("\nres_d\n");
+ d_print_e_mat(1, 2*nb+2*ng, workspace.res_d->pa, 1);
+ printf("\nres_m\n");
+ d_print_e_mat(1, 2*nb+2*ng, workspace.res_m->pa, 1);
+ printf("\nres_mu\n");
+ printf("\n%e\n\n", workspace.res_mu);
+
+ printf("\nipm iter = %d\n", workspace.iter);
+ printf("\nalpha_aff\tmu_aff\t\tsigma\t\talpha\t\tmu\n");
+ d_print_e_tran_mat(5, workspace.iter, workspace.stat, 5);
+
+ printf("\ndense ipm time = %e [s]\n\n", time_dense_ipm);
+
+/************************************************
+* free memory
+************************************************/
+
+ free(qp_mem);
+ free(qp_sol_mem);
+ free(ipm_mem);
+
+/************************************************
+* return
+************************************************/
+
+ return 0;
+
+ }
+
+
diff --git a/test_problems/test_d_ocp.c b/test_problems/test_d_ocp.c
new file mode 100644
index 0000000..cc0d96f
--- /dev/null
+++ b/test_problems/test_d_ocp.c
@@ -0,0 +1,833 @@
+/**************************************************************************************************
+* *
+* 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 <stdlib.h>
+#include <stdio.h>
+#include <math.h>
+#include <sys/time.h>
+
+#include <blasfeo_target.h>
+#include <blasfeo_common.h>
+#include <blasfeo_v_aux_ext_dep.h>
+#include <blasfeo_d_aux_ext_dep.h>
+#include <blasfeo_i_aux_ext_dep.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 "d_tools.h"
+
+
+
+#if ! defined(EXT_DEP)
+/* creates a zero matrix */
+void d_zeros(double **pA, int row, int col)
+ {
+ *pA = malloc((row*col)*sizeof(double));
+ double *A = *pA;
+ int i;
+ for(i=0; i<row*col; i++) A[i] = 0.0;
+ }
+/* frees matrix */
+void d_free(double *pA)
+ {
+ free( pA );
+ }
+/* prints a matrix in column-major format */
+void d_print_mat(int m, int n, double *A, int lda)
+ {
+ int i, j;
+ for(i=0; i<m; i++)
+ {
+ for(j=0; j<n; j++)
+ {
+ printf("%9.5f ", A[i+lda*j]);
+ }
+ printf("\n");
+ }
+ printf("\n");
+ }
+/* prints the transposed of a matrix in column-major format */
+void d_print_tran_mat(int row, int col, double *A, int lda)
+ {
+ int i, j;
+ for(j=0; j<col; j++)
+ {
+ for(i=0; i<row; i++)
+ {
+ printf("%9.5f ", A[i+lda*j]);
+ }
+ printf("\n");
+ }
+ printf("\n");
+ }
+/* prints a matrix in column-major format (exponential notation) */
+void d_print_e_mat(int m, int n, double *A, int lda)
+ {
+ int i, j;
+ for(i=0; i<m; i++)
+ {
+ for(j=0; j<n; j++)
+ {
+ printf("%e\t", A[i+lda*j]);
+ }
+ printf("\n");
+ }
+ printf("\n");
+ }
+/* prints the transposed of a matrix in column-major format (exponential notation) */
+void d_print_e_tran_mat(int row, int col, double *A, int lda)
+ {
+ int i, j;
+ for(j=0; j<col; j++)
+ {
+ for(i=0; i<row; i++)
+ {
+ printf("%e\t", A[i+lda*j]);
+ }
+ printf("\n");
+ }
+ printf("\n");
+ }
+/* creates a zero matrix aligned */
+void int_zeros(int **pA, int row, int col)
+ {
+ void *temp = malloc((row*col)*sizeof(int));
+ *pA = temp;
+ int *A = *pA;
+ int i;
+ for(i=0; i<row*col; i++) A[i] = 0;
+ }
+/* frees matrix */
+void int_free(int *pA)
+ {
+ free( pA );
+ }
+/* prints a matrix in column-major format */
+void int_print_mat(int row, int col, int *A, int lda)
+ {
+ int i, j;
+ for(i=0; i<row; i++)
+ {
+ for(j=0; j<col; j++)
+ {
+ printf("%d ", A[i+lda*j]);
+ }
+ printf("\n");
+ }
+ printf("\n");
+ }
+#endif
+
+
+
+#define KEEP_X0 0
+
+// printing
+#define PRINT 1
+
+/************************************************
+Mass-spring system: nx/2 masses connected each other with springs (in a row), and the first and the last one to walls. nu (<=nx) controls act on the first nu masses. The system is sampled with sampling time Ts.
+************************************************/
+void mass_spring_system(double Ts, int nx, int nu, int N, double *A, double *B, double *b, double *x0)
+ {
+
+ int nx2 = nx*nx;
+
+ int info = 0;
+
+ int pp = nx/2; // number of masses
+
+/************************************************
+* build the continuous time system
+************************************************/
+
+ double *T; d_zeros(&T, pp, pp);
+ int ii;
+ for(ii=0; ii<pp; ii++) T[ii*(pp+1)] = -2;
+ for(ii=0; ii<pp-1; ii++) T[ii*(pp+1)+1] = 1;
+ for(ii=1; ii<pp; ii++) T[ii*(pp+1)-1] = 1;
+
+ double *Z; d_zeros(&Z, pp, pp);
+ double *I; d_zeros(&I, pp, pp); for(ii=0; ii<pp; ii++) I[ii*(pp+1)]=1.0; // = eye(pp);
+ double *Ac; d_zeros(&Ac, nx, nx);
+ dmcopy(pp, pp, Z, pp, Ac, nx);
+ dmcopy(pp, pp, T, pp, Ac+pp, nx);
+ dmcopy(pp, pp, I, pp, Ac+pp*nx, nx);
+ dmcopy(pp, pp, Z, pp, Ac+pp*(nx+1), nx);
+ free(T);
+ free(Z);
+ free(I);
+
+ d_zeros(&I, nu, nu); for(ii=0; ii<nu; ii++) I[ii*(nu+1)]=1.0; //I = eye(nu);
+ double *Bc; d_zeros(&Bc, nx, nu);
+ dmcopy(nu, nu, I, nu, Bc+pp, nx);
+ free(I);
+
+/************************************************
+* compute the discrete time system
+************************************************/
+
+ double *bb; d_zeros(&bb, nx, 1);
+ dmcopy(nx, 1, bb, nx, b, nx);
+
+ dmcopy(nx, nx, Ac, nx, A, nx);
+ dscal_3l(nx2, Ts, A);
+ expm(nx, A);
+
+ d_zeros(&T, nx, nx);
+ d_zeros(&I, nx, nx); for(ii=0; ii<nx; ii++) I[ii*(nx+1)]=1.0; //I = eye(nx);
+ dmcopy(nx, nx, A, nx, T, nx);
+ daxpy_3l(nx2, -1.0, I, T);
+ dgemm_nn_3l(nx, nu, nx, T, nx, Bc, nx, B, nx);
+ free(T);
+ free(I);
+
+ int *ipiv = (int *) malloc(nx*sizeof(int));
+ dgesv_3l(nx, nu, Ac, nx, ipiv, B, nx, &info);
+ free(ipiv);
+
+ free(Ac);
+ free(Bc);
+ free(bb);
+
+
+/************************************************
+* initial state
+************************************************/
+
+ if(nx==4)
+ {
+ x0[0] = 5;
+ x0[1] = 10;
+ x0[2] = 15;
+ x0[3] = 20;
+ }
+ else
+ {
+ int jj;
+ for(jj=0; jj<nx; jj++)
+ x0[jj] = 1;
+ }
+
+ }
+
+
+
+int main()
+ {
+
+
+ // local variables
+
+ int ii, jj;
+
+ int rep, nrep=1000;
+
+ struct timeval tv0, tv1;
+
+
+
+ // problem size
+
+ int nx_ = 8; // number of states (it has to be even for the mass-spring system test problem)
+ int nu_ = 3; // number of inputs (controllers) (it has to be at least 1 and at most nx/2 for the mass-spring system test problem)
+ int N = 5; // horizon lenght
+
+
+
+ // stage-wise variant size
+
+ int nx[N+1];
+#if KEEP_X0
+ nx[0] = nx_;
+#else
+ nx[0] = 0;
+#endif
+ for(ii=1; ii<=N; ii++)
+ nx[ii] = nx_;
+// nx[N] = 0;
+
+ int nu[N+1];
+ for(ii=0; ii<N; ii++)
+ nu[ii] = nu_;
+ nu[N] = 0;
+
+#if 1
+ int nb[N+1];
+#if KEEP_X0
+ nb[0] = nu[0]+nx[0]/2;
+#else
+ nb[0] = nu[0];
+#endif
+ for(ii=1; ii<N; ii++)
+ nb[ii] = nu[1]+nx[1]/2;
+ nb[N] = nx[N]/2;
+
+ int ng[N+1];
+ ng[0] = 0;
+ for(ii=1; ii<N; ii++)
+ ng[ii] = 0;
+ ng[N] = 0;
+#elif 0
+ int nb[N+1];
+ nb[0] = 0;
+ for(ii=1; ii<N; ii++)
+ nb[ii] = 0;
+ nb[N] = 0;
+
+ int ng[N+1];
+#if KEEP_X0
+ ng[0] = nu[0]+nx[0]/2;
+#else
+ ng[0] = nu[0];
+#endif
+ for(ii=1; ii<N; ii++)
+ ng[ii] = nu[1]+nx[1]/2;
+ ng[N] = nx[N]/2;
+#else
+ int nb[N+1];
+ nb[0] = nu[0] + nx[0]/2;
+ for(ii=1; ii<N; ii++)
+ nb[ii] = nu[ii] + nx[ii]/2;
+ nb[N] = nu[N] + nx[N]/2;
+
+ int ng[N+1];
+#if KEEP_X0
+ ng[0] = nx[0]/2;
+#else
+ ng[0] = 0;
+#endif
+ for(ii=1; ii<N; ii++)
+ ng[ii] = nx[1]/2;
+ ng[N] = nx[N]/2;
+#endif
+
+/************************************************
+* dynamical system
+************************************************/
+
+ double *A; d_zeros(&A, nx_, nx_); // states update matrix
+
+ double *B; d_zeros(&B, nx_, nu_); // inputs matrix
+
+ double *b; d_zeros(&b, nx_, 1); // states offset
+ double *x0; d_zeros(&x0, nx_, 1); // initial state
+
+ double Ts = 0.5; // sampling time
+ mass_spring_system(Ts, nx_, nu_, N, A, B, b, x0);
+
+ for(jj=0; jj<nx_; jj++)
+ b[jj] = 0.1;
+
+ for(jj=0; jj<nx_; jj++)
+ x0[jj] = 0;
+ x0[0] = 2.5;
+ x0[1] = 2.5;
+
+ double *b0; d_zeros(&b0, nx_, 1);
+ dgemv_n_3l(nx_, nx_, A, nx_, x0, b0);
+ daxpy_3l(nx_, 1.0, b, b0);
+
+#if PRINT
+ d_print_mat(nx_, nx_, A, nx_);
+ d_print_mat(nx_, nu_, B, nu_);
+ d_print_mat(1, nx_, b, 1);
+ d_print_mat(1, nx_, x0, 1);
+ d_print_mat(1, nx_, b0, 1);
+#endif
+
+/************************************************
+* cost function
+************************************************/
+
+ double *Q; d_zeros(&Q, nx_, nx_);
+ for(ii=0; ii<nx_; ii++) Q[ii*(nx_+1)] = 1.0;
+
+ double *R; d_zeros(&R, nu_, nu_);
+ for(ii=0; ii<nu_; ii++) R[ii*(nu_+1)] = 2.0;
+
+ double *S; d_zeros(&S, nu_, nx_);
+
+ double *q; d_zeros(&q, nx_, 1);
+ for(ii=0; ii<nx_; ii++) q[ii] = 0.1;
+
+ double *r; d_zeros(&r, nu_, 1);
+ for(ii=0; ii<nu_; ii++) r[ii] = 0.2;
+
+ double *r0; d_zeros(&r0, nu_, 1);
+ dgemv_n_3l(nu_, nx_, S, nu_, x0, r0);
+ daxpy_3l(nu_, 1.0, r, r0);
+
+#if PRINT
+ d_print_mat(nx_, nx_, Q, nx_);
+ d_print_mat(nu_, nu_, R, nu_);
+ d_print_mat(nu_, nx_, S, nu_);
+ d_print_mat(1, nx_, q, 1);
+ d_print_mat(1, nu_, r, 1);
+ d_print_mat(1, nu_, r0, 1);
+#endif
+
+ // maximum element in cost functions
+ double mu0 = 2.0;
+
+/************************************************
+* box & general constraints
+************************************************/
+
+ int *idxb0; int_zeros(&idxb0, nb[0], 1);
+ double *d_lb0; d_zeros(&d_lb0, nb[0], 1);
+ double *d_ub0; d_zeros(&d_ub0, nb[0], 1);
+ double *d_lg0; d_zeros(&d_lg0, ng[0], 1);
+ double *d_ug0; d_zeros(&d_ug0, ng[0], 1);
+ for(ii=0; ii<nb[0]; ii++)
+ {
+ if(ii<nu[0]) // input
+ {
+ d_lb0[ii] = - 0.5; // umin
+ d_ub0[ii] = 0.5; // umax
+ }
+ else // state
+ {
+ d_lb0[ii] = - 4.0; // xmin
+ d_ub0[ii] = 4.0; // xmax
+ }
+ idxb0[ii] = ii;
+ }
+ for(ii=0; ii<ng[0]; ii++)
+ {
+ if(ii<nu[0]-nb[0]) // input
+ {
+ d_lg0[ii] = - 0.5; // umin
+ d_ug0[ii] = 0.5; // umax
+ }
+ else // state
+ {
+ d_lg0[ii] = - 4.0; // xmin
+ d_ug0[ii] = 4.0; // xmax
+ }
+ }
+
+ int *idxb1; int_zeros(&idxb1, nb[1], 1);
+ double *d_lb1; d_zeros(&d_lb1, nb[1], 1);
+ double *d_ub1; d_zeros(&d_ub1, nb[1], 1);
+ double *d_lg1; d_zeros(&d_lg1, ng[1], 1);
+ double *d_ug1; d_zeros(&d_ug1, ng[1], 1);
+ for(ii=0; ii<nb[1]; ii++)
+ {
+ if(ii<nu[1]) // input
+ {
+ d_lb1[ii] = - 0.5; // umin
+ d_ub1[ii] = 0.5; // umax
+ }
+ else // state
+ {
+ d_lb1[ii] = - 4.0; // xmin
+ d_ub1[ii] = 4.0; // xmax
+ }
+ idxb1[ii] = ii;
+ }
+ for(ii=0; ii<ng[1]; ii++)
+ {
+ if(ii<nu[1]-nb[1]) // input
+ {
+ d_lg1[ii] = - 0.5; // umin
+ d_ug1[ii] = 0.5; // umax
+ }
+ else // state
+ {
+ d_lg1[ii] = - 4.0; // xmin
+ d_ug1[ii] = 4.0; // xmax
+ }
+ }
+
+
+ int *idxbN; int_zeros(&idxbN, nb[N], 1);
+ double *d_lbN; d_zeros(&d_lbN, nb[N], 1);
+ double *d_ubN; d_zeros(&d_ubN, nb[N], 1);
+ double *d_lgN; d_zeros(&d_lgN, ng[N], 1);
+ double *d_ugN; d_zeros(&d_ugN, ng[N], 1);
+ for(ii=0; ii<nb[N]; ii++)
+ {
+ d_lbN[ii] = - 4.0; // xmin
+ d_ubN[ii] = 4.0; // xmax
+ idxbN[ii] = ii;
+ }
+ for(ii=0; ii<ng[N]; ii++)
+ {
+ d_lgN[ii] = - 4.0; // dmin
+ d_ugN[ii] = 4.0; // dmax
+ }
+
+ double *C0; d_zeros(&C0, ng[0], nx[0]);
+ double *D0; d_zeros(&D0, ng[0], nu[0]);
+ for(ii=0; ii<nu[0]-nb[0] & ii<ng[0]; ii++)
+ D0[ii+(nb[0]+ii)*ng[0]] = 1.0;
+ for(; ii<ng[0]; ii++)
+ C0[ii+(nb[0]+ii-nu[0])*ng[0]] = 1.0;
+
+ double *C1; d_zeros(&C1, ng[1], nx[1]);
+ double *D1; d_zeros(&D1, ng[1], nu[1]);
+ for(ii=0; ii<nu[1]-nb[1] & ii<ng[1]; ii++)
+ D1[ii+(nb[1]+ii)*ng[1]] = 1.0;
+ for(; ii<ng[1]; ii++)
+ C1[ii+(nb[1]+ii-nu[1])*ng[1]] = 1.0;
+
+ double *CN; d_zeros(&CN, ng[N], nx[N]);
+ double *DN; d_zeros(&DN, ng[N], nu[N]);
+ for(ii=0; ii<nu[N]-nb[N] & ii<ng[N]; ii++)
+ DN[ii+(nb[N]+ii)*ng[N]] = 1.0;
+ for(; ii<ng[N]; ii++)
+ CN[ii+(nb[N]+ii-nu[N])*ng[N]] = 1.0;
+
+#if PRINT
+ // box constraints
+ int_print_mat(1, nb[0], idxb0, 1);
+ d_print_mat(1, nb[0], d_lb0, 1);
+ d_print_mat(1, nb[0], d_ub0, 1);
+ int_print_mat(1, nb[1], idxb1, 1);
+ d_print_mat(1, nb[1], d_lb1, 1);
+ d_print_mat(1, nb[1], d_ub1, 1);
+ int_print_mat(1, nb[N], idxbN, 1);
+ d_print_mat(1, nb[N], d_lbN, 1);
+ d_print_mat(1, nb[N], d_ubN, 1);
+ // general constraints
+ d_print_mat(1, ng[0], d_lg0, 1);
+ d_print_mat(1, ng[0], d_ug0, 1);
+ d_print_mat(ng[0], nu[0], D0, ng[0]);
+ d_print_mat(ng[0], nx[0], C0, ng[0]);
+ d_print_mat(1, ng[1], d_lg1, 1);
+ d_print_mat(1, ng[1], d_ug1, 1);
+ d_print_mat(ng[1], nu[1], D1, ng[1]);
+ d_print_mat(ng[1], nx[1], C1, ng[1]);
+ d_print_mat(1, ng[N], d_lgN, 1);
+ d_print_mat(1, ng[N], d_ugN, 1);
+ d_print_mat(ng[N], nu[N], DN, ng[N]);
+ d_print_mat(ng[N], nx[N], CN, ng[N]);
+#endif
+
+/************************************************
+* array of matrices
+************************************************/
+
+ double *hA[N];
+ double *hB[N];
+ double *hb[N];
+ double *hQ[N+1];
+ double *hS[N+1];
+ double *hR[N+1];
+ double *hq[N+1];
+ double *hr[N+1];
+ double *hd_lb[N+1];
+ double *hd_ub[N+1];
+ double *hd_lg[N+1];
+ double *hd_ug[N+1];
+ double *hC[N+1];
+ double *hD[N+1];
+ int *hidxb[N+1];
+
+ hA[0] = A;
+ hB[0] = B;
+ hb[0] = b0;
+ hQ[0] = Q;
+ hS[0] = S;
+ hR[0] = R;
+ hq[0] = q;
+ hr[0] = r0;
+ hidxb[0] = idxb0;
+ hd_lb[0] = d_lb0;
+ hd_ub[0] = d_ub0;
+ hd_lg[0] = d_lg0;
+ hd_ug[0] = d_ug0;
+ hC[0] = C0;
+ hD[0] = D0;
+ for(ii=1; ii<N; ii++)
+ {
+ hA[ii] = A;
+ hB[ii] = B;
+ hb[ii] = b;
+ hQ[ii] = Q;
+ hS[ii] = S;
+ hR[ii] = R;
+ hq[ii] = q;
+ hr[ii] = r;
+ hidxb[ii] = idxb1;
+ hd_lb[ii] = d_lb1;
+ hd_ub[ii] = d_ub1;
+ hd_lg[ii] = d_lg1;
+ hd_ug[ii] = d_ug1;
+ hC[ii] = C1;
+ hD[ii] = D1;
+ }
+ hQ[N] = Q;
+ hS[N] = S;
+ hR[N] = R;
+ hq[N] = q;
+ hr[N] = r;
+ hidxb[N] = idxbN;
+ hd_lb[N] = d_lbN;
+ hd_ub[N] = d_ubN;
+ hd_lg[N] = d_lgN;
+ hd_ug[N] = d_ugN;
+ hC[N] = CN;
+ hD[N] = DN;
+
+/************************************************
+* ocp qp
+************************************************/
+
+ int qp_size = d_memsize_ocp_qp(N, nx, nu, nb, ng);
+ printf("\nqp size = %d\n", qp_size);
+ void *qp_mem = malloc(qp_size);
+
+ struct d_ocp_qp qp;
+ d_create_ocp_qp(N, nx, nu, nb, ng, &qp, qp_mem);
+ d_cvt_colmaj_to_ocp_qp(hA, hB, hb, hQ, hS, hR, hq, hr, hidxb, hd_lb, hd_ub, hC, hD, hd_lg, hd_ug, &qp);
+#if 0
+ printf("\nN = %d\n", qp.N);
+ for(ii=0; ii<N; ii++)
+ d_print_strmat(qp.nu[ii]+qp.nx[ii]+1, qp.nx[ii+1], qp.BAbt+ii, 0, 0);
+ for(ii=0; ii<N; ii++)
+ d_print_tran_strvec(qp.nx[ii+1], qp.b+ii, 0);
+ for(ii=0; ii<=N; ii++)
+ d_print_strmat(qp.nu[ii]+qp.nx[ii]+1, qp.nu[ii]+qp.nx[ii], qp.RSQrq+ii, 0, 0);
+ for(ii=0; ii<=N; ii++)
+ d_print_tran_strvec(qp.nu[ii]+qp.nx[ii], qp.rq+ii, 0);
+ for(ii=0; ii<=N; ii++)
+ int_print_mat(1, nb[ii], qp.idxb[ii], 1);
+ for(ii=0; ii<=N; ii++)
+ d_print_tran_strvec(qp.nb[ii], qp.d_lb+ii, 0);
+ for(ii=0; ii<=N; ii++)
+ d_print_tran_strvec(qp.nb[ii], qp.d_ub+ii, 0);
+ for(ii=0; ii<=N; ii++)
+ d_print_strmat(qp.nu[ii]+qp.nx[ii], qp.ng[ii], qp.DCt+ii, 0, 0);
+ for(ii=0; ii<=N; ii++)
+ d_print_tran_strvec(qp.ng[ii], qp.d_lg+ii, 0);
+ for(ii=0; ii<=N; ii++)
+ d_print_tran_strvec(qp.ng[ii], qp.d_ug+ii, 0);
+ return;
+#endif
+
+/************************************************
+* ocp qp sol
+************************************************/
+
+ int qp_sol_size = d_memsize_ocp_qp_sol(N, nx, nu, nb, ng);
+ printf("\nqp sol size = %d\n", qp_sol_size);
+ void *qp_sol_mem = malloc(qp_sol_size);
+
+ struct d_ocp_qp_sol qp_sol;
+ d_create_ocp_qp_sol(N, nx, nu, nb, ng, &qp_sol, qp_sol_mem);
+
+/************************************************
+* ipm
+************************************************/
+
+ struct d_ipm_hard_ocp_qp_arg arg;
+ arg.alpha_min = 1e-8;
+ arg.mu_max = 1e-12;
+ arg.iter_max = 20;
+ arg.mu0 = 2.0;
+
+ int ipm_size = d_memsize_ipm_hard_ocp_qp(&qp, &arg);
+ printf("\nipm size = %d\n", ipm_size);
+ void *ipm_mem = malloc(ipm_size);
+
+ struct d_ipm_hard_ocp_qp_workspace workspace;
+ d_create_ipm_hard_ocp_qp(&qp, &arg, &workspace, ipm_mem);
+
+ gettimeofday(&tv0, NULL); // start
+
+ for(rep=0; rep<nrep; rep++)
+ {
+// d_solve_ipm_hard_ocp_qp(&qp, &qp_sol, &workspace);
+ d_solve_ipm2_hard_ocp_qp(&qp, &qp_sol, &workspace);
+ }
+
+ gettimeofday(&tv1, NULL); // stop
+
+ double time_ocp_ipm = (tv1.tv_sec-tv0.tv_sec)/(nrep+0.0)+(tv1.tv_usec-tv0.tv_usec)/(nrep*1e6);
+
+/************************************************
+* extract and print solution
+************************************************/
+
+ double *u[N+1]; for(ii=0; ii<=N; ii++) d_zeros(u+ii, nu[ii], 1);
+ double *x[N+1]; for(ii=0; ii<=N; ii++) d_zeros(x+ii, nx[ii], 1);
+ double *pi[N]; for(ii=0; ii<N; ii++) d_zeros(pi+ii, nx[ii+1], 1);
+ double *lam_lb[N+1]; for(ii=0; ii<=N; ii++) d_zeros(lam_lb+ii, nb[ii], 1);
+ double *lam_ub[N+1]; for(ii=0; ii<=N; ii++) d_zeros(lam_ub+ii, nb[ii], 1);
+ double *lam_lg[N+1]; for(ii=0; ii<=N; ii++) d_zeros(lam_lg+ii, ng[ii], 1);
+ double *lam_ug[N+1]; for(ii=0; ii<=N; ii++) d_zeros(lam_ug+ii, ng[ii], 1);
+
+ d_cvt_ocp_qp_sol_to_colmaj(&qp, &qp_sol, u, x, pi, lam_lb, lam_ub, lam_lg, lam_ug);
+
+#if 1
+ printf("\nsolution\n\n");
+ printf("\nu\n");
+ for(ii=0; ii<=N; ii++)
+ d_print_mat(1, nu[ii], u[ii], 1);
+ printf("\nx\n");
+ for(ii=0; ii<=N; ii++)
+ d_print_mat(1, nx[ii], x[ii], 1);
+ printf("\npi\n");
+ for(ii=0; ii<N; ii++)
+ d_print_mat(1, nx[ii+1], pi[ii], 1);
+ printf("\nlam_lb\n");
+ for(ii=0; ii<=N; ii++)
+ d_print_mat(1, nb[ii], lam_lb[ii], 1);
+ printf("\nlam_ub\n");
+ for(ii=0; ii<=N; ii++)
+ d_print_mat(1, nb[ii], lam_ub[ii], 1);
+ printf("\nlam_lg\n");
+ for(ii=0; ii<=N; ii++)
+ d_print_mat(1, ng[ii], lam_lg[ii], 1);
+ printf("\nlam_ug\n");
+ for(ii=0; ii<=N; ii++)
+ d_print_mat(1, ng[ii], lam_ug[ii], 1);
+
+ printf("\nt_lb\n");
+ for(ii=0; ii<=N; ii++)
+ d_print_mat(1, nb[ii], (qp_sol.t_lb+ii)->pa, 1);
+ printf("\nt_ub\n");
+ for(ii=0; ii<=N; ii++)
+ d_print_mat(1, nb[ii], (qp_sol.t_ub+ii)->pa, 1);
+ printf("\nt_lg\n");
+ for(ii=0; ii<=N; ii++)
+ d_print_mat(1, ng[ii], (qp_sol.t_lg+ii)->pa, 1);
+ printf("\nt_ug\n");
+ for(ii=0; ii<=N; ii++)
+ d_print_mat(1, ng[ii], (qp_sol.t_ug+ii)->pa, 1);
+
+ printf("\nresiduals\n\n");
+ printf("\nres_g\n");
+ for(ii=0; ii<=N; ii++)
+ d_print_e_mat(1, nu[ii]+nx[ii], (workspace.res_g+ii)->pa, 1);
+ printf("\nres_b\n");
+ for(ii=0; ii<N; ii++)
+ d_print_e_mat(1, nx[ii+1], (workspace.res_b+ii)->pa, 1);
+ printf("\nres_m_lb\n");
+ for(ii=0; ii<=N; ii++)
+ d_print_e_mat(1, nb[ii], (workspace.res_m_lb+ii)->pa, 1);
+ printf("\nres_m_ub\n");
+ for(ii=0; ii<=N; ii++)
+ d_print_e_mat(1, nb[ii], (workspace.res_m_ub+ii)->pa, 1);
+ printf("\nres_m_lg\n");
+ for(ii=0; ii<=N; ii++)
+ d_print_e_mat(1, ng[ii], (workspace.res_m_lg+ii)->pa, 1);
+ printf("\nres_m_ug\n");
+ for(ii=0; ii<=N; ii++)
+ d_print_e_mat(1, ng[ii], (workspace.res_m_ug+ii)->pa, 1);
+ printf("\nres_d_lb\n");
+ for(ii=0; ii<=N; ii++)
+ d_print_e_mat(1, nb[ii], (workspace.res_d_lb+ii)->pa, 1);
+ printf("\nres_d_ub\n");
+ for(ii=0; ii<=N; ii++)
+ d_print_e_mat(1, nb[ii], (workspace.res_d_ub+ii)->pa, 1);
+ printf("\nres_d_lg\n");
+ for(ii=0; ii<=N; ii++)
+ d_print_e_mat(1, ng[ii], (workspace.res_d_lg+ii)->pa, 1);
+ printf("\nres_d_ug\n");
+ for(ii=0; ii<=N; ii++)
+ d_print_e_mat(1, ng[ii], (workspace.res_d_ug+ii)->pa, 1);
+ printf("\nres_mu\n");
+ printf("\n%e\n\n", workspace.res_mu);
+#endif
+
+ printf("\nipm iter = %d\n", workspace.iter);
+ printf("\nalpha_aff\tmu_aff\t\tsigma\t\talpha\t\tmu\n");
+ d_print_e_tran_mat(5, workspace.iter, workspace.stat, 5);
+
+ printf("\nocp ipm time = %e [s]\n\n", time_ocp_ipm);
+
+/************************************************
+* free memory
+************************************************/
+
+ d_free(A);
+ d_free(B);
+ d_free(b);
+ d_free(x0);
+ d_free(Q);
+ d_free(R);
+ d_free(S);
+ d_free(q);
+ d_free(r);
+ d_free(r0);
+ int_free(idxb0);
+ d_free(d_lb0);
+ d_free(d_ub0);
+ int_free(idxb1);
+ d_free(d_lb1);
+ d_free(d_ub1);
+ int_free(idxbN);
+ d_free(d_lbN);
+ d_free(d_ubN);
+ d_free(C0);
+ d_free(D0);
+ d_free(d_lg0);
+ d_free(d_ug0);
+ d_free(C1);
+ d_free(D1);
+ d_free(d_lg1);
+ d_free(d_ug1);
+ d_free(CN);
+ d_free(DN);
+ d_free(d_lgN);
+ d_free(d_ugN);
+
+ for(ii=0; ii<N; ii++)
+ {
+ d_free(u[ii]);
+ d_free(x[ii]);
+ d_free(pi[ii]);
+ d_free(lam_lb[ii]);
+ d_free(lam_ub[ii]);
+ d_free(lam_lg[ii]);
+ d_free(lam_ug[ii]);
+ }
+ d_free(u[ii]);
+ d_free(x[ii]);
+ d_free(lam_lb[ii]);
+ d_free(lam_ub[ii]);
+ d_free(lam_lg[ii]);
+ d_free(lam_ug[ii]);
+
+ free(qp_mem);
+ free(qp_sol_mem);
+ free(ipm_mem);
+
+/************************************************
+* return
+************************************************/
+
+ return 0;
+
+ }
diff --git a/test_problems/test_m_ocp.c b/test_problems/test_m_ocp.c
new file mode 100644
index 0000000..7bca2d2
--- /dev/null
+++ b/test_problems/test_m_ocp.c
@@ -0,0 +1,876 @@
+/**************************************************************************************************
+* *
+* 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 <stdlib.h>
+#include <stdio.h>
+#include <math.h>
+#include <sys/time.h>
+
+#include <blasfeo_target.h>
+#include <blasfeo_common.h>
+#include <blasfeo_v_aux_ext_dep.h>
+#include <blasfeo_d_aux_ext_dep.h>
+#include <blasfeo_s_aux_ext_dep.h>
+#include <blasfeo_i_aux_ext_dep.h>
+#include <blasfeo_d_aux.h>
+#include <blasfeo_d_blas.h>
+
+#include "../include/hpipm_d_ocp_qp.h"
+#include "../include/hpipm_s_ocp_qp.h"
+#include "../include/hpipm_m_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 "d_tools.h"
+
+
+
+#if ! defined(EXT_DEP)
+/* creates a zero matrix */
+void d_zeros(double **pA, int row, int col)
+ {
+ *pA = malloc((row*col)*sizeof(double));
+ double *A = *pA;
+ int i;
+ for(i=0; i<row*col; i++) A[i] = 0.0;
+ }
+/* frees matrix */
+void d_free(double *pA)
+ {
+ free( pA );
+ }
+/* prints a matrix in column-major format */
+void d_print_mat(int m, int n, double *A, int lda)
+ {
+ int i, j;
+ for(i=0; i<m; i++)
+ {
+ for(j=0; j<n; j++)
+ {
+ printf("%9.5f ", A[i+lda*j]);
+ }
+ printf("\n");
+ }
+ printf("\n");
+ }
+/* prints the transposed of a matrix in column-major format */
+void d_print_tran_mat(int row, int col, double *A, int lda)
+ {
+ int i, j;
+ for(j=0; j<col; j++)
+ {
+ for(i=0; i<row; i++)
+ {
+ printf("%9.5f ", A[i+lda*j]);
+ }
+ printf("\n");
+ }
+ printf("\n");
+ }
+/* prints a matrix in column-major format (exponential notation) */
+void d_print_e_mat(int m, int n, double *A, int lda)
+ {
+ int i, j;
+ for(i=0; i<m; i++)
+ {
+ for(j=0; j<n; j++)
+ {
+ printf("%e\t", A[i+lda*j]);
+ }
+ printf("\n");
+ }
+ printf("\n");
+ }
+/* prints the transposed of a matrix in column-major format (exponential notation) */
+void d_print_e_tran_mat(int row, int col, double *A, int lda)
+ {
+ int i, j;
+ for(j=0; j<col; j++)
+ {
+ for(i=0; i<row; i++)
+ {
+ printf("%e\t", A[i+lda*j]);
+ }
+ printf("\n");
+ }
+ printf("\n");
+ }
+/* creates a zero matrix aligned */
+void int_zeros(int **pA, int row, int col)
+ {
+ void *temp = malloc((row*col)*sizeof(int));
+ *pA = temp;
+ int *A = *pA;
+ int i;
+ for(i=0; i<row*col; i++) A[i] = 0;
+ }
+/* frees matrix */
+void int_free(int *pA)
+ {
+ free( pA );
+ }
+/* prints a matrix in column-major format */
+void int_print_mat(int row, int col, int *A, int lda)
+ {
+ int i, j;
+ for(i=0; i<row; i++)
+ {
+ for(j=0; j<col; j++)
+ {
+ printf("%d ", A[i+lda*j]);
+ }
+ printf("\n");
+ }
+ printf("\n");
+ }
+#endif
+
+
+
+#define KEEP_X0 0
+
+// printing
+#define PRINT 1
+
+/************************************************
+Mass-spring system: nx/2 masses connected each other with springs (in a row), and the first and the last one to walls. nu (<=nx) controls act on the first nu masses. The system is sampled with sampling time Ts.
+************************************************/
+void mass_spring_system(double Ts, int nx, int nu, int N, double *A, double *B, double *b, double *x0)
+ {
+
+ int nx2 = nx*nx;
+
+ int info = 0;
+
+ int pp = nx/2; // number of masses
+
+/************************************************
+* build the continuous time system
+************************************************/
+
+ double *T; d_zeros(&T, pp, pp);
+ int ii;
+ for(ii=0; ii<pp; ii++) T[ii*(pp+1)] = -2;
+ for(ii=0; ii<pp-1; ii++) T[ii*(pp+1)+1] = 1;
+ for(ii=1; ii<pp; ii++) T[ii*(pp+1)-1] = 1;
+
+ double *Z; d_zeros(&Z, pp, pp);
+ double *I; d_zeros(&I, pp, pp); for(ii=0; ii<pp; ii++) I[ii*(pp+1)]=1.0; // = eye(pp);
+ double *Ac; d_zeros(&Ac, nx, nx);
+ dmcopy(pp, pp, Z, pp, Ac, nx);
+ dmcopy(pp, pp, T, pp, Ac+pp, nx);
+ dmcopy(pp, pp, I, pp, Ac+pp*nx, nx);
+ dmcopy(pp, pp, Z, pp, Ac+pp*(nx+1), nx);
+ free(T);
+ free(Z);
+ free(I);
+
+ d_zeros(&I, nu, nu); for(ii=0; ii<nu; ii++) I[ii*(nu+1)]=1.0; //I = eye(nu);
+ double *Bc; d_zeros(&Bc, nx, nu);
+ dmcopy(nu, nu, I, nu, Bc+pp, nx);
+ free(I);
+
+/************************************************
+* compute the discrete time system
+************************************************/
+
+ double *bb; d_zeros(&bb, nx, 1);
+ dmcopy(nx, 1, bb, nx, b, nx);
+
+ dmcopy(nx, nx, Ac, nx, A, nx);
+ dscal_3l(nx2, Ts, A);
+ expm(nx, A);
+
+ d_zeros(&T, nx, nx);
+ d_zeros(&I, nx, nx); for(ii=0; ii<nx; ii++) I[ii*(nx+1)]=1.0; //I = eye(nx);
+ dmcopy(nx, nx, A, nx, T, nx);
+ daxpy_3l(nx2, -1.0, I, T);
+ dgemm_nn_3l(nx, nu, nx, T, nx, Bc, nx, B, nx);
+ free(T);
+ free(I);
+
+ int *ipiv = (int *) malloc(nx*sizeof(int));
+ dgesv_3l(nx, nu, Ac, nx, ipiv, B, nx, &info);
+ free(ipiv);
+
+ free(Ac);
+ free(Bc);
+ free(bb);
+
+
+/************************************************
+* initial state
+************************************************/
+
+ if(nx==4)
+ {
+ x0[0] = 5;
+ x0[1] = 10;
+ x0[2] = 15;
+ x0[3] = 20;
+ }
+ else
+ {
+ int jj;
+ for(jj=0; jj<nx; jj++)
+ x0[jj] = 1;
+ }
+
+ }
+
+
+
+int main()
+ {
+
+
+ // local variables
+
+ int ii, jj;
+
+ int rep, nrep=1000;
+
+ struct timeval tv0, tv1;
+
+
+
+ // problem size
+
+ int nx_ = 8; // number of states (it has to be even for the mass-spring system test problem)
+ int nu_ = 3; // number of inputs (controllers) (it has to be at least 1 and at most nx/2 for the mass-spring system test problem)
+ int N = 5; // horizon lenght
+
+
+
+ // stage-wise variant size
+
+ int nx[N+1];
+#if KEEP_X0
+ nx[0] = nx_;
+#else
+ nx[0] = 0;
+#endif
+ for(ii=1; ii<=N; ii++)
+ nx[ii] = nx_;
+// nx[N] = 0;
+
+ int nu[N+1];
+ for(ii=0; ii<N; ii++)
+ nu[ii] = nu_;
+ nu[N] = 0;
+
+#if 1
+ int nb[N+1];
+#if KEEP_X0
+ nb[0] = nu[0]+nx[0]/2;
+#else
+ nb[0] = nu[0];
+#endif
+ for(ii=1; ii<N; ii++)
+ nb[ii] = nu[1]+nx[1]/2;
+ nb[N] = nx[N]/2;
+
+ int ng[N+1];
+ ng[0] = 0;
+ for(ii=1; ii<N; ii++)
+ ng[ii] = 0;
+ ng[N] = 0;
+#elif 0
+ int nb[N+1];
+ nb[0] = 0;
+ for(ii=1; ii<N; ii++)
+ nb[ii] = 0;
+ nb[N] = 0;
+
+ int ng[N+1];
+#if KEEP_X0
+ ng[0] = nu[0]+nx[0]/2;
+#else
+ ng[0] = nu[0];
+#endif
+ for(ii=1; ii<N; ii++)
+ ng[ii] = nu[1]+nx[1]/2;
+ ng[N] = nx[N]/2;
+#else
+ int nb[N+1];
+ nb[0] = nu[0] + nx[0]/2;
+ for(ii=1; ii<N; ii++)
+ nb[ii] = nu[ii] + nx[ii]/2;
+ nb[N] = nu[N] + nx[N]/2;
+
+ int ng[N+1];
+#if KEEP_X0
+ ng[0] = nx[0]/2;
+#else
+ ng[0] = 0;
+#endif
+ for(ii=1; ii<N; ii++)
+ ng[ii] = nx[1]/2;
+ ng[N] = nx[N]/2;
+#endif
+
+/************************************************
+* dynamical system
+************************************************/
+
+ double *A; d_zeros(&A, nx_, nx_); // states update matrix
+
+ double *B; d_zeros(&B, nx_, nu_); // inputs matrix
+
+ double *b; d_zeros(&b, nx_, 1); // states offset
+ double *x0; d_zeros(&x0, nx_, 1); // initial state
+
+ double Ts = 0.5; // sampling time
+ mass_spring_system(Ts, nx_, nu_, N, A, B, b, x0);
+
+ for(jj=0; jj<nx_; jj++)
+ b[jj] = 0.1;
+
+ for(jj=0; jj<nx_; jj++)
+ x0[jj] = 0;
+ x0[0] = 2.5;
+ x0[1] = 2.5;
+
+ double *b0; d_zeros(&b0, nx_, 1);
+ dgemv_n_3l(nx_, nx_, A, nx_, x0, b0);
+ daxpy_3l(nx_, 1.0, b, b0);
+
+#if PRINT
+ d_print_mat(nx_, nx_, A, nx_);
+ d_print_mat(nx_, nu_, B, nu_);
+ d_print_mat(1, nx_, b, 1);
+ d_print_mat(1, nx_, x0, 1);
+ d_print_mat(1, nx_, b0, 1);
+#endif
+
+/************************************************
+* cost function
+************************************************/
+
+ double *Q; d_zeros(&Q, nx_, nx_);
+ for(ii=0; ii<nx_; ii++) Q[ii*(nx_+1)] = 1.0;
+
+ double *R; d_zeros(&R, nu_, nu_);
+ for(ii=0; ii<nu_; ii++) R[ii*(nu_+1)] = 2.0;
+
+ double *S; d_zeros(&S, nu_, nx_);
+
+ double *q; d_zeros(&q, nx_, 1);
+ for(ii=0; ii<nx_; ii++) q[ii] = 0.1;
+
+ double *r; d_zeros(&r, nu_, 1);
+ for(ii=0; ii<nu_; ii++) r[ii] = 0.2;
+
+ double *r0; d_zeros(&r0, nu_, 1);
+ dgemv_n_3l(nu_, nx_, S, nu_, x0, r0);
+ daxpy_3l(nu_, 1.0, r, r0);
+
+#if PRINT
+ d_print_mat(nx_, nx_, Q, nx_);
+ d_print_mat(nu_, nu_, R, nu_);
+ d_print_mat(nu_, nx_, S, nu_);
+ d_print_mat(1, nx_, q, 1);
+ d_print_mat(1, nu_, r, 1);
+ d_print_mat(1, nu_, r0, 1);
+#endif
+
+ // maximum element in cost functions
+ double mu0 = 2.0;
+
+/************************************************
+* box & general constraints
+************************************************/
+
+ int *idxb0; int_zeros(&idxb0, nb[0], 1);
+ double *d_lb0; d_zeros(&d_lb0, nb[0], 1);
+ double *d_ub0; d_zeros(&d_ub0, nb[0], 1);
+ double *d_lg0; d_zeros(&d_lg0, ng[0], 1);
+ double *d_ug0; d_zeros(&d_ug0, ng[0], 1);
+ for(ii=0; ii<nb[0]; ii++)
+ {
+ if(ii<nu[0]) // input
+ {
+ d_lb0[ii] = - 0.5; // umin
+ d_ub0[ii] = 0.5; // umax
+ }
+ else // state
+ {
+ d_lb0[ii] = - 4.0; // xmin
+ d_ub0[ii] = 4.0; // xmax
+ }
+ idxb0[ii] = ii;
+ }
+ for(ii=0; ii<ng[0]; ii++)
+ {
+ if(ii<nu[0]-nb[0]) // input
+ {
+ d_lg0[ii] = - 0.5; // umin
+ d_ug0[ii] = 0.5; // umax
+ }
+ else // state
+ {
+ d_lg0[ii] = - 4.0; // xmin
+ d_ug0[ii] = 4.0; // xmax
+ }
+ }
+
+ int *idxb1; int_zeros(&idxb1, nb[1], 1);
+ double *d_lb1; d_zeros(&d_lb1, nb[1], 1);
+ double *d_ub1; d_zeros(&d_ub1, nb[1], 1);
+ double *d_lg1; d_zeros(&d_lg1, ng[1], 1);
+ double *d_ug1; d_zeros(&d_ug1, ng[1], 1);
+ for(ii=0; ii<nb[1]; ii++)
+ {
+ if(ii<nu[1]) // input
+ {
+ d_lb1[ii] = - 0.5; // umin
+ d_ub1[ii] = 0.5; // umax
+ }
+ else // state
+ {
+ d_lb1[ii] = - 4.0; // xmin
+ d_ub1[ii] = 4.0; // xmax
+ }
+ idxb1[ii] = ii;
+ }
+ for(ii=0; ii<ng[1]; ii++)
+ {
+ if(ii<nu[1]-nb[1]) // input
+ {
+ d_lg1[ii] = - 0.5; // umin
+ d_ug1[ii] = 0.5; // umax
+ }
+ else // state
+ {
+ d_lg1[ii] = - 4.0; // xmin
+ d_ug1[ii] = 4.0; // xmax
+ }
+ }
+
+
+ int *idxbN; int_zeros(&idxbN, nb[N], 1);
+ double *d_lbN; d_zeros(&d_lbN, nb[N], 1);
+ double *d_ubN; d_zeros(&d_ubN, nb[N], 1);
+ double *d_lgN; d_zeros(&d_lgN, ng[N], 1);
+ double *d_ugN; d_zeros(&d_ugN, ng[N], 1);
+ for(ii=0; ii<nb[N]; ii++)
+ {
+ d_lbN[ii] = - 4.0; // xmin
+ d_ubN[ii] = 4.0; // xmax
+ idxbN[ii] = ii;
+ }
+ for(ii=0; ii<ng[N]; ii++)
+ {
+ d_lgN[ii] = - 4.0; // dmin
+ d_ugN[ii] = 4.0; // dmax
+ }
+
+ double *C0; d_zeros(&C0, ng[0], nx[0]);
+ double *D0; d_zeros(&D0, ng[0], nu[0]);
+ for(ii=0; ii<nu[0]-nb[0] & ii<ng[0]; ii++)
+ D0[ii+(nb[0]+ii)*ng[0]] = 1.0;
+ for(; ii<ng[0]; ii++)
+ C0[ii+(nb[0]+ii-nu[0])*ng[0]] = 1.0;
+
+ double *C1; d_zeros(&C1, ng[1], nx[1]);
+ double *D1; d_zeros(&D1, ng[1], nu[1]);
+ for(ii=0; ii<nu[1]-nb[1] & ii<ng[1]; ii++)
+ D1[ii+(nb[1]+ii)*ng[1]] = 1.0;
+ for(; ii<ng[1]; ii++)
+ C1[ii+(nb[1]+ii-nu[1])*ng[1]] = 1.0;
+
+ double *CN; d_zeros(&CN, ng[N], nx[N]);
+ double *DN; d_zeros(&DN, ng[N], nu[N]);
+ for(ii=0; ii<nu[N]-nb[N] & ii<ng[N]; ii++)
+ DN[ii+(nb[N]+ii)*ng[N]] = 1.0;
+ for(; ii<ng[N]; ii++)
+ CN[ii+(nb[N]+ii-nu[N])*ng[N]] = 1.0;
+
+#if PRINT
+ // box constraints
+ int_print_mat(1, nb[0], idxb0, 1);
+ d_print_mat(1, nb[0], d_lb0, 1);
+ d_print_mat(1, nb[0], d_ub0, 1);
+ int_print_mat(1, nb[1], idxb1, 1);
+ d_print_mat(1, nb[1], d_lb1, 1);
+ d_print_mat(1, nb[1], d_ub1, 1);
+ int_print_mat(1, nb[N], idxbN, 1);
+ d_print_mat(1, nb[N], d_lbN, 1);
+ d_print_mat(1, nb[N], d_ubN, 1);
+ // general constraints
+ d_print_mat(1, ng[0], d_lg0, 1);
+ d_print_mat(1, ng[0], d_ug0, 1);
+ d_print_mat(ng[0], nu[0], D0, ng[0]);
+ d_print_mat(ng[0], nx[0], C0, ng[0]);
+ d_print_mat(1, ng[1], d_lg1, 1);
+ d_print_mat(1, ng[1], d_ug1, 1);
+ d_print_mat(ng[1], nu[1], D1, ng[1]);
+ d_print_mat(ng[1], nx[1], C1, ng[1]);
+ d_print_mat(1, ng[N], d_lgN, 1);
+ d_print_mat(1, ng[N], d_ugN, 1);
+ d_print_mat(ng[N], nu[N], DN, ng[N]);
+ d_print_mat(ng[N], nx[N], CN, ng[N]);
+#endif
+
+/************************************************
+* array of matrices
+************************************************/
+
+ double *hA[N];
+ double *hB[N];
+ double *hb[N];
+ double *hQ[N+1];
+ double *hS[N+1];
+ double *hR[N+1];
+ double *hq[N+1];
+ double *hr[N+1];
+ double *hd_lb[N+1];
+ double *hd_ub[N+1];
+ double *hd_lg[N+1];
+ double *hd_ug[N+1];
+ double *hC[N+1];
+ double *hD[N+1];
+ int *hidxb[N+1];
+
+ hA[0] = A;
+ hB[0] = B;
+ hb[0] = b0;
+ hQ[0] = Q;
+ hS[0] = S;
+ hR[0] = R;
+ hq[0] = q;
+ hr[0] = r0;
+ hidxb[0] = idxb0;
+ hd_lb[0] = d_lb0;
+ hd_ub[0] = d_ub0;
+ hd_lg[0] = d_lg0;
+ hd_ug[0] = d_ug0;
+ hC[0] = C0;
+ hD[0] = D0;
+ for(ii=1; ii<N; ii++)
+ {
+ hA[ii] = A;
+ hB[ii] = B;
+ hb[ii] = b;
+ hQ[ii] = Q;
+ hS[ii] = S;
+ hR[ii] = R;
+ hq[ii] = q;
+ hr[ii] = r;
+ hidxb[ii] = idxb1;
+ hd_lb[ii] = d_lb1;
+ hd_ub[ii] = d_ub1;
+ hd_lg[ii] = d_lg1;
+ hd_ug[ii] = d_ug1;
+ hC[ii] = C1;
+ hD[ii] = D1;
+ }
+ hQ[N] = Q;
+ hS[N] = S;
+ hR[N] = R;
+ hq[N] = q;
+ hr[N] = r;
+ hidxb[N] = idxbN;
+ hd_lb[N] = d_lbN;
+ hd_ub[N] = d_ubN;
+ hd_lg[N] = d_lgN;
+ hd_ug[N] = d_ugN;
+ hC[N] = CN;
+ hD[N] = DN;
+
+/************************************************
+* d ocp qp
+************************************************/
+
+ int d_qp_size = d_memsize_ocp_qp(N, nx, nu, nb, ng);
+ printf("\nd qp size = %d\n", d_qp_size);
+ void *d_qp_mem = malloc(d_qp_size);
+
+ struct d_ocp_qp d_qp;
+ d_create_ocp_qp(N, nx, nu, nb, ng, &d_qp, d_qp_mem);
+ d_cvt_colmaj_to_ocp_qp(hA, hB, hb, hQ, hS, hR, hq, hr, hidxb, hd_lb, hd_ub, hC, hD, hd_lg, hd_ug, &d_qp);
+#if 0
+ printf("\nN = %d\n", d_qp.N);
+ for(ii=0; ii<N; ii++)
+ d_print_strmat(d_qp.nu[ii]+d_qp.nx[ii]+1, d_qp.nx[ii+1], d_qp.BAbt+ii, 0, 0);
+ for(ii=0; ii<N; ii++)
+ d_print_tran_strvec(d_qp.nx[ii+1], d_qp.b+ii, 0);
+ for(ii=0; ii<=N; ii++)
+ d_print_strmat(d_qp.nu[ii]+d_qp.nx[ii]+1, d_qp.nu[ii]+d_qp.nx[ii], d_qp.RSQrq+ii, 0, 0);
+ for(ii=0; ii<=N; ii++)
+ d_print_tran_strvec(d_qp.nu[ii]+d_qp.nx[ii], d_qp.rq+ii, 0);
+ for(ii=0; ii<=N; ii++)
+ int_print_mat(1, nb[ii], d_qp.idxb[ii], 1);
+ for(ii=0; ii<=N; ii++)
+ d_print_tran_strvec(d_qp.nb[ii], d_qp.d_lb+ii, 0);
+ for(ii=0; ii<=N; ii++)
+ d_print_tran_strvec(d_qp.nb[ii], d_qp.d_ub+ii, 0);
+ for(ii=0; ii<=N; ii++)
+ d_print_strmat(d_qp.nu[ii]+d_qp.nx[ii], d_qp.ng[ii], d_qp.DCt+ii, 0, 0);
+ for(ii=0; ii<=N; ii++)
+ d_print_tran_strvec(d_qp.ng[ii], d_qp.d_lg+ii, 0);
+ for(ii=0; ii<=N; ii++)
+ d_print_tran_strvec(d_qp.ng[ii], d_qp.d_ug+ii, 0);
+// return;
+#endif
+
+/************************************************
+* s ocp qp
+************************************************/
+
+ int s_qp_size = s_memsize_ocp_qp(N, nx, nu, nb, ng);
+ printf("\ns qp size = %d\n", s_qp_size);
+ void *s_qp_mem = malloc(s_qp_size);
+
+ struct s_ocp_qp s_qp;
+ s_create_ocp_qp(N, nx, nu, nb, ng, &s_qp, s_qp_mem);
+// d_cvt_colmaj_to_ocp_qp(hA, hB, hb, hQ, hS, hR, hq, hr, hidxb, hd_lb, hd_ub, hC, hD, hd_lg, hd_ug, &d_qp);
+ m_cvt_d_ocp_qp_to_s_ocp_qp(&d_qp, &s_qp);
+
+#if 0
+ printf("\nN = %d\n", s_qp.N);
+ for(ii=0; ii<N; ii++)
+ s_print_strmat(s_qp.nu[ii]+s_qp.nx[ii]+1, s_qp.nx[ii+1], s_qp.BAbt+ii, 0, 0);
+ for(ii=0; ii<N; ii++)
+ s_print_tran_strvec(s_qp.nx[ii+1], s_qp.b+ii, 0);
+ for(ii=0; ii<=N; ii++)
+ s_print_strmat(s_qp.nu[ii]+s_qp.nx[ii]+1, s_qp.nu[ii]+s_qp.nx[ii], s_qp.RSQrq+ii, 0, 0);
+ for(ii=0; ii<=N; ii++)
+ s_print_tran_strvec(s_qp.nu[ii]+s_qp.nx[ii], s_qp.rq+ii, 0);
+ for(ii=0; ii<=N; ii++)
+ int_print_mat(1, nb[ii], s_qp.idxb[ii], 1);
+ for(ii=0; ii<=N; ii++)
+ s_print_tran_strvec(s_qp.nb[ii], s_qp.d_lb+ii, 0);
+ for(ii=0; ii<=N; ii++)
+ s_print_tran_strvec(s_qp.nb[ii], s_qp.d_ub+ii, 0);
+ for(ii=0; ii<=N; ii++)
+ s_print_strmat(s_qp.nu[ii]+s_qp.nx[ii], s_qp.ng[ii], s_qp.DCt+ii, 0, 0);
+ for(ii=0; ii<=N; ii++)
+ s_print_tran_strvec(s_qp.ng[ii], s_qp.d_lg+ii, 0);
+ for(ii=0; ii<=N; ii++)
+ s_print_tran_strvec(s_qp.ng[ii], s_qp.d_ug+ii, 0);
+// return;
+#endif
+
+/************************************************
+* ocp qp sol
+************************************************/
+
+ int d_qp_sol_size = d_memsize_ocp_qp_sol(N, nx, nu, nb, ng);
+ printf("\nd qp sol size = %d\n", d_qp_sol_size);
+ void *d_qp_sol_mem = malloc(d_qp_sol_size);
+
+ struct d_ocp_qp_sol d_qp_sol;
+ d_create_ocp_qp_sol(N, nx, nu, nb, ng, &d_qp_sol, d_qp_sol_mem);
+
+/************************************************
+* ipm
+************************************************/
+
+ struct m_ipm_hard_ocp_qp_arg arg;
+ arg.alpha_min = 1e-8;
+ arg.mu_max = 1e-12;
+ arg.iter_max = 20;
+ arg.mu0 = 2.0;
+
+ int ipm_size = m_memsize_ipm_hard_ocp_qp(&d_qp, &s_qp, &arg);
+ printf("\nipm size = %d\n", ipm_size);
+ void *m_ipm_mem = malloc(ipm_size);
+
+ struct m_ipm_hard_ocp_qp_workspace workspace;
+ m_create_ipm_hard_ocp_qp(&d_qp, &s_qp, &arg, &workspace, m_ipm_mem);
+
+ gettimeofday(&tv0, NULL); // start
+
+ for(rep=0; rep<nrep; rep++)
+ {
+// m_solve_ipm_hard_ocp_qp(&d_qp, &s_qp, &d_qp_sol, &workspace);
+ m_solve_ipm2_hard_ocp_qp(&d_qp, &s_qp, &d_qp_sol, &workspace);
+ }
+
+ gettimeofday(&tv1, NULL); // stop
+
+ double time_ocp_ipm = (tv1.tv_sec-tv0.tv_sec)/(nrep+0.0)+(tv1.tv_usec-tv0.tv_usec)/(nrep*1e6);
+
+/************************************************
+* extract and print solution
+************************************************/
+
+ double *u[N+1]; for(ii=0; ii<=N; ii++) d_zeros(u+ii, nu[ii], 1);
+ double *x[N+1]; for(ii=0; ii<=N; ii++) d_zeros(x+ii, nx[ii], 1);
+ double *pi[N]; for(ii=0; ii<N; ii++) d_zeros(pi+ii, nx[ii+1], 1);
+ double *lam_lb[N+1]; for(ii=0; ii<=N; ii++) d_zeros(lam_lb+ii, nb[ii], 1);
+ double *lam_ub[N+1]; for(ii=0; ii<=N; ii++) d_zeros(lam_ub+ii, nb[ii], 1);
+ double *lam_lg[N+1]; for(ii=0; ii<=N; ii++) d_zeros(lam_lg+ii, ng[ii], 1);
+ double *lam_ug[N+1]; for(ii=0; ii<=N; ii++) d_zeros(lam_ug+ii, ng[ii], 1);
+
+ d_cvt_ocp_qp_sol_to_colmaj(&d_qp, &d_qp_sol, u, x, pi, lam_lb, lam_ub, lam_lg, lam_ug);
+
+#if 1
+ printf("\nsolution\n\n");
+ printf("\nu\n");
+ for(ii=0; ii<=N; ii++)
+ d_print_mat(1, nu[ii], u[ii], 1);
+ printf("\nx\n");
+ for(ii=0; ii<=N; ii++)
+ d_print_mat(1, nx[ii], x[ii], 1);
+ printf("\npi\n");
+ for(ii=0; ii<N; ii++)
+ d_print_mat(1, nx[ii+1], pi[ii], 1);
+ printf("\nlam_lb\n");
+ for(ii=0; ii<=N; ii++)
+ d_print_mat(1, nb[ii], lam_lb[ii], 1);
+ printf("\nlam_ub\n");
+ for(ii=0; ii<=N; ii++)
+ d_print_mat(1, nb[ii], lam_ub[ii], 1);
+ printf("\nlam_lg\n");
+ for(ii=0; ii<=N; ii++)
+ d_print_mat(1, ng[ii], lam_lg[ii], 1);
+ printf("\nlam_ug\n");
+ for(ii=0; ii<=N; ii++)
+ d_print_mat(1, ng[ii], lam_ug[ii], 1);
+
+ printf("\nt_lb\n");
+ for(ii=0; ii<=N; ii++)
+ d_print_mat(1, nb[ii], (d_qp_sol.t_lb+ii)->pa, 1);
+ printf("\nt_ub\n");
+ for(ii=0; ii<=N; ii++)
+ d_print_mat(1, nb[ii], (d_qp_sol.t_ub+ii)->pa, 1);
+ printf("\nt_lg\n");
+ for(ii=0; ii<=N; ii++)
+ d_print_mat(1, ng[ii], (d_qp_sol.t_lg+ii)->pa, 1);
+ printf("\nt_ug\n");
+ for(ii=0; ii<=N; ii++)
+ d_print_mat(1, ng[ii], (d_qp_sol.t_ug+ii)->pa, 1);
+
+ printf("\nresiduals\n\n");
+ printf("\nres_g\n");
+ for(ii=0; ii<=N; ii++)
+ d_print_e_mat(1, nu[ii]+nx[ii], (workspace.res_g+ii)->pa, 1);
+ printf("\nres_b\n");
+ for(ii=0; ii<N; ii++)
+ d_print_e_mat(1, nx[ii+1], (workspace.res_b+ii)->pa, 1);
+ printf("\nres_m_lb\n");
+ for(ii=0; ii<=N; ii++)
+ d_print_e_mat(1, nb[ii], (workspace.res_m_lb+ii)->pa, 1);
+ printf("\nres_m_ub\n");
+ for(ii=0; ii<=N; ii++)
+ d_print_e_mat(1, nb[ii], (workspace.res_m_ub+ii)->pa, 1);
+ printf("\nres_m_lg\n");
+ for(ii=0; ii<=N; ii++)
+ d_print_e_mat(1, ng[ii], (workspace.res_m_lg+ii)->pa, 1);
+ printf("\nres_m_ug\n");
+ for(ii=0; ii<=N; ii++)
+ d_print_e_mat(1, ng[ii], (workspace.res_m_ug+ii)->pa, 1);
+ printf("\nres_d_lb\n");
+ for(ii=0; ii<=N; ii++)
+ d_print_e_mat(1, nb[ii], (workspace.res_d_lb+ii)->pa, 1);
+ printf("\nres_d_ub\n");
+ for(ii=0; ii<=N; ii++)
+ d_print_e_mat(1, nb[ii], (workspace.res_d_ub+ii)->pa, 1);
+ printf("\nres_d_lg\n");
+ for(ii=0; ii<=N; ii++)
+ d_print_e_mat(1, ng[ii], (workspace.res_d_lg+ii)->pa, 1);
+ printf("\nres_d_ug\n");
+ for(ii=0; ii<=N; ii++)
+ d_print_e_mat(1, ng[ii], (workspace.res_d_ug+ii)->pa, 1);
+ printf("\nres_mu\n");
+ printf("\n%e\n\n", workspace.res_mu);
+#endif
+
+ printf("\nipm iter = %d\n", workspace.iter);
+ printf("\nalpha_aff\tmu_aff\t\tsigma\t\talpha\t\tmu\n");
+ d_print_e_tran_mat(5, workspace.iter, workspace.stat, 5);
+
+ printf("\nocp ipm time = %e [s]\n\n", time_ocp_ipm);
+
+/************************************************
+* free memory
+************************************************/
+
+ d_free(A);
+ d_free(B);
+ d_free(b);
+ d_free(x0);
+ d_free(Q);
+ d_free(R);
+ d_free(S);
+ d_free(q);
+ d_free(r);
+ d_free(r0);
+ int_free(idxb0);
+ d_free(d_lb0);
+ d_free(d_ub0);
+ int_free(idxb1);
+ d_free(d_lb1);
+ d_free(d_ub1);
+ int_free(idxbN);
+ d_free(d_lbN);
+ d_free(d_ubN);
+ d_free(C0);
+ d_free(D0);
+ d_free(d_lg0);
+ d_free(d_ug0);
+ d_free(C1);
+ d_free(D1);
+ d_free(d_lg1);
+ d_free(d_ug1);
+ d_free(CN);
+ d_free(DN);
+ d_free(d_lgN);
+ d_free(d_ugN);
+
+ for(ii=0; ii<N; ii++)
+ {
+ d_free(u[ii]);
+ d_free(x[ii]);
+ d_free(pi[ii]);
+ d_free(lam_lb[ii]);
+ d_free(lam_ub[ii]);
+ d_free(lam_lg[ii]);
+ d_free(lam_ug[ii]);
+ }
+ d_free(u[ii]);
+ d_free(x[ii]);
+ d_free(lam_lb[ii]);
+ d_free(lam_ub[ii]);
+ d_free(lam_lg[ii]);
+ d_free(lam_ug[ii]);
+
+ free(d_qp_mem);
+ free(s_qp_mem);
+ free(d_qp_sol_mem);
+ free(m_ipm_mem);
+
+/************************************************
+* return
+************************************************/
+
+ return 0;
+
+ }
diff --git a/test_problems/test_s_dense.c b/test_problems/test_s_dense.c
new file mode 100644
index 0000000..4dbd1fb
--- /dev/null
+++ b/test_problems/test_s_dense.c
@@ -0,0 +1,202 @@
+/**************************************************************************************************
+* *
+* 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 <stdlib.h>
+#include <stdio.h>
+#include <math.h>
+#include <sys/time.h>
+
+#include <blasfeo_target.h>
+#include <blasfeo_common.h>
+#include <blasfeo_v_aux_ext_dep.h>
+#include <blasfeo_s_aux_ext_dep.h>
+#include <blasfeo_i_aux_ext_dep.h>
+#include <blasfeo_s_aux.h>
+#include <blasfeo_s_blas.h>
+
+#include "../include/hpipm_s_dense_qp.h"
+#include "../include/hpipm_s_dense_qp_sol.h"
+#include "../include/hpipm_s_dense_qp_ipm_hard.h"
+
+
+
+#define PRINT 1
+
+
+
+int main()
+ {
+
+ int ii;
+
+/************************************************
+* qp dimension and data
+************************************************/
+
+ int nv = 2;
+ int ne = 1;
+ int nb = 2;
+ int ng = 0;
+
+ float H[] = {4.0, 1.0, 1.0, 2.0};
+ float g[] = {1.0, 1.0};
+ float A[] = {1.0, 1.0};
+ float b[] = {1.0};
+// float d_lb[] = {0.0, 0.0};
+// float d_ub[] = {INFINITY, INFINITY};
+ float d_lb[] = {-1.0, -1.0};
+ float d_ub[] = {1.5, 0.5};
+ int idxb[] = {0, 1};
+ float C[] = {1.0, 0.0, 0.0, 1.0};
+ float d_lg[] = {-1.0, -1.0};
+ float d_ug[] = {1.5, 0.5};
+
+/************************************************
+* dense qp
+************************************************/
+
+ int qp_size = s_memsize_dense_qp(nv, ne, nb, ng);
+ printf("\nqp size = %d\n", qp_size);
+ void *qp_mem = malloc(qp_size);
+
+ struct s_dense_qp qp;
+ s_create_dense_qp(nv, ne, nb, ng, &qp, qp_mem);
+ s_cvt_colmaj_to_dense_qp(H, g, A, b, idxb, d_lb, d_ub, C, d_lg, d_ug, &qp);
+
+ s_print_strmat(nv+1, nv, qp.Hg, 0, 0);
+ s_print_strmat(ne, nv, qp.A, 0, 0);
+ s_print_strmat(nv, ng, qp.Ct, 0, 0);
+ s_print_strvec(nv, qp.g, 0);
+ s_print_strvec(ne, qp.b, 0);
+ s_print_strvec(2*nb+2*ng, qp.d, 0);
+ s_print_strvec(nb, qp.d_lb, 0);
+ s_print_strvec(nb, qp.d_ub, 0);
+ s_print_strvec(ng, qp.d_lg, 0);
+ s_print_strvec(ng, qp.d_ug, 0);
+
+/************************************************
+* dense qp sol
+************************************************/
+
+ int qp_sol_size = s_memsize_dense_qp_sol(nv, ne, nb, ng);
+ printf("\nqp sol size = %d\n", qp_sol_size);
+ void *qp_sol_mem = malloc(qp_sol_size);
+
+ struct s_dense_qp_sol qp_sol;
+ s_create_dense_qp_sol(nv, ne, nb, ng, &qp_sol, qp_sol_mem);
+
+/************************************************
+* ipm
+************************************************/
+
+ struct s_ipm_hard_dense_qp_arg arg;
+ arg.alpha_min = 1e-8;
+ arg.mu_max = 1e-12;
+ arg.iter_max = 20;
+ arg.mu0 = 1.0;
+
+ int ipm_size = s_memsize_ipm_hard_dense_qp(&qp, &arg);
+ printf("\nipm size = %d\n", ipm_size);
+ void *ipm_mem = malloc(ipm_size);
+
+ struct s_ipm_hard_dense_qp_workspace workspace;
+ s_create_ipm_hard_dense_qp(&qp, &arg, &workspace, ipm_mem);
+
+ int rep, nrep=1000;
+
+ struct timeval tv0, tv1;
+
+ gettimeofday(&tv0, NULL); // start
+
+ for(rep=0; rep<nrep; rep++)
+ {
+// s_solve_ipm_hard_dense_qp(&qp, &qp_sol, &workspace);
+ s_solve_ipm2_hard_dense_qp(&qp, &qp_sol, &workspace);
+ }
+
+ gettimeofday(&tv1, NULL); // stop
+
+ float time_dense_ipm = (tv1.tv_sec-tv0.tv_sec)/(nrep+0.0)+(tv1.tv_usec-tv0.tv_usec)/(nrep*1e6);
+
+ printf("\nsolution\n\n");
+ printf("\nv\n");
+ s_print_tran_strvec(nv, qp_sol.v, 0);
+ printf("\npi\n");
+ s_print_tran_strvec(ne, qp_sol.pi, 0);
+ printf("\nlam_lb\n");
+ s_print_tran_strvec(nb, qp_sol.lam_lb, 0);
+ printf("\nlam_ub\n");
+ s_print_tran_strvec(nb, qp_sol.lam_ub, 0);
+ printf("\nlam_lg\n");
+ s_print_tran_strvec(ng, qp_sol.lam_lg, 0);
+ printf("\nlam_ug\n");
+ s_print_tran_strvec(ng, qp_sol.lam_ug, 0);
+ printf("\nt_lb\n");
+ s_print_tran_strvec(nb, qp_sol.t_lb, 0);
+ printf("\nt_ub\n");
+ s_print_tran_strvec(nb, qp_sol.t_ub, 0);
+ printf("\nt_lg\n");
+ s_print_tran_strvec(ng, qp_sol.t_lg, 0);
+ printf("\nt_ug\n");
+ s_print_tran_strvec(ng, qp_sol.t_ug, 0);
+
+ printf("\nresiduals\n\n");
+ printf("\nres_g\n");
+ s_print_e_tran_strvec(nv, workspace.res_g, 0);
+ printf("\nres_b\n");
+ s_print_e_tran_strvec(ne, workspace.res_b, 0);
+ printf("\nres_d\n");
+ s_print_e_tran_strvec(2*nb+2*ng, workspace.res_d, 0);
+ printf("\nres_m\n");
+ s_print_e_tran_strvec(2*nb+2*ng, workspace.res_m, 0);
+ printf("\nres_mu\n");
+ printf("\n%e\n\n", workspace.res_mu);
+
+ printf("\nipm iter = %d\n", workspace.iter);
+ printf("\nalpha_aff\tmu_aff\t\tsigma\t\talpha\t\tmu\n");
+ s_print_e_tran_mat(5, workspace.iter, workspace.stat, 5);
+
+ printf("\ndense ipm time = %e [s]\n\n", time_dense_ipm);
+
+/************************************************
+* free memory
+************************************************/
+
+ free(qp_mem);
+ free(qp_sol_mem);
+ free(ipm_mem);
+
+/************************************************
+* return
+************************************************/
+
+ return 0;
+
+ }
+
+
+
diff --git a/test_problems/test_s_ocp.c b/test_problems/test_s_ocp.c
new file mode 100644
index 0000000..f9d5209
--- /dev/null
+++ b/test_problems/test_s_ocp.c
@@ -0,0 +1,696 @@
+/**************************************************************************************************
+* *
+* 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 <stdlib.h>
+#include <stdio.h>
+#include <math.h>
+#include <sys/time.h>
+
+#include <blasfeo_target.h>
+#include <blasfeo_common.h>
+#include <blasfeo_v_aux_ext_dep.h>
+#include <blasfeo_s_aux_ext_dep.h>
+#include <blasfeo_i_aux_ext_dep.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 "s_tools.h"
+
+
+
+#define KEEP_X0 0
+
+// printing
+#define PRINT 1
+
+/************************************************
+Mass-spring system: nx/2 masses connected each other with springs (in a row), and the first and the last one to walls. nu (<=nx) controls act on the first nu masses. The system is sampled with sampling time Ts.
+************************************************/
+void mass_spring_system(float Ts, int nx, int nu, int N, float *A, float *B, float *b, float *x0)
+ {
+
+ int nx2 = nx*nx;
+
+ int info = 0;
+
+ int pp = nx/2; // number of masses
+
+/************************************************
+* build the continuous time system
+************************************************/
+
+ float *T; s_zeros(&T, pp, pp);
+ int ii;
+ for(ii=0; ii<pp; ii++) T[ii*(pp+1)] = -2;
+ for(ii=0; ii<pp-1; ii++) T[ii*(pp+1)+1] = 1;
+ for(ii=1; ii<pp; ii++) T[ii*(pp+1)-1] = 1;
+
+ float *Z; s_zeros(&Z, pp, pp);
+ float *I; s_zeros(&I, pp, pp); for(ii=0; ii<pp; ii++) I[ii*(pp+1)]=1.0; // = eye(pp);
+ float *Ac; s_zeros(&Ac, nx, nx);
+ smcopy(pp, pp, Z, pp, Ac, nx);
+ smcopy(pp, pp, T, pp, Ac+pp, nx);
+ smcopy(pp, pp, I, pp, Ac+pp*nx, nx);
+ smcopy(pp, pp, Z, pp, Ac+pp*(nx+1), nx);
+ free(T);
+ free(Z);
+ free(I);
+
+ s_zeros(&I, nu, nu); for(ii=0; ii<nu; ii++) I[ii*(nu+1)]=1.0; //I = eye(nu);
+ float *Bc; s_zeros(&Bc, nx, nu);
+ smcopy(nu, nu, I, nu, Bc+pp, nx);
+ free(I);
+
+/************************************************
+* compute the discrete time system
+************************************************/
+
+ float *bb; s_zeros(&bb, nx, 1);
+ smcopy(nx, 1, bb, nx, b, nx);
+
+ smcopy(nx, nx, Ac, nx, A, nx);
+ sscal_3l(nx2, Ts, A);
+ expm(nx, A);
+
+ s_zeros(&T, nx, nx);
+ s_zeros(&I, nx, nx); for(ii=0; ii<nx; ii++) I[ii*(nx+1)]=1.0; //I = eye(nx);
+ smcopy(nx, nx, A, nx, T, nx);
+ saxpy_3l(nx2, -1.0, I, T);
+ sgemm_nn_3l(nx, nu, nx, T, nx, Bc, nx, B, nx);
+ free(T);
+ free(I);
+
+ int *ipiv = (int *) malloc(nx*sizeof(int));
+ sgesv_3l(nx, nu, Ac, nx, ipiv, B, nx, &info);
+ free(ipiv);
+
+ free(Ac);
+ free(Bc);
+ free(bb);
+
+
+/************************************************
+* initial state
+************************************************/
+
+ if(nx==4)
+ {
+ x0[0] = 5;
+ x0[1] = 10;
+ x0[2] = 15;
+ x0[3] = 20;
+ }
+ else
+ {
+ int jj;
+ for(jj=0; jj<nx; jj++)
+ x0[jj] = 1;
+ }
+
+ }
+
+
+
+int main()
+ {
+
+
+ // local variables
+
+ int ii, jj;
+
+ int rep, nrep=1000;
+
+ struct timeval tv0, tv1;
+
+
+
+ // problem size
+
+ int nx_ = 16; // number of states (it has to be even for the mass-spring system test problem)
+ int nu_ = 7; // number of inputs (controllers) (it has to be at least 1 and at most nx/2 for the mass-spring system test problem)
+ int N = 5; // horizon lenght
+
+
+
+ // stage-wise variant size
+
+ int nx[N+1];
+#if KEEP_X0
+ nx[0] = nx_;
+#else
+ nx[0] = 0;
+#endif
+ for(ii=1; ii<=N; ii++)
+ nx[ii] = nx_;
+// nx[N] = 0;
+
+ int nu[N+1];
+ for(ii=0; ii<N; ii++)
+ nu[ii] = nu_;
+ nu[N] = 0;
+
+#if 1
+ int nb[N+1];
+#if KEEP_X0
+ nb[0] = nu[0]+nx[0]/2;
+#else
+ nb[0] = nu[0];
+#endif
+ for(ii=1; ii<N; ii++)
+ nb[ii] = nu[1]+nx[1]/2;
+ nb[N] = nx[N]/2;
+
+ int ng[N+1];
+ ng[0] = 0;
+ for(ii=1; ii<N; ii++)
+ ng[ii] = 0;
+ ng[N] = 0;
+#elif 0
+ int nb[N+1];
+ nb[0] = 0;
+ for(ii=1; ii<N; ii++)
+ nb[ii] = 0;
+ nb[N] = 0;
+
+ int ng[N+1];
+#if KEEP_X0
+ ng[0] = nu[0]+nx[0]/2;
+#else
+ ng[0] = nu[0];
+#endif
+ for(ii=1; ii<N; ii++)
+ ng[ii] = nu[1]+nx[1]/2;
+ ng[N] = nx[N]/2;
+#else
+ int nb[N+1];
+ nb[0] = nu[0] + nx[0]/2;
+ for(ii=1; ii<N; ii++)
+ nb[ii] = nu[ii] + nx[ii]/2;
+ nb[N] = nu[N] + nx[N]/2;
+
+ int ng[N+1];
+#if KEEP_X0
+ ng[0] = nx[0]/2;
+#else
+ ng[0] = 0;
+#endif
+ for(ii=1; ii<N; ii++)
+ ng[ii] = nx[1]/2;
+ ng[N] = nx[N]/2;
+#endif
+
+/************************************************
+* dynamical system
+************************************************/
+
+ float *A; s_zeros(&A, nx_, nx_); // states update matrix
+
+ float *B; s_zeros(&B, nx_, nu_); // inputs matrix
+
+ float *b; s_zeros(&b, nx_, 1); // states offset
+ float *x0; s_zeros(&x0, nx_, 1); // initial state
+
+ float Ts = 0.5; // sampling time
+ mass_spring_system(Ts, nx_, nu_, N, A, B, b, x0);
+
+ for(jj=0; jj<nx_; jj++)
+ b[jj] = 0.1;
+
+ for(jj=0; jj<nx_; jj++)
+ x0[jj] = 0;
+ x0[0] = 2.5;
+ x0[1] = 2.5;
+
+ float *b0; s_zeros(&b0, nx_, 1);
+ sgemv_n_3l(nx_, nx_, A, nx_, x0, b0);
+ saxpy_3l(nx_, 1.0, b, b0);
+
+#if PRINT
+ s_print_mat(nx_, nx_, A, nx_);
+ s_print_mat(nx_, nu_, B, nu_);
+ s_print_mat(1, nx_, b, 1);
+ s_print_mat(1, nx_, x0, 1);
+ s_print_mat(1, nx_, b0, 1);
+#endif
+
+/************************************************
+* cost function
+************************************************/
+
+ float *Q; s_zeros(&Q, nx_, nx_);
+ for(ii=0; ii<nx_; ii++) Q[ii*(nx_+1)] = 1.0;
+
+ float *R; s_zeros(&R, nu_, nu_);
+ for(ii=0; ii<nu_; ii++) R[ii*(nu_+1)] = 2.0;
+
+ float *S; s_zeros(&S, nu_, nx_);
+
+ float *q; s_zeros(&q, nx_, 1);
+ for(ii=0; ii<nx_; ii++) q[ii] = 0.1;
+
+ float *r; s_zeros(&r, nu_, 1);
+ for(ii=0; ii<nu_; ii++) r[ii] = 0.2;
+
+ float *r0; s_zeros(&r0, nu_, 1);
+ sgemv_n_3l(nu_, nx_, S, nu_, x0, r0);
+ saxpy_3l(nu_, 1.0, r, r0);
+
+#if PRINT
+ s_print_mat(nx_, nx_, Q, nx_);
+ s_print_mat(nu_, nu_, R, nu_);
+ s_print_mat(nu_, nx_, S, nu_);
+ s_print_mat(1, nx_, q, 1);
+ s_print_mat(1, nu_, r, 1);
+ s_print_mat(1, nu_, r0, 1);
+#endif
+
+ // maximum element in cost functions
+ float mu0 = 2.0;
+
+/************************************************
+* box & general constraints
+************************************************/
+
+ int *idxb0; int_zeros(&idxb0, nb[0], 1);
+ float *d_lb0; s_zeros(&d_lb0, nb[0], 1);
+ float *d_ub0; s_zeros(&d_ub0, nb[0], 1);
+ float *d_lg0; s_zeros(&d_lg0, ng[0], 1);
+ float *d_ug0; s_zeros(&d_ug0, ng[0], 1);
+ for(ii=0; ii<nb[0]; ii++)
+ {
+ if(ii<nu[0]) // input
+ {
+ d_lb0[ii] = - 0.5; // umin
+ d_ub0[ii] = 0.5; // umax
+ }
+ else // state
+ {
+ d_lb0[ii] = - 4.0; // xmin
+ d_ub0[ii] = 4.0; // xmax
+ }
+ idxb0[ii] = ii;
+ }
+ for(ii=0; ii<ng[0]; ii++)
+ {
+ if(ii<nu[0]-nb[0]) // input
+ {
+ d_lg0[ii] = - 0.5; // umin
+ d_ug0[ii] = 0.5; // umax
+ }
+ else // state
+ {
+ d_lg0[ii] = - 4.0; // xmin
+ d_ug0[ii] = 4.0; // xmax
+ }
+ }
+
+ int *idxb1; int_zeros(&idxb1, nb[1], 1);
+ float *d_lb1; s_zeros(&d_lb1, nb[1], 1);
+ float *d_ub1; s_zeros(&d_ub1, nb[1], 1);
+ float *d_lg1; s_zeros(&d_lg1, ng[1], 1);
+ float *d_ug1; s_zeros(&d_ug1, ng[1], 1);
+ for(ii=0; ii<nb[1]; ii++)
+ {
+ if(ii<nu[1]) // input
+ {
+ d_lb1[ii] = - 0.5; // umin
+ d_ub1[ii] = 0.5; // umax
+ }
+ else // state
+ {
+ d_lb1[ii] = - 4.0; // xmin
+ d_ub1[ii] = 4.0; // xmax
+ }
+ idxb1[ii] = ii;
+ }
+ for(ii=0; ii<ng[1]; ii++)
+ {
+ if(ii<nu[1]-nb[1]) // input
+ {
+ d_lg1[ii] = - 0.5; // umin
+ d_ug1[ii] = 0.5; // umax
+ }
+ else // state
+ {
+ d_lg1[ii] = - 4.0; // xmin
+ d_ug1[ii] = 4.0; // xmax
+ }
+ }
+
+
+ int *idxbN; int_zeros(&idxbN, nb[N], 1);
+ float *d_lbN; s_zeros(&d_lbN, nb[N], 1);
+ float *d_ubN; s_zeros(&d_ubN, nb[N], 1);
+ float *d_lgN; s_zeros(&d_lgN, ng[N], 1);
+ float *d_ugN; s_zeros(&d_ugN, ng[N], 1);
+ for(ii=0; ii<nb[N]; ii++)
+ {
+ d_lbN[ii] = - 4.0; // xmin
+ d_ubN[ii] = 4.0; // xmax
+ idxbN[ii] = ii;
+ }
+ for(ii=0; ii<ng[N]; ii++)
+ {
+ d_lgN[ii] = - 4.0; // dmin
+ d_ugN[ii] = 4.0; // dmax
+ }
+
+ float *C0; s_zeros(&C0, ng[0], nx[0]);
+ float *D0; s_zeros(&D0, ng[0], nu[0]);
+ for(ii=0; ii<nu[0]-nb[0] & ii<ng[0]; ii++)
+ D0[ii+(nb[0]+ii)*ng[0]] = 1.0;
+ for(; ii<ng[0]; ii++)
+ C0[ii+(nb[0]+ii-nu[0])*ng[0]] = 1.0;
+
+ float *C1; s_zeros(&C1, ng[1], nx[1]);
+ float *D1; s_zeros(&D1, ng[1], nu[1]);
+ for(ii=0; ii<nu[1]-nb[1] & ii<ng[1]; ii++)
+ D1[ii+(nb[1]+ii)*ng[1]] = 1.0;
+ for(; ii<ng[1]; ii++)
+ C1[ii+(nb[1]+ii-nu[1])*ng[1]] = 1.0;
+
+ float *CN; s_zeros(&CN, ng[N], nx[N]);
+ float *DN; s_zeros(&DN, ng[N], nu[N]);
+ for(ii=0; ii<nu[N]-nb[N] & ii<ng[N]; ii++)
+ DN[ii+(nb[N]+ii)*ng[N]] = 1.0;
+ for(; ii<ng[N]; ii++)
+ CN[ii+(nb[N]+ii-nu[N])*ng[N]] = 1.0;
+
+#if PRINT
+ // box constraints
+ int_print_mat(1, nb[0], idxb0, 1);
+ s_print_mat(1, nb[0], d_lb0, 1);
+ s_print_mat(1, nb[0], d_ub0, 1);
+ int_print_mat(1, nb[1], idxb1, 1);
+ s_print_mat(1, nb[1], d_lb1, 1);
+ s_print_mat(1, nb[1], d_ub1, 1);
+ int_print_mat(1, nb[N], idxbN, 1);
+ s_print_mat(1, nb[N], d_lbN, 1);
+ s_print_mat(1, nb[N], d_ubN, 1);
+ // general constraints
+ s_print_mat(1, ng[0], d_lg0, 1);
+ s_print_mat(1, ng[0], d_ug0, 1);
+ s_print_mat(ng[0], nu[0], D0, ng[0]);
+ s_print_mat(ng[0], nx[0], C0, ng[0]);
+ s_print_mat(1, ng[1], d_lg1, 1);
+ s_print_mat(1, ng[1], d_ug1, 1);
+ s_print_mat(ng[1], nu[1], D1, ng[1]);
+ s_print_mat(ng[1], nx[1], C1, ng[1]);
+ s_print_mat(1, ng[N], d_lgN, 1);
+ s_print_mat(1, ng[N], d_ugN, 1);
+ s_print_mat(ng[N], nu[N], DN, ng[N]);
+ s_print_mat(ng[N], nx[N], CN, ng[N]);
+#endif
+
+/************************************************
+* array of matrices
+************************************************/
+
+ float *hA[N];
+ float *hB[N];
+ float *hb[N];
+ float *hQ[N+1];
+ float *hS[N+1];
+ float *hR[N+1];
+ float *hq[N+1];
+ float *hr[N+1];
+ float *hd_lb[N+1];
+ float *hd_ub[N+1];
+ float *hd_lg[N+1];
+ float *hd_ug[N+1];
+ float *hC[N+1];
+ float *hD[N+1];
+ int *hidxb[N+1];
+
+ hA[0] = A;
+ hB[0] = B;
+ hb[0] = b0;
+ hQ[0] = Q;
+ hS[0] = S;
+ hR[0] = R;
+ hq[0] = q;
+ hr[0] = r0;
+ hidxb[0] = idxb0;
+ hd_lb[0] = d_lb0;
+ hd_ub[0] = d_ub0;
+ hd_lg[0] = d_lg0;
+ hd_ug[0] = d_ug0;
+ hC[0] = C0;
+ hD[0] = D0;
+ for(ii=1; ii<N; ii++)
+ {
+ hA[ii] = A;
+ hB[ii] = B;
+ hb[ii] = b;
+ hQ[ii] = Q;
+ hS[ii] = S;
+ hR[ii] = R;
+ hq[ii] = q;
+ hr[ii] = r;
+ hidxb[ii] = idxb1;
+ hd_lb[ii] = d_lb1;
+ hd_ub[ii] = d_ub1;
+ hd_lg[ii] = d_lg1;
+ hd_ug[ii] = d_ug1;
+ hC[ii] = C1;
+ hD[ii] = D1;
+ }
+ hQ[N] = Q;
+ hS[N] = S;
+ hR[N] = R;
+ hq[N] = q;
+ hr[N] = r;
+ hidxb[N] = idxbN;
+ hd_lb[N] = d_lbN;
+ hd_ub[N] = d_ubN;
+ hd_lg[N] = d_lgN;
+ hd_ug[N] = d_ugN;
+ hC[N] = CN;
+ hD[N] = DN;
+
+/************************************************
+* ocp qp
+************************************************/
+
+ int qp_size = s_memsize_ocp_qp(N, nx, nu, nb, ng);
+ printf("\nqp size = %d\n", qp_size);
+ void *qp_mem = malloc(qp_size);
+
+ struct s_ocp_qp qp;
+ s_create_ocp_qp(N, nx, nu, nb, ng, &qp, qp_mem);
+ s_cvt_colmaj_to_ocp_qp(hA, hB, hb, hQ, hS, hR, hq, hr, hidxb, hd_lb, hd_ub, hC, hD, hd_lg, hd_ug, &qp);
+#if 0
+ printf("\nN = %d\n", qp.N);
+ for(ii=0; ii<N; ii++)
+ s_print_strmat(qp.nu[ii]+qp.nx[ii]+1, qp.nx[ii+1], qp.BAbt+ii, 0, 0);
+ for(ii=0; ii<N; ii++)
+ s_print_tran_strvec(qp.nx[ii+1], qp.b+ii, 0);
+ for(ii=0; ii<=N; ii++)
+ s_print_strmat(qp.nu[ii]+qp.nx[ii]+1, qp.nu[ii]+qp.nx[ii], qp.RSQrq+ii, 0, 0);
+ for(ii=0; ii<=N; ii++)
+ s_print_tran_strvec(qp.nu[ii]+qp.nx[ii], qp.rq+ii, 0);
+ for(ii=0; ii<=N; ii++)
+ int_print_mat(1, nb[ii], qp.idxb[ii], 1);
+ for(ii=0; ii<=N; ii++)
+ s_print_tran_strvec(qp.nb[ii], qp.d_lb+ii, 0);
+ for(ii=0; ii<=N; ii++)
+ s_print_tran_strvec(qp.nb[ii], qp.d_ub+ii, 0);
+ for(ii=0; ii<=N; ii++)
+ s_print_strmat(qp.nu[ii]+qp.nx[ii], qp.ng[ii], qp.DCt+ii, 0, 0);
+ for(ii=0; ii<=N; ii++)
+ s_print_tran_strvec(qp.ng[ii], qp.d_lg+ii, 0);
+ for(ii=0; ii<=N; ii++)
+ s_print_tran_strvec(qp.ng[ii], qp.d_ug+ii, 0);
+ return;
+#endif
+
+/************************************************
+* ocp qp
+************************************************/
+
+ int qp_sol_size = s_memsize_ocp_qp_sol(N, nx, nu, nb, ng);
+ printf("\nqp sol size = %d\n", qp_sol_size);
+ void *qp_sol_mem = malloc(qp_sol_size);
+
+ struct s_ocp_qp_sol qp_sol;
+ s_create_ocp_qp_sol(N, nx, nu, nb, ng, &qp_sol, qp_sol_mem);
+
+/************************************************
+* ipm
+************************************************/
+
+ struct s_ipm_hard_ocp_qp_arg arg;
+ arg.alpha_min = 1e-8;
+ arg.mu_max = 1e-12;
+ arg.iter_max = 20;
+ arg.mu0 = 2.0;
+
+ int ipm_size = s_memsize_ipm_hard_ocp_qp(&qp, &arg);
+ printf("\nipm size = %d\n", ipm_size);
+ void *ipm_mem = malloc(ipm_size);
+
+ struct s_ipm_hard_ocp_qp_workspace workspace;
+ s_create_ipm_hard_ocp_qp(&qp, &arg, &workspace, ipm_mem);
+
+ gettimeofday(&tv0, NULL); // start
+
+ for(rep=0; rep<nrep; rep++)
+ {
+// s_solve_ipm_hard_ocp_qp(&qp, &qp_sol, &workspace);
+ s_solve_ipm2_hard_ocp_qp(&qp, &qp_sol, &workspace);
+ }
+
+ gettimeofday(&tv1, NULL); // stop
+
+ float time_ocp_ipm = (tv1.tv_sec-tv0.tv_sec)/(nrep+0.0)+(tv1.tv_usec-tv0.tv_usec)/(nrep*1e6);
+
+#if 1
+ printf("\nsolution\n\n");
+ printf("\nux\n");
+ for(ii=0; ii<=N; ii++)
+ s_print_tran_strvec(nu[ii]+nx[ii], qp_sol.ux+ii, 0);
+ printf("\npi\n");
+ for(ii=0; ii<N; ii++)
+ s_print_tran_strvec(nx[ii+1], qp_sol.pi+ii, 0);
+ printf("\nlam_lb\n");
+ for(ii=0; ii<=N; ii++)
+ s_print_tran_strvec(nb[ii], qp_sol.lam_lb+ii, 0);
+ printf("\nlam_ub\n");
+ for(ii=0; ii<=N; ii++)
+ s_print_tran_strvec(nb[ii], qp_sol.lam_ub+ii, 0);
+ printf("\nlam_lg\n");
+ for(ii=0; ii<=N; ii++)
+ s_print_tran_strvec(ng[ii], qp_sol.lam_lg+ii, 0);
+ printf("\nlam_ug\n");
+ for(ii=0; ii<=N; ii++)
+ s_print_tran_strvec(ng[ii], qp_sol.lam_ug+ii, 0);
+ printf("\nt_lb\n");
+ for(ii=0; ii<=N; ii++)
+ s_print_tran_strvec(nb[ii], qp_sol.t_lb+ii, 0);
+ printf("\nt_ub\n");
+ for(ii=0; ii<=N; ii++)
+ s_print_tran_strvec(nb[ii], qp_sol.t_ub+ii, 0);
+ printf("\nt_lg\n");
+ for(ii=0; ii<=N; ii++)
+ s_print_tran_strvec(ng[ii], qp_sol.t_lg+ii, 0);
+ printf("\nt_ug\n");
+ for(ii=0; ii<=N; ii++)
+ s_print_tran_strvec(ng[ii], qp_sol.t_ug+ii, 0);
+
+ printf("\nresiduals\n\n");
+ printf("\nres_g\n");
+ for(ii=0; ii<=N; ii++)
+ s_print_e_tran_strvec(nu[ii]+nx[ii], workspace.res_g+ii, 0);
+ printf("\nres_b\n");
+ for(ii=0; ii<N; ii++)
+ s_print_e_tran_strvec(nx[ii+1], workspace.res_b+ii, 0);
+ printf("\nres_m_lb\n");
+ for(ii=0; ii<=N; ii++)
+ s_print_e_tran_strvec(nb[ii], workspace.res_m_lb+ii, 0);
+ printf("\nres_m_ub\n");
+ for(ii=0; ii<=N; ii++)
+ s_print_e_tran_strvec(nb[ii], workspace.res_m_ub+ii, 0);
+ printf("\nres_m_lg\n");
+ for(ii=0; ii<=N; ii++)
+ s_print_e_tran_strvec(ng[ii], workspace.res_m_lg+ii, 0);
+ printf("\nres_m_ug\n");
+ for(ii=0; ii<=N; ii++)
+ s_print_e_tran_strvec(ng[ii], workspace.res_m_ug+ii, 0);
+ printf("\nres_d_lb\n");
+ for(ii=0; ii<=N; ii++)
+ s_print_e_tran_strvec(nb[ii], workspace.res_d_lb+ii, 0);
+ printf("\nres_d_ub\n");
+ for(ii=0; ii<=N; ii++)
+ s_print_e_tran_strvec(nb[ii], workspace.res_d_ub+ii, 0);
+ printf("\nres_d_lg\n");
+ for(ii=0; ii<=N; ii++)
+ s_print_e_tran_strvec(ng[ii], workspace.res_d_lg+ii, 0);
+ printf("\nres_d_ug\n");
+ for(ii=0; ii<=N; ii++)
+ s_print_e_tran_strvec(ng[ii], workspace.res_d_ug+ii, 0);
+ printf("\nres_mu\n");
+ printf("\n%e\n\n", workspace.res_mu);
+#endif
+
+ printf("\nipm iter = %d\n", workspace.iter);
+ printf("\nalpha_aff\tmu_aff\t\tsigma\t\talpha\t\tmu\n");
+ s_print_e_tran_mat(5, workspace.iter, workspace.stat, 5);
+
+ printf("\nocp ipm time = %e [s]\n\n", time_ocp_ipm);
+
+/************************************************
+* free memory
+************************************************/
+
+ s_free(A);
+ s_free(B);
+ s_free(b);
+ s_free(x0);
+ s_free(Q);
+ s_free(R);
+ s_free(S);
+ s_free(q);
+ s_free(r);
+ s_free(r0);
+ int_free(idxb0);
+ s_free(d_lb0);
+ s_free(d_ub0);
+ int_free(idxb1);
+ s_free(d_lb1);
+ s_free(d_ub1);
+ int_free(idxbN);
+ s_free(d_lbN);
+ s_free(d_ubN);
+ s_free(C0);
+ s_free(D0);
+ s_free(d_lg0);
+ s_free(d_ug0);
+ s_free(C1);
+ s_free(D1);
+ s_free(d_lg1);
+ s_free(d_ug1);
+ s_free(CN);
+ s_free(DN);
+ s_free(d_lgN);
+ s_free(d_ugN);
+
+ free(qp_mem);
+ free(qp_sol_mem);
+ free(ipm_mem);
+
+/************************************************
+* return
+************************************************/
+
+ return 0;
+
+ }