初始提交

.appveyor.yml

@@ -0,0 +1,39 @@
+branches:
+  only:
+    - main
+
+cache:
+  - c:\dev\Eigen3\
+
+environment:
+  matrix:
+    - BUILD_TYPE: Release
+      COMPILER: MinGW-w64
+      PLATFORM: x64
+
+install:
+  - cmd: if not exist c:\dev\Eigen3\include\eigen3\Eigen\Core (
+            curl -L -o eigen-3.2.10.tar.gz https://gitlab.com/libeigen/eigen/-/archive/3.2.10/eigen-3.2.10.tar.gz &&
+            cmake -E tar zxf eigen-3.2.10.tar.gz &&
+            cd eigen-3.2.10 &&
+            mkdir build &&
+            cd build &&
+            cmake -G "Visual Studio 12" -DCMAKE_BUILD_TYPE=Debug -DCMAKE_INSTALL_PREFIX="c:\dev\Eigen3" .. &&
+            cmake --build . --target install --config Release &&
+            cd ..\..
+         ) else (echo Using cached Eigen3)
+
+# NOTE: only static linking, i.e. via .lib file supported
+#  -D BUILD_SHARED_LIBS=OFF;
+# extern "C" {  would have to be added to subprojects
+
+build_script:
+  - git submodule update --init --recursive
+  # MinGW files need to be at the beginning of the PATH
+  - set PATH=C:\mingw-w64\x86_64-7.3.0-posix-seh-rt_v5-rev0\mingw64\bin;%PATH%
+  # Workaround for CMake not wanting sh.exe on PATH for MinGW
+  - rm "C:\Program Files\Git\usr\bin\sh.exe"
+  - cmake -E make_directory build
+  - cmake -E chdir build cmake -DCMAKE_PREFIX_PATH=C:\mingw-w64\x86_64-7.3.0-posix-seh-rt_v5-rev0 -D ACADOS_WITH_QPOASES=ON -D BLASFEO_TARGET=GENERIC -D HPIPM_TARGET=GENERIC -D BUILD_SHARED_LIBS=OFF -D ACADOS_EXAMPLES=ON -D ACADOS_UNIT_TESTS=ON -DEIGEN3_INCLUDE_DIR="C:\dev\Eigen3\include\eigen3" -G "MinGW Makefiles" ..
+  - cmake --build build --config Release
+  - cmake -E chdir build ctest --output-on-failure

.clang-format

@@ -0,0 +1,65 @@
+---
+Language:        Cpp
+BasedOnStyle:  Google
+AccessModifierOffset: -3
+AlignAfterOpenBracket: true
+AlignEscapedNewlinesLeft: true
+AlignOperands:   true
+AlignTrailingComments: true
+AllowAllParametersOfDeclarationOnNextLine: true
+AllowShortBlocksOnASingleLine: false
+AllowShortCaseLabelsOnASingleLine: false
+AllowShortIfStatementsOnASingleLine: true
+AllowShortLoopsOnASingleLine: true
+AllowShortFunctionsOnASingleLine: All
+AlwaysBreakAfterDefinitionReturnType: false
+AlwaysBreakTemplateDeclarations: true
+AlwaysBreakBeforeMultilineStrings: true
+BreakBeforeBinaryOperators: None
+BreakBeforeTernaryOperators: false
+BreakConstructorInitializersBeforeComma: false
+BinPackParameters: true
+BinPackArguments: true
+ColumnLimit:     100
+ConstructorInitializerAllOnOneLineOrOnePerLine: true
+ConstructorInitializerIndentWidth: 4
+DerivePointerAlignment: true
+ExperimentalAutoDetectBinPacking: false
+IndentCaseLabels: true
+IndentWrappedFunctionNames: false
+IndentFunctionDeclarationAfterType: false
+MaxEmptyLinesToKeep: 3
+KeepEmptyLinesAtTheStartOfBlocks: false
+NamespaceIndentation: None
+ObjCBlockIndentWidth: 2
+ObjCSpaceAfterProperty: false
+ObjCSpaceBeforeProtocolList: false
+PenaltyBreakBeforeFirstCallParameter: 1
+PenaltyBreakComment: 300
+PenaltyBreakString: 1000
+PenaltyBreakFirstLessLess: 120
+PenaltyExcessCharacter: 1000000
+PenaltyReturnTypeOnItsOwnLine: 200
+PointerAlignment: Left
+SpacesBeforeTrailingComments: 2
+Cpp11BracedListStyle: true
+Standard:        Auto
+IndentWidth:     4
+TabWidth:        4
+UseTab:          Never
+BreakBeforeBraces: Allman
+SpacesInParentheses: false
+SpacesInSquareBrackets: false
+SpacesInAngles:  false
+SpaceInEmptyParentheses: false
+SpacesInCStyleCastParentheses: false
+SpaceAfterCStyleCast: true
+SpacesInContainerLiterals: true
+SpaceBeforeAssignmentOperators: true
+ContinuationIndentWidth: 4
+CommentPragmas:  '^ IWYU pragma:'
+ForEachMacros:   [ foreach, Q_FOREACH, BOOST_FOREACH ]
+SpaceBeforeParens: ControlStatements
+DisableFormat:   false
+...
+

.gitattributes

@@ -0,0 +1 @@
+qpOASES/doc/html/*.html linguist-documentation

.github/linux/export_paths.sh

@@ -0,0 +1,38 @@
+#!/bin/bash
+#
+# Copyright (c) The acados authors.
+#
+# This file is part of acados.
+#
+# The 2-Clause BSD License
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions are met:
+#
+# 1. Redistributions of source code must retain the above copyright notice,
+# this list of conditions and the following disclaimer.
+#
+# 2. Redistributions in binary form must reproduce the above copyright notice,
+# this list of conditions and the following disclaimer in the documentation
+# and/or other materials provided with the distribution.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.;
+#
+
+echo "ACADOS_SOURCE_DIR=$1/acados" >> $GITHUB_ENV
+echo "ACADOS_INSTALL_DIR=$1/acados" >> $GITHUB_ENV
+echo "LD_LIBRARY_PATH=$1/acados/lib" >> $GITHUB_ENV
+echo "MATLABPATH=$MATLABPATH:$1/acados/interfaces/acados_matlab_octave:$1/acados/interfaces/acados_matlab_octave/acados_template_mex:${1}/acados/external/casadi-matlab" >> $GITHUB_ENV
+echo "OCTAVE_PATH=$OCTAVE_PATH:${1}/acados/interfaces/acados_matlab_octave:${1}/acados/interfaces/acados_matlab_octave/acados_template_mex:${1}/acados/external/casadi-octave" >> $GITHUB_ENV
+echo "LD_RUN_PATH=${1}/acados/examples/acados_matlab_octave/test/c_generated_code:${1}/acados/examples/acados_matlab_octave/getting_started/c_generated_code:${1}/acados/examples/acados_matlab_octave/mocp_transition_example/c_generated_code:${1}/acados/examples/acados_matlab_octave/simple_dae_model/c_generated_code:${1}/acados/examples/acados_matlab_octave/lorentz/c_generated_code" >> $GITHUB_ENV
+echo "ENV_RUN=true" >> $GITHUB_ENV

.github/linux/install_casadi_matlab.sh

@@ -0,0 +1,42 @@
+#!/bin/bash
+#
+# Copyright (c) The acados authors.
+#
+# This file is part of acados.
+#
+# The 2-Clause BSD License
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions are met:
+#
+# 1. Redistributions of source code must retain the above copyright notice,
+# this list of conditions and the following disclaimer.
+#
+# 2. Redistributions in binary form must reproduce the above copyright notice,
+# this list of conditions and the following disclaimer in the documentation
+# and/or other materials provided with the distribution.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.;
+#
+
+# CASADI_VERSION='3.5.3'; # Latest version with Octave 4.2.2 binaries
+CASADI_VERSION='3.6.6';
+MATLAB_VERSION='2018b';
+
+_CASADI_GITHUB_RELEASES="https://github.com/casadi/casadi/releases/download/${CASADI_VERSION}";
+
+CASADI_MATLAB_URL="${_CASADI_GITHUB_RELEASES}/casadi-${CASADI_VERSION}-linux64-matlab${MATLAB_VERSION}.zip";
+
+wget -O casadi-linux-matlab.zip "${CASADI_MATLAB_URL}";
+mkdir -p casadi-matlab;
+unzip casadi-linux-matlab.zip -d ./casadi-matlab;

.github/linux/install_casadi_octave.sh

@@ -0,0 +1,45 @@
+#!/bin/bash
+#
+# Copyright (c) The acados authors.
+#
+# This file is part of acados.
+#
+# The 2-Clause BSD License
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions are met:
+#
+# 1. Redistributions of source code must retain the above copyright notice,
+# this list of conditions and the following disclaimer.
+#
+# 2. Redistributions in binary form must reproduce the above copyright notice,
+# this list of conditions and the following disclaimer in the documentation
+# and/or other materials provided with the distribution.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.;
+#
+
+# CASADI_VERSION='3.5.3'; # Latest version with Octave 4.2.2 binaries
+CASADI_VERSION='3.6.7';
+OCTAVE_VERSION='7.3.0';
+
+_CASADI_GITHUB_RELEASES="https://github.com/casadi/casadi/releases/download/${CASADI_VERSION}";
+
+CASADI_OCTAVE_URL="${_CASADI_GITHUB_RELEASES}/casadi-${CASADI_VERSION}-linux64-octave${OCTAVE_VERSION}.zip";
+
+# URL for Octave new CasADi
+# CASADI_OCTAVE_URL="https://github.com/casadi/casadi/releases/download/nightly-se/casadi-se-linux64-octave7.3.0.zip"
+
+wget -O casadi-linux-octave.zip "${CASADI_OCTAVE_URL}";
+mkdir -p casadi-octave;
+unzip casadi-linux-octave.zip -d ./casadi-octave;

.github/linux/install_eigen.sh

@@ -0,0 +1,38 @@
+#!/bin/bash
+#
+# Copyright (c) The acados authors.
+#
+# This file is part of acados.
+#
+# The 2-Clause BSD License
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions are met:
+#
+# 1. Redistributions of source code must retain the above copyright notice,
+# this list of conditions and the following disclaimer.
+#
+# 2. Redistributions in binary form must reproduce the above copyright notice,
+# this list of conditions and the following disclaimer in the documentation
+# and/or other materials provided with the distribution.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.;
+#
+
+EIGEN_URL="https://gitlab.com/libeigen/eigen/-/archive/3.2.10/eigen-3.2.10.tar.gz";
+
+pushd external;
+	wget -O eigen.tar.gz "${EIGEN_URL}";
+	mkdir -p eigen;
+	tar -xf eigen.tar.gz --strip-components=1 -C eigen;
+popd;

.github/linux/install_new_casadi_matlab.sh

@@ -0,0 +1,37 @@
+#!/bin/bash
+#
+# Copyright (c) The acados authors.
+#
+# This file is part of acados.
+#
+# The 2-Clause BSD License
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions are met:
+#
+# 1. Redistributions of source code must retain the above copyright notice,
+# this list of conditions and the following disclaimer.
+#
+# 2. Redistributions in binary form must reproduce the above copyright notice,
+# this list of conditions and the following disclaimer in the documentation
+# and/or other materials provided with the distribution.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.;
+#
+
+
+CASADI_MATLAB_URL="https://github.com/casadi/casadi/releases/download/nightly-main/casadi-main-linux64-matlab2018b.zip";
+
+wget -O casadi-linux-matlab.zip "${CASADI_MATLAB_URL}";
+mkdir -p casadi-matlab;
+unzip casadi-linux-matlab.zip -d ./casadi-matlab;

.github/linux/install_new_casadi_octave.sh

@@ -0,0 +1,37 @@
+#!/bin/bash
+#
+# Copyright (c) The acados authors.
+#
+# This file is part of acados.
+#
+# The 2-Clause BSD License
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions are met:
+#
+# 1. Redistributions of source code must retain the above copyright notice,
+# this list of conditions and the following disclaimer.
+#
+# 2. Redistributions in binary form must reproduce the above copyright notice,
+# this list of conditions and the following disclaimer in the documentation
+# and/or other materials provided with the distribution.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.;
+#
+
+echo "Installing CasADi for Octave";
+CASADI_OCTAVE_URL="https://github.com/casadi/casadi/releases/download/nightly-main/casadi-main-linux64-octave7.3.0.zip";
+
+wget -O casadi-linux-octave.zip "${CASADI_OCTAVE_URL}";
+mkdir -p casadi-octave;
+unzip casadi-linux-octave.zip -d ./casadi-octave;

.github/linux/install_new_casadi_python.sh

@@ -0,0 +1,40 @@
+#!/bin/bash
+#
+# Copyright (c) The acados authors.
+#
+# This file is part of acados.
+#
+# The 2-Clause BSD License
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions are met:
+#
+# 1. Redistributions of source code must retain the above copyright notice,
+# this list of conditions and the following disclaimer.
+#
+# 2. Redistributions in binary form must reproduce the above copyright notice,
+# this list of conditions and the following disclaimer in the documentation
+# and/or other materials provided with the distribution.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.;
+#
+
+# source virtualenv
+source acadosenv/bin/activate;
+which python;
+
+curl -L --remote-name https://github.com/casadi/casadi/releases/download/nightly-main/casadi-3.6.7.dev+main-cp310-none-manylinux2014_x86_64.whl;
+pip install casadi-3.6.7.dev+main-cp310-none-manylinux2014_x86_64.whl;
+
+
+

.github/linux/install_python.sh

@@ -0,0 +1,42 @@
+#!/bin/bash
+#
+# Copyright (c) The acados authors.
+#
+# This file is part of acados.
+#
+# The 2-Clause BSD License
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions are met:
+#
+# 1. Redistributions of source code must retain the above copyright notice,
+# this list of conditions and the following disclaimer.
+#
+# 2. Redistributions in binary form must reproduce the above copyright notice,
+# this list of conditions and the following disclaimer in the documentation
+# and/or other materials provided with the distribution.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.;
+#
+
+python3 --version;
+
+# install virtualenv
+sudo pip3 install virtualenv;
+# create virtualenv
+virtualenv --python=python3 acadosenv;
+# source virtualenv
+source acadosenv/bin/activate;
+which python;
+
+pip install interfaces/acados_template

.github/linux/install_simde.sh

@@ -0,0 +1,32 @@
+#!/bin/bash
+#
+# Copyright (c) The acados authors.
+#
+# This file is part of acados.
+#
+# The 2-Clause BSD License
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions are met:
+#
+# 1. Redistributions of source code must retain the above copyright notice,
+# this list of conditions and the following disclaimer.
+#
+# 2. Redistributions in binary form must reproduce the above copyright notice,
+# this list of conditions and the following disclaimer in the documentation
+# and/or other materials provided with the distribution.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.;
+#
+
+sudo apt install libsimde-dev

.github/linux/install_tera.sh

@@ -0,0 +1,41 @@
+#!/bin/bash
+#
+# Copyright (c) The acados authors.
+#
+# This file is part of acados.
+#
+# The 2-Clause BSD License
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions are met:
+#
+# 1. Redistributions of source code must retain the above copyright notice,
+# this list of conditions and the following disclaimer.
+#
+# 2. Redistributions in binary form must reproduce the above copyright notice,
+# this list of conditions and the following disclaimer in the documentation
+# and/or other materials provided with the distribution.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.;
+#
+
+# install tera
+TERA_RENDERER_VERSION='0.0.34';
+_TERA_RENDERER_GITHUB_RELEASES="https://github.com/acados/tera_renderer/releases/download/v${TERA_RENDERER_VERSION}/";
+TERA_RENDERER_URL="${_TERA_RENDERER_GITHUB_RELEASES}/t_renderer-v${TERA_RENDERER_VERSION}-linux";
+
+mkdir -p bin;
+pushd bin;
+	wget -O t_renderer "${TERA_RENDERER_URL}";
+	chmod +x t_renderer
+popd;

.github/windows/setup_mingw.m

@@ -0,0 +1,31 @@
+%
+% Copyright (c) The acados authors.
+%
+% This file is part of acados.
+%
+% The 2-Clause BSD License
+%
+% Redistribution and use in source and binary forms, with or without
+% modification, are permitted provided that the following conditions are met:
+%
+% 1. Redistributions of source code must retain the above copyright notice,
+% this list of conditions and the following disclaimer.
+%
+% 2. Redistributions in binary form must reproduce the above copyright notice,
+% this list of conditions and the following disclaimer in the documentation
+% and/or other materials provided with the distribution.
+%
+% THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+% AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+% IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+% ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+% LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+% CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+% SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+% INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+% CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+% ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+% POSSIBILITY OF SUCH DAMAGE.;
+
+setenv("MW_MINGW64_LOC", "C:\ProgramData\mingw64\mingw64");
+mex -v -setup

.github/workflows/c_test_blasfeo_reference.yml

@@ -0,0 +1,67 @@
+name: Test Build and C tests with BLASFEO REFERENCE and OpenMP
+
+on:
+  push:
+    branches-ignore:
+      - 'doc*'
+      - 'wip*'
+  pull_request:
+    branches:
+      - '*'
+
+env:
+  BUILD_TYPE: Release
+  ACADOS_UNIT_TESTS: ON
+  ACADOS_PYTHON: OFF
+  ACADOS_OCTAVE: OFF
+  ACADOS_WITH_OSQP: ON
+  ACADOS_WITH_QPOASES: ON
+  ACADOS_WITH_DAQP: ON
+  ACADOS_WITH_QPDUNES: ON
+  ACADOS_ON_CI: ON
+  ACADOS_WITH_OPENMP: ON
+
+jobs:
+  full_build:
+    runs-on: ubuntu-22.04
+
+    steps:
+    - uses: actions/checkout@v2
+      with:
+        submodules: 'recursive'
+
+    - name: Create Build Environment
+      working-directory: ${{runner.workspace}}
+      run: cmake -E make_directory ${{runner.workspace}}/build
+
+    - name: Install Eigen (C test dependency)
+      working-directory: ${{runner.workspace}}/acados
+      shell: bash
+      # TODO: cache
+      run: ./.github/linux/install_eigen.sh
+
+    - name: Configure CMake
+      shell: bash
+      working-directory: ${{runner.workspace}}/build
+      run: cmake $GITHUB_WORKSPACE -DCMAKE_BUILD_TYPE=$BUILD_TYPE -DACADOS_WITH_QPOASES=$ACADOS_WITH_QPOASES -DACADOS_WITH_DAQP=$ACADOS_WITH_DAQP -DACADOS_WITH_QPDUNES=$ACADOS_WITH_QPDUNES -DACADOS_WITH_OSQP=$ACADOS_WITH_OSQP -DACADOS_PYTHON=$ACADOS_PYTHON -DACADOS_UNIT_TESTS=$ACADOS_UNIT_TESTS -DACADOS_OCTAVE=$ACADOS_OCTAVE -DLA=REFERENCE -DACADOS_WITH_OPENMP=$ACADOS_WITH_OPENMP
+
+    - name: Build & Install
+      working-directory: ${{runner.workspace}}/build
+      shell: bash
+      run: |
+        cmake --build . --config $BUILD_TYPE
+        make install -j4
+
+    - name: Export Paths Lib
+      working-directory: ${{runner.workspace}}/acados
+      shell: bash
+      run: |
+        echo "ACADOS_SOURCE_DIR=$(pwd)" >> $GITHUB_ENV
+        echo "ACADOS_INSTALL_DIR=$(pwd)" >> $GITHUB_ENV
+        echo "LD_LIBRARY_PATH=$LD_LIBRARY_PATH:$(pwd)/lib" >> $GITHUB_ENV
+    - name: Run CMake tests (ctest)
+      working-directory: ${{runner.workspace}}/build
+      shell: bash
+      run: |
+        echo "LD_LIBRARY_PATH=$LD_LIBRARY_PATH"
+        ctest -C $BUILD_TYPE --output-on-failure -j 4 --parallel 4;

.github/workflows/codeql-buildscript.sh

@@ -0,0 +1,5 @@
+#!/usr/bin/env bash
+
+mkdir build && cd build
+cmake ../
+make

.github/workflows/codeql.yml

@@ -0,0 +1,127 @@
+# For most projects, this workflow file will not need changing; you simply need
+# to commit it to your repository.
+#
+# You may wish to alter this file to override the set of languages analyzed,
+# or to provide custom queries or build logic.
+#
+# ******** NOTE ********
+# We have attempted to detect the languages in your repository. Please check
+# the `language` matrix defined below to confirm you have the correct set of
+# supported CodeQL languages.
+#
+name: "CodeQL"
+
+on:
+  # push:
+  #   branches: [ "main", "master" ]
+  # schedule:
+  #   - cron: '0 0 * * *'
+  pull_request:
+    branches: '*'
+
+jobs:
+  analyze:
+    name: Analyze
+    # Runner size impacts CodeQL analysis time. To learn more, please see:
+    #   - https://gh.io/recommended-hardware-resources-for-running-codeql
+    #   - https://gh.io/supported-runners-and-hardware-resources
+    #   - https://gh.io/using-larger-runners
+    # Consider using larger runners for possible analysis time improvements.
+    runs-on: ${{ (matrix.language == 'swift' && 'macos-latest') || 'ubuntu-22.04' }}
+    timeout-minutes: ${{ (matrix.language == 'swift' && 120) || 360 }}
+    permissions:
+      actions: read
+      contents: read
+      security-events: write
+
+    strategy:
+      fail-fast: false
+      matrix:
+        language: [ 'cpp' ]
+        # CodeQL supports [ 'cpp', 'csharp', 'go', 'java', 'javascript', 'python', 'ruby', 'swift' ]
+        # Use only 'java' to analyze code written in Java, Kotlin or both
+        # Use only 'javascript' to analyze code written in JavaScript, TypeScript or both
+        # Learn more about CodeQL language support at https://aka.ms/codeql-docs/language-support
+
+    steps:
+    - name: Checkout repository
+      uses: actions/checkout@v3
+      with:
+        submodules: recursive
+
+    # Initializes the CodeQL tools for scanning.
+    - name: Initialize CodeQL
+      uses: github/codeql-action/init@v2
+      with:
+        languages: ${{ matrix.language }}
+        # If you wish to specify custom queries, you can do so here or in a config file.
+        # By default, queries listed here will override any specified in a config file.
+        # Prefix the list here with "+" to use these queries and those in the config file.
+
+        # For more details on CodeQL's query packs, refer to: https://docs.github.com/en/code-security/code-scanning/automatically-scanning-your-code-for-vulnerabilities-and-errors/configuring-code-scanning#using-queries-in-ql-packs
+        # queries: security-extended,security-and-quality
+        queries: security-and-quality
+
+
+    # Autobuild attempts to build any compiled languages (C/C++, C#, Go, Java, or Swift).
+    # If this step fails, then you should remove it and run the build manually (see below)
+    #- name: Autobuild
+    #  uses: github/codeql-action/autobuild@v2
+
+    # ℹ️ Command-line programs to run using the OS shell.
+    # 📚 See https://docs.github.com/en/actions/using-workflows/workflow-syntax-for-github-actions#jobsjob_idstepsrun
+
+    #   If the Autobuild fails above, remove it and uncomment the following three lines.
+    #   modify them (or add more) to build your code if your project, please refer to the EXAMPLE below for guidance.
+
+    - run: |
+        ./.github/workflows/codeql-buildscript.sh
+
+    - name: Perform CodeQL Analysis
+      uses: github/codeql-action/analyze@v2
+      with:
+        category: "/language:${{matrix.language}}"
+        upload: false
+      id: step1
+
+    # Filter out rules with low severity or high false positve rate
+    # Also filter out warnings in third-party code
+    - name: Filter out unwanted errors and warnings
+      uses: advanced-security/filter-sarif@v1
+      with:
+        patterns: |
+          -**:cpp/path-injection
+          -**:cpp/world-writable-file-creation
+          -**:cpp/poorly-documented-function
+          -**:cpp/potentially-dangerous-function
+          -**:cpp/use-of-goto
+          -**:cpp/integer-multiplication-cast-to-long
+          -**:cpp/comparison-with-wider-type
+          -**:cpp/leap-year/*
+          -**:cpp/ambiguously-signed-bit-field
+          -**:cpp/suspicious-pointer-scaling
+          -**:cpp/suspicious-pointer-scaling-void
+          -**:cpp/unsigned-comparison-zero
+          -**:cpp/commented-out-code
+          -**/cmake*/Modules/**
+        input: ${{ steps.step1.outputs.sarif-output }}/cpp.sarif
+        output: ${{ steps.step1.outputs.sarif-output }}/cpp.sarif
+
+    - name: Upload CodeQL results to code scanning
+      uses: github/codeql-action/upload-sarif@v2
+      with:
+        sarif_file: ${{ steps.step1.outputs.sarif-output }}
+        category: "/language:${{matrix.language}}"
+
+    - name: Upload CodeQL results as an artifact
+      if: success() || failure()
+      uses: actions/upload-artifact@v4
+      with:
+        name: codeql-results
+        path: ${{ steps.step1.outputs.sarif-output }}
+        retention-days: 5
+
+    - name: Fail if an error is found
+      run: |
+        ./.github/workflows/fail_on_error.py \
+          ${{ steps.step1.outputs.sarif-output }}/cpp.sarif

.github/workflows/ext_dep_off.yml

@@ -0,0 +1,69 @@
+name: Test build with EXT_DEP=OFF
+
+on:
+  push:
+    branches-ignore:
+      - 'doc*'
+      - 'wip*'
+  pull_request:
+    branches:
+      - '*'
+
+env:
+  BUILD_TYPE: Debug
+  ACADOS_UNIT_TESTS: OFF
+  ACADOS_PYTHON: OFF
+  ACADOS_OCTAVE: OFF
+  ACADOS_WITH_OSQP: ON
+  ACADOS_WITH_QPOASES: ON
+  ACADOS_WITH_DAQP: ON
+  ACADOS_WITH_QPDUNES: ON
+  ACADOS_ON_CI: ON
+  ACADOS_WITH_OPENMP: ON
+
+jobs:
+  ext_dep_off_test:
+    runs-on: ubuntu-22.04
+
+    steps:
+    - uses: actions/checkout@v2
+      with:
+        submodules: 'recursive'
+
+    - name: Create Build Environment
+      working-directory: ${{runner.workspace}}
+      run: cmake -E make_directory ${{runner.workspace}}/build
+
+    - name: Install Eigen (C test dependency)
+      working-directory: ${{runner.workspace}}/acados
+      shell: bash
+      # TODO: cache
+      run: ./.github/linux/install_eigen.sh
+
+    - name: Configure CMake
+      shell: bash
+      working-directory: ${{runner.workspace}}/build
+      run: cmake $GITHUB_WORKSPACE -DCMAKE_BUILD_TYPE=$BUILD_TYPE -DACADOS_WITH_QPOASES=$ACADOS_WITH_QPOASES -DACADOS_WITH_DAQP=$ACADOS_WITH_DAQP -DACADOS_WITH_QPDUNES=$ACADOS_WITH_QPDUNES -DACADOS_WITH_OSQP=$ACADOS_WITH_OSQP -DACADOS_PYTHON=$ACADOS_PYTHON -DACADOS_UNIT_TESTS=$ACADOS_UNIT_TESTS -DACADOS_OCTAVE=$ACADOS_OCTAVE -DLA=REFERENCE -DACADOS_WITH_OPENMP=$ACADOS_WITH_OPENMP -DEXT_DEP=OFF -DBLASFEO_EXAMPLES=OFF
+
+    - name: Build & Install
+      working-directory: ${{runner.workspace}}/build
+      shell: bash
+      run: |
+        cmake --build . --config $BUILD_TYPE
+        make install -j4
+
+    - name: Export Paths Lib
+      working-directory: ${{runner.workspace}}/acados
+      shell: bash
+      run: |
+        echo "ACADOS_SOURCE_DIR=$(pwd)" >> $GITHUB_ENV
+        echo "ACADOS_INSTALL_DIR=$(pwd)" >> $GITHUB_ENV
+        echo "LD_LIBRARY_PATH=$LD_LIBRARY_PATH:$(pwd)/lib" >> $GITHUB_ENV
+
+    - name: Run example
+      working-directory: ${{runner.workspace}}/acados/examples/c_ext_dep_off/pendulum
+      shell: bash
+      run: |
+        echo "LD_LIBRARY_PATH=$LD_LIBRARY_PATH"
+        make example
+        ./main_pendulum_ode

.github/workflows/fail_on_error.py

@@ -0,0 +1,34 @@
+#!/usr/bin/env python3
+
+import json
+import sys
+
+# Return whether SARIF file contains error-level results
+def codeql_sarif_contain_error(filename):
+    with open(filename, 'r') as f:
+        s = json.load(f)
+
+    for run in s.get('runs', []):
+        rules_metadata = run['tool']['driver']['rules']
+        if not rules_metadata:
+            rules_metadata = run['tool']['extensions'][0]['rules']
+
+        for res in run.get('results', []):
+            if 'ruleIndex' in res:
+                rule_index = res['ruleIndex']
+            elif 'rule' in res and 'index' in res['rule']:
+                rule_index = res['rule']['index']
+            else:
+                continue
+            try:
+                rule_level = rules_metadata[rule_index]['defaultConfiguration']['level']
+            except IndexError as e:
+                print(e, rule_index, len(rules_metadata))
+            else:
+                if rule_level == 'error':
+                    return True
+    return False
+
+if __name__ == "__main__":
+    if codeql_sarif_contain_error(sys.argv[1]):
+        sys.exit(1)

.github/workflows/full_build.yml

@@ -0,0 +1,485 @@
+name: Test Full Build Linux
+
+on:
+  push:
+    branches-ignore:
+      - 'doc*'
+      - 'wip*'
+  pull_request:
+    branches:
+      - '*'
+
+env:
+  BUILD_TYPE: Release
+  ACADOS_PYTHON: ON
+  ACADOS_OCTAVE: ON
+  ACADOS_WITH_OSQP: ON
+  ACADOS_WITH_QPOASES: ON
+  ACADOS_WITH_DAQP: ON
+  ACADOS_WITH_QPDUNES: OFF
+  ACADOS_ON_CI: ON
+
+jobs:
+  core_build:
+    runs-on: ubuntu-22.04
+
+    steps:
+    - uses: actions/checkout@v4
+      with:
+        submodules: 'recursive'
+
+    - name: Create Build Environment
+      working-directory: ${{runner.workspace}}
+      run: cmake -E make_directory ${{runner.workspace}}/acados/build
+
+    - name: Configure CMake
+      shell: bash
+      working-directory: ${{runner.workspace}}/acados/build
+      run: cmake $GITHUB_WORKSPACE -DCMAKE_BUILD_TYPE=$BUILD_TYPE -DACADOS_WITH_QPOASES=$ACADOS_WITH_QPOASES -DACADOS_WITH_DAQP=$ACADOS_WITH_DAQP -DACADOS_WITH_QPDUNES=$ACADOS_WITH_QPDUNES -DACADOS_WITH_OSQP=$ACADOS_WITH_OSQP -DACADOS_PYTHON=$ACADOS_PYTHON -DACADOS_OCTAVE=OFF -DACADOS_WITH_OPENMP=ON -DACADOS_NUM_THREADS=1
+
+    - name: Build & Install
+      working-directory: ${{runner.workspace}}/acados/build
+      shell: bash
+      run: |
+        cmake --build . --config $BUILD_TYPE
+        make install -j4
+
+    - name: Store shared libs (/lib)
+      uses: actions/upload-artifact@v4
+      with:
+        name: lib
+        if-no-files-found: error
+        path: ${{runner.workspace}}/acados/lib/
+        compression-level: 0
+        overwrite: true
+
+    - name: Store build scripts (/build)
+      uses: actions/upload-artifact@v4
+      with:
+        name: build
+        if-no-files-found: error
+        path: |
+          ${{runner.workspace}}/acados/build/
+          !${{runner.workspace}}/**/*.dir
+        compression-level: 0
+        overwrite: true
+        # exclude object files in .dir directories
+
+    - name: Store include directory (/include)
+      uses: actions/upload-artifact@v4
+      with:
+        name: include
+        if-no-files-found: error
+        path: ${{runner.workspace}}/acados/include/
+        compression-level: 0
+        overwrite: true
+
+  python_interface:
+    needs: core_build
+    runs-on: ubuntu-22.04
+
+    steps:
+    - uses: actions/checkout@v4
+      with:
+        submodules: 'recursive'
+
+    - uses: actions/download-artifact@v4
+      with:
+        path: ${{runner.workspace}}/acados
+
+    - name: Export Paths
+      working-directory: ${{runner.workspace}}/acados
+      shell: bash
+      run: |
+        ${{runner.workspace}}/acados/.github/linux/export_paths.sh'' ${{runner.workspace}}
+
+    - name: Install Python interface
+      working-directory: ${{runner.workspace}}/acados
+      shell: bash
+      run: ${{runner.workspace}}/acados/.github/linux/install_python.sh''
+
+    - name: Install Tera
+      working-directory: ${{runner.workspace}}/acados
+      shell: bash
+      run: ${{runner.workspace}}/acados/.github/linux/install_tera.sh
+
+    - name: Run CMake python tests (ctest)
+      working-directory: ${{runner.workspace}}/acados/build
+      shell: bash
+      run: |
+        source ${{runner.workspace}}/acados/acadosenv/bin/activate
+        ctest -C $BUILD_TYPE --output-on-failure -j 4 --parallel 4;
+
+  python_interface_new_casadi:
+    needs: core_build
+    runs-on: ubuntu-22.04
+
+    steps:
+    - uses: actions/checkout@v4
+      with:
+        submodules: 'recursive'
+
+    - uses: actions/download-artifact@v4
+      with:
+        path: ${{runner.workspace}}/acados
+
+    - name: Export Paths
+      working-directory: ${{runner.workspace}}/acados
+      shell: bash
+      run: |
+        ${{runner.workspace}}/acados/.github/linux/export_paths.sh'' ${{runner.workspace}}
+
+    - name: Install Python interface
+      working-directory: ${{runner.workspace}}/acados
+      shell: bash
+      run: ${{runner.workspace}}/acados/.github/linux/install_python.sh''
+
+    - name: Install Tera
+      working-directory: ${{runner.workspace}}/acados
+      shell: bash
+      run: ${{runner.workspace}}/acados/.github/linux/install_tera.sh
+
+    - name: Install simde
+      working-directory: ${{runner.workspace}}/acados
+      shell: bash
+      run: ${{runner.workspace}}/acados/.github/linux/install_simde.sh''
+
+    - name: Install new CasADi Python
+      working-directory: ${{runner.workspace}}/acados
+      shell: bash
+      run: ${{runner.workspace}}/acados/.github/linux/install_new_casadi_python.sh''
+
+    - name: Run Python tests that need new CasADi
+      working-directory: ${{runner.workspace}}/acados/build
+      shell: bash
+      run: |
+        source ${{runner.workspace}}/acados/acadosenv/bin/activate
+        cd ${{runner.workspace}}/acados/examples/acados_python/p_global_example
+        python example_p_global.py
+
+    - name: Run more Python tests
+      working-directory: ${{runner.workspace}}/acados/build
+      shell: bash
+      run: |
+        source ${{runner.workspace}}/acados/acadosenv/bin/activate
+        cd ${{runner.workspace}}/acados/examples/acados_python/tests
+        python test_rti_sqp_residuals.py
+
+    - name: Python sensitivity examples
+      working-directory: ${{runner.workspace}}/acados/build
+      shell: bash
+      run: |
+        source ${{runner.workspace}}/acados/acadosenv/bin/activate
+        cd ${{runner.workspace}}/acados/examples/acados_python/pendulum_on_cart/solution_sensitivities
+        python value_gradient_example.py
+        python policy_gradient_example.py
+        python test_solution_sens_and_exact_hess.py
+        python forw_vs_adj_param_sens.py
+        cd ${{runner.workspace}}/acados/examples/acados_python/solution_sensitivities_convex_example
+        python value_gradient_example_linear.py
+        python batch_adjoint_solution_sensitivity_example.py
+        python non_ocp_example.py
+
+
+    - name: Python Furuta pendulum timeout test
+      working-directory: ${{runner.workspace}}/acados/build
+      shell: bash
+      run: |
+        source ${{runner.workspace}}/acados/acadosenv/bin/activate
+        cd ${{runner.workspace}}/acados/examples/acados_python/furuta_pendulum
+        python main_closed_loop.py
+
+    - name: Python evaluator test
+      working-directory: ${{runner.workspace}}/acados/build
+      shell: bash
+      run: |
+        source ${{runner.workspace}}/acados/acadosenv/bin/activate
+        cd ${{runner.workspace}}/acados/examples/acados_python/evaluation
+        python minimal_example_evaluation.py
+
+  MATLAB_test:
+    needs: core_build
+    runs-on: ubuntu-22.04
+    steps:
+    - uses: actions/checkout@v4
+      with:
+        submodules: 'recursive'
+
+    - uses: actions/download-artifact@v4
+      with:
+        path: ${{runner.workspace}}/acados
+
+    - name: Install Casadi MATLAB
+      working-directory: ${{runner.workspace}}/acados/external
+      shell: bash
+      run: |
+        ${{runner.workspace}}/acados/.github/linux/install_casadi_matlab.sh
+
+    - name: Export Paths
+      working-directory: ${{runner.workspace}}/acados
+      shell: bash
+      run: |
+        ${{runner.workspace}}/acados/.github/linux/export_paths.sh'' ${{runner.workspace}}
+
+    - name: Install MATLAB
+      uses: matlab-actions/setup-matlab@v2
+      if: always()
+      with:
+          release: R2021a
+          cache: true
+
+    # just needed for blasfeo_target.h
+    - name: Configure CMake
+      shell: bash
+      working-directory: ${{runner.workspace}}/acados/build
+      run: cmake $GITHUB_WORKSPACE -DCMAKE_BUILD_TYPE=$BUILD_TYPE -DACADOS_WITH_QPOASES=$ACADOS_WITH_QPOASES -DACADOS_WITH_DAQP=$ACADOS_WITH_DAQP -DACADOS_WITH_QPDUNES=$ACADOS_WITH_QPDUNES -DACADOS_WITH_OSQP=$ACADOS_WITH_OSQP -DACADOS_PYTHON=OFF -DACADOS_OCTAVE=OFF
+
+    - name: Configure MATLAB workspace
+      shell: bash
+      working-directory: ${{runner.workspace}}/acados/examples/acados_matlab_octave/test
+      run: |
+        cd ${{runner.workspace}}/acados/examples/acados_matlab_octave/test
+        source env.sh
+
+    - name: Run Matlab tests
+      uses: matlab-actions/run-command@v2
+      if: always()
+      with:
+        command: |
+            cd ${{runner.workspace}}/acados/examples/acados_matlab_octave/test; run_matlab_tests
+
+  MATLAB_examples_new_casadi:
+    needs: core_build
+    runs-on: ubuntu-22.04
+    steps:
+    - uses: actions/checkout@v4
+      with:
+        submodules: 'recursive'
+
+    - uses: actions/download-artifact@v4
+      with:
+        path: ${{runner.workspace}}/acados
+
+    - name: Install Casadi MATLAB
+      working-directory: ${{runner.workspace}}/acados/external
+      shell: bash
+      run: |
+        ${{runner.workspace}}/acados/.github/linux/install_new_casadi_matlab.sh
+
+    - name: Install simde
+      working-directory: ${{runner.workspace}}/acados
+      shell: bash
+      run: ${{runner.workspace}}/acados/.github/linux/install_simde.sh''
+
+    - name: Export Paths
+      working-directory: ${{runner.workspace}}/acados
+      shell: bash
+      run: |
+        ${{runner.workspace}}/acados/.github/linux/export_paths.sh'' ${{runner.workspace}}
+
+    - name: Install MATLAB
+      uses: matlab-actions/setup-matlab@v2
+      if: always()
+      with:
+          release: R2021a
+          products: Simulink Simulink_Test
+          cache: true
+
+    # just needed for blasfeo_target.h
+    - name: Configure CMake
+      shell: bash
+      working-directory: ${{runner.workspace}}/acados/build
+      run: cmake $GITHUB_WORKSPACE -DCMAKE_BUILD_TYPE=$BUILD_TYPE -DACADOS_WITH_QPOASES=$ACADOS_WITH_QPOASES -DACADOS_WITH_DAQP=$ACADOS_WITH_DAQP -DACADOS_WITH_QPDUNES=$ACADOS_WITH_QPDUNES -DACADOS_WITH_OSQP=$ACADOS_WITH_OSQP -DACADOS_PYTHON=OFF -DACADOS_OCTAVE=OFF
+
+    - name: Configure MATLAB workspace
+      shell: bash
+      working-directory: ${{runner.workspace}}/acados/examples/acados_matlab_octave/test
+      run: |
+        cd ${{runner.workspace}}/acados/examples/acados_matlab_octave/test
+        source env.sh
+
+    - name: Run Matlab tests
+      uses: matlab-actions/run-command@v2
+      if: always()
+      with:
+        command: |
+            cd ${{runner.workspace}}/acados/examples/acados_matlab_octave/test; run_matlab_examples_new_casadi
+
+    - name: Run Simulink MOCP test
+      uses: matlab-actions/run-command@v2
+      if: always()
+      with:
+        command: cd ${{runner.workspace}}/acados/examples/acados_matlab_octave/mocp_transition_example; main_mocp_simulink
+
+  # run selected matlab examples
+  MATLAB_examples:
+    needs: core_build
+    runs-on: ubuntu-22.04
+    steps:
+    - uses: actions/checkout@v4
+      with:
+        submodules: 'recursive'
+
+    - uses: actions/download-artifact@v4
+      with:
+        path: ${{runner.workspace}}/acados
+
+    - name: Install Casadi MATLAB
+      working-directory: ${{runner.workspace}}/acados/external
+      shell: bash
+      run: |
+        ${{runner.workspace}}/acados/.github/linux/install_casadi_matlab.sh
+
+    - name: Export Paths
+      working-directory: ${{runner.workspace}}/acados
+      shell: bash
+      run: |
+        ${{runner.workspace}}/acados/.github/linux/export_paths.sh'' ${{runner.workspace}}
+
+    - name: Install MATLAB
+      uses: matlab-actions/setup-matlab@v2
+      if: always()
+      with:
+          release: R2021a
+          products: Simulink Simulink_Test
+          cache: true
+
+    # just needed for blasfeo_target.h
+    - name: Configure CMake
+      shell: bash
+      working-directory: ${{runner.workspace}}/acados/build
+      run: cmake $GITHUB_WORKSPACE -DCMAKE_BUILD_TYPE=$BUILD_TYPE -DACADOS_WITH_QPOASES=$ACADOS_WITH_QPOASES -DACADOS_WITH_DAQP=$ACADOS_WITH_DAQP -DACADOS_WITH_QPDUNES=$ACADOS_WITH_QPDUNES -DACADOS_WITH_OSQP=$ACADOS_WITH_OSQP -DACADOS_PYTHON=OFF -DACADOS_OCTAVE=OFF
+
+    - name: Configure MATLAB workspace
+      shell: bash
+      working-directory: ${{runner.workspace}}/acados/examples/acados_matlab_octave/test
+      run: |
+        cd ${{runner.workspace}}/acados/examples/acados_matlab_octave/test
+        source env.sh
+
+    - name: Run Matlab examples
+      uses: matlab-actions/run-command@v2
+      if: always()
+      with:
+        command: |
+              cd ${{runner.workspace}}/acados/examples/acados_matlab_octave/test; test_all_examples;
+
+  simulink_test:
+    needs: core_build
+    runs-on: ubuntu-22.04
+
+    steps:
+    - uses: actions/checkout@v4
+      with:
+        submodules: 'recursive'
+
+    - uses: actions/download-artifact@v4
+      with:
+        path: ${{runner.workspace}}/acados
+
+    - name: Install Casadi MATLAB
+      working-directory: ${{runner.workspace}}/acados/external
+      shell: bash
+      run: |
+        ${{runner.workspace}}/acados/.github/linux/install_casadi_matlab.sh
+
+    - name: Export Paths
+      working-directory: ${{runner.workspace}}/acados
+      shell: bash
+      run: |
+        ${{runner.workspace}}/acados/.github/linux/export_paths.sh'' ${{runner.workspace}}
+
+    - name: Install MATLAB
+      uses: matlab-actions/setup-matlab@v2
+      if: always()
+      with:
+        release: R2021a
+        products: Simulink Simulink_Test
+        cache: true
+
+    # just needed for blasfeo_target.h
+    - name: Configure CMake
+      shell: bash
+      working-directory: ${{runner.workspace}}/acados/build
+      run: cmake $GITHUB_WORKSPACE -DCMAKE_BUILD_TYPE=$BUILD_TYPE -DACADOS_WITH_QPOASES=$ACADOS_WITH_QPOASES -DACADOS_WITH_DAQP=$ACADOS_WITH_DAQP -DACADOS_WITH_QPDUNES=$ACADOS_WITH_QPDUNES -DACADOS_WITH_OSQP=$ACADOS_WITH_OSQP -DACADOS_PYTHON=OFF -DACADOS_OCTAVE=OFF
+
+    - name: Run Simulink closed-loop test
+      uses: matlab-actions/run-command@v2
+      if: always()
+      with:
+        command: cd ${{runner.workspace}}/acados/examples/acados_matlab_octave/test; simulink_test
+
+    - name: Run Simulink initialization test
+      uses: matlab-actions/run-command@v2
+      if: always()
+      with:
+        command: cd ${{runner.workspace}}/acados/examples/acados_matlab_octave/test; simulink_init_test
+
+    - name: Run Simulink QP test
+      uses: matlab-actions/run-command@v2
+      if: always()
+      with:
+        command: cd ${{runner.workspace}}/acados/examples/acados_matlab_octave/test; simulink_qp_test
+
+    - name: Run Simulink slack test
+      uses: matlab-actions/run-command@v2
+      if: always()
+      with:
+        command: cd ${{runner.workspace}}/acados/examples/acados_matlab_octave/test; simulink_slack_test
+
+    - name: Run Simulink parameter test
+      uses: matlab-actions/run-command@v2
+      if: always()
+      with:
+        command: cd ${{runner.workspace}}/acados/examples/acados_matlab_octave/test; simulink_param_test
+
+    - name: Run Simulink sparse parameter test
+      uses: matlab-actions/run-command@v2
+      if: always()
+      with:
+        command: cd ${{runner.workspace}}/acados/examples/acados_matlab_octave/test; simulink_sparse_param_test
+
+
+  octave_test:
+    needs: core_build
+    runs-on: ubuntu-22.04
+
+    steps:
+
+    - uses: actions/checkout@v4
+      with:
+        submodules: 'recursive'
+
+    - uses: actions/download-artifact@v4
+      with:
+        path: ${{runner.workspace}}/acados
+
+    - name: Prepare Octave
+      working-directory: ${{runner.workspace}}/acados/external
+      shell: bash
+      run: |
+        sudo apt-get update
+        sudo apt-get install liboctave-dev -y --fix-missing
+        octave --version
+        ${{runner.workspace}}/acados/.github/linux/install_new_casadi_octave.sh
+
+    - name: Install Tera
+      working-directory: ${{runner.workspace}}/acados
+      shell: bash
+      run: ${{runner.workspace}}/acados/.github/linux/install_tera.sh''
+
+    - name: Export Paths
+      working-directory: ${{runner.workspace}}/acados
+      shell: bash
+      run: |
+        ${{runner.workspace}}/acados/.github/linux/export_paths.sh'' ${{runner.workspace}}
+
+    - name: Configure CMake
+      shell: bash
+      working-directory: ${{runner.workspace}}/acados/build
+      run: cmake $GITHUB_WORKSPACE -DCMAKE_BUILD_TYPE=$BUILD_TYPE -DACADOS_WITH_QPOASES=$ACADOS_WITH_QPOASES -DACADOS_WITH_DAQP=$ACADOS_WITH_DAQP -DACADOS_WITH_QPDUNES=$ACADOS_WITH_QPDUNES -DACADOS_WITH_OSQP=$ACADOS_WITH_OSQP -DACADOS_PYTHON=OFF -DACADOS_OCTAVE=$ACADOS_OCTAVE
+
+    - name: Run CMake Octave tests (ctest)
+      working-directory: ${{runner.workspace}}/acados/build
+      shell: bash
+      run: ctest -C $BUILD_TYPE --output-on-failure -j 4 --parallel 4;

.github/workflows/full_build_windows.yml

@@ -0,0 +1,140 @@
+name: Test MATLAB on Windows
+
+on:
+  push:
+    branches-ignore:
+      - 'doc*'
+      - 'wip*'
+  pull_request:
+    branches:
+      - '*'
+
+env:
+  BUILD_TYPE: Release
+  ACADOS_ON_CI: ON
+
+jobs:
+  core_build:
+    runs-on: windows-latest
+
+    steps:
+    - uses: actions/checkout@v4
+      with:
+        submodules: 'recursive'
+
+    - name: Cache MinGW
+      uses: actions/cache@v4
+      id: cache-minGW
+      with:
+        path: C:\tools\mingw64  # This is the default install location for MinGW on Windows.
+        key: ${{ runner.os }}-mingw-${{ hashFiles('**/*.yml') }}  # use yml files for cache key
+        restore-keys: |
+          ${{ runner.os }}-mingw-
+
+    - name: Install Chocolatey
+      run: |
+        Set-ExecutionPolicy Bypass -Scope Process -Force;
+        [System.Net.ServicePointManager]::SecurityProtocol = [System.Net.ServicePointManager]::SecurityProtocol -bor 3072;
+        iex ((New-Object System.Net.WebClient).DownloadString('https://chocolatey.org/install.ps1'))
+
+    - if: ${{ steps.cache-minGW.outputs.cache-hit != 'true' }}
+      name: Install MinGW using Chocolatey
+      run: |
+        choco install mingw --yes
+
+    - name: Add MinGW to PATH
+      run: |
+        echo "C:\tools\mingw64\bin" >> $env:MW_MINGW64_LOC
+
+    - name: Cache CMake
+      id: cache-cmake
+      uses: actions/cache@v4
+      with:
+        path: C:\Program Files\CMake
+        key: ${{ runner.os }}-cmake-${{ hashFiles('**/*.yml') }}
+        restore-keys: |
+          ${{ runner.os }}-cmake-
+
+    - if: ${{ steps.cache-cmake.outputs.cache-hit != 'true' }}
+      name: Install CMake
+      uses: jwlawson/actions-setup-cmake@v2
+
+    - name: Check GCC and CMake versions
+      run: |
+        which gcc
+        gcc --version
+        cmake --version
+
+    - name: Install MATLAB
+      uses: matlab-actions/setup-matlab@v2
+      if: always()
+      with:
+          release: R2021a
+          products: Simulink Simulink_Test
+          cache: true
+
+    - name: Setup MATLAB with MinGW Compiler
+      uses: matlab-actions/run-command@v2
+      if: always()
+      with:
+        command: |
+            cd ${{runner.workspace}}/acados/.github/windows; setup_mingw
+
+    - name: Install acados via MATLAB script
+      uses: matlab-actions/run-command@v2
+      if: always()
+      with:
+        command: |
+            cd ${{runner.workspace}}/acados/interfaces/acados_matlab_octave; acados_install_windows
+
+    - name: Run minimal MATLAB example
+      uses: matlab-actions/run-command@v2
+      if: always()
+      with:
+        command: |
+            cd ${{runner.workspace}}/acados/.github/windows
+            setup_mingw
+            cd ${{runner.workspace}}/acados/examples/acados_matlab_octave
+            acados_env_variables_windows
+            cd getting_started
+            minimal_example_ocp
+
+    - name: Run minimal Simulink example
+      uses: matlab-actions/run-command@v2
+      if: always()
+      with:
+        command: |
+            cd ${{runner.workspace}}/acados/.github/windows
+            setup_mingw
+            cd ${{runner.workspace}}/acados/examples/acados_matlab_octave
+            acados_env_variables_windows
+            cd test
+            simulink_test
+
+    - name: Run MATLAB tests with new CasADi
+      uses: matlab-actions/run-command@v2
+      if: always()
+      with:
+        command: |
+            cd ${{runner.workspace}}/acados/.github/windows
+            setup_mingw
+            cd ${{runner.workspace}}/acados/examples/acados_matlab_octave
+            acados_env_variables_windows
+            cd ${{runner.workspace}}/acados/examples/acados_matlab_octave/p_global_example
+            main
+
+    # This fails undeterministically on Github actions:
+    # Two actions with the same commit, 1 fail, 1 success.
+    # https://github.com/sandmaennchen/acados/actions/runs/11783596401/job/32821083440
+    # https://github.com/acados/acados/actions/runs/11783597483/job/32821085806
+    # - name: Run Simulink test with blazing splines
+    #   uses: matlab-actions/run-command@v2
+    #   if: always()
+    #   with:
+    #     command: |
+    #         cd ${{runner.workspace}}/acados/.github/windows
+    #         setup_mingw
+    #         cd ${{runner.workspace}}/acados/examples/acados_matlab_octave
+    #         acados_env_variables_windows
+    #         cd ${{runner.workspace}}/acados/examples/acados_matlab_octave/p_global_example
+    #         simulink_test_p_global

.gitignore

@@ -0,0 +1,133 @@
+# Compiled source #
+###################
+*.com
+*.class
+*.dll
+*.exe
+*.o
+*.so
+*.out
+*.a
+*.mex*
+
+# Packages #
+############
+# it's better to unpack these files and commit the raw source
+# git has its own built in compression methods
+*.7z
+*.dmg
+*.gz
+*.iso
+*.jar
+*.rar
+*.tar
+*.tar.gz*
+*.zip
+
+# Logs and databases #
+######################
+*.log
+*.sql
+*.sqlite
+
+# OS generated files #
+######################
+.DS_Store
+.DS_Store?
+._*
+.Spotlight-V100
+.Trashes
+ehthumbs.db
+Thumbs.db
+
+# Latex #
+#########
+*.aux
+*.idx
+*.nav
+*.out
+*.snm
+*.toc
+*.bbl
+*.blg
+*.synctex
+*.fdb_latexmk
+*.fls
+
+# dSpace #
+##########
+*.map
+*.sdf
+*.ppc
+*.trc
+*.trz
+*.tmw
+*.hex
+*.srec
+*.mk
+*.dsbuildinfo
+
+# External #
+############
+*external/OOQP
+*external/coinhsl
+*external/matlab
+*external/download
+*external/casadi-matlab*
+*external/download_script
+*external/encrypted_script
+*external/encrypted_script_matlab
+*external/qore
+
+*download_software
+*download_matlab
+
+# Matlab #
+*.m~
+
+# Python #
+__pycache__/
+*.egg-info
+env/
+
+# Examples #
+c_generated_code
+*_generated_code
+
+# Misc #
+########
+*.doc
+*.swp
+build/
+include/
+lib/
+local/
+*.local
+*.dat
+octave-workspace
+.vscode
+notes.txt
+*.dSYM/
+cmakelint
+*.srctrlbm
+*.srctrldb
+*.srctrlprj
+.vs
+build*
+*rti1401
+*.asv
+*.swo
+*.autosave
+_autogen
+tags
+compile_commands.json
+
+*.pkl
+private*
+sandbox
+_out*
+
+qp_in*.txt
+qp_out*.txt
+
+figures

.gitmodules

@@ -0,0 +1,41 @@
+[submodule "hpmpc"]
+	path = external/hpmpc
+	url = https://github.com/giaf/hpmpc.git
+	branch = stable
+[submodule "blasfeo"]
+	path = external/blasfeo
+	url = https://github.com/giaf/blasfeo.git
+[submodule "qpdunes"]
+	path = external/qpdunes
+	url = https://github.com/acados/qpDUNES-dev.git
+[submodule "catch"]
+	path = external/catch
+	url = https://github.com/philsquared/Catch.git
+# [submodule "swig"]
+# 	path = external/swig
+# 	url = https://github.com/jaeandersson/swig
+# 	branch = matlab
+# 	ignore = dirty
+[submodule "hpipm"]
+	path = external/hpipm
+	url = https://github.com/giaf/hpipm.git
+	branch = stable
+[submodule "qpoases"]
+	path = external/qpoases
+	url = https://github.com/acados/qpOASES.git
+[submodule "external/osqp"]
+	path = external/osqp
+	url = https://github.com/osqp/osqp.git
+	branch = develop
+[submodule "interfaces/acados_template/tera_renderer"]
+	path = interfaces/acados_template/tera_renderer
+	url = https://github.com/acados/tera_renderer
+[submodule "external/jsonlab"]
+	path = external/jsonlab
+	url = https://github.com/fangq/jsonlab.git
+[submodule "examples/acados_python/tests/test_data"]
+	path = examples/acados_python/tests/test_data
+	url = https://github.com/acados/test_data
+[submodule "external/daqp"]
+	path = external/daqp
+	url = https://github.com/darnstrom/daqp.git

CMakeLists.txt

@@ -0,0 +1,351 @@
+#
+# Copyright (c) The acados authors.
+#
+# This file is part of acados.
+#
+# The 2-Clause BSD License
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions are met:
+#
+# 1. Redistributions of source code must retain the above copyright notice,
+# this list of conditions and the following disclaimer.
+#
+# 2. Redistributions in binary form must reproduce the above copyright notice,
+# this list of conditions and the following disclaimer in the documentation
+# and/or other materials provided with the distribution.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.;
+#
+
+
+cmake_minimum_required(VERSION 3.7.1)
+
+if(CMAKE_MAKE_PROGRAM)
+    file(TO_CMAKE_PATH ${CMAKE_MAKE_PROGRAM} CMAKE_MAKE_PROGRAM)
+    find_program(CMAKE_MAKE_PROGRAM ${CMAKE_MAKE_PROGRAM})
+endif()
+
+project(acados LANGUAGES C CXX)
+
+list(APPEND CMAKE_MODULE_PATH "${PROJECT_SOURCE_DIR}/cmake")
+
+set(CMAKE_POSITION_INDEPENDENT_CODE TRUE)
+
+### Defaults
+
+if(CMAKE_INSTALL_PREFIX_INITIALIZED_TO_DEFAULT)
+    if(DEFINED ACADOS_INSTALL_DIR)
+        set_property(CACHE CMAKE_INSTALL_PREFIX PROPERTY VALUE "${ACADOS_INSTALL_DIR}")
+    else()
+        set_property(CACHE CMAKE_INSTALL_PREFIX PROPERTY VALUE "${PROJECT_SOURCE_DIR}")
+    endif()
+else()
+    if(DEFINED ACADOS_INSTALL_DIR)
+        if(NOT ${ACADOS_INSTALL_DIR} STREQUAL ${CMAKE_INSTALL_PREFIX})
+            set(CMAKE_INSTALL_PREFIX ${ACADOS_INSTALL_DIR})
+            message(WARNING "CMAKE_INSTALL_PREFIX is overwritten by ACADOS_INSTALL_DIR, installing to ACADOS_INSTALL_DIR = ${ACADOS_INSTALL_DIR}")
+        endif()
+    endif()
+endif()
+
+if ("${CMAKE_HOST_SYSTEM_PROCESSOR}" STREQUAL "aarch64")
+    set(BLASFEO_TARGET "ARMV8A_ARM_CORTEX_A57" CACHE STRING "BLASFEO Target architecture")
+else()
+    set(BLASFEO_TARGET "X64_AUTOMATIC" CACHE STRING "BLASFEO Target architecture")
+endif()
+set(HPIPM_TARGET "X64_AUTOMATIC" CACHE STRING "HPIPM Target architecture")
+set(LA "HIGH_PERFORMANCE" CACHE STRING "Linear algebra optimization level")
+
+if (CMAKE_SYSTEM_NAME MATCHES "Windows")
+    set(BUILD_SHARED_LIBS OFF CACHE STRING "Build shared libraries")
+else()
+    set(BUILD_SHARED_LIBS ON CACHE STRING "Build shared libraries")
+endif()
+
+option(ACADOS_WITH_OPENMP "OpenMP Parallelization" OFF)
+option(ACADOS_SILENT "No console status output" OFF)
+option(ACADOS_DEBUG_SQP_PRINT_QPS_TO_FILE "Print QP inputs and outputs to file in SQP" OFF)
+
+# Additional targets
+option(ACADOS_UNIT_TESTS "Compile Unit tests" OFF)
+option(ACADOS_EXAMPLES "Compile Examples" OFF)
+option(ACADOS_LINT "Compile Lint" OFF)
+# External libs
+option(ACADOS_WITH_QPOASES "qpOASES solver" OFF)
+option(ACADOS_WITH_DAQP "DAQP solver" OFF)
+option(ACADOS_WITH_HPMPC "HPMPC solver" OFF)
+option(ACADOS_WITH_QORE "QORE solver" OFF)
+option(ACADOS_WITH_OOQP "OOQP solver" OFF)
+option(ACADOS_WITH_QPDUNES "qpDUNES solver" OFF)
+option(ACADOS_WITH_OSQP "OSQP solver" OFF)
+# Interfaces
+option(ACADOS_OCTAVE "Octave Interface tests" OFF)
+option(ACADOS_PYTHON "Python Interface tests" OFF)
+# Options to use libraries found via find_package
+option(ACADOS_WITH_SYSTEM_BLASFEO "If ON, use blasfeo found via find_package(blasfeo) instead of compiling it" OFF)
+mark_as_advanced(ACADOS_WITH_SYSTEM_BLASFEO)
+if(ACADOS_WITH_SYSTEM_BLASFEO)
+    message(WARNING "The ACADOS_WITH_SYSTEM_BLASFEO option is enabled. ACADOS is tested by its developers only with ACADOS_WITH_SYSTEM_BLASFEO=OFF, when using an external blasfeo may sure that everything works as expected.")
+endif()
+
+# Set custom path
+set(EXTERNAL_SRC_DIR ${PROJECT_SOURCE_DIR}/external)
+set(BLASFEO_SRC_DIR ${PROJECT_SOURCE_DIR}/external/blasfeo)
+
+# Set up build type
+if(NOT CMAKE_BUILD_TYPE)
+    set(CMAKE_BUILD_TYPE Release
+        CACHE STRING "Valid build types are: \
+        Release (default), None, Debug, WithExternalLibs, RelWithDebInfo, MinSizeRel." FORCE)
+endif()
+
+message(STATUS "Build type is ${CMAKE_BUILD_TYPE}")
+
+enable_testing(true)
+
+if(CMAKE_CXX_COMPILER_ID MATCHES "GNU" AND NOT CMAKE_SYSTEM_NAME MATCHES "Windows")
+    add_definitions(-D _GLIBCXX_USE_CXX11_ABI=0) # Needed for CasADi compatibility
+endif()
+
+if(CMAKE_CXX_COMPILER_ID MATCHES "GNU" AND CMAKE_SYSTEM_NAME MATCHES "Windows")
+    # MinGW: remove prefix and change suffix to match MSVC
+    # (such that Matlab mex recognizes the libraries)
+    set(CMAKE_SHARED_LIBRARY_PREFIX "")
+    set(CMAKE_IMPORT_LIBRARY_SUFFIX ".lib")
+    set(CMAKE_IMPORT_LIBRARY_PREFIX "")
+    set(CMAKE_STATIC_LIBRARY_SUFFIX ".lib")
+    set(CMAKE_STATIC_LIBRARY_PREFIX "")
+    if(BUILD_SHARED_LIBS MATCHES ON)
+        set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -Wl,--export-all-symbols")
+    endif()
+endif()
+
+if(CMAKE_CXX_COMPILER_ID MATCHES "GNU" OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+    set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -std=c99 -Wall -fdiagnostics-show-option")
+    set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++0x -Wall -fdiagnostics-show-option")
+elseif(CMAKE_CXX_COMPILER_ID MATCHES "MSVC")
+    set(CMAKE_C_FLAGS_RELEASE "${CMAKE_C_FLAGS_RELEASE} /MT")
+    set(CMAKE_CXX_FLAGS_RELEASE "${CMAKE_CXX_FLAGS_RELEASE} /MT")
+    if(BUILD_SHARED_LIBS MATCHES ON)
+        set(CMAKE_WINDOWS_EXPORT_ALL_SYMBOLS 1)
+    endif()
+endif()
+
+# OPENMP
+if(ACADOS_WITH_OPENMP)
+    if(${CMAKE_VERSION} VERSION_GREATER_EQUAL "3.9.0")
+        find_package(OpenMP)
+    else()
+        if(NOT TARGET OpenMP::OpenMP_CXX)
+            set(OpenMP_CXX_FLAGS "-fopenmp")
+            set(OpenMP_C_FLAGS "-fopenmp")
+            find_package(Threads REQUIRED)
+            add_library(OpenMP::OpenMP_CXX IMPORTED INTERFACE)
+            set_property(TARGET OpenMP::OpenMP_CXX
+                         PROPERTY INTERFACE_COMPILE_OPTIONS ${OpenMP_CXX_FLAGS})
+            # Only works if the same flag is passed to the linker; use CMake 3.9+ otherwise (Intel, AppleClang)
+            set_property(TARGET OpenMP::OpenMP_CXX
+                         PROPERTY INTERFACE_LINK_LIBRARIES ${OpenMP_CXX_FLAGS} Threads::Threads)
+            set(OpenMP_CXX_FOUND TRUE)
+            set(OpenMP_C_FOUND TRUE)
+        endif()
+    endif()
+
+    if(OpenMP_CXX_FOUND AND OpenMP_C_FOUND)
+        message(STATUS "OpenMP_CXX_FLAGS: ${OpenMP_CXX_FLAGS}, OpenMP_C_FLAGS: ${OpenMP_C_FLAGS}")
+        # add openmp compiler flags
+        set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} ${OpenMP_C_FLAGS}")
+        set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}")
+
+        if(ACADOS_NUM_THREADS)
+            add_definitions(-DACADOS_NUM_THREADS=${ACADOS_NUM_THREADS}) # supported by CMAKE_VERSION < 3.12.0
+        endif()
+    else()
+        message(STATUS "OpenMP NOT found.")
+        set(ACADOS_WITH_OPENMP OFF)
+    endif()
+else()
+    set(ACADOS_WITH_OPENMP OFF)
+    message(STATUS "ACADOS_WITH_OPENMP: ${ACADOS_WITH_OPENMP}")
+endif()
+
+if(ACADOS_SILENT)
+    message(STATUS "ACADOS_SILENT is ON")
+    set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -DACADOS_SILENT")
+    set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DACADOS_SILENT")
+endif()
+
+if(ACADOS_DEBUG_SQP_PRINT_QPS_TO_FILE)
+    message(STATUS "ACADOS_DEBUG_SQP_PRINT_QPS_TO_FILE is ON")
+    set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -DACADOS_DEBUG_SQP_PRINT_QPS_TO_FILE")
+    set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DACADOS_DEBUG_SQP_PRINT_QPS_TO_FILE")
+endif()
+
+# uninstall
+if(NOT TARGET uninstall)
+    # Configure Uninstall
+    # https://gitlab.kitware.com/cmake/community/-/wikis/FAQ#can-i-do-make-uninstall-with-cmake
+    configure_file(
+        "${CMAKE_CURRENT_SOURCE_DIR}/cmake/cmake_uninstall.cmake.in"
+        "${CMAKE_CURRENT_BINARY_DIR}/cmake/cmake_uninstall.cmake"
+        IMMEDIATE @ONLY)
+
+    add_custom_target(uninstall
+        "${CMAKE_COMMAND}" -P "${CMAKE_CURRENT_BINARY_DIR}/cmake/cmake_uninstall.cmake")
+endif()
+
+# Rpath handling, see https://cmake.org/Wiki/CMake_RPATH_handling#What_is_RPATH_.3F
+set(CMAKE_SKIP_BUILD_RPATH FALSE)
+set(CMAKE_BUILD_WITH_INSTALL_RPATH FALSE)
+
+if(CMAKE_BUILD_TYPE MATCHES WithExternalLibs)
+    set(ACADOS_WITH_HPMPC ON CACHE BOOL "Add HPMPC solver")
+    set(ACADOS_WITH_QORE ON CACHE BOOL "Add QORE solver")
+    set(ACADOS_WITH_OOQP ON CACHE BOOL "Add OOQP solver")
+    set(ACADOS_WITH_QPOASES ON CACHE BOOL "Add qpOASES solver")
+	set(ACADOS_WITH_DAQP ON CACHE BOOL "Add DAQP solver")
+    set(ACADOS_WITH_QPDUNES ON CACHE BOOL "Add qpDUNES solver")
+    set(ACADOS_WITH_OSQP ON CACHE BOOL "Add OSQP solver")
+endif()
+
+## External lib checks (
+
+if(ACADOS_WITH_HPMPC MATCHES ON AND CMAKE_C_COMPILER_ID MATCHES "MSVC")
+    set(ACADOS_WITH_HPMPC OFF CACHE BOOL "HPMPC solver" FORCE)
+    message(WARNING "HPMPC has been disabled, not compatible with ${CMAKE_C_COMPILER_ID}")
+endif()
+
+if(ACADOS_WITH_OOQP MATCHES ON)
+
+    if(CMAKE_SYSTEM_NAME MATCHES "dSpace")
+        set(ACADOS_WITH_OOQP OFF CACHE BOOL "Add OOQP solver" FORCE)
+        message(WARNING "OOQP is not compatible with dSpace, OOQP is disabled")
+
+    elseif(NOT EXISTS ${PROJECT_SOURCE_DIR}/external/OOQP)
+        set(ACADOS_WITH_OOQP OFF CACHE BOOL "Add OOQP solver" FORCE)
+        message(WARNING "OOQP folder not found, OOQP has been disabled")
+
+    endif()
+endif()
+
+if(ACADOS_WITH_QORE MATCHES ON)
+
+    if(CMAKE_SYSTEM_NAME MATCHES "dSpace" OR CMAKE_CXX_COMPILER_ID MATCHES "MSVC")
+        set(ACADOS_WITH_QORE OFF CACHE BOOL "Add QORE solver" FORCE)
+        message(WARNING "QORE is not compatible with MSVC or dSpace, QORE is has been disabled")
+
+    elseif(NOT EXISTS ${PROJECT_SOURCE_DIR}/external/qore)
+        set(ACADOS_WITH_QORE OFF CACHE BOOL "Add QORE solver" FORCE)
+        message(WARNING "QORE folder not found, QORE has been disabled")
+
+    endif()
+endif()
+
+if(ACADOS_LINT)
+    if(CMAKE_SYSTEM_NAME MATCHES "Windows" OR CMAKE_SYSTEM_NAME MATCHES "dSpace")
+        set(ACADOS_LINT OFF CACHE BOOL "Lint" FORCE)
+        message(WARNING "Lint has been disabled, not compatible with ${CMAKE_SYSTEM_NAME}")
+    endif()
+endif()
+
+if(ACADOS_OCTAVE OR ACADOS_PYTHON)
+    add_subdirectory(${PROJECT_SOURCE_DIR}/interfaces)
+endif()
+
+## ) External lib checks
+
+# Configure acados
+add_subdirectory(acados)
+
+# Configure interfaces
+add_subdirectory(${PROJECT_SOURCE_DIR}/interfaces/acados_c)
+
+# Configure external libraries
+add_subdirectory(${EXTERNAL_SRC_DIR})
+
+# Configure examples
+if(ACADOS_EXAMPLES)
+    add_subdirectory(examples)
+endif()
+
+# Configure tests
+if(ACADOS_UNIT_TESTS)
+    add_subdirectory(test)
+endif()
+
+# Configure lint
+if(ACADOS_LINT)
+    include(Lint)
+else()
+    add_custom_target(lint echo "Lint is disabled")
+endif()
+
+# Current config messages
+message(STATUS " ")
+message(STATUS "Target: BLASFEO is ${BLASFEO_TARGET}, HPIPM is ${HPIPM_TARGET}")
+message(STATUS "Linear algebra: ${LA}")
+message(STATUS "Octave MEX (${ACADOS_OCTAVE})")
+message(STATUS "System name:version ${CMAKE_SYSTEM_NAME}:${CMAKE_SYSTEM_VERSION}")
+message(STATUS "Build type is ${CMAKE_BUILD_TYPE}")
+message(STATUS "Installation directory is ${CMAKE_INSTALL_PREFIX}")
+if(ACADOS_WITH_OPENMP)
+    message(STATUS "OpenMP parallelization is ON")
+    if(ACADOS_NUM_THREADS)
+        message(STATUS "Number of threads for acados with openMP (ACADOS_NUM_THREADS) ${ACADOS_NUM_THREADS}")
+    else()
+        message(STATUS "Number of threads for acados with openMP (ACADOS_NUM_THREADS) not set, will use omp_get_max_threads")
+    endif()
+else()
+    message(STATUS "OpenMP parallelization is OFF")
+endif()
+
+message(STATUS " ")
+
+# Write libraries to be linked into a json file
+if(${ACADOS_WITH_OPENMP})
+    set(LINK_FLAG_OPENMP ${OpenMP_C_FLAGS})
+endif()
+if(${ACADOS_WITH_QPOASES})
+    set(LINK_FLAG_QPOASES -lqpOASES_e)
+endif()
+if(${ACADOS_WITH_DAQP})
+    set(LINK_FLAG_DAQP -ldaqp)
+endif()
+if(${ACADOS_WITH_QPDUNES})
+    set(LINK_FLAG_QPDUNES -lqpdunes)
+endif()
+# TODO: add -lqdldl (?!)
+if(${ACADOS_WITH_OSQP})
+    set(LINK_FLAG_OSQP -losqp)
+endif()
+if(${ACADOS_WITH_HPMPC})
+    set(LINK_FLAG_HPMPC -lhpmpc)
+endif()
+# if(${ACADOS_WITH_QORE})
+#     set(LINK_FLAG_QORE -lqore)
+# endif()
+if(${ACADOS_WITH_OOQP})
+    set(LINK_FLAG_OOQP -looqp)
+endif()
+
+file(WRITE lib/link_libs.json {\n)
+file(APPEND lib/link_libs.json \t\"openmp\":\ \"${LINK_FLAG_OPENMP}\",\n)
+file(APPEND lib/link_libs.json \t\"qpoases\":\ \"${LINK_FLAG_QPOASES}\",\n)
+file(APPEND lib/link_libs.json \t\"daqp\":\ \"${LINK_FLAG_DAQP}\",\n)
+file(APPEND lib/link_libs.json \t\"qpdunes\":\ \"${LINK_FLAG_QPDUNES}\",\n)
+file(APPEND lib/link_libs.json \t\"osqp\":\ \"${LINK_FLAG_OSQP}\",\n)
+file(APPEND lib/link_libs.json \t\"hpmpc\":\ \"${LINK_FLAG_HPMPC}\",\n)
+# file(APPEND lib/link_libs.json \t\"qore\":\ \"${LINK_FLAG_QORE}\",\n)
+file(APPEND lib/link_libs.json \t\"ooqp\":\ \"${LINK_FLAG_OOQP}\"\n) # no final comma!
+file(APPEND lib/link_libs.json })

CONTRIBUTING.md

@@ -0,0 +1,4 @@
+Thank you for your interest to contribute to `acados`.
+All contributions are licensed under the terms of the [2-Clause BSD License](./LICENSE).
+By making a contribution to `acados` you certify that you have the right to submit the code that you are contributing to the project according to the [Developer Certificate of Origin (DCO)](https://developercertificate.org/).
+Contributions are welcome via pull requests on Github.

CONTRIBUTORS.txt

@@ -0,0 +1,65 @@
+acados contributor list:
+
+- Jonathan Frey (University Freiburg)
+- Robin Verschueren (University Freiburg)
+- Gianluca Frison (University Freiburg)
+- Andrea Zanelli (University Freiburg)
+- Dimitris Kouzoupis (University Freiburg)
+- Branimir Novoselnik (University Freiburg)
+- Tommaso Sartor (University Freiburg)
+- Niels van Duijkeren (2016-2018 KU Leuven, 2018- Robert Bosch GmbH)
+- Rien Quirynen (University Freiburg)
+- Jonas Koenemann (University Freiburg)
+- Martin Kirchengast
+- Katrin Baumgärtner (University Freiburg)
+- Markus Schwienbacher
+- Yutao Chen
+- Rezart Qelibari (University Freiburg)
+- Daniel Arnström (Linköping University)
+- Caspar Gruijthuijsen (Sioux)
+- Tom Geelen (Sioux)
+- Dang Doan (University Freiburg)
+- Tobias Schöls (University Freiburg)
+- Hasan Berkay Çağır (University Freiburg)
+- Jonas Schlagenhauf (University Freiburg)
+- Joris Gillis (KU Leuven)
+- Thomas Jespersen (TKJ electronics)
+- François Bailly
+- Benjamin Michaud
+- Daniel Geweth (Robert Bosch GmbH)
+- Andrea Ghezzi (University Freiburg)
+- Josip Kir Hromatko
+- David Kiessling (KU Leuven)
+- Greg Horn
+- Jacob Grunnet
+- Leonardo Cecchin (Robert Bosch GmbH)
+- Yunfan Gao (Robert Bosch GmbH)
+- Yizhen Wang (University Freiburg)
+- Severin Hänggi (ETH Zürich)
+- Harald Schäfer (comma.ai)
+- Sourish Pramanick
+- Akash John Subash
+- Franek Stark
+- Dirk Reinhardt
+- Henrik Hose
+- Ian McInerney
+- Jan Zwiener
+- Rudolf Reiter (University Freiburg)
+- Mohammed Hababeh
+- Alexander Sherikov
+- Ajinkya Krishnakumar
+- Federico Pizarro Bejarano
+- Anton Pozharskiy (University Freiburg)
+- Silvio Traversaro
+- Adeeb Shihadeh (comma.ai)
+- Martin Koch
+- Miralem Saljanin (Uni Stuttgart)
+- Moritz Diehl (University Freiburg)
+
+
+Notes:
+- This list is roughly sorted by number of commits.
+- Not all commits are created equal.
+- At some point, `acados` switched to squash-merge pull requests, which results in less commits of later contributors and a cleaner git history.
+- Affiliations are added in brackets if they are known.
+- Please reach out, if you or your affiliation is not mentioned correctly.

LICENSE

@@ -0,0 +1,27 @@
+Copyright (c) The acados authors.
+
+This file is part of acados.
+
+The 2-Clause BSD License
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+
+1. Redistributions of source code must retain the above copyright notice,
+this list of conditions and the following disclaimer.
+
+2. Redistributions in binary form must reproduce the above copyright notice,
+this list of conditions and the following disclaimer in the documentation
+and/or other materials provided with the distribution.
+
+THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+POSSIBILITY OF SUCH DAMAGE.

Makefile

@@ -0,0 +1,343 @@
+#
+# Copyright (c) The acados authors.
+#
+# This file is part of acados.
+#
+# The 2-Clause BSD License
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions are met:
+#
+# 1. Redistributions of source code must retain the above copyright notice,
+# this list of conditions and the following disclaimer.
+#
+# 2. Redistributions in binary form must reproduce the above copyright notice,
+# this list of conditions and the following disclaimer in the documentation
+# and/or other materials provided with the distribution.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.;
+#
+
+
+
+
+# default make target
+all: static_library
+
+
+
+# include config & tailored rules
+include ./Makefile.rule
+include ./Makefile.osqp
+
+# acados sources
+OBJS =
+
+# ocp nlp
+OBJS += acados/ocp_nlp/ocp_nlp_common.o
+OBJS += acados/ocp_nlp/ocp_nlp_cost_common.o
+OBJS += acados/ocp_nlp/ocp_nlp_cost_ls.o
+OBJS += acados/ocp_nlp/ocp_nlp_cost_nls.o
+OBJS += acados/ocp_nlp/ocp_nlp_cost_external.o
+OBJS += acados/ocp_nlp/ocp_nlp_cost_conl.o
+OBJS += acados/ocp_nlp/ocp_nlp_constraints_common.o
+OBJS += acados/ocp_nlp/ocp_nlp_constraints_bgh.o
+OBJS += acados/ocp_nlp/ocp_nlp_constraints_bgp.o
+OBJS += acados/ocp_nlp/ocp_nlp_dynamics_common.o
+OBJS += acados/ocp_nlp/ocp_nlp_dynamics_cont.o
+OBJS += acados/ocp_nlp/ocp_nlp_dynamics_disc.o
+OBJS += acados/ocp_nlp/ocp_nlp_sqp.o
+OBJS += acados/ocp_nlp/ocp_nlp_ddp.o
+OBJS += acados/ocp_nlp/ocp_nlp_sqp_rti.o
+OBJS += acados/ocp_nlp/ocp_nlp_reg_common.o
+OBJS += acados/ocp_nlp/ocp_nlp_reg_convexify.o
+OBJS += acados/ocp_nlp/ocp_nlp_reg_mirror.o
+OBJS += acados/ocp_nlp/ocp_nlp_reg_project.o
+OBJS += acados/ocp_nlp/ocp_nlp_reg_project_reduc_hess.o
+OBJS += acados/ocp_nlp/ocp_nlp_reg_noreg.o
+
+OBJS += acados/ocp_nlp/ocp_nlp_globalization_common.o
+OBJS += acados/ocp_nlp/ocp_nlp_globalization_fixed_step.o
+OBJS += acados/ocp_nlp/ocp_nlp_globalization_funnel.o
+OBJS += acados/ocp_nlp/ocp_nlp_globalization_merit_backtracking.o
+
+
+# dense qp
+OBJS += acados/dense_qp/dense_qp_common.o
+OBJS += acados/dense_qp/dense_qp_hpipm.o
+ifeq ($(ACADOS_WITH_QPOASES), 1)
+OBJS += acados/dense_qp/dense_qp_qpoases.o
+endif
+ifeq ($(ACADOS_WITH_QORE), 1)
+OBJS += acados/dense_qp/dense_qp_qore.o
+endif
+# ocp qp
+OBJS += acados/ocp_qp/ocp_qp_common.o
+OBJS += acados/ocp_qp/ocp_qp_common_frontend.o
+OBJS += acados/ocp_qp/ocp_qp_hpipm.o
+ifeq ($(ACADOS_WITH_HPMPC), 1)
+OBJS += acados/ocp_qp/ocp_qp_hpmpc.o
+endif
+ifeq ($(ACADOS_WITH_QPDUNES), 1)
+OBJS += acados/ocp_qp/ocp_qp_qpdunes.o
+endif
+ifeq ($(ACADOS_WITH_OSQP), 1)
+OBJS += acados/ocp_qp/ocp_qp_osqp.o
+endif
+OBJS += acados/ocp_qp/ocp_qp_partial_condensing.o
+OBJS += acados/ocp_qp/ocp_qp_full_condensing.o
+OBJS += acados/ocp_qp/ocp_qp_xcond_solver.o
+# sim
+OBJS += acados/sim/sim_collocation_utils.o
+OBJS += acados/sim/sim_erk_integrator.o
+OBJS += acados/sim/sim_irk_integrator.o
+OBJS += acados/sim/sim_lifted_irk_integrator.o
+OBJS += acados/sim/sim_common.o
+OBJS += acados/sim/sim_gnsf.o
+# utils
+OBJS += acados/utils/math.o
+OBJS += acados/utils/print.o
+OBJS += acados/utils/timing.o
+OBJS += acados/utils/mem.o
+OBJS += acados/utils/external_function_generic.o
+
+# C interface
+ifeq ($(ACADOS_WITH_C_INTERFACE), 1)
+OBJS += interfaces/acados_c/external_function_interface.o
+OBJS += interfaces/acados_c/dense_qp_interface.o
+OBJS += interfaces/acados_c/ocp_nlp_interface.o
+OBJS += interfaces/acados_c/ocp_qp_interface.o
+OBJS += interfaces/acados_c/condensing_interface.o
+OBJS += interfaces/acados_c/sim_interface.o
+endif
+
+# acados dependencies
+STATIC_DEPS = blasfeo_static hpipm_static
+SHARED_DEPS = blasfeo_shared hpipm_shared
+CLEAN_DEPS = blasfeo_clean hpipm_clean
+ifeq ($(ACADOS_WITH_QPOASES), 1)
+STATIC_DEPS += qpoases_static
+SHARED_DEPS += qpoases_shared
+CLEAN_DEPS += qpoases_clean
+LINK_FLAG_QPOASES = -lqpOASES_e
+endif
+ifeq ($(ACADOS_WITH_HPMPC), 1)
+STATIC_DEPS += hpmpc_static
+CLEAN_DEPS += hpmpc_clean
+LINK_FLAG_HPMPC = -lhpmpc
+endif
+ifeq ($(ACADOS_WITH_QPDUNES), 1)
+STATIC_DEPS += qpdunes_static
+CLEAN_DEPS += qpdunes_clean
+LINK_FLAG_QPDUNES = -lqpdunes
+endif
+ifeq ($(ACADOS_WITH_QORE), 1)
+STATIC_DEPS += qore_static
+CLEAN_DEPS += qore_clean
+LINK_FLAG_QORE = -lqore
+endif
+ifeq ($(ACADOS_WITH_OSQP), 1)
+STATIC_DEPS += osqp_static
+SHARED_DEPS += osqp_shared
+CLEAN_DEPS += osqp_clean
+LINK_FLAG_OSQP = -losqp
+endif
+
+ifeq ($(ACADOS_WITH_OPENMP), 1)
+LINK_FLAG_OPENMP = -fopenmp
+endif
+
+
+deprecation_warning:
+	@echo
+	@echo
+	@echo "The Make build system of acados is not as well-tested as CMake."
+	@echo "Please consider using the CMake build system!"
+	@echo
+	@echo
+
+static_library: deprecation_warning $(STATIC_DEPS)
+	( cd acados; $(MAKE) obj TOP=$(TOP) )
+	( cd interfaces/acados_c; $(MAKE) obj CC=$(CC) TOP=$(TOP) )
+	ar rcs libacados.a $(OBJS)
+	mkdir -p lib
+	mv libacados.a lib
+	mkdir -p include/acados
+	cp --parents acados/*/*.h include/
+	mkdir -p include/acados_c
+	cp -r interfaces/acados_c/*.h include/acados_c
+	@echo
+	@echo " libacados.a static library build complete."
+	@echo
+
+shared_library: deprecation_warning link_libs_json $(SHARED_DEPS)
+	( cd acados; $(MAKE) obj TOP=$(TOP) )
+	( cd interfaces/acados_c; $(MAKE) obj  CC=$(CC) TOP=$(TOP) )
+	$(CC) -L./lib -shared -o libacados.so $(OBJS) -lblasfeo -lhpipm -lm -fopenmp
+	mkdir -p lib
+	mv libacados.so lib
+	mkdir -p include/acados
+	cp --parents acados/*/*.h include/
+	mkdir -p include/acados_c
+	cp -r interfaces/acados_c/*.h include/acados_c
+	@echo
+	@echo " libacados.so shared library build complete."
+	@echo
+
+# write linker flags to external libraries into lib/links.json, used in MEX interface
+link_libs_json:
+	echo "{" > ./lib/link_libs.json
+	echo "\t\"openmp\": \"$(LINK_FLAG_OPENMP)\"," >> ./lib/link_libs.json
+	echo "\t\"qpoases\": \"$(LINK_FLAG_QPOASES)\"," >> ./lib/link_libs.json
+	echo "\t\"qpdunes\": \"$(LINK_FLAG_QPDUNES)\"," >> ./lib/link_libs.json
+	echo "\t\"osqp\": \"$(LINK_FLAG_OSQP)\"," >> ./lib/link_libs.json
+	echo "\t\"hpmpc\": \"$(LINK_FLAG_HPMPC)\"," >> ./lib/link_libs.json
+	echo "\t\"ooqp\": \"$(LINK_FLAG_OOQP)\"" >> ./lib/link_libs.json
+	echo "}" >> ./lib/link_libs.json
+
+blasfeo_static: deprecation_warning
+	( cd $(BLASFEO_PATH); $(MAKE) static_library CC=$(CC) LA=$(BLASFEO_VERSION) TARGET=$(BLASFEO_TARGET) MF=PANELMAJ BLAS_API=0 )
+	mkdir -p include/blasfeo/include
+	mkdir -p lib
+	cp $(BLASFEO_PATH)/include/*.h include/blasfeo/include
+	cp $(BLASFEO_PATH)/lib/libblasfeo.a lib
+
+blasfeo_shared: deprecation_warning
+	( cd $(BLASFEO_PATH); $(MAKE) shared_library CC=$(CC) LA=$(BLASFEO_VERSION) TARGET=$(BLASFEO_TARGET) MF=PANELMAJ BLAS_API=0 )
+	mkdir -p include/blasfeo/include
+	mkdir -p lib
+	cp $(BLASFEO_PATH)/include/*.h include/blasfeo/include
+	cp $(BLASFEO_PATH)/lib/libblasfeo.so lib
+
+hpipm_static: deprecation_warning blasfeo_static
+	( cd $(HPIPM_PATH); $(MAKE) static_library CC=$(CC) TARGET=$(HPIPM_TARGET) BLASFEO_PATH=$(BLASFEO_PATH) )
+	mkdir -p include/hpipm/include
+	mkdir -p lib
+	cp $(HPIPM_PATH)/include/*.h include/hpipm/include
+	cp $(HPIPM_PATH)/lib/libhpipm.a lib
+
+hpipm_shared: deprecation_warning blasfeo_shared
+	( cd $(HPIPM_PATH); $(MAKE) shared_library CC=$(CC) TARGET=$(HPIPM_TARGET) BLASFEO_PATH=$(BLASFEO_PATH) )
+	mkdir -p include/hpipm/include
+	mkdir -p lib
+	cp $(HPIPM_PATH)/include/*.h include/hpipm/include
+	cp $(HPIPM_PATH)/lib/libhpipm.so lib
+
+hpmpc_static: blasfeo_static
+	( cd $(HPMPC_PATH); $(MAKE) static_library CC=$(CC) TARGET=$(HPMPC_TARGET) BLASFEO_PATH=$(BLASFEO_PATH)  )
+	mkdir -p include/hpmpc/include
+	mkdir -p lib
+	cp $(HPMPC_PATH)/include/*.h include/hpmpc/include
+	cp $(HPMPC_PATH)/libhpmpc.a lib
+
+qpoases_static:
+	( cd $(QPOASES_PATH); $(MAKE) CC=$(CC) )
+	mkdir -p include/qpoases/include
+	mkdir -p lib
+	cp -r $(QPOASES_PATH)/include/* include/qpoases/include
+	cp $(QPOASES_PATH)/bin/libqpOASES_e.a lib
+
+qpoases_shared:
+	( cd $(QPOASES_PATH); $(MAKE) CC=$(CC) MAKE_STATIC_LIB=0 DLLEXT=so )
+	mkdir -p include/qpoases/include
+	mkdir -p lib
+	cp -r $(QPOASES_PATH)/include/* include/qpoases/include
+	cp $(QPOASES_PATH)/bin/libqpOASES_e.so lib
+
+# TODO how is BLASFEO path set for QORE ?????
+qore_static: blasfeo_static
+	( cd $(QORE_PATH); $(MAKE) static_dense; )
+	mkdir -p include/qore/QPSOLVER_DENSE/include
+	mkdir -p include/qore/QPSOLVER_DENSE/source
+	mkdir -p include/qore/KKTPACK_DENSE/include
+	mkdir -p include/qore/KKTPACK_DENSE/source
+	mkdir -p include/qore/QORE/include
+	mkdir -p lib
+	cp $(QORE_PATH)/qp_types.h include/qore/
+	cp $(QORE_PATH)/QPSOLVER_DENSE/include/*.h include/qore/QPSOLVER_DENSE/include
+	cp $(QORE_PATH)/QPSOLVER_DENSE/source/*.h include/qore/QPSOLVER_DENSE/source
+	cp $(QORE_PATH)/KKTPACK_DENSE/source/*.h include/qore/KKTPACK_DENSE/source
+	cp $(QORE_PATH)/KKTPACK_DENSE/include/*.h include/qore/KKTPACK_DENSE/include
+	cp $(QORE_PATH)/QPCORE/include/*.h include/qore/QORE/include
+	cp $(QORE_PATH)/bin/libqore_dense.a lib
+
+qpdunes_static:
+	( cd $(QPDUNES_PATH); $(MAKE) CC=$(CC) )
+	mkdir -p include/qpdunes/include
+	mkdir -p lib
+	cp -r $(QPDUNES_PATH)/include/* include/qpdunes/include
+	cp $(QPDUNES_PATH)/src/libqpdunes.a lib
+	cp $(QPDUNES_PATH)/externals/qpOASES-3.0beta/bin/libqpOASES.a lib
+
+osqp_static: $(OSQP_LIB_STATIC)
+	mkdir -p include/osqp/include
+	mkdir -p lib
+	cp -r $(OSQP_PATH)/include/* include/osqp/include
+	mv libosqp.a lib
+
+osqp_shared: $(OSQP_LIB_SHARED)
+	mkdir -p include/osqp/include
+	mkdir -p lib
+	cp -r $(OSQP_PATH)/include/* include/osqp/include
+	mv libosqp.so lib
+
+examples_c: deprecation_warning static_library
+	( cd examples/c; $(MAKE) examples TOP=$(TOP) )
+
+run_examples_c: deprecation_warning examples_c
+	( cd examples/c; $(MAKE) run_examples )
+
+unit_tests: deprecation_warning static_library
+	( cd test; $(MAKE) unit_tests TOP=$(TOP) )
+
+run_unit_tests: unit_tests
+	( cd test; $(MAKE) run_unit_tests )
+
+clean:
+	( cd acados; $(MAKE) clean )
+	( cd examples/c; $(MAKE) clean )
+	( cd test; $(MAKE) clean )
+	( cd interfaces/acados_c; $(MAKE) clean )
+
+blasfeo_clean:
+	( cd $(BLASFEO_PATH); $(MAKE) clean )
+
+hpipm_clean:
+	( cd $(HPIPM_PATH); $(MAKE) clean )
+
+hpmpc_clean:
+	( cd $(HPMPC_PATH); $(MAKE) clean )
+
+qpoases_clean:
+	( cd $(QPOASES_PATH); $(MAKE) clean )
+
+qore_clean:
+	( cd $(QORE_PATH); $(MAKE) purge )
+
+qpdunes_clean:
+	( cd $(QPDUNES_PATH); $(MAKE) clean )
+
+osqp_clean:
+	@$(RM) $(OSQP_ALL_OBJ)
+	@$(RM) $(OSQP_QDLDL_INC_DIR)qdldl_types.h
+	@$(RM) $(OSQP_INC_DIR)osqp_configure.h
+
+deep_clean: clean $(CLEAN_DEPS)
+	( cd examples/c; $(MAKE) clean )
+
+clean_models:
+	( cd examples/c; $(MAKE) clean_models )
+
+purge: deep_clean clean_models
+	rm -rf include
+	rm -rf lib

Makefile.osqp

@@ -0,0 +1,202 @@
+#
+# Copyright (c) The acados authors.
+#
+# This file is part of acados.
+#
+# The 2-Clause BSD License
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions are met:
+#
+# 1. Redistributions of source code must retain the above copyright notice,
+# this list of conditions and the following disclaimer.
+#
+# 2. Redistributions in binary form must reproduce the above copyright notice,
+# this list of conditions and the following disclaimer in the documentation
+# and/or other materials provided with the distribution.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.;
+#
+
+
+OSQP_PRINTING = 1
+OSQP_DLONG = 1
+OSQP_DFLOAT = 0
+OSQP_ENABLE_MKL_PARDISO = 1
+
+OSQP_CFLAGS = -fPIC -O3
+
+# ==============================================================================
+#  dirs 
+# ==============================================================================
+
+OSQP_DIR = $(CURDIR)/external/osqp/
+
+OSQP_INC_DIR = $(OSQP_DIR)include/
+OSQP_SRC_DIR = $(OSQP_DIR)src/
+
+OSQP_LIN_SYS_DIR = $(OSQP_DIR)lin_sys/
+
+OSQP_PARDISO_DIR = $(OSQP_DIR)lin_sys/direct/pardiso/
+
+OSQP_QDLDL_DIR = $(OSQP_DIR)lin_sys/direct/qdldl/
+
+OSQP_QDLDL_INC_DIR = $(OSQP_QDLDL_DIR)qdldl_sources/include/
+OSQP_QDLDL_SRC_DIR = $(OSQP_QDLDL_DIR)qdldl_sources/src/
+
+OSQP_AMD_INC_DIR = $(OSQP_QDLDL_DIR)amd/include/
+OSQP_AMD_SRC_DIR = $(OSQP_QDLDL_DIR)amd/src/
+
+OSQP_EXAMPLES_DIR = $(OSQP_DIR)/examples/
+OSQP_EXAMPLES = $(OSQP_EXAMPLES_DIR)osqp_demo
+
+# ==============================================================================
+#  lib 
+# ==============================================================================
+
+OSQP_LIB_STATIC = libosqp.a
+OSQP_LIB_SHARED = libosqp.so
+			
+# ==============================================================================
+#  objs
+# ==============================================================================
+
+OSQP_QDLDL_OBJ = $(OSQP_QDLDL_SRC_DIR)qdldl.o
+
+OSQP_QDLDL_INTERFACE_OBJ = $(OSQP_QDLDL_DIR)qdldl_interface.o
+
+OSQP_AMD_OBJ = $(OSQP_AMD_SRC_DIR)amd_1.o \
+		  $(OSQP_AMD_SRC_DIR)amd_2.o \
+		  $(OSQP_AMD_SRC_DIR)amd_aat.o \
+		  $(OSQP_AMD_SRC_DIR)amd_control.o \
+		  $(OSQP_AMD_SRC_DIR)amd_defaults.o \
+		  $(OSQP_AMD_SRC_DIR)amd_info.o \
+		  $(OSQP_AMD_SRC_DIR)amd_order.o \
+		  $(OSQP_AMD_SRC_DIR)amd_post_tree.o \
+		  $(OSQP_AMD_SRC_DIR)amd_postorder.o \
+		  $(OSQP_AMD_SRC_DIR)amd_preprocess.o \
+		  $(OSQP_AMD_SRC_DIR)amd_valid.o \
+		  $(OSQP_AMD_SRC_DIR)SuiteSparse_config.o \
+		  
+OSQP_PARDISO_OBJ = $(OSQP_PARDISO_DIR)pardiso_interface.o \
+			  $(OSQP_PARDISO_DIR)pardiso_loader.o \
+			  
+OSQP_LIN_SYS_OBJ = $(OSQP_LIN_SYS_DIR)lib_handler.o \
+
+OSQP_OBJ = $(OSQP_SRC_DIR)auxil.o \
+           $(OSQP_SRC_DIR)cs.o \
+           $(OSQP_SRC_DIR)ctrlc.o \
+           $(OSQP_SRC_DIR)kkt.o \
+           $(OSQP_SRC_DIR)lin_alg.o \
+           $(OSQP_SRC_DIR)lin_sys.o \
+           $(OSQP_SRC_DIR)osqp.o \
+           $(OSQP_SRC_DIR)polish.o \
+           $(OSQP_SRC_DIR)proj.o \
+           $(OSQP_SRC_DIR)scaling.o \
+           $(OSQP_SRC_DIR)util.o \
+			  
+OSQP_ALL_OBJ = $(OSQP_QDLDL_OBJ) $(OSQP_AMD_OBJ) $(OSQP_QDLDL_INTERFACE_OBJ) $(OSQP_PARDISO_OBJ) $(OSQP_LIN_SYS_OBJ) $(OSQP_OBJ)
+
+
+
+# ==============================================================================
+#  OSQP make build system
+# ==============================================================================
+
+$(OSQP_LIB_STATIC): osqp_config $(OSQP_ALL_OBJ)
+	$(AR) rcs $@ $(OSQP_ALL_OBJ)
+
+$(OSQP_LIB_SHARED): osqp_config $(OSQP_ALL_OBJ)
+	$(CC) -shared -o $@ $(OSQP_ALL_OBJ)
+
+# compile all object files
+$(OSQP_QDLDL_SRC_DIR)%.o: $(OSQP_QDLDL_SRC_DIR)%.c
+	@echo "Compiling object file" $@
+	@$(CC) $(OSQP_CFLAGS) -I$(OSQP_QDLDL_INC_DIR) -c $< -o $@
+
+$(OSQP_AMD_SRC_DIR)%.o: $(OSQP_AMD_SRC_DIR)%.c
+	@echo "Compiling object file" $@
+	@$(CC) $(OSQP_CFLAGS) -I$(OSQP_AMD_INC_DIR) -I$(OSQP_INC_DIR) -c $< -o $@
+
+$(OSQP_QDLDL_DIR)%.o: $(OSQP_QDLDL_DIR)%.c
+	@echo "Compiling object file" $@
+	@$(CC) $(OSQP_CFLAGS) -I$(OSQP_QDLDL_DIR) -I$(OSQP_QDLDL_INC_DIR) -I$(OSQP_INC_DIR) -I$(OSQP_AMD_INC_DIR) -c $< -o $@
+
+$(OSQP_PARDISO_DIR)%.o: $(OSQP_PARDISO_DIR)%.c
+	@echo "Compiling object file" $@
+	@$(CC) $(OSQP_CFLAGS) -I$(OSQP_PARDISO_DIR) -I$(OSQP_INC_DIR) -I$(OSQP_LIN_SYS_DIR) -c $< -o $@
+
+$(OSQP_LIN_SYS_DIR)%.o: $(OSQP_LIN_SYS_DIR)%.c
+	@echo "Compiling object file" $@
+	@$(CC) $(OSQP_CFLAGS) -I$(OSQP_LIN_SYS_DIR) -I$(OSQP_INC_DIR) -I$(OSQP_QDLDL_INC_DIR) -c $< -o $@
+
+$(OSQP_SRC_DIR)%.o: $(OSQP_SRC_DIR)%.c
+	@echo "Compiling object file" $@
+	@$(CC) $(OSQP_CFLAGS) -I$(OSQP_INC_DIR) -I$(OSQP_QDLDL_DIR) -I$(OSQP_QDLDL_INC_DIR) -I$(OSQP_PARDISO_DIR) -c $< -o $@
+
+osqp_config:
+	@touch $(OSQP_QDLDL_INC_DIR)qdldl_types.h
+	@echo "#ifndef QDLDL_TYPES_H" > $(OSQP_QDLDL_INC_DIR)qdldl_types.h
+	@echo "# define QDLDL_TYPES_H" >> $(OSQP_QDLDL_INC_DIR)qdldl_types.h
+	@echo "# ifdef __cplusplus" >> $(OSQP_QDLDL_INC_DIR)qdldl_types.h
+	@echo "extern "C" {" >> $(OSQP_QDLDL_INC_DIR)qdldl_types.h
+	@echo "# endif /* ifdef __cplusplus */" >> $(OSQP_QDLDL_INC_DIR)qdldl_types.h
+	@echo "// QDLDL integer and float types" >> $(OSQP_QDLDL_INC_DIR)qdldl_types.h
+ifeq ($(OSQP_DLONG), 1)
+	@echo "typedef long long    QDLDL_int;   /* for indices */" >> $(OSQP_QDLDL_INC_DIR)qdldl_types.h
+else
+	@echo "typedef int    QDLDL_int;   /* for indices */" >> $(OSQP_QDLDL_INC_DIR)qdldl_types.h
+endif
+ifeq ($(OSQP_DFLOAT), 1)
+	@echo "typedef float  QDLDL_float; /* for numerical values  */" >> $(OSQP_QDLDL_INC_DIR)qdldl_types.h
+else
+	@echo "typedef double  QDLDL_float; /* for numerical values  */" >> $(OSQP_QDLDL_INC_DIR)qdldl_types.h
+endif
+	@echo "typedef unsigned char   QDLDL_bool;  /* for boolean values  */" >> $(OSQP_QDLDL_INC_DIR)qdldl_types.h
+	@echo "# ifdef __cplusplus" >> $(OSQP_QDLDL_INC_DIR)qdldl_types.h
+	@echo "}" >> $(OSQP_QDLDL_INC_DIR)qdldl_types.h
+	@echo "# endif /* ifdef __cplusplus */" >> $(OSQP_QDLDL_INC_DIR)qdldl_types.h
+	@echo "#endif /* ifndef QDLDL_TYPES_H */" >> $(OSQP_QDLDL_INC_DIR)qdldl_types.h
+	@touch $(OSQP_INC_DIR)osqp_configure.h
+	@echo "#ifndef OSQP_CONFIGURE_H" > $(OSQP_INC_DIR)osqp_configure.h
+	@echo "# define OSQP_CONFIGURE_H" >> $(OSQP_INC_DIR)osqp_configure.h
+	@echo "# ifdef __cplusplus" >> $(OSQP_INC_DIR)osqp_configure.h
+	@echo "extern "C" {" >> $(OSQP_INC_DIR)osqp_configure.h
+	@echo "# endif /* ifdef __cplusplus */" >> $(OSQP_INC_DIR)osqp_configure.h
+	@echo "/* Operative system */" >> $(OSQP_INC_DIR)osqp_configure.h
+ifeq ($(OS), LINUX)
+	@echo "#define IS_LINUX" >> $(OSQP_INC_DIR)osqp_configure.h
+endif
+ifeq ($(OS), MAC)
+	@echo "#define IS_MAC" >> $(OSQP_INC_DIR)osqp_configure.h
+endif
+ifeq ($(OS), WINDOWS)
+	@echo "#define IS_WINDOWS" >> $(OSQP_INC_DIR)osqp_configure.h
+endif
+ifeq ($(OSQP_PRINTING), 1)
+	@echo "/* PRINTING */" >> $(OSQP_INC_DIR)osqp_configure.h
+	@echo "#define PRINTING" >> $(OSQP_INC_DIR)osqp_configure.h
+endif
+ifeq ($(OSQP_DLONG), 1)
+	@echo "/* DLONG */" >> $(OSQP_INC_DIR)osqp_configure.h
+	@echo "#define DLONG" >> $(OSQP_INC_DIR)osqp_configure.h
+endif
+ifeq ($(OSQP_ENABLE_MKL_PARDISO),1)
+	@echo "/* ENABLE_MKL_PARDISO */" >> $(OSQP_INC_DIR)osqp_configure.h
+	@echo "#define ENABLE_MKL_PARDISO" >> $(OSQP_INC_DIR)osqp_configure.h
+endif
+	@echo "# ifdef __cplusplus" >> $(OSQP_INC_DIR)osqp_configure.h
+	@echo "}" >> $(OSQP_INC_DIR)osqp_configure.h
+	@echo "# endif /* ifdef __cplusplus */" >> $(OSQP_INC_DIR)osqp_configure.h
+	@echo "#endif /* ifndef OSQP_CONFIGURE_H */" >> $(OSQP_INC_DIR)osqp_configure.h
+

Makefile.rule

@@ -0,0 +1,175 @@
+#
+# Copyright (c) The acados authors.
+#
+# This file is part of acados.
+#
+# The 2-Clause BSD License
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions are met:
+#
+# 1. Redistributions of source code must retain the above copyright notice,
+# this list of conditions and the following disclaimer.
+#
+# 2. Redistributions in binary form must reproduce the above copyright notice,
+# this list of conditions and the following disclaimer in the documentation
+# and/or other materials provided with the distribution.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.;
+#
+
+
+CC = gcc
+#CC = clang
+#CC = x86_64-w64-mingw32-gcc
+
+# 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
+
+# Installation directory
+TOP = $(shell pwd)
+#PREFIX = /opt
+
+# External projects directory
+EXT_PATH = $(TOP)/external
+
+## BLASFEO path
+BLASFEO_PATH = $(EXT_PATH)/blasfeo
+#BLASFEO_PATH = /home/gianluca/blasfeo
+## BLASFEO version
+BLASFEO_VERSION = HIGH_PERFORMANCE
+# BLASFEO_VERSION = REFERENCE
+# BLASFEO_VERSION = BLAS_WRAPPER
+
+## BLASFEO target
+BLASFEO_TARGET = X64_INTEL_HASWELL
+# BLASFEO_TARGET = X64_INTEL_SANDY_BRIDGE
+# BLASFEO_TARGET = X64_INTEL_CORE
+#
+# BLASFEO_TARGET = X64_AMD_BULLDOZER
+# BLASFEO_TARGET = X86_AMD_JAGUAR
+# BLASFEO_TARGET = X86_AMD_BARCELONA
+#
+# BLASFEO_TARGET = ARMV8A_ARM_CORTEX_A57
+# BLASFEO_TARGET = ARMV8A_ARM_CORTEX_A53
+# BLASFEO_TARGET = ARMV7A_ARM_CORTEX_A15
+# BLASFEO_TARGET = ARMV7A_ARM_CORTEX_A7
+#
+# BLASFEO_TARGET = GENERIC
+
+## HPIPM path
+HPIPM_PATH = $(EXT_PATH)/hpipm
+#HPIPM_PATH = /home/gianluca/hpipm
+
+## HPIPM target
+HPIPM_TARGET = AVX
+# HPIPM_TARGET = GENERIC
+
+## HPMPC path
+HPMPC_PATH = $(EXT_PATH)/hpmpc
+## HPMPC target
+HPMPC_TARGET = X64_AVX
+
+## qpOASES path
+QPOASES_PATH = $(EXT_PATH)/qpoases
+
+# QORE path
+QORE_PATH = $(EXT_PATH)/qore
+
+# QPDUNES path
+QPDUNES_PATH = $(EXT_PATH)/qpdunes
+
+# OSQP path
+OSQP_PATH = $(EXT_PATH)/osqp
+
+# parallelize using openmp
+ACADOS_WITH_OPENMP = 0
+ACADOS_NUM_THREADS = 4
+
+# include QPOASES
+ACADOS_WITH_QPOASES = 0
+
+# include HPMPC
+ACADOS_WITH_HPMPC = 0
+
+# include QPDUNES
+ACADOS_WITH_QPDUNES = 0
+
+# include QORE
+ACADOS_WITH_QORE = 0
+
+# include OSQP
+ACADOS_WITH_OSQP = 0
+
+# C interface
+ACADOS_WITH_C_INTERFACE = 1
+
+# compiler flags
+CFLAGS =
+
+# Possible overide with local options
+mkfile_path := $(abspath $(lastword $(MAKEFILE_LIST)))
+current_dir := $(patsubst %/,%,$(dir $(mkfile_path)))
+# Do not complain if Makefile.local does not exisist
+-include $(current_dir)/Makefile.local
+
+# optimization flags
+CFLAGS += -O2 -fPIC
+
+# turn off assert
+#CFLAGS += -DNDEBUG
+
+# debugging flags
+CFLAGS  += #-g -Wall -pedantic -Wfloat-equal #-pg
+
+ifeq ($(ACADOS_WITH_OPENMP), 1)
+CFLAGS += -DACADOS_WITH_OPENMP -DACADOS_NUM_THREADS=$(ACADOS_NUM_THREADS) -fopenmp
+endif
+ifeq ($(ACADOS_WITH_QPOASES), 1)
+CFLAGS += -DACADOS_WITH_QPOASES
+endif
+ifeq ($(ACADOS_WITH_HPMPC), 1)
+CFLAGS += -DACADOS_WITH_HPMPC
+endif
+ifeq ($(ACADOS_WITH_QPDUNES), 1)
+CFLAGS += -DACADOS_WITH_QPDUNES
+endif
+ifeq ($(ACADOS_WITH_QORE), 1)
+CFLAGS += -DACADOS_WITH_QORE
+endif
+ifeq ($(ACADOS_WITH_OSQP), 1)
+CFLAGS += -DACADOS_WITH_OSQP
+endif
+ifeq ($(ACADOS_WITH_C_INTERFACE), 1)
+CFLAGS += -DACADOS_WITH_C_INTERFACE
+endif
+
+# TEMP
+#CFLAGS += -g -O0 -DRUNTIME_CHECKS -DEXT_DEPS -D__DEBUG__
+
+
+# search directories
+CFLAGS += -I$(TOP) -I$(TOP)/interfaces -I$(TOP)/include -I$(BLASFEO_PATH)/include -I$(HPIPM_PATH)/include -I$(HPMPC_PATH)/include -I$(QPOASES_PATH)/include -I$(TOP)/include/qore/include -I$(QPDUNES_PATH)/include -I$(OSQP_PATH)/include
+LDFLAGS += -L$(TOP)/lib
+
+CXXFLAGS = $(CFLAGS) -std=c++11
+CFLAGS += -std=c99

README.md

@@ -0,0 +1,30 @@
+<!-- # acados -->
+
+![](docs/_static/acados_logo.png)
+<!-- [![Travis Status](https://secure.travis-ci.org/acados/acados.png?branch=master)](http://travis-ci.org/acados/acados) -->
+[![Appveyor status](https://ci.appveyor.com/api/projects/status/q0b2nohk476u5clg?svg=true)](https://ci.appveyor.com/project/roversch/acados)
+![Github actions full build workflow](https://github.com/acados/acados/actions/workflows/full_build.yml/badge.svg)
+<!-- [![codecov](https://codecov.io/gh/acados/acados/branch/master/graph/badge.svg)](https://codecov.io/gh/acados/acados) -->
+
+`acados` provides fast and embedded solvers for nonlinear optimal control.
+It is written in `C` and offers interfaces to the programming languages `Python`, `MATLAB` and `Octave`.
+
+## General
+- `acados` implements
+  1. fast SQP-type solvers for Nonlinear Programming (NLP) formulations with an Optimal Control Problem (OCP) structure
+  2. efficient integration methods, also called *integrators*, to solve initial value problems with dynamic systems given as an ODE or index-1 DAE.
+  These integrators can efficiently compute first and second-order sensitivities of the results.
+
+## Documentation
+- Documentation can be found on [docs.acados.org](https://docs.acados.org/)
+- An overview of the interfaces can be found at [docs.acados.org/interfaces](https://docs.acados.org/interfaces)
+
+## Forum
+- If you have any `acados`-related questions, feel free to post on our forum at [discourse.acados.org](https://discourse.acados.org/)
+
+## Citing
+- References can be found at [docs.acados.org/citing](https://docs.acados.org/citing)
+
+## Installation
+- Instructions can be found at
+[docs.acados.org/installation](https://docs.acados.org/installation)

ROADMAP.md

@@ -0,0 +1,63 @@
+## Roadmap
+- [ ] Test 32 bit
+- [ ] Test OOQP
+- [ ] More flexible solution sensitivities?
+- [ ] get_optimal_value_hessian() - at least for HPIPM and exact Hessians
+- [ ] remove old layer in Matlab interface
+
+#### core
+- [x] propagate cost in integrator for NLS+IRK
+    - or: add support for quadrature state, separate dimension in integrator and OCP solver
+- [ ] faster workspace memory casting
+
+#### `ocp_nlp`
+- [ ] partial tightening
+- [x] RTI implementation similar to ACADO
+- [x] support cost on z for external, NLS
+
+#### `sim`
+- [ ] GNSF Hessians
+- [ ] propagate cost in integrator for CONL+IRK
+- [ ] time in integrator + time dependent model functions
+
+
+## DONE
+- [x] closed loop example MPC + MHE
+- [x] Templates: avoid global memory
+- [x] add support for manual model functions -- partly done: external cost and discrete dynamics
+
+#### `sim`
+- [x] collocation integrators Radau
+    - NOTE: currently always Gauss(-Legendre) Butcher tables
+        - A-stable, but not L-stable
+        - order is 2 * num_stages
+    - implement also Radau IIA collocation methods
+        - L-stable
+        - order is 2 * num_stages - 1
+
+#### C
+- [x] split ocp solve into prepare and feedback
+
+#### build
+- [x] cmake: add openmp parallelization
+
+#### matlab interface
+- [x] detect dimensions
+- [x] detect slack dimensions
+- [x] structure detections for constraints
+- [x] getting started folder
+- [x] add Mex templating support for: ( in prioritized order )
+    - [x] nonlinear least-squares
+    - [x] Vz
+    - [x] exact Hessian
+    - [x] external cost
+    - [x] GNSF
+    - [x] discrete dynamics
+- [x] separate `acados_ocp()` into generating the C object and setting the numerical data
+- [x] support nonuniform grids
+- [x] OCP with DAEs
+
+#### Python interface
+- [x] exact hessian
+- [x] regularization
+- [x] discrete dynamics

acados/CMakeLists.txt

@@ -0,0 +1,203 @@
+#
+# Copyright (c) The acados authors.
+#
+# This file is part of acados.
+#
+# The 2-Clause BSD License
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions are met:
+#
+# 1. Redistributions of source code must retain the above copyright notice,
+# this list of conditions and the following disclaimer.
+#
+# 2. Redistributions in binary form must reproduce the above copyright notice,
+# this list of conditions and the following disclaimer in the documentation
+# and/or other materials provided with the distribution.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.;
+#
+
+
+set(PACKAGE_SO_VERSION "0.1")
+
+include(CMakePackageConfigHelpers)
+
+# Build list with all source files to go into the acados library
+file(GLOB_RECURSE ACADOS_SRC "${PROJECT_SOURCE_DIR}/acados/*.c")
+
+# Exclude files not compatible with MSVC
+if(NOT ACADOS_WITH_HPMPC)
+    list(REMOVE_ITEM ACADOS_SRC "${PROJECT_SOURCE_DIR}/acados/ocp_qp/ocp_qp_hpmpc.c")
+endif()
+
+if(NOT ACADOS_WITH_OOQP)
+    list(REMOVE_ITEM ACADOS_SRC "${PROJECT_SOURCE_DIR}/acados/ocp_qp/ocp_qp_ooqp.c")
+    list(REMOVE_ITEM ACADOS_SRC "${PROJECT_SOURCE_DIR}/acados/dense_qp/dense_qp_ooqp.c")
+endif()
+
+if(NOT ACADOS_WITH_QORE)
+    list(REMOVE_ITEM ACADOS_SRC "${PROJECT_SOURCE_DIR}/acados/dense_qp/dense_qp_qore.c")
+endif()
+
+if(NOT ACADOS_WITH_QPOASES)
+    list(REMOVE_ITEM ACADOS_SRC "${PROJECT_SOURCE_DIR}/acados/dense_qp/dense_qp_qpoases.c")
+endif()
+
+if(NOT ACADOS_WITH_DAQP)
+    list(REMOVE_ITEM ACADOS_SRC "${PROJECT_SOURCE_DIR}/acados/dense_qp/dense_qp_daqp.c")
+endif()
+
+if(NOT ACADOS_WITH_QPDUNES)
+    list(REMOVE_ITEM ACADOS_SRC "${PROJECT_SOURCE_DIR}/acados/ocp_qp/ocp_qp_qpdunes.c")
+endif()
+
+if(NOT ACADOS_WITH_OSQP)
+    list(REMOVE_ITEM ACADOS_SRC "${PROJECT_SOURCE_DIR}/acados/ocp_qp/ocp_qp_osqp.c")
+endif()
+
+# Define acados library
+add_library(acados ${ACADOS_SRC})
+
+target_include_directories(acados PUBLIC
+        $<BUILD_INTERFACE:${PROJECT_SOURCE_DIR}> # to be able to include "acados/header.h"
+        $<BUILD_INTERFACE:${EXTERNAL_SRC_DIR}/hpipm/include>
+        $<BUILD_INTERFACE:${EXTERNAL_SRC_DIR}>
+        $<BUILD_INTERFACE:${PROJECT_SOURCE_DIR}/interfaces>
+        $<INSTALL_INTERFACE:include>)
+
+if(NOT ACADOS_WITH_SYSTEM_BLASFEO)
+    target_include_directories(acados PUBLIC
+        $<BUILD_INTERFACE:${EXTERNAL_SRC_DIR}/blasfeo/include>)
+endif()
+
+# OPENMP
+# find_package(OpenMP)
+if(ACADOS_WITH_OPENMP)
+    target_link_libraries(acados PUBLIC OpenMP::OpenMP_CXX)
+
+    target_compile_definitions(acados PUBLIC ACADOS_WITH_OPENMP)
+endif()
+
+# HPMPC must come before BLASFEO!
+if(ACADOS_WITH_HPMPC)
+    target_link_libraries(acados PUBLIC hpmpc)
+    target_compile_definitions(acados PUBLIC ACADOS_WITH_HPMPC)
+endif()
+
+if(ACADOS_WITH_QORE)
+    target_link_libraries(acados PUBLIC qore)
+
+    target_compile_definitions(acados PUBLIC ACADOS_WITH_QORE)
+
+    target_include_directories(acados PUBLIC
+        $<BUILD_INTERFACE:${EXTERNAL_SRC_DIR}/qore/QPSOLVER_DENSE/include>
+        $<BUILD_INTERFACE:${EXTERNAL_SRC_DIR}/qore/QPSOLVER_DENSE/source>
+        $<BUILD_INTERFACE:${EXTERNAL_SRC_DIR}/qore/KKTPACK_DENSE/include>
+        $<BUILD_INTERFACE:${EXTERNAL_SRC_DIR}/qore/KKTPACK_DENSE/source>
+        $<BUILD_INTERFACE:${EXTERNAL_SRC_DIR}/qore/QPCORE/include>
+        $<BUILD_INTERFACE:${EXTERNAL_SRC_DIR}/qore>
+    )
+endif()
+
+if(ACADOS_WITH_QPOASES)
+    target_link_libraries(acados PUBLIC qpOASES_e)
+
+    target_compile_definitions(acados PUBLIC ACADOS_WITH_QPOASES)
+    target_compile_definitions(acados PUBLIC USE_ACADOS_TYPES)
+endif()
+
+if(ACADOS_WITH_DAQP)
+    target_link_libraries(acados PUBLIC daqp)
+
+    target_compile_definitions(acados PUBLIC ACADOS_WITH_DAQP)
+endif()
+
+if(ACADOS_WITH_QPDUNES)
+    target_link_libraries(acados PUBLIC qpdunes)
+
+    target_compile_definitions(acados PUBLIC ACADOS_WITH_QPDUNES)
+    target_compile_definitions(acados PUBLIC USE_ACADOS_TYPES)
+endif()
+
+if(ACADOS_WITH_OSQP)
+    target_link_libraries(acados PUBLIC osqp)
+
+    target_compile_definitions(acados PUBLIC ACADOS_WITH_OSQP)
+    # TODO(dimitris): needed or not?
+    # target_compile_definitions(acados PUBLIC USE_ACADOS_TYPES)
+endif()
+
+if(ACADOS_WITH_OOQP)
+    target_compile_definitions(acados PUBLIC ACADOS_WITH_OOQP)
+    target_link_libraries(acados PUBLIC ooqp)
+endif()
+
+if(CMAKE_C_COMPILER_ID MATCHES MSVC) # no explicit math library
+    target_link_libraries(acados PUBLIC hpipm blasfeo)
+else() # add explicit math library
+    target_link_libraries(acados PUBLIC hpipm blasfeo m)
+endif()
+
+if(CMAKE_BUILD_TYPE MATCHES Debug)
+    target_compile_definitions(acados PRIVATE DEBUG)
+endif()
+
+if(EXT_DEP MATCHES OFF)
+    target_compile_definitions(acados PUBLIC BLASFEO_EXT_DEP_OFF)
+endif()
+
+# Only test acados library for coverage
+if(COVERAGE MATCHES "lcov")
+    include(CodeCoverage)
+    setup_target_for_coverage(acados_coverage ctest coverage)
+    set(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -O0 -fprofile-arcs -ftest-coverage")
+    set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -O0 -fprofile-arcs -ftest-coverage")
+    target_link_libraries(acados PRIVATE --coverage)
+endif()
+
+install(TARGETS acados EXPORT acadosTargets
+    LIBRARY DESTINATION lib
+    ARCHIVE DESTINATION lib
+    RUNTIME DESTINATION bin)
+
+install(DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}
+    DESTINATION include
+    FILES_MATCHING PATTERN "*.h"
+    PATTERN "simplified" EXCLUDE)
+
+install(EXPORT acadosTargets DESTINATION cmake)
+
+configure_package_config_file(${PROJECT_SOURCE_DIR}/cmake/acadosConfig.cmake.in
+                              ${CMAKE_CURRENT_BINARY_DIR}/cmake/acadosConfig.cmake
+                              INSTALL_DESTINATION cmake
+                              PATH_VARS ACADOS_WITH_HPMPC
+                                        ACADOS_WITH_QORE
+                                        ACADOS_WITH_QPOASES
+                                        ACADOS_WITH_DAQP
+                                        ACADOS_WITH_QPDUNES
+                                        ACADOS_WITH_OSQP
+                                        ACADOS_WITH_OOQP)
+
+install(FILES ${CMAKE_CURRENT_BINARY_DIR}/cmake/acadosConfig.cmake
+        DESTINATION cmake)
+
+install(
+  FILES ${PROJECT_SOURCE_DIR}/cmake/FindEigen3.cmake
+        ${PROJECT_SOURCE_DIR}/cmake/FindFortranLibs.cmake
+        ${PROJECT_SOURCE_DIR}/cmake/FindOpenBLAS.cmake
+  DESTINATION cmake)
+
+# set_target_properties(acados
+#     PROPERTIES SOVERSION ${PACKAGE_SO_VERSION})
+# export(TARGETS acados FILE ${PROJECT_BINARY_DIR}/acadosConfig.cmake)

acados/Makefile

@@ -0,0 +1,46 @@
+#
+# Copyright (c) The acados authors.
+#
+# This file is part of acados.
+#
+# The 2-Clause BSD License
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions are met:
+#
+# 1. Redistributions of source code must retain the above copyright notice,
+# this list of conditions and the following disclaimer.
+#
+# 2. Redistributions in binary form must reproduce the above copyright notice,
+# this list of conditions and the following disclaimer in the documentation
+# and/or other materials provided with the distribution.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.;
+#
+
+
+include ../Makefile.rule
+
+obj:
+	(cd ocp_nlp; $(MAKE) obj )
+	(cd dense_qp; $(MAKE) obj )
+	(cd ocp_qp; $(MAKE) obj )
+	(cd sim; $(MAKE) obj )
+	(cd utils; $(MAKE) obj )
+
+clean:
+	(cd ocp_nlp; $(MAKE) clean )
+	(cd dense_qp; $(MAKE) clean )
+	(cd ocp_qp; $(MAKE) clean )
+	(cd sim; $(MAKE) clean )
+	(cd utils; $(MAKE) clean )

acados/dense_qp/Makefile

@@ -0,0 +1,52 @@
+#
+# Copyright (c) The acados authors.
+#
+# This file is part of acados.
+#
+# The 2-Clause BSD License
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions are met:
+#
+# 1. Redistributions of source code must retain the above copyright notice,
+# this list of conditions and the following disclaimer.
+#
+# 2. Redistributions in binary form must reproduce the above copyright notice,
+# this list of conditions and the following disclaimer in the documentation
+# and/or other materials provided with the distribution.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.;
+#
+
+
+include ../../Makefile.rule
+
+OBJS =
+
+OBJS += dense_qp_common.o
+OBJS += dense_qp_hpipm.o
+ifeq ($(ACADOS_WITH_QPOASES), 1)
+OBJS += dense_qp_qpoases.o
+endif
+ifeq ($(ACADOS_WITH_DAQP), 1)
+OBJS += dense_qp_daqp.o
+endif
+ifeq ($(ACADOS_WITH_QORE), 1)
+OBJS += dense_qp_qore.o
+endif
+
+obj: $(OBJS)
+
+clean:
+	rm -f *.o
+	rm -f *.s

acados/dense_qp/dense_qp_common.c

@@ -0,0 +1,657 @@
+/*
+ * Copyright (c) The acados authors.
+ *
+ * This file is part of acados.
+ *
+ * The 2-Clause BSD License
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.;
+ */
+
+
+// external
+#include <stdlib.h>
+#include <stdio.h>
+#include <assert.h>
+#include <stddef.h>
+#include <string.h>
+
+// hpipm
+#include "hpipm/include/hpipm_d_dense_qp.h"
+#include "hpipm/include/hpipm_d_dense_qp_ipm.h"
+#include "hpipm/include/hpipm_d_dense_qp_kkt.h"
+#include "hpipm/include/hpipm_d_dense_qp_res.h"
+#include "hpipm/include/hpipm_d_dense_qp_sol.h"
+#include "hpipm/include/hpipm_d_dense_qp_dim.h"
+
+// blasfeo
+#include "blasfeo_d_aux.h"
+#include "blasfeo_d_aux_ext_dep.h"
+#include "blasfeo_d_blas.h"
+// acados
+#include "acados/dense_qp/dense_qp_common.h"
+#include "acados/utils/types.h"
+#include "acados/utils/mem.h"
+
+
+
+
+/************************************************
+ * config
+ ************************************************/
+
+acados_size_t dense_qp_solver_config_calculate_size()
+{
+    acados_size_t size = 0;
+
+    size += sizeof(qp_solver_config);
+
+    return size;
+}
+
+
+
+qp_solver_config *dense_qp_solver_config_assign(void *raw_memory)
+{
+    char *c_ptr = raw_memory;
+
+    qp_solver_config *config = (qp_solver_config *) c_ptr;
+    c_ptr += sizeof(qp_solver_config);
+
+    return config;
+}
+
+
+
+/************************************************
+ * dims
+ ************************************************/
+
+acados_size_t dense_qp_dims_calculate_size()
+{
+    acados_size_t size = sizeof(dense_qp_dims);
+
+    size += d_dense_qp_dim_memsize();
+
+    return size;
+}
+
+
+
+dense_qp_dims *dense_qp_dims_assign(void *raw_memory)
+{
+    char *c_ptr = (char *) raw_memory;
+
+    dense_qp_dims *dims = (dense_qp_dims *) c_ptr;
+    c_ptr += sizeof(dense_qp_dims);
+
+    d_dense_qp_dim_create(dims, c_ptr);
+    c_ptr += d_dense_qp_dim_memsize();
+
+    assert((char *) raw_memory + dense_qp_dims_calculate_size() == c_ptr);
+
+    return dims;
+}
+
+
+void dense_qp_dims_set(void *config_, void *dims_, const char *field, const int* value)
+{
+    // wrap hpipm function
+    // char field_copy[MAX_STR_LEN];
+    // strcpy(field_copy, field);
+    char *field_copy = (char *) field;
+
+    dense_qp_dims *dims = (dense_qp_dims *) dims_;
+
+    d_dense_qp_dim_set(field_copy, *value, dims);
+}
+
+
+
+/************************************************
+ * in
+ ************************************************/
+
+acados_size_t dense_qp_in_calculate_size(dense_qp_dims *dims)
+{
+    acados_size_t size = sizeof(dense_qp_in);
+    size += sizeof(dense_qp_dims);
+    size += d_dense_qp_memsize(dims);
+    size += 8;  // align for d_dense_qp
+    make_int_multiple_of(8, &size);
+
+    return size;
+}
+
+
+
+dense_qp_in *dense_qp_in_assign(dense_qp_dims *dims, void *raw_memory)
+{
+    char *c_ptr = (char *) raw_memory;
+
+    dense_qp_in *qp_in = (dense_qp_in *) c_ptr;
+    c_ptr += sizeof(dense_qp_in);
+
+    align_char_to(8, &c_ptr);
+    d_dense_qp_create(dims, qp_in, c_ptr);
+    c_ptr += d_dense_qp_memsize(dims);
+
+    qp_in->dim = (dense_qp_dims *) c_ptr;
+    c_ptr += sizeof(dense_qp_dims);
+
+    qp_in->dim->nv = dims->nv;
+    qp_in->dim->ne = dims->ne;
+    qp_in->dim->nb = dims->nb;
+    qp_in->dim->ng = dims->ng;
+    qp_in->dim->ns = dims->ns;
+    qp_in->dim->nsb = dims->nsb;
+    qp_in->dim->nsg = dims->nsg;
+
+    assert((char *) raw_memory + dense_qp_in_calculate_size(dims) >= c_ptr);
+
+    return qp_in;
+}
+
+
+
+/************************************************
+ * out
+ ************************************************/
+
+acados_size_t dense_qp_out_calculate_size(dense_qp_dims *dims)
+{
+    acados_size_t size = sizeof(dense_qp_out);
+    size += d_dense_qp_sol_memsize(dims);
+    size += sizeof(qp_info);
+    size += 8;
+    make_int_multiple_of(8, &size);
+
+    return size;
+}
+
+
+
+dense_qp_out *dense_qp_out_assign(dense_qp_dims *dims, void *raw_memory)
+{
+    char *c_ptr = (char *) raw_memory;
+
+    dense_qp_out *qp_out = (dense_qp_out *) c_ptr;
+    c_ptr += sizeof(dense_qp_out);
+    align_char_to(8, &c_ptr);
+
+    d_dense_qp_sol_create(dims, qp_out, c_ptr);
+    c_ptr += d_dense_qp_sol_memsize(dims);
+
+    qp_out->misc = (void *) c_ptr;
+    c_ptr += sizeof(qp_info);
+
+    assert((char *) raw_memory + dense_qp_out_calculate_size(dims) >= c_ptr);
+
+    return qp_out;
+}
+
+
+
+void dense_qp_out_get(dense_qp_out *out, const char *field, void *value)
+{
+    if(!strcmp(field, "qp_info"))
+    {
+        qp_info **ptr = value;
+        *ptr = out->misc;
+    }
+    else
+    {
+        printf("\nerror: dense_qp_out_get: field %s not available\n", field);
+        exit(1);
+    }
+
+    return;
+}
+
+
+
+/************************************************
+ * res
+ ************************************************/
+
+acados_size_t dense_qp_res_calculate_size(dense_qp_dims *dims)
+{
+    acados_size_t size = sizeof(dense_qp_res);
+    size += d_dense_qp_res_memsize(dims);
+
+    make_int_multiple_of(8, &size);
+    return size;
+}
+
+
+
+dense_qp_res *dense_qp_res_assign(dense_qp_dims *dims, void *raw_memory)
+{
+    char *c_ptr = (char *) raw_memory;
+
+    dense_qp_res *qp_res = (dense_qp_res *) c_ptr;
+    c_ptr += sizeof(dense_qp_res);
+
+    align_char_to(8, &c_ptr);
+    d_dense_qp_res_create(dims, qp_res, c_ptr);
+    c_ptr += d_dense_qp_res_memsize(dims);
+    assert((char *) raw_memory + dense_qp_res_calculate_size(dims) >= c_ptr);
+
+    return qp_res;
+}
+
+
+
+acados_size_t dense_qp_res_workspace_calculate_size(dense_qp_dims *dims)
+{
+    acados_size_t size = sizeof(dense_qp_res_ws);
+
+    size += d_dense_qp_res_ws_memsize(dims);
+    make_int_multiple_of(8, &size);
+
+    return size;
+}
+
+
+
+dense_qp_res_ws *dense_qp_res_workspace_assign(dense_qp_dims *dims, void *raw_memory)
+{
+    char *c_ptr = (char *) raw_memory;
+
+    dense_qp_res_ws *res_ws = (dense_qp_res_ws *) c_ptr;
+    c_ptr += sizeof(dense_qp_res_ws);
+
+    align_char_to(8, &c_ptr);
+
+    d_dense_qp_res_ws_create(dims, res_ws, c_ptr);
+    c_ptr += d_dense_qp_res_ws_memsize(dims);
+
+    assert((char *) raw_memory + dense_qp_res_workspace_calculate_size(dims) >= c_ptr);
+
+    return res_ws;
+}
+
+
+
+void dense_qp_compute_t(dense_qp_in *qp_in, dense_qp_out *qp_out)
+{
+    int nvd = qp_in->dim->nv;
+    // int ned = qp_in->dim->ne;
+    int nbd = qp_in->dim->nb;
+    int ngd = qp_in->dim->ng;
+    int nsd = qp_in->dim->ns;
+
+    int *idxb = qp_in->idxb;
+    int *idxs_rev = qp_in->idxs_rev;
+
+    int ii, idx;
+
+    // compute slacks for bounds
+    blasfeo_dvecex_sp(nbd, 1.0, idxb, qp_out->v, 0, qp_out->t, nbd+ngd);
+    blasfeo_daxpby(nbd, 1.0, qp_out->t, nbd+ngd, -1.0, qp_in->d, 0, qp_out->t, 0);
+    blasfeo_daxpby(nbd, -1.0, qp_out->t, nbd+ngd, -1.0, qp_in->d, nbd + ngd, qp_out->t, nbd + ngd);
+
+    // compute slacks for general constraints
+    blasfeo_dgemv_t(nvd, ngd, 1.0, qp_in->Ct, 0, 0, qp_out->v, 0, -1.0, qp_in->d, nbd, qp_out->t,
+                    nbd);
+    blasfeo_dgemv_t(nvd, ngd, -1.0, qp_in->Ct, 0, 0, qp_out->v, 0, -1.0, qp_in->d, 2 * nbd + ngd,
+                    qp_out->t, 2 * nbd + ngd);
+
+    // soft
+//    blasfeo_dvecad_sp(nsd, 1.0, qp_out->v, nvd, idxs, qp_out->t, 0);
+//    blasfeo_dvecad_sp(nsd, 1.0, qp_out->v, nvd+nsd, idxs, qp_out->t, nbd+ngd);
+    for(ii=0; ii<nbd+ngd; ii++)
+    {
+        idx = idxs_rev[ii];
+        if(idx!=-1)
+        {
+        BLASFEO_DVECEL(qp_out->t, ii)         += BLASFEO_DVECEL(qp_out->v, nvd+idx);
+        BLASFEO_DVECEL(qp_out->t, nbd+ngd+ii) += BLASFEO_DVECEL(qp_out->v, nvd+nsd+idx);
+        }
+    }
+    blasfeo_dveccp(2*nsd, qp_out->v, nvd, qp_out->t, 2*nbd+2*ngd);
+    blasfeo_daxpy(2*nsd, -1.0, qp_in->d, 2*nbd+2*ngd, qp_out->t, 2*nbd+2*ngd, qp_out->t,
+        2*nbd+2*ngd);
+
+}
+
+
+
+void dense_qp_res_compute(dense_qp_in *qp_in, dense_qp_out *qp_out, dense_qp_res *qp_res,
+                          dense_qp_res_ws *res_ws)
+{
+    qp_info *info = (qp_info *) qp_out->misc;
+
+    if (info->t_computed == 0)
+    {
+        dense_qp_compute_t(qp_in, qp_out);
+        info->t_computed = 1;
+    }
+
+    // compute residuals
+    d_dense_qp_res_compute(qp_in, qp_out, qp_res, res_ws);
+}
+
+
+
+void dense_qp_res_compute_nrm_inf(dense_qp_res *qp_res, double res[4])
+{
+    int nv = qp_res->dim->nv;
+    int nb = qp_res->dim->nb;
+    int ne = qp_res->dim->ne;
+    int ng = qp_res->dim->ng;
+    int ns = qp_res->dim->ns;
+
+    blasfeo_dvecnrm_inf(nv + 2 * ns, qp_res->res_g, 0, &res[0]);
+    blasfeo_dvecnrm_inf(ne, qp_res->res_b, 0, &res[1]);
+    blasfeo_dvecnrm_inf(2 * nb + 2 * ng + 2 * ns, qp_res->res_d, 0, &res[2]);
+    blasfeo_dvecnrm_inf(2 * nb + 2 * ng + 2 * ns, qp_res->res_m, 0, &res[3]);
+}
+
+
+
+void dense_qp_stack_slacks_dims(dense_qp_dims *in, dense_qp_dims *out)
+{
+    out->nv = in->nv + 2 * in->ns;
+    out->ne = in->ne;
+    out->nb = in->nb - in->nsb + 2 * in->ns;
+    out->ng = in->ns > 0 ? in->ng + in->nsb : in->ng;
+    out->ns = 0;
+    out->nsb = 0;
+    out->nsg = 0;
+}
+
+
+
+void dense_qp_stack_slacks(dense_qp_in *in, dense_qp_in *out)
+{
+    int nv = in->dim->nv;
+    int ne = in->dim->ne;
+    int nb = in->dim->nb;
+    int ng = in->dim->ng;
+    int ns = in->dim->ns;
+    int nsb = in->dim->nsb;
+    // int nsg = in->dim->nsg;
+    int *idxs_rev = in->idxs_rev;
+    int *idxb = in->idxb;
+
+    int nv2 = out->dim->nv;
+    int ne2 = out->dim->ne;
+    int nb2 = out->dim->nb;
+    int ng2 = out->dim->ng;
+
+    assert(nv2 == nv+2*ns && "Dimensions are wrong!");
+    assert(nb2 == nb-nsb+2*ns && "Dimensions are wrong!");
+    assert(ng2 == ng+nsb && "Dimensions are wrong!");
+
+    // set matrices to 0.0
+    blasfeo_dgese(nv2, nv2, 0.0, out->Hv, 0, 0);
+    blasfeo_dgese(ne2, nv2, 0.0, out->A, 0, 0);
+    blasfeo_dgese(nv2, ng2, 0.0, out->Ct, 0, 0);
+
+    // copy in->Hv to upper left corner of out->Hv, out->Hv = [in->Hv 0; 0 0]
+    blasfeo_dgecp(nv, nv, in->Hv, 0, 0, out->Hv, 0, 0);
+
+    // copy in->Z to main diagonal of out->Hv, out->Hv = [in->Hv 0; 0 Z]
+    blasfeo_ddiain(2 * ns, 1.0, in->Z, 0, out->Hv, nv, nv);
+
+    // copy in->gz to out->gz
+    blasfeo_dveccp(nv + 2 * ns, in->gz, 0, out->gz, 0);
+
+    if (ne > 0)
+    {
+        // copy in->A to out->A
+        blasfeo_dgecp(ne, nv, in->A, 0, 0, out->A, 0, 0);
+
+        // copy in->b to out->b
+        blasfeo_dveccp(ne, in->b, 0, out->b, 0);
+    }
+
+    // copy in->Ct to out->Ct, out->Ct = [in->Ct 0; 0 0]
+    blasfeo_dgecp(nv, ng, in->Ct, 0, 0, out->Ct, 0, 0);
+
+    if (ns > 0)
+    {
+        // set flags for non-softened box constraints
+        // use out->m temporarily for this
+        for (int ii = 0; ii < nb; ii++)
+        {
+            // TODO(bnovoselnik): pick up some workspace for this
+            BLASFEO_DVECEL(out->m, ii) = 1.0;
+        }
+
+        int col_b = ng;
+//        for (int ii = 0; ii < ns; ii++)
+//        {
+//            int js = idxs[ii];
+        for(int js=0; js<nb+ng; js++)
+        {
+            int ii = idxs_rev[js];
+            if(ii!=-1)
+            {
+
+                // int idx_v_ls0 = nv+ii;
+                // int idx_v_us0 = nv+ns+ii;
+                int idx_v_ls1 = nv+ii;
+                int idx_v_us1 = nv+ns+ii;
+
+                int idx_d_ls0 = js;
+                int idx_d_us0 = nb+ng+js;
+                int idx_d_ls1;
+                int idx_d_us1;
+
+                if (js < nb)  // soft box constraint
+                {
+                    // index of a soft box constraint
+                    int jv = idxb[js];
+
+                    idx_d_ls1 = nb2+col_b;
+                    idx_d_us1 = 2*nb2+ng2+col_b;
+
+                    // softened box constraint, set its flag to -1
+                    BLASFEO_DVECEL(out->m, js) = -1.0;
+
+                    // insert softened box constraint into out->Ct, lb_i <= x_i + sl_i - su_i <= ub_i
+                    BLASFEO_DMATEL(out->Ct, jv, col_b) = 1.0;
+                    BLASFEO_DMATEL(out->Ct, idx_v_ls1, col_b) = +1.0;
+                    BLASFEO_DMATEL(out->Ct, idx_v_us1, col_b) = -1.0;
+                    BLASFEO_DVECEL(out->d, idx_d_ls1) = BLASFEO_DVECEL(in->d, idx_d_ls0);
+                    BLASFEO_DVECEL(out->d, idx_d_us1) = -BLASFEO_DVECEL(in->d, idx_d_us0);
+
+                    col_b++;
+                }
+                else  // soft general constraint
+                {
+                    // index of a soft general constraint
+                    int col_g = js - nb;
+
+                    // soft general constraint, lg_i <= C_i x + sl_i - su_i <= ug_i
+                    BLASFEO_DMATEL(out->Ct, idx_v_ls1, col_g) = +1.0;
+                    BLASFEO_DMATEL(out->Ct, idx_v_us1, col_g) = -1.0;
+                }
+
+                // slack variables have box constraints
+                out->idxb[nb-nsb+ii] = ii + nv;
+                out->idxb[nb-nsb+ns+ii] = ii + nv + ns;
+            }
+        }
+
+        int k_nsb = 0;
+        for (int ii = 0; ii < nb; ii++)
+        {
+            if (BLASFEO_DVECEL(out->m, ii) > 0)
+            {
+                // copy nonsoftened box constraint bounds to out->d
+                BLASFEO_DVECEL(out->d, k_nsb) = BLASFEO_DVECEL(in->d, ii);
+                BLASFEO_DVECEL(out->d, nb2+ng2+k_nsb) = -BLASFEO_DVECEL(in->d, nb+ng+ii);
+                out->idxb[k_nsb] = ii;
+                k_nsb++;
+            }
+        }
+
+        assert(k_nsb == nb-nsb && "Dimensions are wrong!");
+
+        // copy ls and us to out->lb, jump over nonsoftened box constraints
+        blasfeo_dveccp(2*ns, in->d, 2*nb+2*ng, out->d, k_nsb);
+
+        // for slack variables out->ub is +INFTY
+        blasfeo_dvecse(2*ns, 1.0e6, out->d, nb2+ng2+k_nsb);
+
+        // copy in->lg to out->lg
+        blasfeo_dveccp(ng, in->d, nb, out->d, nb2);
+
+        // copy in->ug to out->ug
+        blasfeo_dveccpsc(ng, -1.0, in->d, 2*nb+ng, out->d, 2*nb2+ng2);
+
+        // flip signs for ub and ug
+        blasfeo_dvecsc(nb2+ng2, -1.0, out->d, nb2+ng2);
+
+        // set out->m to 0.0
+        blasfeo_dvecse(2*nb2+2*ng2, 0.0, out->m, 0);
+    }
+    else
+    {
+        blasfeo_dveccp(2*nb+2*ng, in->d, 0, out->d, 0);
+        blasfeo_dveccp(2*nb+2*ng, in->m, 0, out->m, 0);
+        for (int ii = 0; ii < nb; ii++) out->idxb[ii] = in->idxb[ii];
+    }
+}
+
+
+
+void dense_qp_unstack_slacks(dense_qp_out *in, dense_qp_in *qp_out, dense_qp_out *out)
+{
+    int nv = qp_out->dim->nv;
+    int ne = qp_out->dim->ne;
+    int nb = qp_out->dim->nb;
+    int ng = qp_out->dim->ng;
+    int ns = qp_out->dim->ns;
+    int nsb = qp_out->dim->nsb;
+    // int nsg = qp_out->dim->nsg;
+
+    int *idxs_rev = qp_out->idxs_rev;
+
+    int nv2 = in->dim->nv;
+    // int ne2 = in->dim->ne;
+    int nb2 = in->dim->nb;
+    int ng2 = in->dim->ng;
+
+    UNUSED(nsb);
+    UNUSED(nv2);
+
+    assert(nv2 == nv+2*ns && "Dimensions are wrong!");
+    assert(nb2 == nb-nsb+2*ns && "Dimensions are wrong!");
+    assert(ng2 == 2*ng+2*nsb && "Dimensions are wrong!");
+
+    // inequality constraints multipliers and slacks
+    if (ns > 0)
+    {
+
+        // set flags for non-softened box constraints
+        // use out->v temporarily for this
+        // XXX assume that nb<=nv
+        assert(nv+2*ns >= nb);
+
+        for (int ii = 0; ii < nb; ii++)
+        {
+            // TODO(bnovoselnik): pick up some workspace for this
+            BLASFEO_DVECEL(out->v, ii) = 1.0;
+        }
+
+        int col_b = 2*ng;
+//        for (int ii = 0; ii < ns; ii++)
+//        {
+//            int js = idxs[ii];
+        for(int js=0; js<nb+ng; js++)
+        {
+            int ii = idxs_rev[js];
+            if(ii!=-1)
+            {
+
+                int idx_d_ls0 = js;
+                int idx_d_us0 = nb+ng+js;
+                int idx_d_ls1;
+                int idx_d_us1;
+
+                if (js < nb)
+                {
+                    // softened box constraint, set its flag to -1
+                    BLASFEO_DVECEL(out->v, js) = -1.0;
+
+                    idx_d_ls1 = nb2+col_b;
+                    idx_d_us1 = 2*nb2+ng2+col_b;
+
+                    BLASFEO_DVECEL(out->lam, idx_d_ls0) = BLASFEO_DVECEL(in->lam, idx_d_ls1);
+                    BLASFEO_DVECEL(out->lam, idx_d_us0) = BLASFEO_DVECEL(in->lam, idx_d_us1);
+
+                    BLASFEO_DVECEL(out->t, idx_d_ls0) = BLASFEO_DVECEL(in->t, idx_d_ls1);
+                    BLASFEO_DVECEL(out->t, idx_d_us0) = BLASFEO_DVECEL(in->t, idx_d_us1);
+
+                    col_b++;
+                }
+            }
+        }
+
+        int k_nsb = 0;  // number of non-softed bounds
+        for (int ii = 0; ii < nb; ii++)
+        {
+            int idx_d_ls0 = ii;
+            int idx_d_us0 = nb+ng+ii;
+            int idx_d_ls1;
+            int idx_d_us1;
+
+            if (BLASFEO_DVECEL(out->v, ii) > 0)
+            {
+                idx_d_ls1 = k_nsb;
+                idx_d_us1 = nb2+ng2+k_nsb;
+
+                BLASFEO_DVECEL(out->lam, idx_d_ls0) = BLASFEO_DVECEL(in->lam, idx_d_ls1);
+                BLASFEO_DVECEL(out->lam, idx_d_us0) = BLASFEO_DVECEL(in->lam, idx_d_us1);
+
+                BLASFEO_DVECEL(out->t, idx_d_ls0) = BLASFEO_DVECEL(in->t, idx_d_ls1);
+                BLASFEO_DVECEL(out->t, idx_d_us0) = BLASFEO_DVECEL(in->t, idx_d_us1);
+
+                k_nsb++;
+            }
+        }
+
+        assert(k_nsb == nb-nsb && "Dimensions are wrong!");
+
+        blasfeo_dveccp(2*ns, in->t, k_nsb, out->t, 2*nb+2*ng);
+        blasfeo_dveccp(ng, in->t, 2*nb2+ng2, out->t, 2*nb+ng);
+        blasfeo_dveccp(ng, in->t, nb2, out->t, nb);
+
+        blasfeo_dveccp(2*ns, in->lam, k_nsb, out->lam, 2*nb+2*ng);
+        blasfeo_dveccp(ng, in->lam, 2*nb2+ng2, out->lam, 2*nb+ng);
+        blasfeo_dveccp(ng, in->lam, nb2, out->lam, nb);
+
+        // variables
+        blasfeo_dveccp(nv+2*ns, in->v, 0, out->v, 0);
+
+        // equality constraints multipliers
+        blasfeo_dveccp(ne, in->pi, 0, out->pi, 0);
+
+    }
+
+    return;
+}

acados/dense_qp/dense_qp_common.h

@@ -0,0 +1,152 @@
+/*
+ * Copyright (c) The acados authors.
+ *
+ * This file is part of acados.
+ *
+ * The 2-Clause BSD License
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.;
+ */
+
+
+#ifndef ACADOS_DENSE_QP_DENSE_QP_COMMON_H_
+#define ACADOS_DENSE_QP_DENSE_QP_COMMON_H_
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+// hpipm
+#include "hpipm/include/hpipm_d_dense_qp.h"
+#include "hpipm/include/hpipm_d_dense_qp_res.h"
+#include "hpipm/include/hpipm_d_dense_qp_sol.h"
+// acados
+#include "acados/utils/types.h"
+
+typedef struct d_dense_qp_dim dense_qp_dims;
+typedef struct d_dense_qp dense_qp_in;
+typedef struct d_dense_qp_sol dense_qp_out;
+typedef struct d_dense_qp_res dense_qp_res;
+typedef struct d_dense_qp_res_ws dense_qp_res_ws;
+
+
+
+#ifndef QP_SOLVER_CONFIG_
+#define QP_SOLVER_CONFIG_
+typedef struct
+{
+    void (*dims_set)(void *config_, void *dims_, const char *field, const int* value);
+    acados_size_t (*opts_calculate_size)(void *config, void *dims);
+    void *(*opts_assign)(void *config, void *dims, void *raw_memory);
+    void (*opts_initialize_default)(void *config, void *dims, void *args);
+    void (*opts_update)(void *config, void *dims, void *args);
+    void (*opts_set)(void *config_, void *opts_, const char *field, void* value);
+    void (*opts_get)(void *config_, void *opts_, const char *field, void* value);
+    acados_size_t (*memory_calculate_size)(void *config, void *dims, void *args);
+    void *(*memory_assign)(void *config, void *dims, void *args, void *raw_memory);
+    void (*memory_get)(void *config_, void *mem_, const char *field, void* value);
+    acados_size_t (*workspace_calculate_size)(void *config, void *dims, void *args);
+    int (*evaluate)(void *config, void *qp_in, void *qp_out, void *opts, void *mem, void *work);
+    void (*solver_get)(void *config_, void *qp_in_, void *qp_out_, void *opts_, void *mem_, const char *field, int stage, void* value, int size1, int size2);
+    void (*memory_reset)(void *config, void *qp_in, void *qp_out, void *opts, void *mem, void *work);
+    void (*eval_sens)(void *config, void *qp_in, void *qp_out, void *opts, void *mem, void *work);
+    void (*eval_adj_sens)(void *config, void *qp_in, void *qp_out, void *opts, void *mem, void *work);
+    void (*terminate)(void *config, void *mem, void *work);
+} qp_solver_config;
+#endif
+
+
+
+#ifndef QP_INFO_
+#define QP_INFO_
+typedef struct
+{
+    double solve_QP_time;
+    double condensing_time;
+    double interface_time;
+    double total_time;
+    int num_iter;
+    int t_computed;
+} qp_info;
+#endif
+
+
+
+/* config */
+//
+acados_size_t dense_qp_solver_config_calculate_size();
+//
+qp_solver_config *dense_qp_solver_config_assign(void *raw_memory);
+
+/* dims */
+//
+acados_size_t dense_qp_dims_calculate_size();
+//
+dense_qp_dims *dense_qp_dims_assign(void *raw_memory);
+//
+void dense_qp_dims_set(void *config_, void *dims_, const char *field, const int* value);
+//
+
+/* in */
+//
+acados_size_t dense_qp_in_calculate_size(dense_qp_dims *dims);
+//
+dense_qp_in *dense_qp_in_assign(dense_qp_dims *dims, void *raw_memory);
+
+/* out */
+//
+acados_size_t dense_qp_out_calculate_size(dense_qp_dims *dims);
+//
+dense_qp_out *dense_qp_out_assign(dense_qp_dims *dims, void *raw_memory);
+//
+void dense_qp_out_get(dense_qp_out *out, const char *field, void *value);
+
+/* res */
+//
+acados_size_t dense_qp_res_calculate_size(dense_qp_dims *dims);
+//
+dense_qp_res *dense_qp_res_assign(dense_qp_dims *dims, void *raw_memory);
+//
+acados_size_t dense_qp_res_workspace_calculate_size(dense_qp_dims *dims);
+//
+dense_qp_res_ws *dense_qp_res_workspace_assign(dense_qp_dims *dims, void *raw_memory);
+//
+void dense_qp_compute_t(dense_qp_in *qp_in, dense_qp_out *qp_out);
+//
+void dense_qp_res_compute(dense_qp_in *qp_in, dense_qp_out *qp_out, dense_qp_res *qp_res, dense_qp_res_ws *res_ws);
+//
+void dense_qp_res_compute_nrm_inf(dense_qp_res *qp_res, double res[4]);
+
+/* misc */
+//
+void dense_qp_stack_slacks_dims(dense_qp_dims *in, dense_qp_dims *out);
+//
+void dense_qp_stack_slacks(dense_qp_in *in, dense_qp_in *out);
+//
+void dense_qp_unstack_slacks(dense_qp_out *in, dense_qp_in *qp_out, dense_qp_out *out);
+
+#ifdef __cplusplus
+} /* extern "C" */
+#endif
+
+#endif  // ACADOS_DENSE_QP_DENSE_QP_COMMON_H_

acados/dense_qp/dense_qp_daqp.c

@@ -0,0 +1,845 @@
+/*
+ * Copyright (c) The acados authors.
+ *
+ * This file is part of acados.
+ *
+ * The 2-Clause BSD License
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.;
+ */
+
+
+// external
+#include <stdlib.h>
+#include <assert.h>
+#include <string.h>
+#include <stdio.h>
+// blasfeo
+#include "blasfeo_d_aux.h"
+#include "blasfeo_d_blas.h"
+
+// daqp
+#define SOFT_WEIGHTS
+#include "daqp/include/types.h"
+#include "daqp/include/api.h"
+#include "daqp/include/daqp.h"
+#include "daqp/include/utils.h"
+
+// acados
+#include "acados/dense_qp/dense_qp_common.h"
+#include "acados/dense_qp/dense_qp_daqp.h"
+#include "acados/utils/mem.h"
+#include "acados/utils/timing.h"
+#include "acados/utils/print.h"
+#include "acados/utils/math.h"
+
+#include "acados_c/dense_qp_interface.h"
+
+
+/************************************************
+ * opts
+ ************************************************/
+
+acados_size_t dense_qp_daqp_opts_calculate_size(void *config_, dense_qp_dims *dims)
+{
+    acados_size_t size = 0;
+    size += sizeof(dense_qp_daqp_opts);
+    size += sizeof(DAQPSettings);
+
+    make_int_multiple_of(8, &size);
+
+    return size;
+}
+
+
+
+void *dense_qp_daqp_opts_assign(void *config_, dense_qp_dims *dims, void *raw_memory)
+{
+    dense_qp_daqp_opts *opts;
+
+    char *c_ptr = (char *) raw_memory;
+
+    opts = (dense_qp_daqp_opts *) c_ptr;
+    c_ptr += sizeof(dense_qp_daqp_opts);
+
+    opts->daqp_opts = (DAQPSettings *) c_ptr;
+    c_ptr += sizeof(DAQPSettings);
+
+    assert((char *) raw_memory + dense_qp_daqp_opts_calculate_size(config_, dims) >= c_ptr);
+
+    return (void *) opts;
+}
+
+
+
+void dense_qp_daqp_opts_initialize_default(void *config_, dense_qp_dims *dims, void *opts_)
+{
+    dense_qp_daqp_opts *opts = (dense_qp_daqp_opts *) opts_;
+    daqp_default_settings(opts->daqp_opts);
+    opts->warm_start=1;
+    return;
+}
+
+
+
+void dense_qp_daqp_opts_update(void *config_, dense_qp_dims *dims, void *opts_)
+{
+    return;
+}
+
+
+
+void dense_qp_daqp_opts_set(void *config_, void *opts_, const char *field, void *value)
+{
+    dense_qp_daqp_opts *opts = opts_;
+    if (!strcmp(field, "tol_stat"))
+    {
+        // DAQP always "aims" at a stationary point
+    }
+    else if (!strcmp(field, "tol_eq"))
+    {
+        // Equality constraints are explicitly
+        // handled by the working set
+    }
+    else if (!strcmp(field, "tol_ineq"))
+    {
+        double *tol = value;
+        opts->daqp_opts->primal_tol = *tol;
+    }
+    else if (!strcmp(field, "tol_comp"))
+    {
+        // Complementary slackness is implicitly
+        // handled by the working set
+    }
+    else if (!strcmp(field, "iter_max"))
+    {
+        int *iter_max= value;
+        opts->daqp_opts->iter_limit = *iter_max;
+    }
+    else if (!strcmp(field, "warm_start"))
+    {
+        int *warm_start = value;
+        opts->warm_start = *warm_start;
+    }
+    else
+    {
+        printf("\nerror: dense_qp_daqp_opts_set: wrong field: %s\n", field);
+        exit(1);
+    }
+
+    return;
+}
+
+void dense_qp_daqp_opts_get(void *config_, void *opts_, const char *field, void *value)
+{
+    // dense_qp_daqp_opts *opts = opts_;
+    printf("\nerror: dense_qp_daqp_opts_get: not implemented for field: %s\n", field);
+    exit(1);
+}
+
+
+
+/************************************************
+ * memory
+ ************************************************/
+
+static acados_size_t daqp_workspace_calculate_size(int n, int m, int ms, int ns)
+{
+    acados_size_t size = 0;
+
+    size += sizeof(DAQPWorkspace);
+    size += sizeof(DAQPProblem);
+
+    size += n * n * sizeof(c_float); // H
+    size += 1 * n * sizeof(c_float); // f
+    size += n * (m-ms) * sizeof(c_float); // A
+    size += 2 * m * sizeof(c_float); // bupper/blower
+
+    size += n * (m-ms) * sizeof(c_float); // M
+    size += 2 * m * sizeof(c_float); // dupper/dlower
+    size += (n+1)*n/2 * sizeof(c_float); // Rinv
+    size += n * sizeof(c_float); // v
+    size += m * sizeof(c_float); // scaling
+
+    size += 2 * n * sizeof(c_float); // x & xold
+    size += 2*(n+ns+1) * sizeof(c_float); // lam & lam_star
+    size += n * sizeof(c_float); // u
+
+    size += (n+ns+2)*(n+ns+1)/2 * sizeof(c_float); // L
+    size += (n+ns+1) * sizeof(c_float); // D
+
+    size += 2*(n+ns+1) * sizeof(c_float); //xldl & zldl
+
+    size += 4 * m * sizeof(c_float); // d_ls, d_us,  rho_ls, rho_us
+
+    size += m * sizeof(int); // work->sense
+    size += (n+ns+1) * sizeof(int); // WS
+
+    make_int_multiple_of(8, &size);
+
+    return size;
+}
+
+
+acados_size_t dense_qp_daqp_memory_calculate_size(void *config_, dense_qp_dims *dims, void *opts_)
+{
+    int n = dims->nv;
+    int m = dims->nv + dims->ng + dims->ne;
+    int ms = dims->nv;
+    int nb = dims->nb;
+    int ns = dims->ns;
+
+    acados_size_t size = sizeof(dense_qp_daqp_memory);
+
+    size += daqp_workspace_calculate_size(n, m, ms, ns);
+
+    size += nb * 2 * sizeof(c_float); // lb_tmp & ub_tmp
+    size += nb * 1 * sizeof(int); // idbx
+    size += n *  1 * sizeof(int); // idxv_to_idxb;
+    size += ns * 1 * sizeof(int); // idbs
+    size += m  * 1 * sizeof(int); // idxdaqp_to_idxs;
+
+    size += ns * 6 * sizeof(c_float); // Zl,Zu,zl,zu,d_ls,d_us
+    make_int_multiple_of(8, &size);
+
+    return size;
+}
+
+
+static void *daqp_workspace_assign(int n, int m, int ms, int ns, void *raw_memory)
+{
+    DAQPWorkspace *work;
+    char *c_ptr = (char *) raw_memory;
+
+    work = (DAQPWorkspace *) c_ptr;
+    c_ptr += sizeof(DAQPWorkspace);
+
+    work->qp = (DAQPProblem *) c_ptr;
+    c_ptr += sizeof(DAQPProblem);
+
+    align_char_to(8, &c_ptr);
+
+    // double
+    work->qp->H = (c_float*) c_ptr;
+    c_ptr += n * n * sizeof(c_float);
+
+    work->qp->f = (c_float*) c_ptr;
+    c_ptr += 1 * n * sizeof(c_float);
+
+    work->qp->A = (c_float*) c_ptr;
+    c_ptr += n * (m-ms) * sizeof(c_float);
+
+    work->qp->bupper = (c_float*) c_ptr;
+    c_ptr += 1 * m * sizeof(c_float);
+
+    work->qp->blower = (c_float*) c_ptr;
+    c_ptr += 1 * m * sizeof(c_float);
+
+    work->M = (c_float *) c_ptr;
+    c_ptr += n * (m - ms) * sizeof(c_float);
+
+    work->dupper = (c_float *) c_ptr;
+    c_ptr += 1 * m * sizeof(c_float);
+
+    work->dlower = (c_float *) c_ptr;
+    c_ptr += 1 * m * sizeof(c_float);
+
+    work->Rinv = (c_float *) c_ptr;
+    c_ptr += (n + 1) * n / 2 * sizeof(c_float);
+
+    work->v = (c_float *) c_ptr;
+    c_ptr += n * sizeof(c_float);
+
+    work->scaling = (c_float *) c_ptr;
+    c_ptr += m * sizeof(c_float);
+
+    work->x = (c_float *) c_ptr;
+    c_ptr += n * sizeof(c_float);
+
+    work->xold = (c_float *) c_ptr;
+    c_ptr += n * sizeof(c_float);
+
+    work->lam = (c_float *) c_ptr;
+    c_ptr += (n+ns+1) * sizeof(c_float);
+
+    work->lam_star = (c_float *) c_ptr;
+    c_ptr += (n+ns+1) * sizeof(c_float);
+
+    work->u = (c_float *) c_ptr;
+    c_ptr += n * sizeof(c_float);
+
+    work->D = (c_float *) c_ptr;
+    c_ptr += (n+ns+1) * sizeof(c_float);
+
+    work->xldl = (c_float *) c_ptr;
+    c_ptr += (n+ns+1) * sizeof(c_float);
+
+    work->zldl = (c_float *) c_ptr;
+    c_ptr += (n+ns+1) * sizeof(c_float);
+
+    work->L = (c_float *) c_ptr;
+    c_ptr += (n+ns+2)*(n+ns+1)/2 * sizeof(c_float);
+
+    work->d_ls = (c_float *) c_ptr;
+    c_ptr += m * sizeof(c_float);
+
+    work->d_us = (c_float *) c_ptr;
+    c_ptr += m * sizeof(c_float);
+
+    work->rho_ls = (c_float *) c_ptr;
+    c_ptr += m * sizeof(c_float);
+
+    work->rho_us = (c_float *) c_ptr;
+    c_ptr += m * sizeof(c_float);
+
+    // ints
+    work->sense = (int *) c_ptr;
+    c_ptr += m * sizeof(int);
+
+    work->WS= (int *) c_ptr;
+    c_ptr += (n+ns+1) * sizeof(int);
+
+    // Initialize constants of workspace
+    work->qp->nb = 0;
+    work->qp->bin_ids = NULL;
+
+    work->n = n;
+    work->m = m;
+    work->ms = ms;
+    work->fval = -1;
+    work->n_active = 0;
+    work->iterations = 0;
+    work->sing_ind  = 0;
+    work->soft_slack = 0;
+
+    work->bnb = NULL; // No need to solve MIQP
+
+    // initialize d_ls, d_us and sense
+    for (int ii=0; ii<m; ii++)
+    {
+        work->d_ls[ii] = 0;
+        work->d_us[ii] = 0;
+        work->sense[ii] = 0;
+    }
+
+    return work;
+}
+
+
+void *dense_qp_daqp_memory_assign(void *config_, dense_qp_dims *dims, void *opts_,
+                                     void *raw_memory)
+{
+    dense_qp_daqp_memory *mem;
+
+    int n = dims->nv;
+    int m = dims->nv + dims->ng + dims->ne;
+    int ms = dims->nv;
+    int nb = dims->nb;
+    int ns = dims->ns;
+
+    // char pointer
+    char *c_ptr = (char *) raw_memory;
+
+    mem = (dense_qp_daqp_memory *) c_ptr;
+    c_ptr += sizeof(dense_qp_daqp_memory);
+
+    assert((size_t) c_ptr % 8 == 0 && "memory not 8-byte aligned!");
+
+    // Assign raw memory to workspace
+    mem->daqp_work = daqp_workspace_assign(n, m, ms, ns, c_ptr);
+    c_ptr += daqp_workspace_calculate_size(n, m, ms, ns);
+
+    assert((size_t) c_ptr % 8 == 0 && "double not 8-byte aligned!");
+
+    mem->lb_tmp = (c_float *) c_ptr;
+    c_ptr += nb * 1 * sizeof(c_float);
+
+    mem->ub_tmp = (c_float *) c_ptr;
+    c_ptr += nb * 1 * sizeof(c_float);
+
+    mem->idxb = (int *) c_ptr;
+    c_ptr += nb * 1 * sizeof(int);
+
+    mem->idxv_to_idxb = (int *) c_ptr;
+    c_ptr += n * 1 * sizeof(int);
+
+    mem->idxs= (int *) c_ptr;
+    c_ptr += ns * 1 * sizeof(int);
+
+    mem->idxdaqp_to_idxs = (int *) c_ptr;
+    c_ptr += m * 1 * sizeof(int);
+
+    mem->Zl = (c_float *) c_ptr;
+    c_ptr += ns * 1 * sizeof(c_float);
+
+    mem->Zu = (c_float *) c_ptr;
+    c_ptr += ns * 1 * sizeof(c_float);
+
+    mem->zl = (c_float *) c_ptr;
+    c_ptr += ns * 1 * sizeof(c_float);
+
+    mem->zu = (c_float *) c_ptr;
+    c_ptr += ns * 1 * sizeof(c_float);
+
+    mem->d_ls = (c_float *) c_ptr;
+    c_ptr += ns * 1 * sizeof(c_float);
+
+    mem->d_us = (c_float *) c_ptr;
+    c_ptr += ns * 1 * sizeof(c_float);
+
+    assert((char *) raw_memory + dense_qp_daqp_memory_calculate_size(config_, dims, opts_) >=
+           c_ptr);
+
+    return mem;
+}
+
+
+
+void dense_qp_daqp_memory_get(void *config_, void *mem_, const char *field, void* value)
+{
+    // qp_solver_config *config = config_;
+    dense_qp_daqp_memory *mem = mem_;
+
+    if (!strcmp(field, "time_qp_solver_call"))
+    {
+        double *tmp_ptr = value;
+        *tmp_ptr = mem->time_qp_solver_call;
+    }
+    else if (!strcmp(field, "iter"))
+    {
+        int *tmp_ptr = value;
+        *tmp_ptr = mem->iter;
+    }
+    else
+    {
+        printf("\nerror: dense_qp_daqp_memory_get: field %s not available\n", field);
+        exit(1);
+    }
+
+    return;
+
+}
+
+/************************************************
+ * workspace
+ ************************************************/
+
+acados_size_t dense_qp_daqp_workspace_calculate_size(void *config_, dense_qp_dims *dims, void *opts_)
+{
+    return 0;
+}
+
+
+/************************************************
+ * functions
+ ************************************************/
+
+// NOTE on transcription of acados dense QP into DAQP formulation:
+
+
+// DAQP constraints are: [bounds on ALL x; linear constraints (ng); equality constraints (ne)]
+
+// A_DAQP = [C; A]
+// blower = [lower bounds on ALL x (-INF if not set); lg; b_eq]
+// bupper = [upper bounds on ALL x (+INF if not set); ug; b_eq]
+
+
+
+static void dense_qp_daqp_update_memory(dense_qp_in *qp_in, const dense_qp_daqp_opts *opts, dense_qp_daqp_memory *mem)
+{
+    // extract dense qp size
+    DAQPWorkspace * work = mem->daqp_work;
+    int nv = qp_in->dim->nv;
+    int nb = qp_in->dim->nb;
+    int ns = qp_in->dim->ns;
+    int ng = qp_in->dim->ng;
+    int ne = qp_in->dim->ne;
+
+    // extract daqp data
+    double *lb_tmp = mem->lb_tmp;
+    double *ub_tmp = mem->ub_tmp;
+    int *idxb = mem->idxb;
+    int *idxs = mem->idxs;
+
+    // fill in the upper triangular of H in dense_qp
+    blasfeo_dtrtr_l(nv, qp_in->Hv, 0, 0, qp_in->Hv, 0, 0);
+
+    // extract data from qp_in in row-major
+    d_dense_qp_get_all_rowmaj(qp_in, work->qp->H, work->qp->f,  // objective
+        work->qp->A+nv*ng, work->qp->bupper+nv+ng,  // equalities
+        idxb, lb_tmp, ub_tmp,  // bounds
+        work->qp->A, work->qp->blower+nv, work->qp->bupper+nv,  // general linear constraints
+        mem->Zl, mem->Zu, mem->zl, mem->zu, idxs, mem->d_ls, mem->d_us  // slacks
+    );
+
+    // printf("\nDAQP: matrix A\n");
+    // int m = qp_in->dim->nv + qp_in->dim->ng + qp_in->dim->ne;
+    // int ms = qp_in->dim->nv;
+    // d_print_exp_tran_mat(nv, m-ms, work->qp->A, nv);
+
+    // "Unignore" all general linear inequalites (ng)
+    for (int ii = nv; ii < nv+ng; ii++)
+        SET_MUTABLE(ii);
+
+    // Setup upper/lower bounds
+    for (int ii = 0; ii < nv; ii++)
+    {
+        // "ignore" bounds that are not in acados dense QP
+        work->qp->blower[ii] = -DAQP_INF;
+        work->qp->bupper[ii] = +DAQP_INF;
+        SET_IMMUTABLE(ii);
+    }
+    for (int ii = 0; ii < nb; ii++)
+    {
+        // "Unignore" bounds that are in acados dense QP and set bound values
+        work->qp->blower[idxb[ii]] = lb_tmp[ii];
+        work->qp->bupper[idxb[ii]] = ub_tmp[ii];
+        SET_MUTABLE(idxb[ii]);
+        mem->idxv_to_idxb[idxb[ii]] = ii;
+    }
+
+    // printf("DAQP: dmask\n");
+    // blasfeo_print_tran_dvec(2*(nb+ng), qp_in->d_mask, 0);
+    for (int ii = 0; ii < nb; ii++)
+    {
+        // NOTE: DAQP always works with double sided constraints, so currently one can not only ignore the upper bound or the lower bound.
+
+        // "ignore" bounds that are marked as unconstrained in qp_in via dmask
+        if (BLASFEO_DVECEL(qp_in->d_mask, ii) == 0.0)
+        {
+            work->qp->blower[idxb[ii]] = -DAQP_INF;
+        }
+        if (BLASFEO_DVECEL(qp_in->d_mask, ii+ng+nb) == 0.0)
+        {
+            work->qp->bupper[idxb[ii]] = +DAQP_INF;
+        }
+    }
+    // ignore some general linear constraints.
+    for (int ii = 0; ii < ng; ii++)
+    {
+        if (BLASFEO_DVECEL(qp_in->d_mask, nb+ii) == 0.0)
+        {
+            work->qp->blower[ii+nv] = -DAQP_INF;
+        }
+        if (BLASFEO_DVECEL(qp_in->d_mask, 2*nb+ng+ii) == 0.0)
+        {
+            work->qp->bupper[ii+nv] = +DAQP_INF;
+        }
+    }
+
+
+
+    // Mark equality constraints
+    for (int ii = 0; ii < ne; ii++)
+    {
+        // NOTE: b_eq values are ONLY in bupper, but sense status is default upper, thus fine.
+        work->sense[nv+ng+ii] &= ACTIVE+IMMUTABLE;
+        // SET_ACTIVE(nv+ng+ii);
+        // SET_IMMUTABLE(nv+ng+ii);
+    }
+
+    // Soft constraints
+    int idxdaqp;  // index of soft constraint within DAQP ordering
+    for (int ii = 0; ii < ns; ii++)
+    {
+        idxdaqp = idxs[ii] < nb ? idxb[idxs[ii]] : nv+idxs[ii]-nb;
+        mem->idxdaqp_to_idxs[idxdaqp] = ii;
+
+        SET_SOFT(idxdaqp);
+
+        // Quadratic slack penalty needs to be nonzero in DAQP
+        mem->Zl[ii] = MAX(1e-8,mem->Zl[ii]);
+        mem->Zu[ii] = MAX(1e-8,mem->Zu[ii]);
+
+        // Setup soft weight used in DAQP
+        work->rho_ls[idxdaqp] = 1/mem->Zl[ii];
+        work->rho_us[idxdaqp] = 1/mem->Zu[ii];
+
+        // Shift QP to handle linear terms on slack
+        // DAQP penalizes soft slacks s using a quadratic penalty s' s, bounded by s >= d_l
+        // (instead of weighting the soft slacks in the objective as in acados,
+        // the soft slacks are weighted in the constraint by rho=1/Z).
+        // To remove the linear term from acados we use the transformation
+        //             s_daqp = (Z*s_acados+z/Z),
+        // which will shift blower/bupper and scale the nominal slack bounds with 1/Z
+        work->qp->blower[idxdaqp]+=mem->zl[ii]/mem->Zl[ii];
+        work->qp->bupper[idxdaqp]-=mem->zu[ii]/mem->Zu[ii];
+
+        work->d_ls[idxdaqp] = MAX(0,mem->zl[ii]+mem->Zl[ii]*mem->d_ls[ii]);
+        work->d_us[idxdaqp] = MAX(0,mem->zu[ii]+mem->Zu[ii]*mem->d_us[ii]);
+
+        // The default state in DAQP is that the soft slacks are active at their bounds
+        // => shift bupper/blower with these bounds
+        work->qp->blower[idxdaqp] -= work->d_ls[idxdaqp]/mem->Zl[ii];
+        work->qp->bupper[idxdaqp] += work->d_us[idxdaqp]/mem->Zu[ii];
+    }
+}
+
+
+
+static void dense_qp_daqp_fill_output(dense_qp_daqp_memory *mem, const dense_qp_out *qp_out, const dense_qp_in *qp_in)
+{
+    int *idxv_to_idxb = mem->idxv_to_idxb;
+    int *idxs = mem->idxs;
+    int *idxb = mem->idxb;
+    int i;
+    int nv = qp_in->dim->nv;
+    int nb = qp_in->dim->nb;
+    int ng = qp_in->dim->ng;
+    int ns = qp_in->dim->ns;
+    DAQPWorkspace *work = mem->daqp_work;
+
+    struct blasfeo_dvec *v = qp_out->v;
+    struct blasfeo_dvec *lambda = qp_out->lam;
+
+    // print DAQP solution before expansion:
+    // printf("\n\nDAQP solution\n");
+    // printf("------------------\n");
+    // printf("\nx (primals):\n\n");
+    // for (i = 0; i<nv; i++)
+    //     printf("%e\t", work->x[i]);
+    // printf("\nlambda (duals):\n\n");
+    // for (i = 0; i<work->n_active; i++)
+    //     printf("%e\t", work->lam_star[i]);
+    // printf("\n\n");
+
+    // primal variables
+    blasfeo_pack_dvec(nv, work->x, 1, v, 0);
+
+
+    // dual variables
+    blasfeo_dvecse(2 * nb + 2 * ng + 2 * ns, 0.0, lambda, 0);
+    c_float lam;
+    for (i = 0; i < work->n_active; i++)
+    {
+        lam = work->lam_star[i];
+        if (work->WS[i] < nv) // bound constraint
+        {
+            if (lam >= 0.0)
+                BLASFEO_DVECEL(lambda, nb+ng+idxv_to_idxb[work->WS[i]]) = lam;
+            else
+                BLASFEO_DVECEL(lambda, idxv_to_idxb[work->WS[i]]) = -lam;
+        }
+        else if (work->WS[i] < nv+ng)// general constraint
+        {
+            if (lam >= 0.0)
+                BLASFEO_DVECEL(lambda, 2*nb+ng+work->WS[i]-nv) = lam;
+            else
+                BLASFEO_DVECEL(lambda, nb+work->WS[i]-nv) = -lam;
+        }
+        else // equality constraint
+            BLASFEO_DVECEL(qp_out->pi, work->WS[i]-nv-ng) = lam;
+    }
+
+    // soft slacks
+    int idx;
+    for (i = 0; i < ns; i++)
+    {
+        idx = idxs[i] < nb ? idxb[idxs[i]] : nv+idxs[i]-nb;
+        // shift back QP
+        work->qp->blower[idx]-=(mem->zl[i]-work->d_ls[idx]/work->scaling[idx])/mem->Zl[i];
+        work->qp->bupper[idx]+=(mem->zu[i]-work->d_us[idx]/work->scaling[idx])/mem->Zu[i];
+
+        // lower
+        if (BLASFEO_DVECEL(lambda, idxs[i]) == 0) // inactive soft => active slack bound
+        {
+            BLASFEO_DVECEL(v, nv+i) = mem->d_ls[i];
+            BLASFEO_DVECEL(lambda, 2*nb+2*ng+i) = mem->d_ls[i]/mem->Zl[i]+mem->zl[i];
+        }
+        else
+        { // if soft active => compute slack directly from equality
+            BLASFEO_DVECEL(v, nv+i) = work->qp->blower[idx];
+            if (idx<nv)
+                BLASFEO_DVECEL(v, nv+i) -= BLASFEO_DVECEL(v, idx);
+            else
+            { // general constraint
+                for (int j=0, disp = (idx-nv)*nv; j < nv; j++, disp++)
+                {
+                    BLASFEO_DVECEL(v, nv+i) -= work->qp->A[disp] * BLASFEO_DVECEL(v, j);
+                }
+            }
+            // compute dual variable from stationarity condition
+            BLASFEO_DVECEL(lambda, 2*(nb+ng)+i) = mem->Zl[i] * BLASFEO_DVECEL(v, nv+i) + mem->zl[i]
+                - BLASFEO_DVECEL(lambda, idxs[i]);
+        }
+
+        // upper
+        if (BLASFEO_DVECEL(lambda, idxs[i]+nb+ng) == 0) // inactive soft => active slack bound
+        {
+            BLASFEO_DVECEL(v, nv+ns+i) = mem->d_us[i];
+            BLASFEO_DVECEL(lambda, 2*nb+2*ng+ns+i) = mem->d_us[i]/mem->Zu[i]+mem->zu[i];
+        }
+        else
+        { // if soft active => compute slack directly from equality
+            BLASFEO_DVECEL(v, nv+ns+i) = -work->qp->bupper[idx];
+            if (idx<nv)
+                BLASFEO_DVECEL(v, nv+ns+i) += BLASFEO_DVECEL(v, idx);
+            else
+            { // general constraint
+                for (int j=0, disp = (idx-nv)*nv; j < nv; j++, disp++)
+                    BLASFEO_DVECEL(v, nv+ns+i) += work->qp->A[disp] * BLASFEO_DVECEL(v, j);
+            }
+            // compute dual variable from stationarity condition
+            BLASFEO_DVECEL(lambda, 2*(nb+ng)+ns+i) = mem->Zu[i] * BLASFEO_DVECEL(v, nv+ns+i) + mem->zu[i]
+                - BLASFEO_DVECEL(lambda, idxs[i]+nb+ng);
+        }
+    }
+}
+
+
+
+int dense_qp_daqp(void* config_, dense_qp_in *qp_in, dense_qp_out *qp_out, void *opts_, void *memory_, void *work_)
+{
+    qp_info *info = (qp_info *) qp_out->misc;
+    acados_timer tot_timer, qp_timer, interface_timer;
+
+    // Uncomment to print dense QPs to file for solution with matlab;
+    // d_dense_qp_codegen_matlab("dense_qp_for_matlab.m", "a", qp_in->dim, qp_in);
+
+    acados_tic(&tot_timer);
+    acados_tic(&interface_timer);
+
+    // cast structures
+    dense_qp_daqp_opts *opts = (dense_qp_daqp_opts *) opts_;
+    dense_qp_daqp_memory *memory = (dense_qp_daqp_memory *) memory_;
+
+    // Move data into daqp workspace
+    dense_qp_daqp_update_memory(qp_in,opts,memory);
+    info->interface_time = acados_toc(&interface_timer);
+
+    // Extract workspace and update settings
+    DAQPWorkspace* work = memory->daqp_work;
+    work->settings = opts->daqp_opts;
+
+    // === Solve starts ===
+    acados_tic(&qp_timer);
+    if (opts->warm_start==0) deactivate_constraints(work);
+    // setup LDP
+    int update_mask,daqp_status;
+    update_mask= (opts->warm_start==2) ?
+        UPDATE_v+UPDATE_d: UPDATE_Rinv+UPDATE_M+UPDATE_v+UPDATE_d;
+    daqp_status = update_ldp(update_mask,work);
+    // if setup failed, abort
+    if(daqp_status < 0)
+        return daqp_status;
+    // solve LDP
+    if (opts->warm_start==1)
+        activate_constraints(work);
+
+    // TODO: shift active set? - not in SQP but would be nice as an option in SQP_RTI.
+
+    daqp_status = daqp_ldp(memory->daqp_work);
+    ldp2qp_solution(work);
+
+    // extract primal and dual solution
+    dense_qp_daqp_fill_output(memory,qp_out,qp_in);
+    info->solve_QP_time = acados_toc(&qp_timer);
+
+    acados_tic(&interface_timer);
+
+    // compute slacks
+    dense_qp_compute_t(qp_in, qp_out);
+    info->t_computed = 1;
+
+    // log solve info
+    info->interface_time += acados_toc(&interface_timer);
+    info->total_time = acados_toc(&tot_timer);
+    info->num_iter = memory->daqp_work->iterations;
+    memory->time_qp_solver_call = info->solve_QP_time;
+    memory->iter = memory->daqp_work->iterations;
+
+    // status
+    int acados_status = daqp_status;
+    if (daqp_status == EXIT_OPTIMAL || daqp_status == EXIT_SOFT_OPTIMAL)
+        acados_status = ACADOS_SUCCESS;
+    else if (daqp_status == EXIT_ITERLIMIT)
+        acados_status = ACADOS_MAXITER;
+    // NOTE: There are also:
+    // EXIT_INFEASIBLE, EXIT_CYCLE, EXIT_UNBOUNDED, EXIT_NONCONVEX, EXIT_OVERDETERMINED_INITIAL
+
+    return acados_status;
+}
+
+
+void dense_qp_daqp_eval_sens(void *config_, void *qp_in, void *qp_out, void *opts_, void *mem_, void *work_)
+{
+    printf("\nerror: dense_qp_daqp_eval_sens: not implemented yet\n");
+    exit(1);
+}
+
+
+void dense_qp_daqp_eval_adj_sens(void *config_, void *qp_in, void *qp_out, void *opts_, void *mem_, void *work_)
+{
+    printf("\nerror: dense_qp_daqp_eval_adj_sens: not implemented yet\n");
+    exit(1);
+}
+
+void dense_qp_daqp_memory_reset(void *config, void *qp_in, void *qp_out, void *opts, void *mem, void *work)
+{
+    printf("\nerror: dense_qp_daqp_memory_reset: not implemented yet\n");
+    exit(1);
+}
+
+void dense_qp_daqp_solver_get(void *config_, void *qp_in_, void *qp_out_, void *opts_, void *mem_, const char *field, int stage, void* value, int size1, int size2)
+{
+    printf("\nerror: dense_qp_daqp_solver_get: not implemented yet\n");
+    exit(1);
+}
+
+
+
+void dense_qp_daqp_terminate(void *config_, void *mem_, void *work_)
+{
+    return;
+}
+
+
+
+
+void dense_qp_daqp_config_initialize_default(void *config_)
+{
+    qp_solver_config *config = config_;
+
+    config->opts_calculate_size = (acados_size_t (*)(void *, void *)) & dense_qp_daqp_opts_calculate_size;
+    config->opts_assign = (void *(*) (void *, void *, void *) ) & dense_qp_daqp_opts_assign;
+    config->opts_initialize_default =
+        (void (*)(void *, void *, void *)) & dense_qp_daqp_opts_initialize_default;
+    config->opts_update = (void (*)(void *, void *, void *)) & dense_qp_daqp_opts_update;
+    config->opts_set = &dense_qp_daqp_opts_set;
+    config->opts_get = &dense_qp_daqp_opts_get;
+    config->memory_calculate_size =
+        (acados_size_t (*)(void *, void *, void *)) & dense_qp_daqp_memory_calculate_size;
+    config->memory_assign =
+        (void *(*) (void *, void *, void *, void *) ) & dense_qp_daqp_memory_assign;
+    config->memory_get = &dense_qp_daqp_memory_get;
+    config->workspace_calculate_size =
+        (acados_size_t (*)(void *, void *, void *)) & dense_qp_daqp_workspace_calculate_size;
+    config->eval_sens = &dense_qp_daqp_eval_sens;
+    config->eval_adj_sens = &dense_qp_daqp_eval_adj_sens;
+    config->evaluate = (int (*)(void *, void *, void *, void *, void *, void *)) & dense_qp_daqp;
+    config->memory_reset = &dense_qp_daqp_memory_reset;
+    config->solver_get = &dense_qp_daqp_solver_get;
+    config->terminate = &dense_qp_daqp_terminate;
+
+    return;
+}

acados/dense_qp/dense_qp_daqp.h

@@ -0,0 +1,115 @@
+/*
+ * Copyright (c) The acados authors.
+ *
+ * This file is part of acados.
+ *
+ * The 2-Clause BSD License
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.;
+ */
+
+
+#ifndef ACADOS_DENSE_QP_DENSE_QP_DAQP_H_
+#define ACADOS_DENSE_QP_DENSE_QP_DAQP_H_
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+// blasfeo
+#include "blasfeo_common.h"
+
+// daqp
+#include "daqp/include/types.h"
+
+// acados
+#include "acados/dense_qp/dense_qp_common.h"
+#include "acados/utils/types.h"
+
+
+typedef struct dense_qp_daqp_opts_
+{
+    DAQPSettings* daqp_opts;
+    int warm_start;
+} dense_qp_daqp_opts;
+
+
+typedef struct dense_qp_daqp_memory_
+{
+    double* lb_tmp;
+    double* ub_tmp;
+    int* idxb;
+    int* idxv_to_idxb;
+    int* idxs;
+    int* idxdaqp_to_idxs;
+
+    double* Zl;
+    double* Zu;
+    double* zl;
+    double* zu;
+    double* d_ls;
+    double* d_us;
+
+    double time_qp_solver_call;
+    int iter;
+    DAQPWorkspace * daqp_work;
+
+} dense_qp_daqp_memory;
+
+// opts
+acados_size_t dense_qp_daqp_opts_calculate_size(void *config, dense_qp_dims *dims);
+//
+void *dense_qp_daqp_opts_assign(void *config, dense_qp_dims *dims, void *raw_memory);
+//
+void dense_qp_daqp_opts_initialize_default(void *config, dense_qp_dims *dims, void *opts_);
+//
+void dense_qp_daqp_opts_update(void *config, dense_qp_dims *dims, void *opts_);
+//
+// memory
+acados_size_t dense_qp_daqp_workspace_calculate_size(void *config, dense_qp_dims *dims, void *opts_);
+//
+void *dense_qp_daqp_workspace_assign(void *config, dense_qp_dims *dims, void *raw_memory);
+//
+acados_size_t dense_qp_daqp_memory_calculate_size(void *config, dense_qp_dims *dims, void *opts_);
+//
+void *dense_qp_daqp_memory_assign(void *config, dense_qp_dims *dims, void *opts_, void *raw_memory);
+//
+// functions
+int dense_qp_daqp(void *config, dense_qp_in *qp_in, dense_qp_out *qp_out, void *opts_, void *memory_, void *work_);
+//
+void dense_qp_daqp_eval_sens(void *config_, void *qp_in, void *qp_out, void *opts_, void *mem_, void *work_);
+//
+void dense_qp_daqp_memory_reset(void *config_, void *qp_in, void *qp_out, void *opts_, void *mem_, void *work_);
+//
+void dense_qp_daqp_config_initialize_default(void *config_);
+//
+void dense_qp_daqp_memory_reset(void *config, void *qp_in, void *qp_out, void *opts, void *mem, void *work);
+//
+void dense_qp_daqp_solver_get(void *config_, void *qp_in_, void *qp_out_, void *opts_, void *mem_, const char *field, int stage, void* value, int size1, int size2);
+
+
+#ifdef __cplusplus
+} /* extern "C" */
+#endif
+
+#endif  // ACADOS_DENSE_QP_DENSE_QP_DAQP_H_

acados/dense_qp/dense_qp_hpipm.c

@@ -0,0 +1,394 @@
+/*
+ * Copyright (c) The acados authors.
+ *
+ * This file is part of acados.
+ *
+ * The 2-Clause BSD License
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.;
+ */
+
+
+// external
+#include <stdlib.h>
+#include <assert.h>
+#include <string.h>
+// hpipm
+#include "hpipm/include/hpipm_d_dense_qp.h"
+#include "hpipm/include/hpipm_d_dense_qp_ipm.h"
+#include "hpipm/include/hpipm_d_dense_qp_sol.h"
+// acados
+#include "acados/dense_qp/dense_qp_common.h"
+#include "acados/dense_qp/dense_qp_hpipm.h"
+#include "acados/utils/mem.h"
+#include "acados/utils/timing.h"
+
+
+
+/************************************************
+ * opts
+ ************************************************/
+
+acados_size_t dense_qp_hpipm_opts_calculate_size(void *config_, void *dims_)
+{
+    dense_qp_dims *dims = dims_;
+
+    acados_size_t size = 0;
+    size += sizeof(dense_qp_hpipm_opts);
+    size += sizeof(struct d_dense_qp_ipm_arg);
+    size += 8;  // align for d_dense_qp_ipm_arg
+    size += d_dense_qp_ipm_arg_memsize(dims);
+
+    make_int_multiple_of(8, &size);
+
+    return size;
+}
+
+
+
+void *dense_qp_hpipm_opts_assign(void *config_, void *dims_, void *raw_memory)
+{
+    dense_qp_dims *dims = dims_;
+    dense_qp_hpipm_opts *opts;
+
+    char *c_ptr = (char *) raw_memory;
+
+    opts = (dense_qp_hpipm_opts *) c_ptr;
+    c_ptr += sizeof(dense_qp_hpipm_opts);
+
+    opts->hpipm_opts = (struct d_dense_qp_ipm_arg *) c_ptr;
+    c_ptr += sizeof(struct d_dense_qp_ipm_arg);
+
+    align_char_to(8, &c_ptr);
+
+    d_dense_qp_ipm_arg_create(dims, opts->hpipm_opts, c_ptr);
+    c_ptr += d_dense_qp_ipm_arg_memsize(dims);
+
+    assert((char *) raw_memory + dense_qp_hpipm_opts_calculate_size(config_, dims) >= c_ptr);
+
+    return (void *) opts;
+}
+
+static void dense_qp_hpipm_opts_overwrite_mode_opts(dense_qp_hpipm_opts *opts)
+{
+    // overwrite some default options
+    opts->hpipm_opts->res_g_max = 1e-6;
+    opts->hpipm_opts->res_b_max = 1e-8;
+    opts->hpipm_opts->res_d_max = 1e-8;
+    opts->hpipm_opts->res_m_max = 1e-8;
+    opts->hpipm_opts->iter_max = 50;
+    opts->hpipm_opts->stat_max = 50;
+    opts->hpipm_opts->alpha_min = 1e-8;
+    opts->hpipm_opts->mu0 = 1e0;
+}
+
+
+void dense_qp_hpipm_opts_initialize_default(void *config_, void *dims_, void *opts_)
+{
+    dense_qp_hpipm_opts *opts = opts_;
+
+    d_dense_qp_ipm_arg_set_default(BALANCE, opts->hpipm_opts);
+    dense_qp_hpipm_opts_overwrite_mode_opts(opts);
+
+    opts->print_level = 0;
+
+    return;
+}
+
+
+
+void dense_qp_hpipm_opts_update(void *config_, void *dims_, void *opts_)
+{
+    //    dense_qp_hpipm_opts *opts = (dense_qp_hpipm_opts *)opts_;
+
+    return;
+}
+
+
+void dense_qp_hpipm_opts_set(void *config_, void *opts_, const char *field, void *value)
+{
+    dense_qp_hpipm_opts *opts = opts_;
+
+    const char *mode;
+    if (!strcmp(field, "hpipm_mode"))
+    {
+        mode = (const char *) value;
+        if (!strcmp(mode, "BALANCE"))
+            d_dense_qp_ipm_arg_set_default(BALANCE, opts->hpipm_opts);
+        else if (!strcmp(mode, "SPEED"))
+            d_dense_qp_ipm_arg_set_default(SPEED, opts->hpipm_opts);
+        else if (!strcmp(mode, "SPEED_ABS"))
+            d_dense_qp_ipm_arg_set_default(SPEED_ABS, opts->hpipm_opts);
+        else if (!strcmp(mode, "ROBUST"))
+            d_dense_qp_ipm_arg_set_default(ROBUST, opts->hpipm_opts);
+
+        dense_qp_hpipm_opts_overwrite_mode_opts(opts);
+
+    }
+    else if (!strcmp(field, "print_level"))
+    {
+        int* print_level = (int *) value;
+        opts->print_level = *print_level;
+    }
+    else
+    {
+        d_dense_qp_ipm_arg_set((char *) field, value, opts->hpipm_opts);
+    }
+
+
+    return;
+}
+
+
+void dense_qp_hpipm_opts_get(void *config_, void *opts_, const char *field, void *value)
+{
+    dense_qp_hpipm_opts *opts = opts_;
+
+    d_dense_qp_ipm_arg_get((char *) field, opts->hpipm_opts, value);
+
+    return;
+}
+
+
+
+/************************************************
+ * memory
+ ************************************************/
+
+acados_size_t dense_qp_hpipm_memory_calculate_size(void *config_, void *dims_, void *opts_)
+{
+    dense_qp_dims *dims = dims_;
+    dense_qp_hpipm_opts *opts = opts_;
+
+    acados_size_t size = 0;
+    size += sizeof(dense_qp_hpipm_memory);
+    size += sizeof(struct d_dense_qp_ipm_ws);
+
+    size += d_dense_qp_ipm_ws_memsize(dims, opts->hpipm_opts);
+    size += 8;  // align d_dense_qp_ipm_ws
+    make_int_multiple_of(8, &size);
+
+    return size;
+}
+
+
+
+void *dense_qp_hpipm_memory_assign(void *config_, void *dims_, void *opts_, void *raw_memory)
+{
+    dense_qp_dims *dims = dims_;
+    dense_qp_hpipm_opts *opts = opts_;
+    dense_qp_hpipm_memory *mem;
+
+    char *c_ptr = (char *) raw_memory;
+
+    mem = (dense_qp_hpipm_memory *) c_ptr;
+    c_ptr += sizeof(dense_qp_hpipm_memory);
+
+    mem->hpipm_workspace = (struct d_dense_qp_ipm_ws *) c_ptr;
+    c_ptr += sizeof(struct d_dense_qp_ipm_ws);
+
+    struct d_dense_qp_ipm_ws *ipm_workspace = mem->hpipm_workspace;
+
+    align_char_to(8, &c_ptr);
+
+    // ipm workspace structure
+    d_dense_qp_ipm_ws_create(dims, opts->hpipm_opts, ipm_workspace, c_ptr);
+    c_ptr += ipm_workspace->memsize;
+
+    assert((char *) raw_memory + dense_qp_hpipm_memory_calculate_size(config_, dims, opts) >= c_ptr);
+
+    return mem;
+}
+
+
+
+void dense_qp_hpipm_memory_get(void *config_, void *mem_, const char *field, void* value)
+{
+    // qp_solver_config *config = config_;
+    dense_qp_hpipm_memory *mem = mem_;
+
+    if (!strcmp(field, "time_qp_solver_call"))
+    {
+        double *tmp_ptr = value;
+        *tmp_ptr = mem->time_qp_solver_call;
+    }
+    else if (!strcmp(field, "iter"))
+    {
+        int *tmp_ptr = value;
+        *tmp_ptr = mem->iter;
+    }
+    else
+    {
+        printf("\nerror: dense_qp_hpipm_memory_get: field %s not available\n", field);
+        exit(1);
+    }
+
+    return;
+
+}
+
+
+
+/************************************************
+ * workspace
+ ************************************************/
+
+acados_size_t dense_qp_hpipm_workspace_calculate_size(void *config_, void *dims_, void *opts_)
+{
+    return 0;
+}
+
+
+
+/************************************************
+ * functions
+ ************************************************/
+
+int dense_qp_hpipm(void *config, void *qp_in_, void *qp_out_, void *opts_, void *mem_, void *work_)
+{
+    dense_qp_in *qp_in = qp_in_;
+    dense_qp_out *qp_out = qp_out_;
+
+    qp_info *info = (qp_info *) qp_out->misc;
+    acados_timer tot_timer, qp_timer;
+
+    acados_tic(&tot_timer);
+
+    // cast structures
+    dense_qp_hpipm_opts *opts = opts_;
+    dense_qp_hpipm_memory *mem = mem_;
+
+    // zero primal solution
+    // TODO add a check if warm start of first SQP iteration is implemented !!!!!!
+    int nv = qp_in->dim->nv;
+    int ns = qp_in->dim->ns;
+    blasfeo_dvecse(nv+2*ns, 0.0, qp_out->v, 0);
+
+    // solve ipm
+    acados_tic(&qp_timer);
+    int hpipm_status;
+    d_dense_qp_ipm_solve(qp_in, qp_out, opts->hpipm_opts, mem->hpipm_workspace);
+    d_dense_qp_ipm_get_status(mem->hpipm_workspace, &hpipm_status);
+
+    info->solve_QP_time = acados_toc(&qp_timer);
+    info->interface_time = 0;  // there are no conversions for hpipm
+    info->total_time = acados_toc(&tot_timer);
+    info->num_iter = mem->hpipm_workspace->iter;
+    info->t_computed = 1;
+
+    mem->time_qp_solver_call = info->solve_QP_time;
+    mem->iter = mem->hpipm_workspace->iter;
+
+#ifndef BLASFEO_EXT_DEP_OFF
+    // print HPIPM statistics:
+    if (opts->print_level > 0)
+    {
+        double *stat; d_dense_qp_ipm_get_stat(mem->hpipm_workspace, &stat);
+        int stat_m; d_dense_qp_ipm_get_stat_m(mem->hpipm_workspace, &stat_m);
+        printf("\nalpha_aff\tmu_aff\t\tsigma\t\talpha_prim\talpha_dual\tmu\t\tres_stat\tres_eq\t\tres_ineq\tres_comp\tobj\t\tlq fact\t\titref pred\titref corr\tlin res stat\tlin res eq\tlin res ineq\tlin res comp\n");
+        d_print_exp_tran_mat(stat_m, mem->iter+1, stat, stat_m);
+    }
+#endif
+
+    // check exit conditions
+    int acados_status = hpipm_status;
+    if (hpipm_status == 0) acados_status = ACADOS_SUCCESS;
+    if (hpipm_status == 1) acados_status = ACADOS_MAXITER;
+    if (hpipm_status == 2) acados_status = ACADOS_MINSTEP;
+    return acados_status;
+}
+
+
+
+void dense_qp_hpipm_eval_sens(void *config_, void *param_qp_in_, void *sens_qp_out_, void *opts_, void *mem_, void *work_)
+{
+    dense_qp_in *param_qp_in = param_qp_in_;
+    dense_qp_out *sens_qp_out = sens_qp_out_;
+
+    dense_qp_hpipm_opts *opts = opts_;
+    dense_qp_hpipm_memory *memory = mem_;
+
+    d_dense_qp_ipm_sens(param_qp_in, sens_qp_out, opts->hpipm_opts, memory->hpipm_workspace);
+
+    return;
+}
+
+
+void dense_qp_hpipm_eval_adj_sens(void *config_, void *param_qp_in_, void *sens_qp_out_, void *opts_, void *mem_, void *work_)
+{
+    dense_qp_in *param_qp_in = param_qp_in_;
+    dense_qp_out *sens_qp_out = sens_qp_out_;
+
+    dense_qp_hpipm_opts *opts = opts_;
+    dense_qp_hpipm_memory *memory = mem_;
+
+    d_dense_qp_ipm_sens_adj(param_qp_in, sens_qp_out, opts->hpipm_opts, memory->hpipm_workspace);
+
+    return;
+}
+
+
+
+void dense_qp_hpipm_memory_reset(void *config, void *qp_in, void *qp_out, void *opts, void *mem, void *work)
+{
+    printf("\nerror: dense_qp_hpipm_memory_reset: not implemented yet\n");
+    exit(1);
+}
+
+void dense_qp_hpipm_solver_get(void *config_, void *qp_in_, void *qp_out_, void *opts_, void *mem_, const char *field, int stage, void* value, int size1, int size2)
+{
+    printf("\nerror: dense_qp_hpipm_solver_get: not implemented yet\n");
+    exit(1);
+}
+
+
+void dense_qp_hpipm_terminate(void *config_, void *mem_, void *work_)
+{
+    return;
+}
+
+
+void dense_qp_hpipm_config_initialize_default(void *config_)
+{
+    qp_solver_config *config = config_;
+
+    config->dims_set = &dense_qp_dims_set;
+    config->opts_calculate_size = &dense_qp_hpipm_opts_calculate_size;
+    config->opts_assign = &dense_qp_hpipm_opts_assign;
+    config->opts_initialize_default = &dense_qp_hpipm_opts_initialize_default;
+    config->opts_update = &dense_qp_hpipm_opts_update;
+    config->opts_set = &dense_qp_hpipm_opts_set;
+    config->opts_get = &dense_qp_hpipm_opts_get;
+    config->memory_calculate_size = &dense_qp_hpipm_memory_calculate_size;
+    config->memory_assign = &dense_qp_hpipm_memory_assign;
+    config->memory_get = &dense_qp_hpipm_memory_get;
+    config->workspace_calculate_size = &dense_qp_hpipm_workspace_calculate_size;
+    config->evaluate = &dense_qp_hpipm;
+    config->eval_sens = &dense_qp_hpipm_eval_sens;
+    config->eval_adj_sens = &dense_qp_hpipm_eval_adj_sens;
+    config->memory_reset = &dense_qp_hpipm_memory_reset;
+    config->solver_get = &dense_qp_hpipm_solver_get;
+    config->terminate = &dense_qp_hpipm_terminate;
+
+    return;
+}

acados/dense_qp/dense_qp_hpipm.h

@@ -0,0 +1,97 @@
+/*
+ * Copyright (c) The acados authors.
+ *
+ * This file is part of acados.
+ *
+ * The 2-Clause BSD License
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.;
+ */
+
+
+#ifndef ACADOS_DENSE_QP_DENSE_QP_HPIPM_H_
+#define ACADOS_DENSE_QP_DENSE_QP_HPIPM_H_
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+// hpipm
+#include "hpipm/include/hpipm_d_dense_qp.h"
+#include "hpipm/include/hpipm_d_dense_qp_ipm.h"
+#include "hpipm/include/hpipm_d_dense_qp_sol.h"
+// acados
+#include "acados/dense_qp/dense_qp_common.h"
+#include "acados/utils/types.h"
+
+
+
+typedef struct dense_qp_hpipm_opts_
+{
+    struct d_dense_qp_ipm_arg *hpipm_opts;
+    int print_level;
+} dense_qp_hpipm_opts;
+
+
+
+typedef struct dense_qp_hpipm_memory_
+{
+    struct d_dense_qp_ipm_ws *hpipm_workspace;
+    double time_qp_solver_call;
+    int iter;
+
+} dense_qp_hpipm_memory;
+
+
+
+//
+acados_size_t dense_qp_hpipm_opts_calculate_size(void *config, void *dims);
+//
+void *dense_qp_hpipm_opts_assign(void *config, void *dims, void *raw_memory);
+//
+void dense_qp_hpipm_opts_initialize_default(void *config, void *dims, void *opts_);
+//
+void dense_qp_hpipm_opts_update(void *config, void *dims, void *opts_);
+//
+acados_size_t dense_qp_hpipm_calculate_memory_size(void *dims, void *opts_);
+//
+void *dense_qp_hpipm_assign_memory(void *dims, void *opts_, void *raw_memory);
+//
+acados_size_t dense_qp_hpipm_calculate_workspace_size(void *dims, void *opts_);
+//
+int dense_qp_hpipm(void *config, void *qp_in, void *qp_out, void *opts_, void *mem_, void *work_);
+//
+void dense_qp_hpipm_eval_sens(void *config_, void *qp_in, void *qp_out, void *opts_, void *mem_, void *work_);
+//
+void dense_qp_hpipm_config_initialize_default(void *config_);
+//
+void dense_qp_hpipm_memory_reset(void *config, void *qp_in, void *qp_out, void *opts, void *mem, void *work);
+//
+void dense_qp_hpipm_solver_get(void *config_, void *qp_in_, void *qp_out_, void *opts_, void *mem_, const char *field, int stage, void* value, int size1, int size2);
+
+
+#ifdef __cplusplus
+} /* extern "C" */
+#endif
+
+#endif  // ACADOS_DENSE_QP_DENSE_QP_HPIPM_H_

acados/dense_qp/dense_qp_ooqp.c

@@ -0,0 +1,682 @@
+/*
+ * Copyright (c) The acados authors.
+ *
+ * This file is part of acados.
+ *
+ * The 2-Clause BSD License
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.;
+ */
+
+
+
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <assert.h>
+
+#include "ooqp/cQpGenDense.h"
+
+// blasfeo
+#include "blasfeo_d_aux.h"
+#include "blasfeo_d_aux_ext_dep.h"
+
+// acados
+#include "acados/dense_qp/dense_qp_ooqp.h"
+#include "acados/utils/timing.h"
+#include "acados/utils/print.h"
+#include "acados/utils/mem.h"
+
+
+
+static void update_gradient(const dense_qp_in *in, dense_qp_ooqp_memory *mem)
+{
+    dense_qp_dims *dims = in->dim;
+
+    blasfeo_unpack_dvec(dims->nv, in->gz, 0, &mem->c[0]);
+}
+
+
+
+static void update_hessian_data(const dense_qp_in *in, dense_qp_ooqp_memory *mem)
+{
+    dense_qp_dims *dims = in->dim;
+
+    // fill in the upper triangular of H in dense_qp
+    blasfeo_dtrtr_l(dims->nv, in->Hv, 0, 0, in->Hv, 0, 0);
+
+    // copy H to dQ, transpose to get row-major ordering
+    blasfeo_unpack_tran_dmat(dims->nv, dims->nv, in->Hv, 0, 0, mem->dQ, dims->nv);
+}
+
+
+
+static void update_b_vector(const dense_qp_in *in, dense_qp_ooqp_memory *mem)
+{
+    dense_qp_dims *dims = in->dim;
+
+    blasfeo_unpack_dvec(dims->ne, in->b, 0, mem->bA);
+}
+
+
+
+static void update_bounds(const dense_qp_in *in, dense_qp_ooqp_memory *mem)
+{
+    dense_qp_dims *dims = in->dim;
+
+    for (int ii = 0; ii < dims->nv; ii++)
+    {
+        mem->ixlow[ii] = (char)0;
+        mem->ixupp[ii] = (char)0;
+        mem->xlow[ii] = 0.0;
+        mem->xupp[ii] = 0.0;
+    }
+
+    for (int ii = 0; ii < dims->nb; ii++)
+    {
+        int idx = in->idxb[ii];
+        // TODO(dimitris): check if cast is redundant
+        // NOTE(dimitris): OOQP can give wrong results if there are 1e12 bounds
+        if (BLASFEO_DVECEL(in->d, ii) > -1e10)
+        {  // TODO(dimitris): use acados inf
+            mem->ixlow[idx] = (char)1;
+            mem->xlow[idx] = BLASFEO_DVECEL(in->d, ii);
+        }
+        if (-BLASFEO_DVECEL(in->d, ii+dims->nb+dims->ng) < 1e10)
+        {  // TODO(dimitris): same here
+            mem->ixupp[idx] = (char)1;
+            mem->xupp[idx] = -BLASFEO_DVECEL(in->d, ii+dims->nb+dims->ng);
+        }
+    }
+}
+
+
+
+static void update_ineq_bounds(const dense_qp_in *in, dense_qp_ooqp_memory *mem)
+{
+    dense_qp_dims *dims = in->dim;
+
+    for (int ii = 0; ii < dims->ng; ii++)
+    {
+        mem->iclow[ii] = (char)1;
+        mem->clow[ii] = BLASFEO_DVECEL(in->d, ii+dims->nb);  // lg
+        mem->icupp[ii] = (char)1;
+        mem->cupp[ii] = -BLASFEO_DVECEL(in->d, ii+2*dims->nb+dims->ng);  // ug
+    }
+}
+
+
+
+static void update_inequalities_data(const dense_qp_in *in, dense_qp_ooqp_memory *mem)
+{
+    dense_qp_dims *dims = in->dim;
+
+    blasfeo_unpack_dmat(dims->nv, dims->ng, in->Ct, 0, 0, mem->dC, dims->nv);
+}
+
+
+
+static void dense_qp_ooqp_update_memory(const dense_qp_in *in, const dense_qp_ooqp_opts *opts,
+                                        dense_qp_ooqp_memory *mem)
+{
+    // ------- Update objective
+    update_gradient(in, mem);
+
+    if (mem->firstRun == 1 || opts->fixHessian == 0)
+    {
+        update_hessian_data(in, mem);
+    }
+
+    // ------- Update equality constraints
+    update_b_vector(in, mem);
+
+    // ------- Update bounds
+    update_bounds(in, mem);
+
+    // ------- Update inequality constraints
+    update_ineq_bounds(in, mem);
+
+    if (mem->firstRun == 1 || opts->fixInequalities == 0)
+    {
+        update_inequalities_data(in, mem);
+    }
+
+    mem->firstRun = 0;
+}
+
+
+
+static void print_inputs(dense_qp_ooqp_memory *mem)
+{
+    printf("\n----------> OOQP INPUTS <----------\n\n");
+    printf("NUMBER OF PRIMAL VARIABLES: %d\n", mem->nx);
+    printf("NUMBER OF EQUALITY CONSTRAINTS: %d\n", mem->my);
+    printf("NUMBER OF INEQUALITY CONSTRAINTS: %d\n", mem->mz);
+    printf("\n-----------------------------------\n\n");
+
+    printf("\nOBJECTIVE FUNCTION:\n");
+    for (int jj = 0; jj < mem->nx; jj++)
+    {
+        for (int ii = 0; ii < mem->nx; ii++)
+        {
+            printf("%9.5f ", mem->dQ[ii+mem->nx*jj]);
+        }
+        printf("\n");
+    }
+    printf("\n");
+
+    for (int ii = 0; ii < mem->nx; ii++)
+        printf("===> c[%d] = %f\n", ii + 1, mem->c[ii]);
+
+    printf("\nBOUNDS:\n");
+    for (int ii = 0; ii < mem->nx; ii++)
+    {
+        printf(
+            "ixlow[%d] = %d \t xlow[%d] = %4.2f \t ixupp[%d] = %d \t xupp[%d] "
+            "= %4.2f\n",
+            ii + 1, mem->ixlow[ii], ii + 1, mem->xlow[ii], ii + 1, mem->ixupp[ii], ii + 1,
+            mem->xupp[ii]);
+    }
+
+    printf("\n");
+    printf("\nINEQUALITY CONSTRAINTS:\n");
+    for (int ii = 0; ii < mem->mz; ii++)
+    {
+        for (int jj = 0; jj < mem->nx; jj++)
+        {
+            printf("%9.5f ", mem->dC[ii*mem->nx + jj]);
+        }
+        printf("\n");
+    }
+
+    for (int ii = 0; ii < mem->mz; ii++)
+    {
+        printf("===> clow[%d] = %4.2f \t cupp[%d] = %4.2f\n", ii + 1, mem->clow[ii], ii + 1,
+               mem->cupp[ii]);
+    }
+}
+
+
+
+static void print_outputs(dense_qp_ooqp_memory *mem, dense_qp_ooqp_workspace *work,
+                          int return_value)
+{
+    int_t ii;
+
+    printf("\n----------> OOQP OUTPUTS <---------\n\n");
+    printf("RETURN STATUS: %d\n", return_value);
+    printf("OBJECTIVE VALUE: %f\n", work->objectiveValue);
+    printf("FIRST AND LAST ELEMENT OF SOLUTION:\n");
+    printf("x[0] = %f\n", work->x[0]);
+    printf("x[%d] = %f\n", mem->nx, work->x[mem->nx - 1]);
+    printf("\n----------------------------------\n\n");
+
+    printf("\nPRIMAL SOLUTION:\n");
+    for (ii = 0; ii < mem->nx; ii++)
+    {
+        printf("=====> x[%d] = %f\n", ii + 1, work->x[ii]);
+    }
+
+    // printf("\nGAMMA:\n");
+    // for (ii = 0; ii < mem->nx; ii++)
+    // {
+    //     printf("=====> gamma[%d] = %f\n", ii + 1, work->gamma[ii]);
+    // }
+
+    // printf("\nPHI:\n");
+    // for (ii = 0; ii < mem->nx; ii++)
+    // {
+    //     printf("=====> phi[%d] = %f\n", ii + 1, work->phi[ii]);
+    // }
+
+    printf("\nLAMBDA:\n");
+    for (ii = 0; ii < mem->mz; ii++)
+    {
+        printf("=====> lambda[%d] = %f\n", ii + 1, work->lambda[ii]);
+    }
+
+    printf("\nPI:\n");
+    for (ii = 0; ii < mem->mz; ii++)
+    {
+        printf("=====> pi[%d] = %f\n", ii + 1, work->pi[ii]);
+    }
+}
+
+
+
+static void fill_in_qp_out(const dense_qp_in *in, dense_qp_out *out, dense_qp_ooqp_workspace *work)
+{
+    int nb = in->dim->nb;
+    int ng = in->dim->ng;
+    int ns = in->dim->ns;
+    int nv = in->dim->nv;
+    int *idxb = in->idxb;
+
+    blasfeo_pack_dvec(nv, &work->x[0], out->v, 0);
+
+    for (int ii = 0; ii < 2 * nb + 2 * ng + 2 * ns; ii++) out->lam->pa[ii] = 0.0;
+
+    for (int ii = 0; ii < nb; ii++)
+    {
+        double delta = work->gamma[idxb[ii]] - work->phi[idxb[ii]];
+
+        if (delta >= 0)
+            out->lam->pa[ii] = delta;
+        else
+            out->lam->pa[nb + ng + ii] = -delta;
+    }
+
+
+    for (int ii = 0; ii < ng; ii++)
+    {
+        double delta = work->lambda[ii] - work->pi[ii];
+
+        if (delta >= 0)
+            out->lam->pa[nb+ii] = delta;
+        else
+            out->lam->pa[2*nb+ng+ii] = -delta;
+    }
+
+}
+
+
+
+acados_size_t dense_qp_ooqp_opts_calculate_size(void *config_, dense_qp_dims *dims)
+{
+    acados_size_t size = 0;
+    size += sizeof(dense_qp_ooqp_opts);
+    return size;
+}
+
+
+
+void *dense_qp_ooqp_opts_assign(void *config_, dense_qp_dims *dims, void *raw_memory)
+{
+    dense_qp_ooqp_opts *opts;
+
+    char *c_ptr = (char *) raw_memory;
+
+    opts = (dense_qp_ooqp_opts *) c_ptr;
+    c_ptr += sizeof(dense_qp_ooqp_opts);
+
+    assert((char *) raw_memory + dense_qp_ooqp_opts_calculate_size(config_, dims) >= c_ptr);
+
+    return (void *) opts;
+}
+
+
+
+void dense_qp_ooqp_opts_initialize_default(void *config_, dense_qp_dims *dims, void *opts_)
+{
+    dense_qp_ooqp_opts *opts = (dense_qp_ooqp_opts *) opts_;
+
+    opts->printLevel = 0;
+    opts->fixHessian = 0;
+    opts->fixDynamics = 0;
+    opts->fixInequalities = 0;
+
+    return;
+}
+
+
+
+void dense_qp_ooqp_opts_update(void *config_, dense_qp_dims *dims, void *opts_)
+{
+    return;
+}
+
+
+
+void dense_qp_ooqp_opts_set(void *config_, void *opts_, const char *field, void *value)
+{
+    dense_qp_ooqp_opts *opts = opts_;
+
+    if (!strcmp(field, "tol_stat"))
+    {
+        // TODO set solver exit tolerance
+    }
+    else if (!strcmp(field, "tol_eq"))
+    {
+        // TODO set solver exit tolerance
+    }
+    else if (!strcmp(field, "tol_ineq"))
+    {
+        // TODO set solver exit tolerance
+    }
+    else if (!strcmp(field, "tol_comp"))
+    {
+        // TODO set solver exit tolerance
+    }
+    else if (!strcmp(field, "warm_start"))
+    {
+        // TODO set solver warm start
+    }
+    else
+    {
+        printf("\nerror: dense_qp_ooqp_opts_set: wrong field: %s\n", field);
+        exit(1);
+    }
+
+    return;
+}
+
+
+void dense_qp_ooqp_opts_get(void *config_, void *opts_, const char *field, void *value)
+{
+    // dense_qp_ooqp_opts *opts = opts_;
+    printf("\nerror: dense_qp_ooqp_opts_get: not implemented for field: %s\n", field);
+    exit(1);
+}
+
+
+
+acados_size_t dense_qp_ooqp_memory_calculate_size(void *config_, dense_qp_dims *dims, void *opts_)
+{
+    int nv = dims->nv;
+    int ne = dims->ne;
+    int ng = dims->ng;
+    // int nb = dims->nb;
+    // int ns = dims->ns;
+    // int nsb = dims->nsb;
+    // int nsg = dims->nsg;
+
+    acados_size_t size = 0;
+    size += sizeof(dense_qp_ooqp_memory);
+
+    size += 1 * nv * nv * sizeof(double);  // dQ
+    size += 1 * nv *      sizeof(double);  // c
+    size += 2 * nv *      sizeof(char);    // ixlow, ixupp
+    size += 2 * nv *      sizeof(double);  // xlow, xupp
+    size += 1 * nv * ne * sizeof(double);  // dA
+    size += 1 * ne *      sizeof(double);  // bA
+    size += 1 * nv * ng * sizeof(double);  // dC
+    size += 2 * ng *      sizeof(double);  // clow, cupp
+    size += 2 * ng *      sizeof(char);    // iclow, icupp
+
+    make_int_multiple_of(8, &size);
+
+    return size;
+}
+
+
+
+void *dense_qp_ooqp_memory_assign(void *config_, dense_qp_dims *dims, void *opts_, void *raw_memory)
+{
+    // dense_qp_ooqp_opts *opts = (dense_qp_ooqp_opts *)opts_;
+    dense_qp_ooqp_memory *mem;
+
+    int nv = dims->nv;
+    int ne = dims->ne;
+    int ng = dims->ng;
+    // int nb = dims->nb;
+    // int ns = dims->ns;
+    // int nsb = dims->nsb;
+    // int nsg = dims->nsg;
+
+    // char pointer
+    char *c_ptr = (char *) raw_memory;
+
+    mem = (dense_qp_ooqp_memory *) c_ptr;
+    c_ptr += sizeof(dense_qp_ooqp_memory);
+
+    assert((size_t) c_ptr % 8 == 0 && "memory not 8-byte aligned!");
+
+    assign_and_advance_double(nv * nv, &mem->dQ, &c_ptr);
+    assign_and_advance_double(nv, &mem->c, &c_ptr);
+    assign_and_advance_double(nv, &mem->xlow, &c_ptr);
+    assign_and_advance_double(nv, &mem->xupp, &c_ptr);
+    assign_and_advance_double(nv * ne, &mem->dA, &c_ptr);
+    assign_and_advance_double(ne, &mem->bA, &c_ptr);
+    assign_and_advance_double(nv * ng, &mem->dC, &c_ptr);
+    assign_and_advance_double(ng, &mem->clow, &c_ptr);
+    assign_and_advance_double(ng, &mem->cupp, &c_ptr);
+
+    assert((size_t) c_ptr % 8 == 0 && "double not 8-byte aligned!");
+
+    assign_and_advance_char(nv, &mem->ixlow, &c_ptr);
+    assign_and_advance_char(nv, &mem->ixupp, &c_ptr);
+    assign_and_advance_char(ng, &mem->iclow, &c_ptr);
+    assign_and_advance_char(ng, &mem->icupp, &c_ptr);
+
+    // initialize memory
+    mem->firstRun = 1;
+    mem->nx = nv;
+    mem->my = ne;
+    mem->mz = ng;
+
+    assert((char *) raw_memory + dense_qp_ooqp_memory_calculate_size(config_, dims, opts_) >=
+           c_ptr);
+
+    return mem;
+}
+
+
+
+void dense_qp_ooqp_memory_get(void *config_, void *mem_, const char *field, void* value)
+{
+    qp_solver_config *config = config_;
+    dense_qp_ooqp_memory *mem = mem_;
+
+    if(!strcmp(field, "time_qp_solver_call"))
+    {
+        double *tmp_ptr = value;
+        *tmp_ptr = mem->time_qp_solver_call;
+    }
+    else if(!strcmp(field, "iter"))
+    {
+        int *tmp_ptr = value;
+        *tmp_ptr = mem->iter;
+    }
+    else
+    {
+        printf("\nerror: dense_qp_ooqp_memory_get: field %s not available\n", field);
+        exit(1);
+    }
+
+    return;
+
+}
+
+
+
+/************************************************
+ * workspace
+ ************************************************/
+
+acados_size_t dense_qp_ooqp_workspace_calculate_size(void *config_, dense_qp_dims *dims, void *opts_)
+{
+    // dense_qp_ooqp_opts *opts = (dense_qp_ooqp_opts *)opts_;
+
+    acados_size_t size = 0;
+    acados_size_t nx, my, mz;
+
+    nx = dims->nv;
+    my = dims->ne;
+    mz = dims->ng;
+
+    size += sizeof(dense_qp_ooqp_workspace);
+    size += sizeof(double) * (3 * nx + my + 3 * mz);
+
+    return size;
+}
+
+
+
+static void dense_qp_ooqp_cast_workspace(dense_qp_ooqp_workspace *work, dense_qp_ooqp_memory *mem)
+{
+    char *ptr = (char *)work;
+
+    ptr += sizeof(dense_qp_ooqp_workspace);
+    work->x = (double *)ptr;
+    ptr += (mem->nx) * sizeof(double);
+    work->gamma = (double *)ptr;
+    ptr += (mem->nx) * sizeof(double);
+    work->phi = (double *)ptr;
+    ptr += (mem->nx) * sizeof(double);
+    work->y = (double *)ptr;
+    ptr += (mem->my) * sizeof(double);
+    work->z = (double *)ptr;
+    ptr += (mem->mz) * sizeof(double);
+    work->lambda = (double *)ptr;
+    ptr += (mem->mz) * sizeof(double);
+    work->pi = (double *)ptr;
+    ptr += (mem->mz) * sizeof(double);
+}
+
+
+
+int_t dense_qp_ooqp(void *config_, dense_qp_in *qp_in, dense_qp_out *qp_out, void *opts_,
+                    void *memory_, void *work_)
+{
+    int ns = qp_in->dim->ns;
+
+    if (ns > 0)
+    {
+        printf("\nOOQP interface can not handle ns>0 yet: what about implementing it? :)\n");
+        exit(1);
+    }
+
+    acados_timer tot_timer, qp_timer, interface_timer;
+    qp_info *info = (qp_info *) qp_out->misc;
+    acados_tic(&tot_timer);
+
+    dense_qp_ooqp_opts *opts = (dense_qp_ooqp_opts *)opts_;
+    dense_qp_ooqp_memory *mem = (dense_qp_ooqp_memory *)memory_;
+    dense_qp_ooqp_workspace *work = (dense_qp_ooqp_workspace *)work_;
+
+    acados_tic(&interface_timer);
+    // NOTE: has to be called after setting up the memory which contains the problem dimensions
+    dense_qp_ooqp_cast_workspace(work, mem);
+    dense_qp_ooqp_update_memory(qp_in, opts, mem);
+    info->interface_time = acados_toc(&interface_timer);
+
+    if (0)
+    {
+        print_dense_qp_in(qp_in);
+        print_inputs(mem);
+    }
+
+    // TODO(dimitris): implement dense OOQP
+    // call sparse OOQP
+    acados_tic(&qp_timer);
+    int ooqp_status;
+    qpsolvede(mem->c,    mem->nx,      mem->dQ,
+              mem->xlow, mem->ixlow,
+              mem->xupp, mem->ixupp,
+              mem->dA,   mem->my,      mem->bA,
+              mem->dC,   mem->mz,
+              mem->clow, mem->iclow,
+              mem->cupp, mem->icupp,
+              work->x,   work->gamma,  work->phi,
+              work->y,
+              work->z,   work->lambda, work->pi,
+              &work->objectiveValue,
+              opts->printLevel, &ooqp_status);
+    info->solve_QP_time = acados_toc(&qp_timer);
+
+    mem->time_qp_solver_call = info->solve_QP_time;
+    mem->iter = -1;
+
+    if (0) print_outputs(mem, work, ooqp_status);
+    acados_tic(&interface_timer);
+    fill_in_qp_out(qp_in, qp_out, work);
+    dense_qp_compute_t(qp_in, qp_out);
+    info->interface_time += acados_toc(&interface_timer);
+
+    info->total_time = acados_toc(&tot_timer);
+    info->num_iter = -1;
+    info->t_computed = 1;
+
+    int acados_status = ooqp_status;
+    if (ooqp_status == DENSE_SUCCESSFUL_TERMINATION) acados_status = ACADOS_SUCCESS;
+    if (ooqp_status == DENSE_MAX_ITS_EXCEEDED) acados_status = ACADOS_MAXITER;
+    return acados_status;
+}
+
+
+
+void dense_qp_ooqp_destroy(void *mem_, void *work)
+{
+    free(work);
+    free(mem_);
+}
+
+
+
+void dense_qp_ooqp_eval_sens(void *config_, void *qp_in, void *qp_out, void *opts_, void *mem_, void *work_)
+{
+    printf("\nerror: dense_qp_ooqp_eval_sens: not implemented yet\n");
+    exit(1);
+}
+
+
+void dense_qp_ooqp_eval_adj_sens(void *config_, void *qp_in, void *qp_out, void *opts_, void *mem_, void *work_)
+{
+    printf("\nerror: dense_qp_ooqp_eval_adj_sens: not implemented yet\n");
+    exit(1);
+}
+
+
+void dense_qp_ooqp_memory_reset(void *config, void *qp_in, void *qp_out, void *opts, void *mem, void *work)
+{
+    printf("\nerror: dense_qp_ooqp_memory_reset: not implemented yet\n");
+    exit(1);
+}
+
+void dense_qp_ooqp_solver_get(void *config_, void *qp_in_, void *qp_out_, void *opts_, void *mem_, const char *field, int stage, void* value, int size1, int size2)
+{
+    printf("\nerror: dense_qp_ooqp_solver_get: not implemented yet\n");
+    exit(1);
+}
+
+
+void dense_qp_ooqp_terminate(void *config_, void *mem_, void *work_)
+{
+    return;
+}
+
+void dense_qp_ooqp_config_initialize_default(void *config_)
+{
+    qp_solver_config *config = config_;
+
+    config->opts_calculate_size = (acados_size_t (*)(void *, void *)) & dense_qp_ooqp_opts_calculate_size;
+    config->opts_assign = (void *(*) (void *, void *, void *) ) & dense_qp_ooqp_opts_assign;
+    config->opts_initialize_default =
+        (void (*)(void *, void *, void *)) & dense_qp_ooqp_opts_initialize_default;
+    config->opts_update = (void (*)(void *, void *, void *)) & dense_qp_ooqp_opts_update;
+    config->opts_set = &dense_qp_ooqp_opts_set;
+    config->opts_set = &dense_qp_ooqp_opts_get;
+    config->memory_calculate_size =
+        (acados_size_t (*)(void *, void *, void *)) & dense_qp_ooqp_memory_calculate_size;
+    config->memory_assign =
+        (void *(*) (void *, void *, void *, void *) ) & dense_qp_ooqp_memory_assign;
+    config->memory_get = &dense_qp_ooqp_memory_get;
+    config->workspace_calculate_size =
+        (acados_size_t (*)(void *, void *, void *)) & dense_qp_ooqp_workspace_calculate_size;
+    config->evaluate = (int (*)(void *, void *, void *, void *, void *, void *)) & dense_qp_ooqp;
+    config->eval_sens = &dense_qp_ooqp_eval_sens;
+    config->eval_adj_sens = &dense_qp_ooqp_eval_adj_sens;
+    config->memory_reset = &dense_qp_ooqp_memory_reset;
+    config->solver_get = &dense_qp_ooqp_solver_get;
+    config->terminate = &dense_qp_ooqp_terminate;
+}

acados/dense_qp/dense_qp_ooqp.h

@@ -0,0 +1,129 @@
+/*
+ * Copyright (c) The acados authors.
+ *
+ * This file is part of acados.
+ *
+ * The 2-Clause BSD License
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.;
+ */
+
+
+#ifndef ACADOS_DENSE_QP_DENSE_QP_OOQP_H_
+#define ACADOS_DENSE_QP_DENSE_QP_OOQP_H_
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include "acados/dense_qp/dense_qp_common.h"
+#include "acados/utils/types.h"
+
+enum dense_qp_ooqp_termination_code
+{
+  DENSE_SUCCESSFUL_TERMINATION = 0,
+  DENSE_NOT_FINISHED,
+  DENSE_MAX_ITS_EXCEEDED,
+  DENSE_INFEASIBLE,
+  DENSE_UNKNOWN
+};
+
+typedef struct dense_qp_ooqp_opts_
+{
+    int printLevel;
+    int useDiagonalWeights;  // TODO(dimitris): implement option
+    int fixHessian;
+    int fixDynamics;
+    int fixInequalities;
+} dense_qp_ooqp_opts;
+
+typedef struct dense_qp_ooqp_workspace_
+{
+    double *x;
+    double *gamma;
+    double *phi;
+    double *y;
+    double *z;
+    double *lambda;
+    double *pi;
+    double objectiveValue;
+} dense_qp_ooqp_workspace;
+
+typedef struct dense_qp_ooqp_memory_
+{
+    int firstRun;
+    int nx;
+    int my;
+    int mz;
+    double *c;
+    double *dQ;
+    double *xlow;
+    char *ixlow;
+    double *xupp;
+    char *ixupp;
+    double *dA;
+    double *bA;
+    double *dC;
+    double *clow;
+    char *iclow;
+    double *cupp;
+    char *icupp;
+    double time_qp_solver_call;
+    int iter;
+
+} dense_qp_ooqp_memory;
+
+//
+acados_size_t dense_qp_ooqp_opts_calculate_size(void *config_, dense_qp_dims *dims);
+//
+void *dense_qp_ooqp_opts_assign(void *config_, dense_qp_dims *dims, void *raw_memory);
+//
+void dense_qp_ooqp_opts_initialize_default(void *config_, dense_qp_dims *dims, void *opts_);
+//
+void dense_qp_ooqp_opts_update(void *config_, dense_qp_dims *dims, void *opts_);
+//
+acados_size_t dense_qp_ooqp_memory_calculate_size(void *config_, dense_qp_dims *dims, void *opts_);
+//
+void *dense_qp_ooqp_memory_assign(void *config_, dense_qp_dims *dims, void *opts_,
+                                  void *raw_memory);
+//
+acados_size_t dense_qp_ooqp_workspace_calculate_size(void *config_, dense_qp_dims *dims, void *opts_);
+//
+int dense_qp_ooqp(void *config_, dense_qp_in *qp_in, dense_qp_out *qp_out, void *opts_,
+                  void *memory_, void *work_);
+//
+void dense_qp_ooqp_destroy(void *mem_, void *work);
+//
+void dense_qp_ooqp_eval_sens(void *config_, void *qp_in, void *qp_out, void *opts_, void *mem_, void *work_);
+//
+void dense_qp_ooqp_memory_reset(void *config, void *qp_in, void *qp_out, void *opts, void *mem, void *work);
+//
+void dense_qp_ooqp_solver_get(void *config_, void *qp_in_, void *qp_out_, void *opts_, void *mem_, const char *field, int stage, void* value, int size1, int size2);
+//
+void dense_qp_ooqp_config_initialize_default(void *config_);
+
+#ifdef __cplusplus
+} /* extern "C" */
+#endif
+
+#endif  // ACADOS_DENSE_QP_DENSE_QP_OOQP_H_

acados/dense_qp/dense_qp_qore.c

@@ -0,0 +1,618 @@
+/*
+ * Copyright (c) The acados authors.
+ *
+ * This file is part of acados.
+ *
+ * The 2-Clause BSD License
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.;
+ */
+
+
+// external
+#include <assert.h>
+#include <math.h>
+#include <string.h>
+// blasfeo
+#include "blasfeo_d_aux.h"
+#include "blasfeo_d_aux_ext_dep.h"
+// acados
+#include "acados/dense_qp/dense_qp_common.h"
+#include "acados/dense_qp/dense_qp_qore.h"
+#include "acados/utils/mem.h"
+#include "acados/utils/timing.h"
+
+
+
+/************************************************
+ * opts
+ ************************************************/
+
+acados_size_t dense_qp_qore_opts_calculate_size(void *config_, dense_qp_dims *dims)
+{
+    acados_size_t size = 0;
+    size += sizeof(dense_qp_qore_opts);
+
+    return size;
+}
+
+
+
+void *dense_qp_qore_opts_assign(void *config_, dense_qp_dims *dims, void *raw_memory)
+{
+    dense_qp_qore_opts *opts;
+
+    char *c_ptr = (char *) raw_memory;
+
+    opts = (dense_qp_qore_opts *) c_ptr;
+    c_ptr += sizeof(dense_qp_qore_opts);
+
+    assert((char *) raw_memory + dense_qp_qore_opts_calculate_size(config_, dims) >= c_ptr);
+
+    return (void *) opts;
+}
+
+
+
+void dense_qp_qore_opts_initialize_default(void *config_, dense_qp_dims *dims, void *opts_)
+{
+    dense_qp_qore_opts *opts = (dense_qp_qore_opts *) opts_;
+
+    opts->print_freq = -1;
+    opts->warm_start = 0;
+    opts->warm_strategy = 0;
+    opts->nsmax = 400;
+    opts->hot_start = 0;
+    opts->max_iter = 1000;
+    opts->compute_t = 1;
+
+    return;
+}
+
+
+
+void dense_qp_qore_opts_update(void *config_, dense_qp_dims *dims, void *opts_)
+{
+    //    dense_qp_qore_opts *opts = (dense_qp_qore_opts *)opts_;
+
+    return;
+}
+
+
+
+void dense_qp_qore_opts_set(void *config_, void *opts_, const char *field, void *value)
+{
+    dense_qp_qore_opts *opts = opts_;
+
+    if (!strcmp(field, "tol_stat"))
+    {
+        // TODO set solver exit tolerance
+    }
+    else if (!strcmp(field, "tol_eq"))
+    {
+        // TODO set solver exit tolerance
+    }
+    else if (!strcmp(field, "tol_ineq"))
+    {
+        // TODO set solver exit tolerance
+    }
+    else if (!strcmp(field, "tol_comp"))
+    {
+        // TODO set solver exit tolerance
+    }
+    else if (!strcmp(field, "warm_start"))
+    {
+        // TODO set solver warm start
+    }
+    else
+    {
+        printf("\nerror: dense_qp_qore_opts_set: wrong field: %s\n", field);
+        exit(1);
+    }
+
+    return;
+}
+
+
+void dense_qp_qore_opts_get(void *config_, void *opts_, const char *field, void *value)
+{
+    // dense_qp_qore_opts *opts = opts_;
+    printf("\nerror: dense_qp_qore_opts_get: not implemented for field: %s\n", field);
+    exit(1);
+}
+
+
+
+
+/************************************************
+ * memory
+ ************************************************/
+
+acados_size_t dense_qp_qore_memory_calculate_size(void *config_, dense_qp_dims *dims, void *opts_)
+{
+    dense_qp_qore_opts *opts = (dense_qp_qore_opts *) opts_;
+    dense_qp_dims dims_stacked;
+
+    int nv = dims->nv;
+    int ne = dims->ne;
+    int ng = dims->ng;
+    int nb = dims->nb;
+    int ns = dims->ns;
+    int nsb = dims->nsb;
+    // int nsg = dims->nsg;
+    int nsmax = (2 * nv >= opts->nsmax) ? opts->nsmax : 2 * nv;
+
+    int nv2 = nv + 2*ns;
+    int ng2 = (ns > 0) ? ng + nsb : ng;
+    int nb2 = nb - nsb + 2*ns;
+
+    // size in bytes
+    acados_size_t size = sizeof(dense_qp_qore_memory);
+
+    size += 1 * nv * nv * sizeof(double);      // H
+    size += 1 * nv2 * nv2 * sizeof(double);    // HH
+    size += 1 * nv2 * ne * sizeof(double);     // A
+    size += 2 * nv * ng * sizeof(double);      // C, Ct
+    size += 2 * nv2 * ng2 * sizeof(double);    // CC, CCt
+    size += 1 * nv * sizeof(double);           // g
+    size += 3 * nv2 * sizeof(double);          // gg d_lb d_ub
+    size += 1 * ne * sizeof(double);           // b
+    size += 2 * nb2 * sizeof(double);          // d_lb0 d_ub0
+    size += 2 * ng2 * sizeof(double);          // d_lg d_ug
+    size += 1 * nb * sizeof(int);              // idxb
+    size += 1 * nb2 * sizeof(int);             // idxb_stacked
+    size += 1 * ns * sizeof(int);
+    size += 2 * (nv2 + ng2) * sizeof(double);  // lb, ub
+    size += 1 * (nv2 + ng2) * sizeof(double);  // prim_sol
+    size += 1 * (nv2 + ng2) * sizeof(double);  // dual_sol
+    size += 6 * ns * sizeof(double);           // Zl, Zu, zl, zu, d_ls, d_us
+    size += QPDenseSize(nv2, ng2, nsmax);
+
+    if (ns > 0)
+    {
+        dense_qp_stack_slacks_dims(dims, &dims_stacked);
+        size += dense_qp_in_calculate_size(&dims_stacked);
+    }
+
+    make_int_multiple_of(8, &size);
+
+    return size;
+}
+
+
+
+void *dense_qp_qore_memory_assign(void *config_, dense_qp_dims *dims, void *opts_, void *raw_memory)
+{
+    dense_qp_qore_memory *mem;
+    dense_qp_qore_opts *opts = (dense_qp_qore_opts *) opts_;
+    dense_qp_dims dims_stacked;
+
+    int nv = dims->nv;
+    int ne = dims->ne;
+    int ng = dims->ng;
+    int nb = dims->nb;
+    int ns = dims->ns;
+    int nsb = dims->nsb;
+    // int nsg = dims->nsg;
+    int nsmax = (2 * nv >= opts->nsmax) ? opts->nsmax : 2 * nv;
+
+    int nv2 = nv + 2*ns;
+    int ng2 = (ns > 0) ? ng + nsb : ng;
+    int nb2 = nb - nsb + 2*ns;
+
+    // char pointer
+    char *c_ptr = (char *) raw_memory;
+
+    mem = (dense_qp_qore_memory *) c_ptr;
+    c_ptr += sizeof(dense_qp_qore_memory);
+
+    assert((size_t) c_ptr % 8 == 0 && "memory not 8-byte aligned!");
+
+    if (ns > 0)
+    {
+        dense_qp_stack_slacks_dims(dims, &dims_stacked);
+        mem->qp_stacked = dense_qp_in_assign(&dims_stacked, c_ptr);
+        c_ptr += dense_qp_in_calculate_size(&dims_stacked);
+    }
+    else
+    {
+        mem->qp_stacked = NULL;
+    }
+
+    assert((size_t) c_ptr % 8 == 0 && "double not 8-byte aligned!");
+
+    assign_and_advance_double(nv * nv, &mem->H, &c_ptr);
+    assign_and_advance_double(nv2 * nv2, &mem->HH, &c_ptr);
+    assign_and_advance_double(nv2 * ne, &mem->A, &c_ptr);
+    assign_and_advance_double(nv * ng, &mem->C, &c_ptr);
+    assign_and_advance_double(nv * ng, &mem->Ct, &c_ptr);
+    assign_and_advance_double(nv2 * ng2, &mem->CC, &c_ptr);
+    assign_and_advance_double(nv2 * ng2, &mem->CCt, &c_ptr);
+    assign_and_advance_double(nv, &mem->g, &c_ptr);
+    assign_and_advance_double(nv2, &mem->gg, &c_ptr);
+    assign_and_advance_double(ne, &mem->b, &c_ptr);
+    assign_and_advance_double(nb2, &mem->d_lb0, &c_ptr);
+    assign_and_advance_double(nb2, &mem->d_ub0, &c_ptr);
+    assign_and_advance_double(nv2, &mem->d_lb, &c_ptr);
+    assign_and_advance_double(nv2, &mem->d_ub, &c_ptr);
+    assign_and_advance_double(ng2, &mem->d_lg, &c_ptr);
+    assign_and_advance_double(ng2, &mem->d_ug, &c_ptr);
+    assign_and_advance_double(ns, &mem->Zl, &c_ptr);
+    assign_and_advance_double(ns, &mem->Zu, &c_ptr);
+    assign_and_advance_double(ns, &mem->zl, &c_ptr);
+    assign_and_advance_double(ns, &mem->zu, &c_ptr);
+    assign_and_advance_double(ns, &mem->d_ls, &c_ptr);
+    assign_and_advance_double(ns, &mem->d_us, &c_ptr);
+    assign_and_advance_double(nv2 + ng2, &mem->lb, &c_ptr);
+    assign_and_advance_double(nv2 + ng2, &mem->ub, &c_ptr);
+    assign_and_advance_double(nv2 + ng2, &mem->prim_sol, &c_ptr);
+    assign_and_advance_double(nv2 + ng2, &mem->dual_sol, &c_ptr);
+
+    assert((size_t) c_ptr % 8 == 0 && "double not 8-byte aligned!");
+
+    mem->QP = (QoreProblemDense *) c_ptr;
+    QPDenseCreate(&mem->QP, nv2, ng2, nsmax, c_ptr);
+    c_ptr += QPDenseSize(nv2, ng2, nsmax);
+
+    // int stuff
+    assign_and_advance_int(nb, &mem->idxb, &c_ptr);
+    assign_and_advance_int(nb2, &mem->idxb_stacked, &c_ptr);
+    assign_and_advance_int(ns, &mem->idxs, &c_ptr);
+
+    assert((char *) raw_memory + dense_qp_qore_memory_calculate_size(config_, dims, opts_) >=
+           c_ptr);
+
+    return mem;
+}
+
+
+
+void dense_qp_qore_memory_get(void *config_, void *mem_, const char *field, void* value)
+{
+    qp_solver_config *config = config_;
+    dense_qp_qore_memory *mem = mem_;
+
+    if(!strcmp(field, "time_qp_solver_call"))
+    {
+        double *tmp_ptr = value;
+        *tmp_ptr = mem->time_qp_solver_call;
+    }
+    else if(!strcmp(field, "iter"))
+    {
+        int *tmp_ptr = value;
+        *tmp_ptr = mem->iter;
+    }
+    else
+    {
+        printf("\nerror: dense_qp_qore_memory_get: field %s not available\n", field);
+        exit(1);
+    }
+
+    return;
+
+}
+
+
+
+/************************************************
+ * workspace
+ ************************************************/
+
+acados_size_t dense_qp_qore_workspace_calculate_size(void *config_, dense_qp_dims *dims, void *opts_)
+{
+    return 0;
+}
+
+
+
+/************************************************
+ * functions
+ ************************************************/
+
+int dense_qp_qore(void *config_, dense_qp_in *qp_in, dense_qp_out *qp_out, void *opts_,
+                  void *memory_, void *work_)
+{
+    qp_info *info = (qp_info *) qp_out->misc;
+    acados_timer tot_timer, qp_timer, interface_timer;
+
+    acados_tic(&tot_timer);
+    acados_tic(&interface_timer);
+    info->t_computed = 0;
+
+    // cast structures
+    dense_qp_qore_opts *opts = (dense_qp_qore_opts *) opts_;
+    dense_qp_qore_memory *memory = (dense_qp_qore_memory *) memory_;
+
+    // extract qpoases data
+    double *H = memory->H;
+    double *HH = memory->HH;
+    double *A = memory->A;
+    double *C = memory->C;
+    double *CC = memory->CC;
+    double *Ct = memory->Ct;
+    double *CCt = memory->CCt;
+    // double *g = memory->g;
+    double *gg = memory->gg;
+    double *b = memory->b;
+    double *d_lb0 = memory->d_lb0;
+    double *d_ub0 = memory->d_ub0;
+    double *d_lb = memory->d_lb;
+    double *d_ub = memory->d_ub;
+    double *d_lg = memory->d_lg;
+    double *d_ug = memory->d_ug;
+    double *lb = memory->lb;
+    double *ub = memory->ub;
+    double *Zl = memory->Zl;
+    double *Zu = memory->Zu;
+    double *zl = memory->zl;
+    double *zu = memory->zu;
+    double *d_ls = memory->d_ls;
+    double *d_us = memory->d_us;
+    int *idxb = memory->idxb;
+    int *idxb_stacked = memory->idxb_stacked;
+    int *idxs = memory->idxs;
+    QoreProblemDense *QP = memory->QP;
+    double *prim_sol = memory->prim_sol;
+    double *dual_sol = memory->dual_sol;
+    dense_qp_in *qp_stacked = memory->qp_stacked;
+
+    // extract dense qp size
+    int nv = qp_in->dim->nv;
+    int ng = qp_in->dim->ng;
+    int nb = qp_in->dim->nb;
+    int ns = qp_in->dim->ns;
+    int nsb = qp_in->dim->nsb;
+    // int nsg = qp_in->dim->nsg;
+
+    int nv2 = nv + 2*ns;
+    int ng2 = (ns > 0) ? ng + nsb : ng;
+    int nb2 = nb - nsb + 2 * ns;
+
+    // fill in the upper triangular of H in dense_qp
+    blasfeo_dtrtr_l(nv, qp_in->Hv, 0, 0, qp_in->Hv, 0, 0);
+
+    // extract data from qp_in in col-major
+    d_dense_qp_get_all(qp_in, H, gg, A, b, idxb, d_lb0, d_ub0, C, d_lg, d_ug,
+                             Zl, Zu, zl, zu, idxs, d_ls, d_us);
+
+    // reorder bounds
+    for (int ii = 0; ii < nv2; ii++)
+    {
+        d_lb[ii] = -INFINITY;
+        d_ub[ii] = +INFINITY;
+    }
+
+    if (ns > 0)
+    {
+        dense_qp_stack_slacks(qp_in, qp_stacked);
+        d_dense_qp_get_all(qp_stacked, HH, gg, A, b, idxb_stacked, d_lb0, d_ub0, CC, d_lg,
+            d_ug, NULL, NULL, NULL, NULL, NULL, NULL, NULL);
+
+        for (int ii = 0; ii < nb2; ii++)
+        {
+            d_lb[idxb_stacked[ii]] = d_lb0[ii];
+            d_ub[idxb_stacked[ii]] = d_ub0[ii];
+        }
+
+        // transpose CC as expected by QORE
+        for (int j = 0; j < nv2; j++)
+        {
+            for (int i = 0; i < ng2; i++)
+            {
+                CCt[j + i * nv2] = CC[i + j * ng2];
+            }
+        }
+    }
+    else
+    {
+        for (int ii = 0; ii < nb; ii++)
+        {
+            d_lb[idxb[ii]] = d_lb0[ii];
+            d_ub[idxb[ii]] = d_ub0[ii];
+        }
+
+        // transpose C as expected by QORE
+        for (int j = 0; j < nv; j++)
+        {
+            for (int i = 0; i < ng; i++)
+            {
+                Ct[j + i * nv] = C[i + j * ng];
+            }
+        }
+    }
+
+    memcpy(lb, d_lb, nv2 * sizeof(double));
+    memcpy(lb + nv2, d_lg, ng2 * sizeof(double));
+
+    memcpy(ub, d_ub, nv2 * sizeof(double));
+    memcpy(ub + nv2, d_ug, ng2 * sizeof(double));
+
+
+    info->interface_time = acados_toc(&interface_timer);
+
+    // solve dense qp
+    acados_tic(&qp_timer);
+
+    if (opts->warm_start)
+    {
+        QPDenseSetInt(QP, "warmstrategy", opts->warm_strategy);
+        (ns > 0) ? QPDenseUpdateMatrices(QP, nv2, ng2, CCt, HH) :
+                   QPDenseUpdateMatrices(QP, nv, ng, Ct, H);
+    }
+    else if (!opts->hot_start)
+    {
+        (ns > 0) ? QPDenseSetData(QP, nv2, ng2, CCt, HH) :
+                   QPDenseSetData(QP, nv, ng, Ct, H);
+    }
+
+    QPDenseSetInt(QP, "maxiter", opts->max_iter);
+    QPDenseSetInt(QP, "prtfreq", opts->print_freq);
+    QPDenseOptimize(QP, lb, ub, gg, 0, 0);
+    int qore_status;
+    QPDenseGetInt(QP, "status", &qore_status);
+
+    QPDenseGetDblVector(QP, "primalsol", prim_sol);
+    QPDenseGetDblVector(QP, "dualsol", dual_sol);
+    int num_iter;
+    QPDenseGetInt(QP, "itercount", &num_iter);
+    memory->num_iter = num_iter;
+
+    info->solve_QP_time = acados_toc(&qp_timer);
+    acados_tic(&interface_timer);
+
+    // copy prim_sol and dual_sol to qpd_sol
+    blasfeo_pack_dvec(nv2, prim_sol, qp_out->v, 0);
+    for (int ii = 0; ii < 2 * nb + 2 * ng + 2 * ns; ii++) qp_out->lam->pa[ii] = 0.0;
+    for (int ii = 0; ii < nb; ii++)
+    {
+        if (dual_sol[idxb[ii]] >= 0.0)
+            qp_out->lam->pa[ii] = dual_sol[idxb[ii]];
+        else
+            qp_out->lam->pa[nb + ng + ii] = -dual_sol[idxb[ii]];
+    }
+
+    for (int ii = 0; ii < ng; ii++)
+    {
+        if (dual_sol[nv2 + ii] >= 0.0)
+            qp_out->lam->pa[nb + ii] = dual_sol[nv2 + ii];
+        else
+            qp_out->lam->pa[2 * nb + ng + ii] = -dual_sol[nv2 + ii];
+    }
+
+    int k = 0;
+    for (int ii = 0; ii < ns; ii++)
+    {
+        int js = idxs[ii];
+
+        double offset_l = 0.0;
+        double offset_u = 0.0;
+
+        if (js < nb)
+        {
+            if (dual_sol[nv2 + ng + k] <= 0.0)  // softened upper box constraints
+            {
+                qp_out->lam->pa[nb + ng + js] = -dual_sol[nv2 + ng + k];
+                offset_u = -dual_sol[nv2 + ng + k];
+            }
+            else  // softened lower box constraints
+            {
+                qp_out->lam->pa[js] = dual_sol[nv2 + ng + k];
+                offset_l = dual_sol[nv2 + ng + k];
+            }
+
+            k++;
+        }
+        else
+        {
+            offset_l = qp_out->lam->pa[nb+js-nb];
+            offset_u = qp_out->lam->pa[2*nb+ng+js-nb];
+        }
+
+        // dual variables for sl >= d_ls
+        if (dual_sol[nv + ii] >= 0)
+            qp_out->lam->pa[2*nb + 2*ng + ii] = dual_sol[nv + ii] - offset_u;
+
+        // dual variables for su >= d_us
+        if (dual_sol[nv + ns + ii] >= 0)
+            qp_out->lam->pa[2*nb + 2*ng + ns + ii] = dual_sol[nv + ns + ii] - offset_l;
+    }
+
+    info->interface_time += acados_toc(&interface_timer);
+    info->total_time = acados_toc(&tot_timer);
+    info->num_iter = num_iter;
+
+    mem->time_qp_solver_call = info->solve_QP_time;
+    mem->iter = num_iter;
+
+    // compute slacks
+    if (opts->compute_t)
+    {
+        dense_qp_compute_t(qp_in, qp_out);
+        info->t_computed = 1;
+    }
+
+    int acados_status = qore_status;
+    if (qore_status == QPSOLVER_DENSE_OPTIMAL) acados_status = ACADOS_SUCCESS;
+    if (qore_status == QPSOLVER_DENSE_ITER_LIMIT) acados_status = ACADOS_MAXITER;
+    return acados_status;
+}
+
+
+
+void dense_qp_qore_eval_sens(void *config_, void *qp_in, void *qp_out, void *opts_, void *mem_, void *work_)
+{
+    printf("\nerror: dense_qp_qore_eval_sens: not implemented yet\n");
+    exit(1);
+}
+
+void dense_qp_qore_eval_adj_sens(void *config_, void *qp_in, void *qp_out, void *opts_, void *mem_, void *work_)
+{
+    printf("\nerror: dense_qp_qore_eval_adj_sens: not implemented yet\n");
+    exit(1);
+}
+
+
+void dense_qp_qore_memory_reset(void *config, void *qp_in, void *qp_out, void *opts, void *mem, void *work)
+{
+    printf("\nerror: dense_qp_qore_memory_reset: not implemented yet\n");
+    exit(1);
+}
+
+void dense_qp_qore_solver_get(void *config_, void *qp_in_, void *qp_out_, void *opts_, void *mem_, const char *field, int stage, void* value, int size1, int size2)
+{
+    printf("\nerror: dense_qp_qore_solver_get: not implemented yet\n");
+    exit(1);
+}
+
+void dense_qp_qore_terminate(void *config_, void *mem_, void *work_)
+{
+    return;
+}
+
+void dense_qp_qore_config_initialize_default(void *config_)
+{
+    qp_solver_config *config = config_;
+
+    config->opts_calculate_size = (acados_size_t (*)(void *, void *)) & dense_qp_qore_opts_calculate_size;
+    config->opts_assign = (void *(*) (void *, void *, void *) ) & dense_qp_qore_opts_assign;
+    config->opts_initialize_default =
+        (void (*)(void *, void *, void *)) & dense_qp_qore_opts_initialize_default;
+    config->opts_update = (void (*)(void *, void *, void *)) & dense_qp_qore_opts_update;
+    config->opts_set = &dense_qp_qore_opts_set;
+    config->opts_get = &dense_qp_qore_opts_get;
+    config->memory_calculate_size =
+        (acados_size_t (*)(void *, void *, void *)) & dense_qp_qore_memory_calculate_size;
+    config->memory_assign =
+        (void *(*) (void *, void *, void *, void *) ) & dense_qp_qore_memory_assign;
+    config->memory_get = &dense_qp_qore_memory_get;
+    config->workspace_calculate_size =
+        (acados_size_t (*)(void *, void *, void *)) & dense_qp_qore_workspace_calculate_size;
+    config->evaluate = (int (*)(void *, void *, void *, void *, void *, void *)) & dense_qp_qore;
+    config->eval_sens = &dense_qp_qore_eval_sens;
+    config->eval_adj_sens = &dense_qp_qore_eval_adj_sens;
+    config->memory_reset = &dense_qp_qore_memory_reset;
+    config->solver_get = &dense_qp_qore_solver_get;
+    config->terminate = &dense_qp_qore_terminate;
+
+    return;
+}

acados/dense_qp/dense_qp_qore.h

@@ -0,0 +1,128 @@
+/*
+ * Copyright (c) The acados authors.
+ *
+ * This file is part of acados.
+ *
+ * The 2-Clause BSD License
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.;
+ */
+
+
+#ifndef ACADOS_DENSE_QP_DENSE_QP_QORE_H_
+#define ACADOS_DENSE_QP_DENSE_QP_QORE_H_
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+// qore
+#include "qore/QPSOLVER_DENSE/include/qpsolver_dense.h"
+// acados
+#include "acados/dense_qp/dense_qp_common.h"
+#include "acados/utils/types.h"
+
+typedef struct dense_qp_qore_opts_
+{
+    int nsmax;       // maximum size of Schur complement
+    int print_freq;  // print frequency,
+                     // prtfreq  < 0: disable printing;
+                     // prtfreq == 0: print on each call and include working set changes;
+                     // prtfreq  > 0: print on every prtfreq seconds, but do not include working set
+                     // changes;
+    int warm_start;  // warm start with updated matrices H and C
+    int warm_strategy;  // 0: ramp-up from zero homotopy; 1: setup homotopy from the previous
+                        // solution
+    int hot_start;      // hot start with unchanged matrices H and C
+    int max_iter;       // maximum number of iterations
+    int compute_t;      // compute t in qp_out (to have correct residuals in NLP)
+} dense_qp_qore_opts;
+
+typedef struct dense_qp_qore_memory_
+{
+    double *H;
+    double *HH;
+    double *g;
+    double *gg;
+    double *Zl;
+    double *Zu;
+    double *zl;
+    double *zu;
+    double *A;
+    double *b;
+    double *C;
+    double *CC;
+    double *Ct;
+    double *CCt;
+    double *d_lb0;
+    double *d_ub0;
+    double *d_lb;
+    double *d_ub;
+    double *d_lg;
+    double *d_ug;
+    double *d_ls;
+    double *d_us;
+    double *lb;
+    double *ub;
+    int *idxb;
+    int *idxb_stacked;
+    int *idxs;
+    double *prim_sol;
+    double *dual_sol;
+    QoreProblemDense *QP;
+    int num_iter;
+    dense_qp_in *qp_stacked;
+    double time_qp_solver_call;
+    int iter;
+
+} dense_qp_qore_memory;
+
+acados_size_t dense_qp_qore_opts_calculate_size(void *config, dense_qp_dims *dims);
+//
+void *dense_qp_qore_opts_assign(void *config, dense_qp_dims *dims, void *raw_memory);
+//
+void dense_qp_qore_opts_initialize_default(void *config, dense_qp_dims *dims, void *opts_);
+//
+void dense_qp_qore_opts_update(void *config, dense_qp_dims *dims, void *opts_);
+//
+acados_size_t dense_qp_qore_memory_calculate_size(void *config, dense_qp_dims *dims, void *opts_);
+//
+void *dense_qp_qore_memory_assign(void *config, dense_qp_dims *dims, void *opts_, void *raw_memory);
+//
+acados_size_t dense_qp_qore_workspace_calculate_size(void *config, dense_qp_dims *dims, void *opts_);
+//
+int dense_qp_qore(void *config, dense_qp_in *qp_in, dense_qp_out *qp_out, void *opts_, void *memory_, void *work_);
+//
+void dense_qp_qore_eval_sens(void *config_, void *qp_in, void *qp_out, void *opts_, void *mem_, void *work_);
+//
+void dense_qp_qore_memory_reset(void *config, void *qp_in, void *qp_out, void *opts, void *mem, void *work);
+//
+void dense_qp_qore_solver_get(void *config_, void *qp_in_, void *qp_out_, void *opts_, void *mem_, const char *field, int stage, void* value, int size1, int size2);
+//
+void dense_qp_qore_config_initialize_default(void *config);
+
+#ifdef __cplusplus
+} /* extern "C" */
+#endif
+
+#endif  // ACADOS_DENSE_QP_DENSE_QP_QORE_H_

acados/dense_qp/dense_qp_qpoases.c

@@ -0,0 +1,826 @@
+/*
+ * Copyright (c) The acados authors.
+ *
+ * This file is part of acados.
+ *
+ * The 2-Clause BSD License
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.;
+ */
+
+
+// external
+#include <stdlib.h>
+#include <assert.h>
+#include <string.h>
+// blasfeo
+#include "blasfeo_d_aux.h"
+#include "blasfeo_d_blas.h"
+
+/* Ignore compiler warnings from qpOASES */
+#if defined(__clang__)
+#pragma clang diagnostic push
+#pragma clang diagnostic ignored "-Wtautological-pointer-compare"
+#pragma clang diagnostic ignored "-Wunused-parameter"
+#pragma clang diagnostic ignored "-Wunused-function"
+#include "qpOASES_e/QProblem.h"
+#include "qpOASES_e/QProblemB.h"
+#pragma clang diagnostic pop
+#elif defined(__GNUC__)
+#if __GNUC__ >= 6
+#pragma GCC diagnostic push
+#pragma GCC diagnostic ignored "-Wunused-but-set-parameter"
+#pragma GCC diagnostic ignored "-Wunused-parameter"
+#pragma GCC diagnostic ignored "-Wunused-function"
+#include "qpOASES_e/QProblem.h"
+#include "qpOASES_e/QProblemB.h"
+#pragma GCC diagnostic pop
+#else
+#pragma GCC diagnostic ignored "-Wunused-parameter"
+#pragma GCC diagnostic ignored "-Wunused-function"
+#include "qpOASES_e/QProblem.h"
+#include "qpOASES_e/QProblemB.h"
+#endif
+#else
+#include "qpOASES_e/QProblem.h"
+#include "qpOASES_e/QProblemB.h"
+#endif
+
+// acados
+#include "acados/dense_qp/dense_qp_common.h"
+#include "acados/dense_qp/dense_qp_qpoases.h"
+#include "acados/utils/mem.h"
+#include "acados/utils/timing.h"
+#include "acados/utils/print.h"
+#include "acados/utils/math.h"
+
+#include "acados_c/dense_qp_interface.h"
+
+
+/************************************************
+ * opts
+ ************************************************/
+
+acados_size_t dense_qp_qpoases_opts_calculate_size(void *config_, dense_qp_dims *dims)
+{
+    acados_size_t size = 0;
+    size += sizeof(dense_qp_qpoases_opts);
+
+    make_int_multiple_of(8, &size);
+
+    return size;
+}
+
+
+
+void *dense_qp_qpoases_opts_assign(void *config_, dense_qp_dims *dims, void *raw_memory)
+{
+    dense_qp_qpoases_opts *opts;
+
+    char *c_ptr = (char *) raw_memory;
+
+    opts = (dense_qp_qpoases_opts *) c_ptr;
+    c_ptr += sizeof(dense_qp_qpoases_opts);
+
+    assert((char *) raw_memory + dense_qp_qpoases_opts_calculate_size(config_, dims) >= c_ptr);
+
+    return (void *) opts;
+}
+
+
+
+void dense_qp_qpoases_opts_initialize_default(void *config_, dense_qp_dims *dims, void *opts_)
+{
+    dense_qp_qpoases_opts *opts = (dense_qp_qpoases_opts *) opts_;
+
+    opts->max_cputime = 1000.0;
+    opts->warm_start = 1;
+    opts->max_nwsr = 1000;
+    opts->use_precomputed_cholesky = 0;
+    opts->hotstart = 0;
+    opts->set_acado_opts = 1;
+    opts->compute_t = 1;
+    opts->tolerance = 1e-4;
+
+    return;
+}
+
+
+
+void dense_qp_qpoases_opts_update(void *config_, dense_qp_dims *dims, void *opts_)
+{
+    //    dense_qp_qpoases_opts *opts = (dense_qp_qpoases_opts *)opts_;
+
+    return;
+}
+
+
+
+void dense_qp_qpoases_opts_set(void *config_, void *opts_, const char *field, void *value)
+{
+    dense_qp_qpoases_opts *opts = opts_;
+
+    if (!strcmp(field, "tol_stat"))
+    {
+        double *tol = value;
+        opts->tolerance = MIN(opts->tolerance, *tol);
+    }
+    else if (!strcmp(field, "tol_eq"))
+    {
+        double *tol = value;
+        opts->tolerance = MIN(opts->tolerance, *tol);
+    }
+    else if (!strcmp(field, "tol_ineq"))
+    {
+        double *tol = value;
+        opts->tolerance = MIN(opts->tolerance, *tol);
+    }
+    else if (!strcmp(field, "tol_comp"))
+    {
+        double *tol = value;
+        opts->tolerance = MIN(opts->tolerance, *tol);
+    }
+    else if (!strcmp(field, "warm_start"))
+    {
+        int *warm_start = value;
+        if (*warm_start == 0)
+        {
+            opts->warm_start = 0;
+            opts->hotstart = 0;
+        }
+        else if (*warm_start == 1)
+        {
+            opts->warm_start = 1;
+            opts->hotstart = 0;
+        }
+        else if (*warm_start == 2)
+        {
+            opts->warm_start = 1;
+            opts->hotstart = 1;
+        }
+        else
+        {
+            printf("\ndense_qp_qpoases: setting warm_start: supported values are: 0 - cold, 1 - warm, 2 - hot\n");
+            exit(1);
+        }
+    }
+    else if (!strcmp(field, "iter_max"))
+    {
+        int *max_iter = value;
+        opts->max_nwsr = *max_iter;
+    }
+    else
+    {
+        printf("\nerror: dense_qp_qpoases_opts_set: wrong field: %s\n", field);
+        exit(1);
+    }
+
+    return;
+}
+
+
+void dense_qp_qpoases_opts_get(void *config_, void *opts_, const char *field, void *value)
+{
+    // dense_qp_qpoases_opts *opts = opts_;
+    printf("\nerror: dense_qp_qpoases_opts_get: not implemented for field: %s\n", field);
+    exit(1);
+}
+
+
+/************************************************
+ * memory
+ ************************************************/
+
+acados_size_t dense_qp_qpoases_memory_calculate_size(void *config_, dense_qp_dims *dims, void *opts_)
+{
+    dense_qp_dims dims_stacked;
+
+    int nv  = dims->nv;
+    int ne  = dims->ne;
+    int ng  = dims->ng;
+    int nb  = dims->nb;
+    int nsb = dims->nsb;
+    // int nsg = dims->nsg;
+    int ns  = dims->ns;
+
+    int nv2 = nv + 2*ns;
+    int ng2 = (ns > 0) ? ng + nsb : ng;
+    int nb2 = nb - nsb + 2 * ns;
+
+    // size in bytes
+    acados_size_t size = sizeof(dense_qp_qpoases_memory);
+
+    size += 1 * nv * nv * sizeof(double);      // H
+    size += 1 * nv2 * nv2 * sizeof(double);    // HH
+    size += 1 * nv2 * nv2 * sizeof(double);    // R
+    size += 1 * nv2 * ne * sizeof(double);     // A
+    size += 1 * nv * ng * sizeof(double);      // C
+    size += 1 * nv2 * ng2 * sizeof(double);    // CC
+    size += 1 * nv * sizeof(double);           // g
+    size += 1 * nv2 * sizeof(double);          // gg
+    size += 2 * nv2 * sizeof(double);          // d_lb d_ub
+    size += 1 * ne * sizeof(double);           // b
+    size += 2 * nb2 * sizeof(double);          // d_lb0 d_ub0
+    size += 2 * ng * sizeof(double);           // d_lg0 d_ug0
+    size += 2 * ng2 * sizeof(double);          // d_lg d_ug
+    size += 1 * nb * sizeof(int);              // idxb
+    size += 1 * nb2 * sizeof(int);             // idxb_stacked
+    size += 1 * ns * sizeof(int);              // idxs
+    size += 1 * nv2 * sizeof(double);          // prim_sol
+    size += 1 * (nv2 + ng2) * sizeof(double);  // dual_sol
+    size += 6 * ns * sizeof(double);           // Zl, Zu, zl, zu, d_ls, d_us
+
+    if (ns > 0)
+    {
+        dense_qp_stack_slacks_dims(dims, &dims_stacked);
+        size += dense_qp_in_calculate_size(&dims_stacked);
+    }
+
+    if (ng > 0 || ns > 0)  // QProblem
+        size += QProblem_calculateMemorySize(nv2, ng2);
+    else  // QProblemB
+        size += QProblemB_calculateMemorySize(nv);
+
+    make_int_multiple_of(8, &size);
+
+    return size;
+}
+
+
+
+void *dense_qp_qpoases_memory_assign(void *config_, dense_qp_dims *dims, void *opts_,
+                                     void *raw_memory)
+{
+    dense_qp_qpoases_memory *mem;
+    dense_qp_dims dims_stacked;
+
+    int nv  = dims->nv;
+    int ne  = dims->ne;
+    int ng  = dims->ng;
+    int nb  = dims->nb;
+    int nsb = dims->nsb;
+    // int nsg = dims->nsg;
+    int ns  = dims->ns;
+
+    int nv2 = nv + 2*ns;
+    int ng2 = (ns > 0) ? ng + nsb : ng;
+    int nb2 = nb - nsb + 2 * ns;
+
+    // char pointer
+    char *c_ptr = (char *) raw_memory;
+
+    mem = (dense_qp_qpoases_memory *) c_ptr;
+    c_ptr += sizeof(dense_qp_qpoases_memory);
+
+    assert((size_t) c_ptr % 8 == 0 && "memory not 8-byte aligned!");
+
+    if (ns > 0)
+    {
+        dense_qp_stack_slacks_dims(dims, &dims_stacked);
+        mem->qp_stacked = dense_qp_in_assign(&dims_stacked, c_ptr);
+        c_ptr += dense_qp_in_calculate_size(&dims_stacked);
+    }
+    else
+    {
+        mem->qp_stacked = NULL;
+    }
+
+    assert((size_t) c_ptr % 8 == 0 && "memory not 8-byte aligned!");
+
+    assign_and_advance_double(nv * nv, &mem->H, &c_ptr);
+    assign_and_advance_double(nv2 * nv2, &mem->HH, &c_ptr);
+    assign_and_advance_double(nv2 * nv2, &mem->R, &c_ptr);
+    assign_and_advance_double(nv2 * ne, &mem->A, &c_ptr);
+    assign_and_advance_double(nv * ng, &mem->C, &c_ptr);
+    assign_and_advance_double(nv2 * ng2, &mem->CC, &c_ptr);
+    assign_and_advance_double(nv, &mem->g, &c_ptr);
+    assign_and_advance_double(nv2, &mem->gg, &c_ptr);
+    assign_and_advance_double(ne, &mem->b, &c_ptr);
+    assign_and_advance_double(nb2, &mem->d_lb0, &c_ptr);
+    assign_and_advance_double(nb2, &mem->d_ub0, &c_ptr);
+    assign_and_advance_double(nv2, &mem->d_lb, &c_ptr);
+    assign_and_advance_double(nv2, &mem->d_ub, &c_ptr);
+    assign_and_advance_double(ng, &mem->d_lg0, &c_ptr);
+    assign_and_advance_double(ng, &mem->d_ug0, &c_ptr);
+    assign_and_advance_double(ng2, &mem->d_lg, &c_ptr);
+    assign_and_advance_double(ng2, &mem->d_ug, &c_ptr);
+    assign_and_advance_double(ns, &mem->d_ls, &c_ptr);
+    assign_and_advance_double(ns, &mem->d_us, &c_ptr);
+    assign_and_advance_double(ns, &mem->Zl, &c_ptr);
+    assign_and_advance_double(ns, &mem->Zu, &c_ptr);
+    assign_and_advance_double(ns, &mem->zl, &c_ptr);
+    assign_and_advance_double(ns, &mem->zu, &c_ptr);
+    assign_and_advance_double(nv2, &mem->prim_sol, &c_ptr);
+    assign_and_advance_double(nv2 + ng2, &mem->dual_sol, &c_ptr);
+
+    // TODO(dimitris): update assign syntax in qpOASES
+    assert((size_t) c_ptr % 8 == 0 && "double not 8-byte aligned!");
+
+    if (ng > 0 || ns > 0)
+    {  // QProblem
+        QProblem_assignMemory(nv2, ng2, (QProblem **) &(mem->QP), c_ptr);
+        c_ptr += QProblem_calculateMemorySize(nv2, ng2);
+    }
+    else
+    {  // QProblemB
+        QProblemB_assignMemory(nv, (QProblemB **) &(mem->QPB), c_ptr);
+        c_ptr += QProblemB_calculateMemorySize(nv);
+    }
+
+    assign_and_advance_int(nb, &mem->idxb, &c_ptr);
+    assign_and_advance_int(nb2, &mem->idxb_stacked, &c_ptr);
+    assign_and_advance_int(ns, &mem->idxs, &c_ptr);
+
+    assert((char *) raw_memory + dense_qp_qpoases_memory_calculate_size(config_, dims, opts_) >=
+           c_ptr);
+
+    // assign default values to fields stored in the memory
+    mem->first_it = 1;  // only used if hotstart (only constant data matrices) is enabled
+
+    return mem;
+}
+
+
+
+void dense_qp_qpoases_memory_get(void *config_, void *mem_, const char *field, void* value)
+{
+    // qp_solver_config *config = config_;
+    dense_qp_qpoases_memory *mem = mem_;
+
+    if (!strcmp(field, "time_qp_solver_call"))
+    {
+        double *tmp_ptr = value;
+        *tmp_ptr = mem->time_qp_solver_call;
+    }
+    else if (!strcmp(field, "iter"))
+    {
+        int *tmp_ptr = value;
+        *tmp_ptr = mem->iter;
+    }
+    else
+    {
+        printf("\nerror: dense_qp_qpoases_memory_get: field %s not available\n", field);
+        exit(1);
+    }
+
+    return;
+
+}
+
+
+
+/************************************************
+ * workspace
+ ************************************************/
+
+acados_size_t dense_qp_qpoases_workspace_calculate_size(void *config_, dense_qp_dims *dims, void *opts_)
+{
+    return 0;
+}
+
+
+
+/************************************************
+ * functions
+ ************************************************/
+
+int dense_qp_qpoases(void *config_, dense_qp_in *qp_in, dense_qp_out *qp_out, void *opts_,
+                     void *memory_, void *work_)
+{
+    qp_info *info = (qp_info *) qp_out->misc;
+    acados_timer tot_timer, qp_timer, interface_timer;
+
+    acados_tic(&tot_timer);
+    acados_tic(&interface_timer);
+    info->t_computed = 0;
+
+    // cast structures
+    dense_qp_qpoases_opts *opts = (dense_qp_qpoases_opts *) opts_;
+    dense_qp_qpoases_memory *memory = (dense_qp_qpoases_memory *) memory_;
+
+    // extract qpoases data
+    double *H = memory->H;
+    double *HH = memory->HH;
+    double *A = memory->A;
+    double *C = memory->C;
+    double *CC = memory->CC;
+    double *g = memory->g;
+    double *gg = memory->gg;
+    double *b = memory->b;
+    double *d_lb0 = memory->d_lb0;
+    double *d_ub0 = memory->d_ub0;
+    double *d_lb = memory->d_lb;
+    double *d_ub = memory->d_ub;
+    double *d_lg0 = memory->d_lg0;
+    double *d_ug0 = memory->d_ug0;
+    double *d_lg = memory->d_lg;
+    double *d_ug = memory->d_ug;
+    double *Zl = memory->Zl;
+    double *Zu = memory->Zu;
+    double *zl = memory->zl;
+    double *zu = memory->zu;
+    double *d_ls = memory->d_ls;
+    double *d_us = memory->d_us;
+    int *idxb = memory->idxb;
+    int *idxb_stacked = memory->idxb_stacked;
+    int *idxs = memory->idxs;
+    double *prim_sol = memory->prim_sol;
+    double *dual_sol = memory->dual_sol;
+    QProblemB *QPB = memory->QPB;
+    QProblem *QP = memory->QP;
+    dense_qp_in *qp_stacked = memory->qp_stacked;
+
+    // extract dense qp size
+    int nv  = qp_in->dim->nv;
+    // int ne  = qp_in->dim->ne;
+    int ng  = qp_in->dim->ng;
+    int nb  = qp_in->dim->nb;
+    int nsb = qp_in->dim->nsb;
+    // int nsg = qp_in->dim->nsg;
+    int ns  = qp_in->dim->ns;
+
+    int nv2 = nv + 2*ns;
+    int ng2 = (ns > 0) ? ng + nsb : ng;
+    int nb2 = nb - nsb + 2 * ns;
+
+    // fill in the upper triangular of H in dense_qp
+    blasfeo_dtrtr_l(nv, qp_in->Hv, 0, 0, qp_in->Hv, 0, 0);
+
+    // extract data from qp_in in row-major
+    d_dense_qp_get_all_rowmaj(qp_in, H, g, A, b, idxb, d_lb0, d_ub0, C, d_lg0, d_ug0,
+                                 Zl, Zu, zl, zu, idxs, d_ls, d_us);
+
+    // reorder box constraints bounds
+    for (int ii = 0; ii < nv2; ii++)
+    {
+        d_lb[ii] = -QPOASES_INFTY;
+        d_ub[ii] = +QPOASES_INFTY;
+    }
+
+    if (ns > 0)
+    {
+        dense_qp_stack_slacks(qp_in, qp_stacked);
+        d_dense_qp_get_all_rowmaj(qp_stacked, HH, gg, A, b, idxb_stacked, d_lb0, d_ub0, CC, d_lg,
+            d_ug, NULL, NULL, NULL, NULL, NULL, NULL, NULL);
+
+        for (int ii = 0; ii < nb2; ii++)
+        {
+            d_lb[idxb_stacked[ii]] = d_lb0[ii];
+            d_ub[idxb_stacked[ii]] = d_ub0[ii];
+        }
+    }
+    else
+    {
+        for (int ii = 0; ii < nb; ii++)
+        {
+            d_lb[idxb[ii]] = d_lb0[ii];
+            d_ub[idxb[ii]] = d_ub0[ii];
+        }
+    }
+
+    // cholesky factorization of H
+    // blasfeo_dpotrf_l(nvd, qpd->Hv, 0, 0, sR, 0, 0);
+
+    // fill in upper triangular of R
+    // blasfeo_dtrtr_l(nvd, sR, 0, 0, sR, 0, 0);
+
+    // extract R
+    // blasfeo_unpack_dmat(nvd, nvd, sR, 0, 0, R, nvd);
+
+    info->interface_time = acados_toc(&interface_timer);
+    acados_tic(&qp_timer);
+
+    // solve dense qp
+    int nwsr = opts->max_nwsr;
+    double cputime = opts->max_cputime;
+
+    int qpoases_status = 0;
+    if (opts->hotstart == 1)
+    {  // only to be used with fixed data matrices!
+        if (ng > 0 || ns > 0)
+        {  // QProblem
+            if (memory->first_it == 1)
+            {
+                QProblemCON(QP, nv2, ng2, HST_POSDEF);
+                QProblem_setPrintLevel(QP, PL_MEDIUM);
+                // QProblem_setPrintLevel(QP, PL_DEBUG_ITER);
+                if (opts->set_acado_opts)
+                {
+                    static Options options;
+                    Options_setToMPC(&options);
+                    options.terminationTolerance = opts->tolerance;
+                    QProblem_setOptions(QP, options);
+                }
+
+                qpoases_status = (ns > 0) ?
+                    QProblem_init(QP, HH, gg, CC, d_lb, d_ub, d_lg, d_ug, &nwsr, &cputime) :
+                    QProblem_init(QP, H, g, C, d_lb, d_ub, d_lg0, d_ug0, &nwsr, &cputime);
+                memory->first_it = 0;
+
+                QProblem_getPrimalSolution(QP, prim_sol);
+                QProblem_getDualSolution(QP, dual_sol);
+            }
+            else
+            {
+                qpoases_status = (ns > 0) ?
+                    QProblem_hotstart(QP, gg, d_lb, d_ub, d_lg, d_ug, &nwsr, &cputime) :
+                    QProblem_hotstart(QP, g, d_lb, d_ub, d_lg0, d_ug0, &nwsr, &cputime);
+
+                QProblem_getPrimalSolution(QP, prim_sol);
+                QProblem_getDualSolution(QP, dual_sol);
+            }
+        }
+        else
+        {
+            if (memory->first_it == 1)
+            {
+                QProblemBCON(QPB, nv, HST_POSDEF);
+                QProblemB_setPrintLevel(QPB, PL_MEDIUM);
+                // QProblemB_setPrintLevel(QPB, PL_DEBUG_ITER);
+                if (opts->set_acado_opts)
+                {
+                    static Options options;
+                    Options_setToMPC(&options);
+                    options.terminationTolerance = opts->tolerance;
+                    QProblem_setOptions(QP, options);
+                }
+                QProblemB_init(QPB, H, g, d_lb, d_ub, &nwsr, &cputime);
+                memory->first_it = 0;
+
+                QProblemB_getPrimalSolution(QPB, prim_sol);
+                QProblemB_getDualSolution(QPB, dual_sol);
+            }
+            else
+            {
+                QProblemB_hotstart(QPB, g, d_lb, d_ub, &nwsr, &cputime);
+
+                QProblemB_getPrimalSolution(QPB, prim_sol);
+                QProblemB_getDualSolution(QPB, dual_sol);
+            }
+        }
+    }
+    else
+    {  // hotstart = 0
+        if (ng > 0 || ns > 0)
+        {
+            QProblemCON(QP, nv2, ng2, HST_POSDEF);
+            // QProblem_setPrintLevel(QP, PL_HIGH);
+            QProblem_setPrintLevel(QP, PL_DEBUG_ITER);
+            QProblem_printProperties(QP);
+            if (opts->use_precomputed_cholesky == 1)
+            {
+                // NOTE(oj): why are there no options set in this case?
+                // static Options options;
+                // Options_setToDefault( &options );
+                // options.initialStatusBounds = ST_INACTIVE;
+                // options.terminationTolerance = opts->tolerance;
+                // QProblem_setOptions( QP, options );
+
+
+                qpoases_status = (ns > 0) ?
+                    QProblem_initW(QP, HH, gg, CC, d_lb, d_ub, d_lg, d_ug, &nwsr, &cputime,
+                                   /* primal_sol */ NULL, /* dual sol */ NULL,
+                                   /* guessed bounds */ NULL, /* guessed constraints */ NULL,
+                                   /* R */ memory->R) :
+                    QProblem_initW(QP, H, g, C, d_lb, d_ub, d_lg0, d_ug0, &nwsr, &cputime,
+                                   /* primal_sol */ NULL, /* dual sol */ NULL,
+                                   /* guessed bounds */ NULL, /* guessed constraints */ NULL,
+                                   /* R */ memory->R);  // NOTE(dimitris): can pass either NULL or 0
+            }
+            else
+            {
+                if (opts->set_acado_opts)
+                {
+                    static Options options;
+                    Options_setToMPC(&options);
+                    options.terminationTolerance = opts->tolerance;
+                    QProblem_setOptions(QP, options);
+                }
+                if (opts->warm_start)
+                {
+                    qpoases_status = (ns > 0) ?
+                        QProblem_initW(QP, HH, gg, CC, d_lb, d_ub, d_lg, d_ug, &nwsr, &cputime,
+                                      /* primal_sol */ NULL, dual_sol, /* guessed bounds */ NULL,
+                                      /* guessed constraints */ NULL, /* R */ NULL) :
+                        QProblem_initW(QP, H, g, C, d_lb, d_ub, d_lg0, d_ug0, &nwsr,
+                                       &cputime, NULL, dual_sol, NULL, NULL, NULL);
+                }
+                else
+                {
+                    qpoases_status = (ns > 0) ?
+                        QProblem_init(QP, HH, gg, CC, d_lb, d_ub, d_lg, d_ug, &nwsr, &cputime) :
+                        QProblem_init(QP, H, g, C, d_lb, d_ub, d_lg0, d_ug0, &nwsr, &cputime);
+                }
+            }
+            QProblem_getPrimalSolution(QP, prim_sol);
+            QProblem_getDualSolution(QP, dual_sol);
+        }
+        else
+        {  // QProblemB
+            QProblemBCON(QPB, nv, HST_POSDEF);
+            // QProblemB_setPrintLevel(QPB, PL_MEDIUM);
+            QProblemB_setPrintLevel(QPB, PL_DEBUG_ITER);
+            QProblemB_printProperties(QPB);
+            if (opts->use_precomputed_cholesky == 1)
+            {
+                // NOTE(oj): why are there no options set in this case?
+                // static Options options;
+                // Options_setToDefault( &options );
+                // options.initialStatusBounds = ST_INACTIVE;
+                // QProblemB_setOptions( QPB, options );
+                qpoases_status = QProblemB_initW(QPB, H, g, d_lb, d_ub, &nwsr, &cputime,
+                                                 /* primal_sol */ NULL, /* dual sol */ NULL,
+                                                 /* guessed bounds */ NULL,
+                                                 /* R */ memory->R);
+            }
+            else
+            {
+                if (opts->set_acado_opts)
+                {
+                    static Options options;
+                    Options_setToMPC(&options);
+                    options.terminationTolerance = opts->tolerance;
+                    QProblemB_setOptions(QPB, options);
+                }
+                if (opts->warm_start)
+                {
+                    qpoases_status = QProblemB_initW(QPB, H, g, d_lb, d_ub, &nwsr, &cputime,
+                                                     /* primal sol */ NULL, /* dual sol */ dual_sol,
+                                                     /* guessed bounds */ NULL,
+                                                     /* R */ NULL);
+                }
+                else
+                {
+                    qpoases_status = QProblemB_init(QPB, H, g, d_lb, d_ub, &nwsr, &cputime);
+                }
+            }
+            QProblemB_getPrimalSolution(QPB, prim_sol);
+            QProblemB_getDualSolution(QPB, dual_sol);
+        }
+    }
+
+    // save solution statistics to memory
+    memory->cputime = cputime;
+    memory->nwsr = nwsr;
+    info->solve_QP_time = acados_toc(&qp_timer);
+
+    acados_tic(&interface_timer);
+    // copy prim_sol and dual_sol to qp_out
+    blasfeo_pack_dvec(nv2, prim_sol, 1, qp_out->v, 0);
+    blasfeo_dvecse(2 * nb + 2 * ng + 2 * ns, 0.0, qp_out->lam, 0);
+    for (int ii = 0; ii < nb; ii++)
+    {
+        if (dual_sol[idxb[ii]] >= 0.0)
+            qp_out->lam->pa[ii] = dual_sol[idxb[ii]];
+        else
+            qp_out->lam->pa[nb + ng + ii] = -dual_sol[idxb[ii]];
+    }
+
+    for (int ii = 0; ii < ng; ii++)
+    {
+        if (dual_sol[nv2 + ii] >= 0.0)
+            qp_out->lam->pa[nb + ii] = dual_sol[nv2 + ii];
+        else
+            qp_out->lam->pa[2 * nb + ng + ii] = -dual_sol[nv2 + ii];
+    }
+
+    int k = 0;
+    for (int ii = 0; ii < ns; ii++)
+    {
+        int js = idxs[ii];
+
+        double offset_l = 0.0;
+        double offset_u = 0.0;
+
+        if (js < nb)
+        {
+            if (dual_sol[nv2 + ng + k] <= 0.0)  // softened upper box constraints
+            {
+                qp_out->lam->pa[nb + ng + js] = -dual_sol[nv2 + ng + k];
+                offset_u = -dual_sol[nv2 + ng + k];
+            }
+            else  // softened lower box constraints
+            {
+                qp_out->lam->pa[js] = dual_sol[nv2 + ng + k];
+                offset_l = dual_sol[nv2 + ng + k];
+            }
+
+            k++;
+        }
+        else
+        {
+            offset_l = qp_out->lam->pa[nb+js-nb];
+            offset_u = qp_out->lam->pa[2*nb+ng+js-nb];
+        }
+
+        // dual variables for sl >= d_ls
+        if (dual_sol[nv + ii] >= 0)
+            qp_out->lam->pa[2*nb + 2*ng + ii] = dual_sol[nv + ii] - offset_u;
+
+        // dual variables for su >= d_us
+        if (dual_sol[nv + ns + ii] >= 0)
+            qp_out->lam->pa[2*nb + 2*ng + ns + ii] = dual_sol[nv + ns + ii] - offset_l;
+    }
+
+    info->interface_time += acados_toc(&interface_timer);
+    info->total_time = acados_toc(&tot_timer);
+    info->num_iter = nwsr;
+
+    memory->time_qp_solver_call = info->solve_QP_time;
+    memory->iter = nwsr;
+
+    // compute slacks
+    if (opts->compute_t)
+    {
+        dense_qp_compute_t(qp_in, qp_out);
+        info->t_computed = 1;
+    }
+
+    int acados_status = qpoases_status;
+    if (qpoases_status == SUCCESSFUL_RETURN) acados_status = ACADOS_SUCCESS;
+    if (qpoases_status == RET_MAX_NWSR_REACHED) acados_status = ACADOS_MAXITER;
+    return acados_status;
+}
+
+
+
+void dense_qp_qpoases_eval_sens(void *config_, void *qp_in, void *qp_out, void *opts_, void *mem_, void *work_)
+{
+    printf("\nerror: dense_qp_qpoases_eval_sens: not implemented yet\n");
+    exit(1);
+}
+
+
+void dense_qp_qpoases_eval_adj_sens(void *config_, void *qp_in, void *qp_out, void *opts_, void *mem_, void *work_)
+{
+    printf("\nerror: dense_qp_qpoases_eval_adj_sens: not implemented yet\n");
+    exit(1);
+}
+
+
+void dense_qp_qpoases_memory_reset(void *config, void *qp_in, void *qp_out, void *opts, void *mem, void *work)
+{
+    printf("\nerror: dense_qp_qpoases_memory_reset: not implemented yet\n");
+    exit(1);
+}
+
+void dense_qp_qpoases_solver_get(void *config_, void *qp_in_, void *qp_out_, void *opts_, void *mem_, const char *field, int stage, void* value, int size1, int size2)
+{
+    printf("\nerror: dense_qp_qpoases_solver_get: not implemented yet\n");
+    exit(1);
+}
+
+
+
+void dense_qp_qpoases_terminate(void *config_, void *mem_, void *work_)
+{
+    return;
+}
+
+void dense_qp_qpoases_config_initialize_default(void *config_)
+{
+    qp_solver_config *config = config_;
+
+    config->opts_calculate_size = (acados_size_t (*)(void *, void *)) & dense_qp_qpoases_opts_calculate_size;
+    config->opts_assign = (void *(*) (void *, void *, void *) ) & dense_qp_qpoases_opts_assign;
+    config->opts_initialize_default =
+        (void (*)(void *, void *, void *)) & dense_qp_qpoases_opts_initialize_default;
+    config->opts_update = (void (*)(void *, void *, void *)) & dense_qp_qpoases_opts_update;
+    config->opts_set = &dense_qp_qpoases_opts_set;
+    config->opts_get = &dense_qp_qpoases_opts_get;
+    config->memory_calculate_size =
+        (acados_size_t (*)(void *, void *, void *)) & dense_qp_qpoases_memory_calculate_size;
+    config->memory_assign =
+        (void *(*) (void *, void *, void *, void *) ) & dense_qp_qpoases_memory_assign;
+    config->memory_get = &dense_qp_qpoases_memory_get;
+    config->workspace_calculate_size =
+        (acados_size_t (*)(void *, void *, void *)) & dense_qp_qpoases_workspace_calculate_size;
+    config->eval_sens = &dense_qp_qpoases_eval_sens;
+    config->eval_adj_sens = &dense_qp_qpoases_eval_adj_sens;
+    config->evaluate = (int (*)(void *, void *, void *, void *, void *, void *)) & dense_qp_qpoases;
+    config->memory_reset = &dense_qp_qpoases_memory_reset;
+    config->solver_get = &dense_qp_qpoases_solver_get;
+    config->terminate = &dense_qp_qpoases_terminate;
+
+    return;
+}

acados/dense_qp/dense_qp_qpoases.h

@@ -0,0 +1,131 @@
+/*
+ * Copyright (c) The acados authors.
+ *
+ * This file is part of acados.
+ *
+ * The 2-Clause BSD License
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.;
+ */
+
+
+#ifndef ACADOS_DENSE_QP_DENSE_QP_QPOASES_H_
+#define ACADOS_DENSE_QP_DENSE_QP_QPOASES_H_
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+// blasfeo
+#include "blasfeo_common.h"
+
+// acados
+#include "acados/dense_qp/dense_qp_common.h"
+#include "acados/utils/types.h"
+
+typedef struct dense_qp_qpoases_opts_
+{
+    double max_cputime;  // maximum cpu time in seconds
+    int max_nwsr;        // maximum number of working set recalculations
+    int warm_start;      // warm start with dual_sol in memory
+    int use_precomputed_cholesky;
+    int hotstart;  // this option requires constant data matrices! (eg linear MPC, inexact schemes
+                   // with frozen sensitivities)
+    int set_acado_opts;  // use same options as in acado code generation
+    int compute_t;       // compute t in qp_out (to have correct residuals in NLP)
+    double tolerance;  // terminationTolerance
+} dense_qp_qpoases_opts;
+
+typedef struct dense_qp_qpoases_memory_
+{
+    double *H;
+    double *HH;
+    double *R;
+    double *g;
+    double *gg;
+    double *Zl;
+    double *Zu;
+    double *zl;
+    double *zu;
+    double *A;
+    double *b;
+    double *d_lb0;
+    double *d_ub0;
+    double *d_lb;
+    double *d_ub;
+    double *C;
+    double *CC;
+    double *d_lg0;
+    double *d_ug0;
+    double *d_lg;
+    double *d_ug;
+    double *d_ls;
+    double *d_us;
+    int *idxb;
+    int *idxb_stacked;
+    int *idxs;
+    double *prim_sol;
+    double *dual_sol;
+    void *QPB;       // NOTE(giaf): cast to QProblemB to use
+    void *QP;        // NOTE(giaf): cast to QProblem to use
+    double cputime;  // cputime of qpoases
+    int nwsr;        // performed number of working set recalculations
+    int first_it;    // to be used with hotstart
+    dense_qp_in *qp_stacked;
+    double time_qp_solver_call; // equal to cputime
+    int iter;
+
+} dense_qp_qpoases_memory;
+
+acados_size_t dense_qp_qpoases_opts_calculate_size(void *config, dense_qp_dims *dims);
+//
+void *dense_qp_qpoases_opts_assign(void *config, dense_qp_dims *dims, void *raw_memory);
+//
+void dense_qp_qpoases_opts_initialize_default(void *config, dense_qp_dims *dims, void *opts_);
+//
+void dense_qp_qpoases_opts_update(void *config, dense_qp_dims *dims, void *opts_);
+//
+acados_size_t dense_qp_qpoases__memorycalculate_size(void *config, dense_qp_dims *dims, void *opts_);
+//
+void *dense_qp_qpoases_memory_assign(void *config, dense_qp_dims *dims, void *opts_, void *raw_memory);
+//
+acados_size_t dense_qp_qpoases_workspace_calculate_size(void *config, dense_qp_dims *dims, void *opts_);
+//
+int dense_qp_qpoases(void *config, dense_qp_in *qp_in, dense_qp_out *qp_out, void *opts_, void *memory_, void *work_);
+//
+void dense_qp_qpoases_eval_sens(void *config_, void *qp_in, void *qp_out, void *opts_, void *mem_, void *work_);
+//
+void dense_qp_qpoases_memory_reset(void *config_, void *qp_in, void *qp_out, void *opts_, void *mem_, void *work_);
+//
+void dense_qp_qpoases_config_initialize_default(void *config_);
+//
+void dense_qp_qpoases_memory_reset(void *config, void *qp_in, void *qp_out, void *opts, void *mem, void *work);
+//
+void dense_qp_qpoases_solver_get(void *config_, void *qp_in_, void *qp_out_, void *opts_, void *mem_, const char *field, int stage, void* value, int size1, int size2);
+
+
+#ifdef __cplusplus
+} /* extern "C" */
+#endif
+
+#endif  // ACADOS_DENSE_QP_DENSE_QP_QPOASES_H_

acados/ocp_nlp/Makefile

@@ -0,0 +1,67 @@
+#
+# Copyright (c) The acados authors.
+#
+# This file is part of acados.
+#
+# The 2-Clause BSD License
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions are met:
+#
+# 1. Redistributions of source code must retain the above copyright notice,
+# this list of conditions and the following disclaimer.
+#
+# 2. Redistributions in binary form must reproduce the above copyright notice,
+# this list of conditions and the following disclaimer in the documentation
+# and/or other materials provided with the distribution.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.;
+#
+
+
+include ../../Makefile.rule
+
+OBJS =
+
+OBJS += ocp_nlp_common.o
+OBJS += ocp_nlp_cost_common.o
+OBJS += ocp_nlp_cost_ls.o
+OBJS += ocp_nlp_cost_nls.o
+OBJS += ocp_nlp_cost_conl.o
+OBJS += ocp_nlp_cost_external.o
+OBJS += ocp_nlp_constraints_common.o
+OBJS += ocp_nlp_constraints_bgh.o
+OBJS += ocp_nlp_constraints_bgp.o
+OBJS += ocp_nlp_dynamics_common.o
+OBJS += ocp_nlp_dynamics_cont.o
+OBJS += ocp_nlp_dynamics_disc.o
+OBJS += ocp_nlp_globalization_common.o
+OBJS += ocp_nlp_globalization_fixed_step.o
+OBJS += ocp_nlp_globalization_funnel.o
+OBJS += ocp_nlp_globalization_merit_backtracking.o
+OBJS += ocp_nlp_sqp.o
+OBJS += ocp_nlp_sqp_with_feasible_qp.o
+OBJS += ocp_nlp_ddp.o
+OBJS += ocp_nlp_sqp_rti.o
+OBJS += ocp_nlp_reg_common.o
+OBJS += ocp_nlp_reg_convexify.o
+OBJS += ocp_nlp_reg_mirror.o
+OBJS += ocp_nlp_reg_project.o
+OBJS += ocp_nlp_reg_project_reduc_hess.o
+OBJS += ocp_nlp_reg_noreg.o
+
+obj: $(OBJS)
+
+clean:
+	rm -f *.o
+	rm -f *.s

acados/ocp_nlp/ocp_nlp_common.h

@@ -0,0 +1,610 @@
+/*
+ * Copyright (c) The acados authors.
+ *
+ * This file is part of acados.
+ *
+ * The 2-Clause BSD License
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.;
+ */
+
+
+
+/// \defgroup ocp_nlp ocp_nlp
+/// @{
+/// @}
+
+/// \defgroup ocp_nlp_solver ocp_nlp_solver
+/// @{
+/// @}
+
+/// \ingroup ocp_nlp
+/// @{
+
+/// \ingroup ocp_nlp_solver
+/// @{
+
+/// \defgroup ocp_nlp_common ocp_nlp_common
+/// @{
+
+#ifndef ACADOS_OCP_NLP_OCP_NLP_COMMON_H_
+#define ACADOS_OCP_NLP_OCP_NLP_COMMON_H_
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include "acados/ocp_nlp/ocp_nlp_constraints_common.h"
+#include "acados/ocp_nlp/ocp_nlp_cost_common.h"
+#include "acados/ocp_nlp/ocp_nlp_dynamics_common.h"
+#include "acados/ocp_nlp/ocp_nlp_reg_common.h"
+#include "acados/ocp_nlp/ocp_nlp_globalization_common.h"
+#include "acados/ocp_qp/ocp_qp_common.h"
+#include "acados/ocp_qp/ocp_qp_xcond_solver.h"
+#include "acados/sim/sim_common.h"
+#include "acados/utils/external_function_generic.h"
+#include "acados/utils/types.h"
+
+
+
+/************************************************
+ * config
+ ************************************************/
+// NOTE: in here only void* arguments, as ocp_nlp_in etc are defined based on config.
+typedef struct ocp_nlp_config
+{
+    int N;  // number of stages
+    int with_feasible_qp;
+
+    // solver-specific implementations of memory management functions
+    acados_size_t (*opts_calculate_size)(void *config, void *dims);
+    void *(*opts_assign)(void *config, void *dims, void *raw_memory);
+    void (*opts_initialize_default)(void *config, void *dims, void *opts_);
+    void (*opts_update)(void *config, void *dims, void *opts_);
+    acados_size_t (*memory_calculate_size)(void *config, void *dims, void *opts_, void *in);
+    void *(*memory_assign)(void *config, void *dims, void *opts_, void *in, void *raw_memory);
+    acados_size_t (*workspace_calculate_size)(void *config, void *dims, void *opts_, void *in);
+    void (*opts_set)(void *config_, void *opts_, const char *field, void* value);
+    void (*opts_set_at_stage)(void *config_, void *opts_, size_t stage, const char *field, void* value);
+    // evaluate solver // TODO rename into solve
+    int (*evaluate)(void *config, void *dims, void *nlp_in, void *nlp_out, void *opts_, void *mem, void *work);
+    int (*setup_qp_matrices_and_factorize)(void *config, void *dims, void *nlp_in, void *nlp_out, void *opts_, void *mem, void *work);
+
+    void (*eval_kkt_residual)(void *config, void *dims, void *nlp_in, void *nlp_out, void *opts_, void *mem, void *work);
+    void (*eval_param_sens)(void *config, void *dims, void *opts_, void *mem, void *work,
+                            char *field, int stage, int index, void *sens_nlp_out);
+    void (*eval_lagr_grad_p)(void *config, void *dims, void *nlp_in, void *opts_, void *mem, void *work,
+                            const char *field, void *grad_p);
+    void (*eval_solution_sens_adj_p)(void *config_, void *dims_,
+                        void *opts_, void *mem_, void *work_, void *sens_nlp_out,
+                        const char *field, int stage, void *grad_p);
+    void (*step_update)(void *config, void *dims, void *in,
+            void *out_start, void *opts, void *mem, void *work,
+            void *out_destination, void* solver_mem, double alpha, bool full_step_dual);
+    // prepare memory
+    int (*precompute)(void *config, void *dims, void *nlp_in, void *nlp_out, void *opts_, void *mem, void *work);
+    void (*memory_reset_qp_solver)(void *config, void *dims, void *nlp_in, void *nlp_out, void *opts_, void *mem, void *work);
+    // initialize this struct with default values
+    void (*config_initialize_default)(void *config);
+    // general getter
+    void (*get)(void *config_, void *dims, void *mem_, const char *field, void *return_value_);
+    void (*opts_get)(void *config_, void *dims, void *opts_, const char *field, void *return_value_);
+    void (*work_get)(void *config_, void *dims, void *work_, const char *field, void *return_value_);
+    //
+    void (*terminate)(void *config, void *mem, void *work);
+
+    bool (*is_real_time_algorithm)();
+
+
+    // config structs of submodules
+    ocp_qp_xcond_solver_config *qp_solver; // TODO rename xcond_solver
+    ocp_qp_xcond_solver_config *relaxed_qp_solver;
+    ocp_nlp_dynamics_config **dynamics;
+    ocp_nlp_cost_config **cost;
+    ocp_nlp_constraints_config **constraints;
+    ocp_nlp_reg_config *regularize;
+    ocp_nlp_globalization_config *globalization;
+
+} ocp_nlp_config;
+
+//
+acados_size_t ocp_nlp_config_calculate_size(int N);
+//
+ocp_nlp_config *ocp_nlp_config_assign(int N, void *raw_memory);
+
+
+
+/************************************************
+ * dims
+ ************************************************/
+
+/// Structure to store dimensions/number of variables.
+typedef struct ocp_nlp_dims
+{
+    void **cost;
+    void **dynamics;
+    void **constraints;
+    ocp_qp_xcond_solver_dims *qp_solver;  // xcond solver instead ??
+    ocp_qp_xcond_solver_dims *relaxed_qp_solver;  // xcond solver instead ??
+    ocp_nlp_reg_dims *regularize;
+
+    int *nv;  // number of primal variables (states+controls+slacks)
+    int *nx;  // number of differential states
+    int *nu;  // number of inputs
+    int *nz;  // number of algebraic variables
+    int *ns;  // number of slack variables
+    int *np;  // number of parameters
+    // constraints
+    int *ni;  // number of two-sided inequality constraints: nb+ng+nh+ns+nphi
+    int *nb;  // number of two-sided bounds
+    int *ng;  // number of two-sided general linear constraints
+    int *ni_nl;  // number of two-sided nonlinear inequalities
+
+    int np_global;  // number of global parameters
+    int n_global_data;  // size of global_data; expressions that only depend on p_global; detected automatically during code generation
+    int N;    // number of shooting nodes
+
+    void *raw_memory; // Pointer to allocated memory, to be used for freeing
+} ocp_nlp_dims;
+
+//
+acados_size_t ocp_nlp_dims_calculate_size(void *config);
+//
+ocp_nlp_dims *ocp_nlp_dims_assign(void *config, void *raw_memory);
+
+/// Sets the dimension of optimization variables
+/// (states, controls, algebraic variables, slack variables).
+///
+/// \param config_ The configuration struct.
+/// \param dims_ The dimension struct.
+/// \param field The type of optimization variables, either nx, nu, nz, or ns.
+/// \param value_array Number of variables for each stage.
+void ocp_nlp_dims_set_opt_vars(void *config_, void *dims_,
+                               const char *field, const void* value_array);
+
+/// Sets the dimensions of constraints functions for a stage
+/// (bounds on states, bounds on controls, equality constraints,
+/// inequality constraints).
+///
+/// \param config_ The configuration struct.
+/// \param dims_ The dimension struct.
+/// \param stage Stage number.
+/// \param field The type of constraint/bound, either nbx, nbu, ng, or nh.
+/// \param value_field Number of constraints/bounds for the given stage.
+void ocp_nlp_dims_set_constraints(void *config_, void *dims_, int stage,
+                                  const char *field, const void* value_field);
+
+/// Sets the dimensions of the cost terms for a stage.
+///
+/// \param config_ The configuration struct.
+/// \param dims_ The dimension struct.
+/// \param stage Stage number.
+/// \param field Type of cost dimension
+/// \param value_field Number of cost terms/residuals for the given stage.
+void ocp_nlp_dims_set_cost(void *config_, void *dims_, int stage, const char *field,
+                           const void* value_field);
+
+/// Sets the dimensions of the dynamics for a stage.
+///
+/// \param config_ The configuration struct.
+/// \param dims_ The dimension struct.
+/// \param stage Stage number.
+/// \param field TBD
+/// \param value TBD
+void ocp_nlp_dims_set_dynamics(void *config_, void *dims_, int stage, const char *field,
+                               const void* value);
+
+void ocp_nlp_dims_set_global(void *config_, void *dims_, const char *field, int value_field);
+
+
+/************************************************
+ * Inputs
+ ************************************************/
+
+/// Struct for storing the inputs of an OCP NLP solver
+typedef struct ocp_nlp_in
+{
+    /// Timesteps.
+    double *Ts;
+
+    /// Parameter values.
+    double **parameter_values;
+
+    /// Global data
+    double *global_data;
+
+    /// Pointers to cost functions (TBC).
+    void **cost;
+
+    /// Pointers to dynamics functions (TBC).
+    void **dynamics;
+
+    /// Pointers to constraints functions (TBC).
+    void **constraints;
+
+    /// Pointer to allocated memory, to be used for freeing.
+    void *raw_memory;
+
+} ocp_nlp_in;
+
+//
+acados_size_t ocp_nlp_in_calculate_size(ocp_nlp_config *config, ocp_nlp_dims *dims);
+//
+ocp_nlp_in *ocp_nlp_in_assign(ocp_nlp_config *config, ocp_nlp_dims *dims, void *raw_memory);
+
+
+/************************************************
+ * out
+ ************************************************/
+
+typedef struct ocp_nlp_out
+{
+    struct blasfeo_dvec *ux;  // NOTE: this contains [u; x; s_l; s_u]! - rename to uxs?
+    struct blasfeo_dvec *z;  // algebraic variables
+    struct blasfeo_dvec *pi;  // multipliers for dynamics
+    struct blasfeo_dvec *lam;  // inequality multipliers
+
+    // NOTE: the inequalities are internally organized in the following order:
+    // [ lbu lbx lg lh lphi ubu ubx ug uh uphi; lsbu lsbx lsg lsh lsphi usbu usbx usg ush usphi]
+    double inf_norm_res;
+
+    void *raw_memory; // Pointer to allocated memory, to be used for freeing
+
+} ocp_nlp_out;
+
+//
+acados_size_t ocp_nlp_out_calculate_size(ocp_nlp_config *config, ocp_nlp_dims *dims);
+//
+ocp_nlp_out *ocp_nlp_out_assign(ocp_nlp_config *config, ocp_nlp_dims *dims,
+                                void *raw_memory);
+
+
+
+/************************************************
+ * options
+ ************************************************/
+typedef struct ocp_nlp_opts
+{
+    ocp_qp_xcond_solver_opts *qp_solver_opts; // xcond solver opts instead ???
+    void *regularize;
+    void *globalization;  // globalization_opts
+    void **dynamics;     // dynamics_opts
+    void **cost;         // cost_opts
+    void **constraints;  // constraints_opts
+    double levenberg_marquardt;  // LM factor to be added to the hessian before regularization
+    int reuse_workspace;
+    int num_threads;
+    int print_level;
+    int fixed_hess;
+    int log_primal_step_norm; // compute and log the max norm of the primal steps
+    int max_iter; // maximum number of (SQP/DDP) iterations
+    int qp_iter_max; // maximum iter of QP solver, stored to remember.
+    double tau_min;  // minimum value of the barrier parameter, for IPMs
+
+    // Flag for usage of adaptive levenberg marquardt strategy
+    bool with_adaptive_levenberg_marquardt;
+    double adaptive_levenberg_marquardt_lam;
+    double adaptive_levenberg_marquardt_mu_min;
+    double adaptive_levenberg_marquardt_mu0;
+
+    int with_solution_sens_wrt_params;
+    int with_value_sens_wrt_params;
+    double solution_sens_qp_t_lam_min;
+
+    int ext_qp_res;
+    int qp_warm_start;
+
+    bool store_iterates; // flag indicating whether intermediate iterates should be stored
+
+
+} ocp_nlp_opts;
+
+//
+acados_size_t ocp_nlp_opts_calculate_size(void *config, void *dims);
+//
+void *ocp_nlp_opts_assign(void *config, void *dims, void *raw_memory);
+//
+void ocp_nlp_opts_initialize_default(void *config, void *dims, void *opts);
+//
+void ocp_nlp_opts_update(void *config, void *dims, void *opts);
+//
+void ocp_nlp_opts_set(void *config_, void *opts_, const char *field, void* value);
+//
+void ocp_nlp_opts_set_at_stage(void *config, void *opts, int stage, const char *field, void *value);
+
+
+/************************************************
+ * residuals
+ ************************************************/
+
+typedef struct ocp_nlp_res
+{
+    struct blasfeo_dvec *res_stat;  // stationarity
+    struct blasfeo_dvec *res_eq;  // dynamics
+    struct blasfeo_dvec *res_ineq;  // inequality constraints
+    struct blasfeo_dvec *res_comp;  // complementarity
+    struct blasfeo_dvec tmp;  // tmp
+    double inf_norm_res_stat;
+    double inf_norm_res_eq;
+    double inf_norm_res_ineq;
+    double inf_norm_res_comp;
+    acados_size_t memsize;
+} ocp_nlp_res;
+
+//
+acados_size_t ocp_nlp_res_calculate_size(ocp_nlp_dims *dims);
+//
+ocp_nlp_res *ocp_nlp_res_assign(ocp_nlp_dims *dims, void *raw_memory);
+//
+void ocp_nlp_res_get_inf_norm(ocp_nlp_res *res, double *out);
+
+/************************************************
+ * timings
+ ************************************************/
+
+typedef struct ocp_nlp_timings
+{
+    // these timers are reset at every solver call
+    double time_qp_sol;
+    double time_qp_solver_call;
+    double time_qp_xcond;
+    double time_lin;
+    double time_reg;
+    double time_tot;
+    double time_glob;
+    double time_sim;
+    double time_sim_la;
+    double time_sim_ad;
+    // these are not
+    double time_solution_sensitivities;
+    double time_feedback;
+    double time_preparation;
+} ocp_nlp_timings;
+
+
+void ocp_nlp_timings_get(ocp_nlp_config *config, ocp_nlp_timings *timings, const char *field, void *return_value_);
+
+void ocp_nlp_timings_reset(ocp_nlp_timings *timings);
+
+
+/************************************************
+ * memory
+ ************************************************/
+
+typedef struct ocp_nlp_memory
+{
+//    void *qp_solver_mem; // xcond solver mem instead ???
+    ocp_qp_xcond_solver_memory *qp_solver_mem; // xcond solver mem instead ???
+    void *regularize_mem;
+    void *globalization; // globalization memory
+    void **dynamics;     // dynamics memory
+    void **cost;         // cost memory
+    void **constraints;  // constraints memory
+
+    // intermediate iterates
+    struct ocp_nlp_out ** iterates;
+
+    // residuals
+    ocp_nlp_res *nlp_res;
+
+    // timings
+    ocp_nlp_timings *nlp_timings;
+
+    // qp in & out
+    ocp_qp_in *qp_in;
+    ocp_qp_out *qp_out;
+
+    // QP stuff not entering the qp_in struct
+    struct blasfeo_dmat *dzduxt; // dzdux transposed
+    struct blasfeo_dvec *z_alg; // z_alg, output algebraic variables
+
+    struct blasfeo_dvec *cost_grad;
+    struct blasfeo_dvec *ineq_fun;
+    struct blasfeo_dvec *ineq_adj;
+    struct blasfeo_dvec *dyn_fun;
+    struct blasfeo_dvec *dyn_adj;
+
+    // optimal value gradient wrt params
+    struct blasfeo_dmat *jac_lag_stat_p_global;  // jacobian of stationarity condition wrt p_global (nv, np_global)
+    struct blasfeo_dmat *jac_ineq_p_global;  // jacobian of nonlinear inequalities wrt p_global (ni_nl, np_global)
+    struct blasfeo_dmat *jac_dyn_p_global;  // jacobian of dynamics wrt p_global (nx_next, np_global)
+    struct blasfeo_dvec out_np_global;
+
+    double cost_value;
+    double qp_cost_value;
+    double predicted_infeasibility_reduction; // used for funnel globalization
+    double predicted_optimality_reduction; // // used for funnel globalization
+    double objective_multiplier; // used for funnel globalization
+    int compute_hess;
+
+    int status;
+    int iter;
+
+    double adaptive_levenberg_marquardt_mu;
+    double adaptive_levenberg_marquardt_mu_bar;
+
+    bool *set_sim_guess; // indicate if there is new explicitly provided guess for integration variables
+    struct blasfeo_dvec *sim_guess;
+    acados_size_t workspace_size;
+
+} ocp_nlp_memory;
+
+//
+acados_size_t ocp_nlp_memory_calculate_size(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_opts *opts, ocp_nlp_in *in);
+//
+ocp_nlp_memory *ocp_nlp_memory_assign(ocp_nlp_config *config, ocp_nlp_dims *dims,
+                                      ocp_nlp_opts *opts, ocp_nlp_in *in, void *raw_memory);
+//
+void ocp_nlp_memory_get(ocp_nlp_config *config, ocp_nlp_memory *nlp_mem, const char *field, void *return_value_);
+
+/************************************************
+ * workspace
+ ************************************************/
+
+typedef struct ocp_nlp_workspace
+{
+
+    void *qp_work;
+    void **dynamics;     // dynamics_workspace
+    void **cost;         // cost_workspace
+    void **constraints;  // constraints_workspace
+
+    // temp QP in & out (to be used as workspace in param sens) and merit line search
+    ocp_qp_in *tmp_qp_in;
+    ocp_qp_out *tmp_qp_out;
+
+    // qp residuals
+    ocp_qp_res *qp_res;
+    ocp_qp_res_ws *qp_res_ws;
+
+    // for globalization: -> move to module?!
+    ocp_nlp_out *tmp_nlp_out;
+    ocp_nlp_out *weight_merit_fun;
+    struct blasfeo_dvec tmp_nv;
+    struct blasfeo_dvec tmp_2ni;
+    struct blasfeo_dvec dxnext_dy;
+
+    // optimal value gradient wrt params
+    struct blasfeo_dvec tmp_np_global;
+    // AS-RTI
+    double *tmp_nv_double;
+
+    int *tmp_nins;
+
+} ocp_nlp_workspace;
+
+//
+acados_size_t ocp_nlp_workspace_calculate_size(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_opts *opts, ocp_nlp_in *nlp_in);
+//
+ocp_nlp_workspace *ocp_nlp_workspace_assign(ocp_nlp_config *config, ocp_nlp_dims *dims,
+                                ocp_nlp_opts *opts, ocp_nlp_in *nlp_in, ocp_nlp_memory *mem, void *raw_memory);
+
+
+
+/************************************************
+ * function
+ ************************************************/
+//
+void ocp_nlp_alias_memory_to_submodules(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_in *in,
+            ocp_nlp_out *out, ocp_nlp_opts *opts, ocp_nlp_memory *mem, ocp_nlp_workspace *work);
+//
+void ocp_nlp_initialize_submodules(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_in *in,
+            ocp_nlp_out *out, ocp_nlp_opts *opts, ocp_nlp_memory *mem, ocp_nlp_workspace *work);
+//
+void ocp_nlp_set_primal_variable_pointers_in_submodules(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_in *nlp_in,
+                                                       ocp_nlp_out *nlp_out, ocp_nlp_memory *nlp_mem);
+//
+void ocp_nlp_approximate_qp_matrices(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_in *in,
+             ocp_nlp_out *out, ocp_nlp_opts *opts, ocp_nlp_memory *mem, ocp_nlp_workspace *work);
+//
+void ocp_nlp_approximate_qp_vectors_sqp(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_in *in,
+                 ocp_nlp_out *out, ocp_nlp_opts *opts, ocp_nlp_memory *mem, ocp_nlp_workspace *work);
+//
+void ocp_nlp_zero_order_qp_update(ocp_nlp_config *config,
+    ocp_nlp_dims *dims, ocp_nlp_in *in, ocp_nlp_out *out, ocp_nlp_opts *opts,
+    ocp_nlp_memory *mem, ocp_nlp_workspace *work);
+//
+void ocp_nlp_level_c_update(ocp_nlp_config *config,
+    ocp_nlp_dims *dims, ocp_nlp_in *in, ocp_nlp_out *out, ocp_nlp_opts *opts,
+    ocp_nlp_memory *mem, ocp_nlp_workspace *work);
+//
+void ocp_nlp_update_variables_sqp(void *config_, void *dims_,
+            void *in_, void *out_, void *opts_, void *mem_, void *work_,
+            void *out_destination_, void *solver_mem, double alpha, bool full_step_dual);
+//
+int ocp_nlp_precompute_common(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_in *in,
+            ocp_nlp_out *out, ocp_nlp_opts *opts, ocp_nlp_memory *mem, ocp_nlp_workspace *work);
+
+//
+void ocp_nlp_initialize_qp_from_nlp(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_qp_in *qp_in,
+            ocp_nlp_out *out, ocp_qp_out *qp_out);
+
+//
+void ocp_nlp_res_compute(ocp_nlp_dims *dims, ocp_nlp_opts *opts, ocp_nlp_in *in, ocp_nlp_out *out,
+                         ocp_nlp_res *res, ocp_nlp_memory *mem, ocp_nlp_workspace *work);
+//
+void copy_ocp_nlp_out(ocp_nlp_dims *dims, ocp_nlp_out *from, ocp_nlp_out *to);
+
+//
+void ocp_nlp_cost_compute(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_in *in,
+            ocp_nlp_out *out, ocp_nlp_opts *opts, ocp_nlp_memory *mem, ocp_nlp_workspace *work);
+//
+void ocp_nlp_get_cost_value_from_submodules(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_in *in,
+            ocp_nlp_out *out, ocp_nlp_opts *opts, ocp_nlp_memory *mem, ocp_nlp_workspace *work);
+//
+int ocp_nlp_common_setup_qp_matrices_and_factorize(ocp_nlp_config *config, ocp_nlp_dims *dims_, ocp_nlp_in *nlp_in, ocp_nlp_out *nlp_out,
+                ocp_nlp_opts *nlp_opts, ocp_nlp_memory *nlp_mem, ocp_nlp_workspace *nlp_work);
+//
+void ocp_nlp_params_jac_compute(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_in *in, ocp_nlp_opts *opts, ocp_nlp_memory *mem, ocp_nlp_workspace *work);
+
+void ocp_nlp_common_eval_param_sens(ocp_nlp_config *config, ocp_nlp_dims *dims,
+                        ocp_nlp_opts *opts, ocp_nlp_memory *mem, ocp_nlp_workspace *work,
+                        char *field, int stage, int index, ocp_nlp_out *sens_nlp_out);
+//
+void ocp_nlp_common_eval_lagr_grad_p(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_in *in,
+                        ocp_nlp_opts *opts, ocp_nlp_memory *mem, ocp_nlp_workspace *work,
+                        const char *field, void *grad_p);
+//
+void ocp_nlp_common_eval_solution_sens_adj_p(ocp_nlp_config *config, ocp_nlp_dims *dims,
+                        ocp_nlp_opts *opts, ocp_nlp_memory *mem, ocp_nlp_workspace *work,
+                        ocp_nlp_out *sens_nlp_out, const char *field, int stage, void *grad_p);
+//
+void ocp_nlp_add_levenberg_marquardt_term(ocp_nlp_config *config, ocp_nlp_dims *dims,
+    ocp_nlp_in *in, ocp_nlp_out *out, ocp_nlp_opts *opts, ocp_nlp_memory *mem,
+    ocp_nlp_workspace *work, double alpha, int iter, ocp_qp_in *qp_in);
+//
+double ocp_nlp_compute_dual_pi_norm_inf(ocp_nlp_dims *dims, ocp_nlp_out *nlp_out);
+//
+double ocp_nlp_compute_dual_lam_norm_inf(ocp_nlp_dims *dims, ocp_nlp_out *nlp_out);
+//
+double ocp_nlp_get_l1_infeasibility(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_memory *nlp_mem);
+//
+int ocp_nlp_solve_qp_and_correct_dual(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_opts *nlp_opts,
+                     ocp_nlp_memory *nlp_mem, ocp_nlp_workspace *nlp_work,
+                     bool precondensed_lhs, ocp_qp_in *qp_in_, ocp_qp_out *qp_out_,
+                     ocp_qp_xcond_solver *xcond_solver);
+//
+int ocp_nlp_solve_qp(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_opts *nlp_opts,
+    ocp_nlp_memory *nlp_mem, ocp_nlp_workspace *nlp_work,
+    ocp_qp_in *qp_in_, ocp_qp_out *qp_out_,
+    ocp_qp_xcond_solver *xcond_solver);
+//
+double ocp_nlp_compute_qp_objective_value(ocp_nlp_dims *dims, ocp_qp_in *qp_in, ocp_qp_out *qp_out, ocp_nlp_workspace *nlp_work);
+//
+double ocp_nlp_compute_gradient_directional_derivative(ocp_nlp_dims *dims, ocp_qp_in *qp_in, ocp_qp_out *qp_out);
+
+// print / debug functionality
+void ocp_nlp_dump_qp_out_to_file(ocp_qp_out *qp_out, int sqp_iter, int soc);
+void ocp_nlp_dump_qp_in_to_file(ocp_qp_in *qp_in, int sqp_iter, int soc);
+void ocp_nlp_common_print_iteration_header();
+void ocp_nlp_common_print_iteration(int iter_count, ocp_nlp_res *nlp_res);
+
+
+#ifdef __cplusplus
+} /* extern "C" */
+#endif
+
+#endif  // ACADOS_OCP_NLP_OCP_NLP_COMMON_H_
+/// @}
+/// @}
+/// @}

acados/ocp_nlp/ocp_nlp_constraints_bgh.h

@@ -0,0 +1,255 @@
+/*
+ * Copyright (c) The acados authors.
+ *
+ * This file is part of acados.
+ *
+ * The 2-Clause BSD License
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.;
+ */
+
+
+/// \addtogroup ocp_nlp
+/// @{
+/// \addtogroup ocp_nlp_constraints
+/// @{
+
+#ifndef ACADOS_OCP_NLP_OCP_NLP_CONSTRAINTS_BGH_H_
+#define ACADOS_OCP_NLP_OCP_NLP_CONSTRAINTS_BGH_H_
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+// acados
+#include "acados/ocp_qp/ocp_qp_common.h"
+#include "acados/ocp_nlp/ocp_nlp_constraints_common.h"
+#include "acados/utils/external_function_generic.h"
+#include "acados/utils/types.h"
+
+
+
+/************************************************
+ * dims
+ ************************************************/
+
+typedef struct
+{
+    int nx;
+    int nu;
+    int nz;
+    int nb;  // nbx + nbu
+    int nbu; // number of input box constraints
+    int nbx; // number of state box constraints
+    int ng;  // number of general linear constraints
+    int nh;  // number of nonlinear path constraints
+    int ns;  // nsbu + nsbx + nsg + nsh
+    int nsbu;  // number of softened input bounds
+    int nsbx;  // number of softened state bounds
+    int nsg;  // number of softened general linear constraints
+    int nsh;  // number of softened nonlinear constraints
+    int nbue; // number of input box constraints which are equality
+    int nbxe; // number of state box constraints which are equality
+    int nge;  // number of general linear constraints which are equality
+    int nhe;  // number of nonlinear path constraints which are equality
+    int np_global;
+} ocp_nlp_constraints_bgh_dims;
+
+//
+acados_size_t ocp_nlp_constraints_bgh_dims_calculate_size(void *config);
+//
+void *ocp_nlp_constraints_bgh_dims_assign(void *config, void *raw_memory);
+//
+void ocp_nlp_constraints_bgh_dims_get(void *config_, void *dims_, const char *field, int* value);
+//
+void ocp_nlp_constraints_bgh_dims_set(void *config_, void *dims_,
+                                      const char *field, const int* value);
+
+
+/************************************************
+ * model
+ ************************************************/
+
+typedef struct
+{
+    int *idxb;
+    int *idxs;
+    int *idxe;
+    struct blasfeo_dvec d;  // gathers bounds
+    struct blasfeo_dmat DCt;  // general linear constraint matrix
+            // lg <= [D, C] * [u; x] <= ug
+    external_function_generic *nl_constr_h_fun;  // nonlinear: lh <= h(x,u) <= uh
+    external_function_generic *nl_constr_h_fun_jac;  // nonlinear: lh <= h(x,u) <= uh
+    external_function_generic *nl_constr_h_fun_jac_hess;  // nonlinear: lh <= h(x,u) <= uh
+    external_function_generic *nl_constr_h_jac_p_hess_xu_p;
+    external_function_generic *nl_constr_h_adj_p;
+} ocp_nlp_constraints_bgh_model;
+
+//
+acados_size_t ocp_nlp_constraints_bgh_model_calculate_size(void *config, void *dims);
+//
+void *ocp_nlp_constraints_bgh_model_assign(void *config, void *dims, void *raw_memory);
+//
+int ocp_nlp_constraints_bgh_model_set(void *config_, void *dims_,
+                         void *model_, const char *field, void *value);
+
+//
+void ocp_nlp_constraints_bgh_model_get(void *config_, void *dims_,
+                         void *model_, const char *field, void *value);
+
+
+/************************************************
+ * options
+ ************************************************/
+
+typedef struct
+{
+    int compute_adj;
+    int compute_hess;
+    int with_solution_sens_wrt_params;
+} ocp_nlp_constraints_bgh_opts;
+
+//
+acados_size_t ocp_nlp_constraints_bgh_opts_calculate_size(void *config, void *dims);
+//
+void *ocp_nlp_constraints_bgh_opts_assign(void *config, void *dims, void *raw_memory);
+//
+void ocp_nlp_constraints_bgh_opts_initialize_default(void *config, void *dims, void *opts);
+//
+void ocp_nlp_constraints_bgh_opts_update(void *config, void *dims, void *opts);
+//
+void ocp_nlp_constraints_bgh_opts_set(void *config, void *opts, char *field, void *value);
+
+
+
+/************************************************
+ * memory
+ ************************************************/
+
+typedef struct
+{
+    struct blasfeo_dvec fun;
+    struct blasfeo_dvec adj;
+    struct blasfeo_dmat *jac_lag_stat_p_global;  // pointer to jacobian of to stationarity condition wrt p_global (nx+nu, np_global)
+    struct blasfeo_dmat *jac_ineq_p_global;  // jacobian of h wrt p_global (nh, np_global)
+    struct blasfeo_dvec constr_eval_no_bounds;
+    struct blasfeo_dvec *ux;     // pointer to ux in nlp_out
+    struct blasfeo_dvec *lam;    // pointer to lam in nlp_out
+    struct blasfeo_dvec *z_alg;  // pointer to z_alg in ocp_nlp memory
+    struct blasfeo_dvec *dmask;  // pointer to dmask in ocp_nlp memory
+    struct blasfeo_dmat *DCt;    // pointer to DCt in qp_in
+    struct blasfeo_dmat *RSQrq;  // pointer to RSQrq in qp_in
+    struct blasfeo_dmat *dzduxt; // pointer to dzduxt in ocp_nlp memory
+    int *idxb;                   // pointer to idxb[ii] in qp_in
+    int *idxs_rev;               // pointer to idxs_rev[ii] in qp_in
+    int *idxe;                   // pointer to idxe[ii] in qp_in
+} ocp_nlp_constraints_bgh_memory;
+
+//
+acados_size_t ocp_nlp_constraints_bgh_memory_calculate_size(void *config, void *dims, void *opts);
+//
+void *ocp_nlp_constraints_bgh_memory_assign(void *config, void *dims, void *opts, void *raw_memory);
+//
+struct blasfeo_dvec *ocp_nlp_constraints_bgh_memory_get_fun_ptr(void *memory_);
+//
+struct blasfeo_dvec *ocp_nlp_constraints_bgh_memory_get_adj_ptr(void *memory_);
+//
+void ocp_nlp_constraints_bgh_memory_set_ux_ptr(struct blasfeo_dvec *ux, void *memory_);
+//
+void ocp_nlp_constraints_bgh_memory_set_lam_ptr(struct blasfeo_dvec *lam, void *memory_);
+//
+void ocp_nlp_constraints_bgh_memory_set_DCt_ptr(struct blasfeo_dmat *DCt, void *memory);
+//
+void ocp_nlp_constraints_bgh_memory_set_RSQrq_ptr(struct blasfeo_dmat *RSQrq, void *memory_);
+//
+void ocp_nlp_constraints_bgh_memory_set_z_alg_ptr(struct blasfeo_dvec *z_alg, void *memory_);
+//
+void ocp_nlp_constraints_bgh_memory_set_dzduxt_ptr(struct blasfeo_dmat *dzduxt, void *memory_);
+//
+void ocp_nlp_constraints_bgh_memory_set_idxb_ptr(int *idxb, void *memory_);
+//
+void ocp_nlp_constraints_bgh_memory_set_idxs_rev_ptr(int *idxs_rev, void *memory_);
+//
+void ocp_nlp_constraints_bgh_memory_set_idxe_ptr(int *idxe, void *memory_);
+//
+void ocp_nlp_constraints_bgh_memory_set_jac_lag_stat_p_global_ptr(struct blasfeo_dmat *jac_lag_stat_p_global, void *memory_);
+//
+void ocp_nlp_constraints_bgh_memory_set_jac_ineq_p_global_ptr(struct blasfeo_dmat *jac_ineq_p_global, void *memory_);
+
+
+
+/************************************************
+ * workspace
+ ************************************************/
+
+typedef struct
+{
+    struct blasfeo_dmat jac_lag_p_global;
+    struct blasfeo_dmat tmp_nv_nv;
+    struct blasfeo_dmat tmp_nz_nh;
+    struct blasfeo_dmat tmp_nv_nh;
+    struct blasfeo_dmat tmp_nz_nv;
+    struct blasfeo_dmat hess_z;
+    struct blasfeo_dvec tmp_ni;
+    struct blasfeo_dvec tmp_nh;
+} ocp_nlp_constraints_bgh_workspace;
+
+//
+acados_size_t ocp_nlp_constraints_bgh_workspace_calculate_size(void *config, void *dims, void *opts);
+//
+size_t ocp_nlp_constraints_bgh_get_external_fun_workspace_requirement(void *config_, void *dims_, void *opts_, void *model_);
+//
+void ocp_nlp_constraints_bgh_set_external_fun_workspaces(void *config_, void *dims_, void *opts_, void *model_, void *workspace_);
+
+/* functions */
+
+//
+void ocp_nlp_constraints_bgh_config_initialize_default(void *config, int stage);
+//
+void ocp_nlp_constraints_bgh_initialize(void *config, void *dims, void *model, void *opts,
+                                    void *mem, void *work);
+//
+void ocp_nlp_constraints_bgh_update_qp_matrices(void *config_, void *dims, void *model_,
+                                            void *opts_, void *memory_, void *work_);
+
+//
+void ocp_nlp_constraints_bgh_compute_fun(void *config_, void *dims, void *model_,
+                                            void *opts_, void *memory_, void *work_);
+//
+void ocp_nlp_constraints_bgh_bounds_update(void *config_, void *dims, void *model_,
+                                            void *opts_, void *memory_, void *work_);
+//
+void ocp_nlp_constraints_bgh_compute_jac_hess_p(void *config_, void *dims_, void *model_,
+                                            void *opts_, void *memory_, void *work_);
+//
+void ocp_nlp_constraints_bgh_compute_adj_p(void* config_, void *dims_, void *model_,
+                                    void *opts_, void *mem_, void *work_, struct blasfeo_dvec *out);
+
+
+#ifdef __cplusplus
+} /* extern "C" */
+#endif
+
+#endif  // ACADOS_OCP_NLP_OCP_NLP_CONSTRAINTS_BGH_H_
+/// @}
+/// @}

acados/ocp_nlp/ocp_nlp_constraints_bgp.h

@@ -0,0 +1,227 @@
+/*
+ * Copyright (c) The acados authors.
+ *
+ * This file is part of acados.
+ *
+ * The 2-Clause BSD License
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.;
+ */
+
+
+/// \addtogroup ocp_nlp
+/// @{
+/// \addtogroup ocp_nlp_constraints
+/// @{
+
+#ifndef ACADOS_OCP_NLP_OCP_NLP_CONSTRAINTS_BGP_H_
+#define ACADOS_OCP_NLP_OCP_NLP_CONSTRAINTS_BGP_H_
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+// acados
+#include "acados/ocp_qp/ocp_qp_common.h"
+#include "acados/ocp_nlp/ocp_nlp_constraints_common.h"
+#include "acados/utils/external_function_generic.h"
+#include "acados/utils/types.h"
+
+
+
+/* dims */
+
+typedef struct
+{
+    int nx;
+    int nu;
+    int nz;
+    int nb;   // nbx + nbu
+    int nbu;
+    int nbx;
+    int ng;   // number of general linear constraints
+    int nphi; // dimension of convex outer part
+    int ns;   // nsbu + nsbx + nsg + nsphi
+    int nsbu; // number of softened input bounds
+    int nsbx; // number of softened state bounds
+    int nsg;  // number of softened general linear constraints
+    int nsphi;  // number of softened nonlinear constraints
+    int nr;   // dimension of nonlinear function in convex_over_nonlinear constraint
+    int nbue; // number of input box constraints which are equality
+    int nbxe; // number of state box constraints which are equality
+    int nge;  // number of general linear constraints which are equality
+    int nphie;  // number of nonlinear path constraints which are equality
+} ocp_nlp_constraints_bgp_dims;
+
+//
+acados_size_t ocp_nlp_constraints_bgp_dims_calculate_size(void *config);
+//
+void *ocp_nlp_constraints_bgp_dims_assign(void *config, void *raw_memory);
+//
+void ocp_nlp_constraints_bgp_dims_get(void *config_, void *dims_, const char *field, int* value);
+
+
+/* model */
+
+typedef struct
+{
+    //  ocp_nlp_constraints_bgp_dims *dims;
+    int *idxb;
+    int *idxs;
+    int *idxe;
+    struct blasfeo_dvec d;
+    struct blasfeo_dmat DCt;
+    external_function_generic *nl_constr_phi_o_r_fun_phi_jac_ux_z_phi_hess_r_jac_ux;
+    external_function_generic *nl_constr_phi_o_r_fun;
+    external_function_generic *nl_constr_r_fun_jac;
+} ocp_nlp_constraints_bgp_model;
+
+//
+acados_size_t ocp_nlp_constraints_bgp_calculate_size(void *config, void *dims);
+//
+void *ocp_nlp_constraints_bgp_assign(void *config, void *dims, void *raw_memory);
+//
+int ocp_nlp_constraints_bgp_model_set(void *config_, void *dims_,
+                         void *model_, const char *field, void *value);
+//
+void ocp_nlp_constraints_bgp_model_get(void *config_, void *dims_,
+                         void *model_, const char *field, void *value);
+
+/* options */
+
+typedef struct
+{
+    int compute_adj;
+    int compute_hess;
+} ocp_nlp_constraints_bgp_opts;
+
+//
+acados_size_t ocp_nlp_constraints_bgp_opts_calculate_size(void *config, void *dims);
+//
+void *ocp_nlp_constraints_bgp_opts_assign(void *config, void *dims, void *raw_memory);
+//
+void ocp_nlp_constraints_bgp_opts_initialize_default(void *config, void *dims, void *opts);
+//
+void ocp_nlp_constraints_bgp_opts_update(void *config, void *dims, void *opts);
+//
+void ocp_nlp_constraints_bgp_opts_set(void *config, void *opts, char *field, void *value);
+
+/* memory */
+
+typedef struct
+{
+    struct blasfeo_dvec fun;
+    struct blasfeo_dvec adj;
+    struct blasfeo_dvec constr_eval_no_bounds;
+    struct blasfeo_dvec *ux;     // pointer to ux in nlp_out
+    struct blasfeo_dvec *lam;    // pointer to lam in nlp_out
+    struct blasfeo_dvec *z_alg;  // pointer to z_alg in ocp_nlp memory
+    struct blasfeo_dvec *dmask;  // pointer to dmask in qp_in
+    struct blasfeo_dmat *DCt;    // pointer to DCt in qp_in
+    struct blasfeo_dmat *RSQrq;  // pointer to RSQrq in qp_in
+    struct blasfeo_dmat *dzduxt; // pointer to dzduxt in ocp_nlp memory
+    int *idxb;                   // pointer to idxb[ii] in qp_in
+    int *idxs_rev;                   // pointer to idxs_rev[ii] in qp_in
+    int *idxe;                   // pointer to idxe[ii] in qp_in
+} ocp_nlp_constraints_bgp_memory;
+
+//
+acados_size_t ocp_nlp_constraints_bgp_memory_calculate_size(void *config, void *dims, void *opts);
+//
+void *ocp_nlp_constraints_bgp_memory_assign(void *config, void *dims, void *opts,
+    void *raw_memory);
+//
+struct blasfeo_dvec *ocp_nlp_constraints_bgp_memory_get_fun_ptr(void *memory_);
+//
+struct blasfeo_dvec *ocp_nlp_constraints_bgp_memory_get_adj_ptr(void *memory_);
+//
+void ocp_nlp_constraints_bgp_memory_set_ux_ptr(struct blasfeo_dvec *ux, void *memory_);
+//
+void ocp_nlp_constraints_bgp_memory_set_lam_ptr(struct blasfeo_dvec *lam, void *memory_);
+//
+void ocp_nlp_constraints_bgp_memory_set_DCt_ptr(struct blasfeo_dmat *DCt, void *memory);
+//
+void ocp_nlp_constraints_bgp_memory_set_z_alg_ptr(struct blasfeo_dvec *z_alg, void *memory_);
+//
+void ocp_nlp_constraints_bgp_memory_set_dmask_ptr(struct blasfeo_dvec *dmask, void *memory_);
+//
+void ocp_nlp_constraints_bgp_memory_set_dzduxt_ptr(struct blasfeo_dmat *dzduxt, void *memory_);
+//
+void ocp_nlp_constraints_bgp_memory_set_idxb_ptr(int *idxb, void *memory_);
+//
+void ocp_nlp_constraints_bgp_memory_set_idxs_rev_ptr(int *idxs_rev, void *memory_);
+//
+void ocp_nlp_constraints_bgp_memory_set_idxe_ptr(int *idxe, void *memory_);
+//
+void ocp_nlp_constraints_bgp_memory_set_jac_lag_stat_p_global_ptr(struct blasfeo_dmat *jac_lag_stat_p_global, void *memory_);
+//
+void ocp_nlp_constraints_bgp_memory_set_jac_ineq_p_global_ptr(struct blasfeo_dmat *jac_ineq_p_global, void *memory_);
+
+
+/* workspace */
+
+typedef struct
+{
+    struct blasfeo_dmat jac_r_ux_tran;
+    struct blasfeo_dmat tmp_nr_nphi_nr;
+    struct blasfeo_dmat tmp_nv_nr;
+    struct blasfeo_dmat tmp_nv_nphi;
+    struct blasfeo_dmat tmp_nz_nphi;
+    struct blasfeo_dvec tmp_ni;
+} ocp_nlp_constraints_bgp_workspace;
+
+//
+acados_size_t ocp_nlp_constraints_bgp_workspace_calculate_size(void *config, void *dims, void *opts);
+//
+size_t ocp_nlp_constraints_bgp_get_external_fun_workspace_requirement(void *config_, void *dims_, void *opts_, void *model_);
+//
+void ocp_nlp_constraints_bgp_set_external_fun_workspaces(void *config_, void *dims_, void *opts_, void *model_, void *workspace_);
+
+/* functions */
+
+//
+void ocp_nlp_constraints_bgp_config_initialize_default(void *config, int stage);
+//
+void ocp_nlp_constraints_bgp_initialize(void *config, void *dims, void *model,
+        void *opts, void *mem, void *work);
+//
+void ocp_nlp_constraints_bgp_update_qp_matrices(void *config_, void *dims,
+        void *model_, void *opts_, void *memory_, void *work_);
+//
+void ocp_nlp_constraints_bgp_compute_fun(void *config_, void *dims,
+        void *model_, void *opts_, void *memory_, void *work_);
+//
+void ocp_nlp_constraints_bgp_update_qp_vectors(void *config_, void *dims, void *model_,
+        void *opts_, void *memory_, void *work_);
+//
+void ocp_nlp_constraints_bgp_compute_jac_hess_p(void *config_, void *dims, void *model_,
+        void *opts_, void *memory_, void *work_);
+
+
+#ifdef __cplusplus
+} /* extern "C" */
+#endif
+
+#endif  // ACADOS_OCP_NLP_OCP_NLP_CONSTRAINTS_BGP_H_
+/// @}
+/// @}

acados/ocp_nlp/ocp_nlp_constraints_common.c

@@ -0,0 +1,68 @@
+/*
+ * Copyright (c) The acados authors.
+ *
+ * This file is part of acados.
+ *
+ * The 2-Clause BSD License
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.;
+ */
+
+
+#include "acados/ocp_nlp/ocp_nlp_constraints_common.h"
+
+#include <assert.h>
+#include <stdlib.h>
+#include <string.h>
+
+// blasfeo
+#include "blasfeo_d_aux.h"
+#include "blasfeo_d_blas.h"
+// acados
+#include "acados/ocp_qp/ocp_qp_common.h"
+#include "acados/utils/mem.h"
+
+/************************************************
+ * config
+ ************************************************/
+
+acados_size_t ocp_nlp_constraints_config_calculate_size()
+{
+    acados_size_t size = 0;
+
+    size += sizeof(ocp_nlp_constraints_config);
+
+    return size;
+}
+
+
+
+ocp_nlp_constraints_config *ocp_nlp_constraints_config_assign(void *raw_memory)
+{
+    char *c_ptr = raw_memory;
+
+    ocp_nlp_constraints_config *config = (ocp_nlp_constraints_config *) c_ptr;
+    c_ptr += sizeof(ocp_nlp_constraints_config);
+
+    return config;
+}

acados/ocp_nlp/ocp_nlp_constraints_common.h

@@ -0,0 +1,117 @@
+/*
+ * Copyright (c) The acados authors.
+ *
+ * This file is part of acados.
+ *
+ * The 2-Clause BSD License
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.;
+ */
+
+
+/// \ingroup ocp_nlp
+/// @{
+
+/// \defgroup ocp_nlp_constraints ocp_nlp_constraints
+/// @{
+
+#ifndef ACADOS_OCP_NLP_OCP_NLP_CONSTRAINTS_COMMON_H_
+#define ACADOS_OCP_NLP_OCP_NLP_CONSTRAINTS_COMMON_H_
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+// acados
+#include "acados/ocp_qp/ocp_qp_common.h"
+#include "acados/utils/external_function_generic.h"
+#include "acados/utils/types.h"
+
+
+
+/************************************************
+ * config
+ ************************************************/
+
+typedef struct
+{
+    acados_size_t (*dims_calculate_size)(void *config);
+    void *(*dims_assign)(void *config, void *raw_memory);
+    acados_size_t (*model_calculate_size)(void *config, void *dims);
+    void *(*model_assign)(void *config, void *dims, void *raw_memory);
+    int (*model_set)(void *config_, void *dims_, void *model_, const char *field, void *value);
+    void (*model_get)(void *config_, void *dims_, void *model_, const char *field, void *value);
+    acados_size_t (*opts_calculate_size)(void *config, void *dims);
+    void *(*opts_assign)(void *config, void *dims, void *raw_memory);
+    void (*opts_initialize_default)(void *config, void *dims, void *opts);
+    void (*opts_update)(void *config, void *dims, void *opts);
+    void (*opts_set)(void *config, void *opts, char *field, void *value);
+    acados_size_t (*memory_calculate_size)(void *config, void *dims, void *opts);
+    struct blasfeo_dvec *(*memory_get_fun_ptr)(void *memory);
+    struct blasfeo_dvec *(*memory_get_adj_ptr)(void *memory);
+    void (*memory_set_ux_ptr)(struct blasfeo_dvec *ux, void *memory);
+    void (*memory_set_lam_ptr)(struct blasfeo_dvec *lam, void *memory);
+    void (*memory_set_DCt_ptr)(struct blasfeo_dmat *DCt, void *memory);
+    void (*memory_set_RSQrq_ptr)(struct blasfeo_dmat *RSQrq, void *memory);
+    void (*memory_set_z_alg_ptr)(struct blasfeo_dvec *z_alg, void *memory);
+    void (*memory_set_dmask_ptr)(struct blasfeo_dvec *dmask, void *memory);
+    void (*memory_set_dzdux_tran_ptr)(struct blasfeo_dmat *dzduxt, void *memory);
+    void (*memory_set_idxb_ptr)(int *idxb, void *memory);
+    void (*memory_set_idxs_rev_ptr)(int *idxs_rev, void *memory);
+    void (*memory_set_idxe_ptr)(int *idxe, void *memory);
+    void (*memory_set_jac_lag_stat_p_global_ptr)(struct blasfeo_dmat *jac_lag_stat_p_global, void *memory);
+    void (*memory_set_jac_ineq_p_global_ptr)(struct blasfeo_dmat *jac_ineq_p_global, void *memory);
+
+    void *(*memory_assign)(void *config, void *dims, void *opts, void *raw_memory);
+    acados_size_t (*workspace_calculate_size)(void *config, void *dims, void *opts);
+    acados_size_t (*get_external_fun_workspace_requirement)(void *config, void *dims, void *opts_, void *in);
+    void (*set_external_fun_workspaces)(void *config, void *dims, void *opts_, void *in, void *work_);
+    void (*initialize)(void *config, void *dims, void *model, void *opts, void *mem, void *work);
+    //
+    void (*update_qp_matrices)(void *config, void *dims, void *model, void *opts, void *mem, void *work);
+    void (*update_qp_vectors)(void *config, void *dims, void *model, void *opts, void *mem, void *work);
+    void (*compute_fun)(void *config, void *dims, void *model, void *opts, void *mem, void *work);
+    void (*compute_jac_hess_p)(void *config, void *dims, void *model, void *opts, void *mem, void *work);
+    void (*compute_adj_p)(void *config, void *dims, void *model, void *opts, void *memory, void *work, struct blasfeo_dvec *out);
+    void (*config_initialize_default)(void *config, int stage);
+    // dimension setters
+    void (*dims_set)(void *config_, void *dims_, const char *field, const int *value);
+    void (*dims_get)(void *config_, void *dims_, const char *field, int* value);
+    // stage information
+    int stage;
+} ocp_nlp_constraints_config;
+
+//
+acados_size_t ocp_nlp_constraints_config_calculate_size();
+//
+ocp_nlp_constraints_config *ocp_nlp_constraints_config_assign(void *raw_memory);
+
+
+
+#ifdef __cplusplus
+} /* extern "C" */
+#endif
+
+#endif  // ACADOS_OCP_NLP_OCP_NLP_CONSTRAINTS_COMMON_H_
+/// @}
+/// @}

acados/ocp_nlp/ocp_nlp_cost_common.c

@@ -0,0 +1,67 @@
+/*
+ * Copyright (c) The acados authors.
+ *
+ * This file is part of acados.
+ *
+ * The 2-Clause BSD License
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.;
+ */
+
+
+#include "acados/ocp_nlp/ocp_nlp_cost_common.h"
+
+#include <assert.h>
+#include <stdlib.h>
+#include <string.h>
+
+// blasfeo
+#include "blasfeo_d_aux.h"
+#include "blasfeo_d_blas.h"
+// acados
+#include "acados/utils/mem.h"
+
+
+
+/************************************************
+ * config
+ ************************************************/
+
+acados_size_t ocp_nlp_cost_config_calculate_size()
+{
+    acados_size_t size = 0;
+
+    size += sizeof(ocp_nlp_cost_config);
+
+    return size;
+}
+
+ocp_nlp_cost_config *ocp_nlp_cost_config_assign(void *raw_memory)
+{
+    char *c_ptr = raw_memory;
+
+    ocp_nlp_cost_config *config = (ocp_nlp_cost_config *) c_ptr;
+    c_ptr += sizeof(ocp_nlp_cost_config);
+
+    return config;
+}

acados/ocp_nlp/ocp_nlp_cost_common.h

@@ -0,0 +1,119 @@
+/*
+ * Copyright (c) The acados authors.
+ *
+ * This file is part of acados.
+ *
+ * The 2-Clause BSD License
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.;
+ */
+
+
+///
+/// \defgroup ocp_nlp_cost ocp_nlp_cost
+///
+
+/// \addtogroup ocp_nlp_cost ocp_nlp_cost
+/// @{
+/// \addtogroup ocp_nlp_cost_common ocp_nlp_cost_common
+/// @{
+
+#ifndef ACADOS_OCP_NLP_OCP_NLP_COST_COMMON_H_
+#define ACADOS_OCP_NLP_OCP_NLP_COST_COMMON_H_
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+// acados
+#include "acados/utils/external_function_generic.h"
+#include "acados/utils/types.h"
+
+
+
+/************************************************
+ * config
+ ************************************************/
+
+typedef struct
+{
+    acados_size_t (*dims_calculate_size)(void *config);
+    void *(*dims_assign)(void *config, void *raw_memory);
+    void (*dims_set)(void *config_, void *dims_, const char *field, int *value);
+    void (*dims_get)(void *config_, void *dims_, const char *field, int *value);
+    acados_size_t (*model_calculate_size)(void *config, void *dims);
+    void *(*model_assign)(void *config, void *dims, void *raw_memory);
+    int (*model_set)(void *config_, void *dims_, void *model_, const char *field, void *value_);
+    int (*model_get)(void *config_, void *dims_, void *model_, const char *field, void *value_);
+    acados_size_t (*opts_calculate_size)(void *config, void *dims);
+    void *(*opts_assign)(void *config, void *dims, void *raw_memory);
+    void (*opts_initialize_default)(void *config, void *dims, void *opts);
+    void (*opts_update)(void *config, void *dims, void *opts);
+    void (*opts_set)(void *config, void *opts, const char *field, void *value);
+    acados_size_t (*memory_calculate_size)(void *config, void *dims, void *opts);
+    double *(*memory_get_fun_ptr)(void *memory);
+    struct blasfeo_dvec *(*memory_get_grad_ptr)(void *memory);
+    struct blasfeo_dvec *(*model_get_y_ref_ptr)(void *memory);
+    struct blasfeo_dmat *(*memory_get_W_chol_ptr)(void *memory_);
+    struct blasfeo_dvec *(*memory_get_W_chol_diag_ptr)(void *memory_);
+    double *(*get_outer_hess_is_diag_ptr)(void *memory_, void *model_);
+    void (*memory_set_ux_ptr)(struct blasfeo_dvec *ux, void *memory);
+    void (*memory_set_z_alg_ptr)(struct blasfeo_dvec *z_alg, void *memory);
+    void (*memory_set_dzdux_tran_ptr)(struct blasfeo_dmat *dzdux, void *memory);
+    void (*memory_set_RSQrq_ptr)(struct blasfeo_dmat *RSQrq, void *memory);
+    void (*memory_set_Z_ptr)(struct blasfeo_dvec *Z, void *memory);
+    void (*memory_set_jac_lag_stat_p_global_ptr)(struct blasfeo_dmat *jac_lag_stat_p_global, void *memory);
+    void *(*memory_assign)(void *config, void *dims, void *opts, void *raw_memory);
+    acados_size_t (*workspace_calculate_size)(void *config, void *dims, void *opts);
+    acados_size_t (*get_external_fun_workspace_requirement)(void *config, void *dims, void *opts_, void *in);
+    void (*set_external_fun_workspaces)(void *config, void *dims, void *opts_, void *in, void *work_);
+    void (*initialize)(void *config_, void *dims, void *model_, void *opts_, void *mem_, void *work_);
+
+    // computes the function value, gradient and hessian (approximation) of the cost function
+    void (*update_qp_matrices)(void *config_, void *dims, void *model_, void *opts_, void *mem_, void *work_);
+    // computes the cost function value (intended for globalization)
+    void (*compute_fun)(void *config_, void *dims, void *model_, void *opts_, void *mem_, void *work_);
+    // computes the cost jacobian wrt parameters (intended for solution sensitivities)
+    void (*compute_jac_p)(void *config_, void *dims, void *model_, void *opts_, void *mem_, void *work_);
+    void (*eval_grad_p)(void *config_, void *dim, void* model, void *opts, void *mem, void *work, struct blasfeo_dvec *out);
+    void (*compute_gradient)(void *config_, void *dims, void *model_, void *opts_, void *mem_, void *work_);
+    void (*config_initialize_default)(void *config, int stage);
+    void (*precompute)(void *config_, void *dims_, void *model_, void *opts_, void *memory_, void *work_);
+    // stage information
+    int stage;
+} ocp_nlp_cost_config;
+
+//
+acados_size_t ocp_nlp_cost_config_calculate_size();
+//
+ocp_nlp_cost_config *ocp_nlp_cost_config_assign(void *raw_memory);
+
+
+
+#ifdef __cplusplus
+} /* extern "C" */
+#endif
+
+#endif  // ACADOS_OCP_NLP_OCP_NLP_COST_COMMON_H_
+/// @}
+/// @}

acados/ocp_nlp/ocp_nlp_cost_conl.h

@@ -0,0 +1,218 @@
+/*
+ * Copyright (c) The acados authors.
+ *
+ * This file is part of acados.
+ *
+ * The 2-Clause BSD License
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.;
+ */
+
+
+/// \addtogroup ocp_nlp
+/// @{
+/// \addtogroup ocp_nlp_cost ocp_nlp_cost
+/// @{
+/// \addtogroup ocp_nlp_cost_conl ocp_nlp_cost_conl
+/// \brief This module implements convex-over-nonlinear costs of the form
+/// \f$\min_{x,u,z} \psi(y(x,u,z,p) - y_{\text{ref}}, p)\f$,
+
+
+#ifndef ACADOS_OCP_NLP_OCP_NLP_COST_CONL_H_
+#define ACADOS_OCP_NLP_OCP_NLP_COST_CONL_H_
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+// blasfeo
+#include "blasfeo_common.h"
+
+// acados
+#include "acados/ocp_nlp/ocp_nlp_cost_common.h"
+#include "acados/utils/external_function_generic.h"
+#include "acados/utils/types.h"
+
+
+
+/************************************************
+ * dims
+ ************************************************/
+
+typedef struct
+{
+    int nx;  // number of states
+    int nz;  // number of algebraic variables
+    int nu;  // number of inputs
+    int ny;  // number of outputs
+    int ns;  // number of slacks
+} ocp_nlp_cost_conl_dims;
+
+//
+acados_size_t ocp_nlp_cost_conl_dims_calculate_size(void *config);
+//
+void *ocp_nlp_cost_conl_dims_assign(void *config, void *raw_memory);
+//
+void ocp_nlp_cost_conl_dims_initialize(void *config, void *dims, int nx, int nu, int ny, int ns, int nz);
+//
+void ocp_nlp_cost_conl_dims_set(void *config_, void *dims_, const char *field, int* value);
+//
+void ocp_nlp_cost_conl_dims_get(void *config_, void *dims_, const char *field, int* value);
+
+
+
+/************************************************
+ * model
+ ************************************************/
+
+typedef struct
+{
+    // slack penalty has the form z^T * s + .5 * s^T * Z * s
+    external_function_generic *conl_cost_fun;
+    external_function_generic *conl_cost_fun_jac_hess;
+    struct blasfeo_dvec y_ref;
+    struct blasfeo_dvec Z;              // diagonal Hessian of slacks as vector
+    struct blasfeo_dvec z;              // gradient of slacks as vector
+    double scaling;
+    double t; // time (always zero) to match signature of external function wrt cost integration
+} ocp_nlp_cost_conl_model;
+
+//
+acados_size_t ocp_nlp_cost_conl_model_calculate_size(void *config, void *dims);
+//
+void *ocp_nlp_cost_conl_model_assign(void *config, void *dims, void *raw_memory);
+//
+int ocp_nlp_cost_conl_model_set(void *config_, void *dims_, void *model_, const char *field, void *value_);
+
+
+
+/************************************************
+ * options
+ ************************************************/
+
+typedef struct
+{
+    bool gauss_newton_hess;  // dummy options, we always use a gauss-newton hessian
+    int integrator_cost; // > 0 indicating that cost is propagated within integrator instead of cost module, only add slack contributions
+} ocp_nlp_cost_conl_opts;
+
+//
+acados_size_t ocp_nlp_cost_conl_opts_calculate_size(void *config, void *dims);
+//
+void *ocp_nlp_cost_conl_opts_assign(void *config, void *dims, void *raw_memory);
+//
+void ocp_nlp_cost_conl_opts_initialize_default(void *config, void *dims, void *opts);
+//
+void ocp_nlp_cost_conl_opts_update(void *config, void *dims, void *opts);
+//
+void ocp_nlp_cost_conl_opts_set(void *config, void *opts, const char *field, void *value);
+
+
+
+/************************************************
+ * memory
+ ************************************************/
+typedef struct
+{
+    struct blasfeo_dvec grad;    // gradient of cost function
+    struct blasfeo_dvec *ux;     // pointer to ux in nlp_out
+    struct blasfeo_dmat *RSQrq;  // pointer to RSQrq in qp_in
+    struct blasfeo_dvec *Z;      // pointer to Z in qp_in
+    struct blasfeo_dvec *z_alg;         ///< pointer to z in sim_out
+    struct blasfeo_dmat *dzdux_tran;    ///< pointer to sensitivity of a wrt ux in sim_out
+    struct blasfeo_dmat W_chol;        // cholesky factor of hessian of outer loss function
+    struct blasfeo_dvec W_chol_diag;   // cholesky factor of hessian of outer loss function if Hessian is diagonal
+        // NOTE: could be in work, but needed for compatibility with NLS and cost integration
+    double fun;                         ///< value of the cost function
+    double outer_hess_is_diag;
+} ocp_nlp_cost_conl_memory;
+
+//
+acados_size_t ocp_nlp_cost_conl_memory_calculate_size(void *config, void *dims, void *opts);
+//
+void *ocp_nlp_cost_conl_memory_assign(void *config, void *dims, void *opts, void *raw_memory);
+//
+double *ocp_nlp_cost_conl_memory_get_fun_ptr(void *memory_);
+//
+struct blasfeo_dvec *ocp_nlp_cost_conl_memory_get_grad_ptr(void *memory_);
+//
+void ocp_nlp_cost_conl_memory_set_RSQrq_ptr(struct blasfeo_dmat *RSQrq, void *memory);
+//
+void ocp_nlp_cost_conl_memory_set_Z_ptr(struct blasfeo_dvec *Z, void *memory);
+//
+void ocp_nlp_cost_conl_memory_set_ux_ptr(struct blasfeo_dvec *ux, void *memory_);
+//
+void ocp_nlp_cost_conl_memory_set_z_alg_ptr(struct blasfeo_dvec *z_alg, void *memory_);
+//
+void ocp_nlp_cost_conl_memory_set_dzdux_tran_ptr(struct blasfeo_dmat *dzdux_tran, void *memory_);
+
+/************************************************
+ * workspace
+ ************************************************/
+
+typedef struct
+{
+    struct blasfeo_dmat W;             // hessian of outer loss function
+    struct blasfeo_dmat Jt_ux;         // jacobian of inner residual function
+    struct blasfeo_dmat Jt_ux_tilde;   // jacobian of inner residual function plus gradient contribution of algebraic variables
+    struct blasfeo_dmat Jt_z;          // jacobian of inner residual function wrt algebraic variables
+    struct blasfeo_dmat tmp_nv_ny;
+    struct blasfeo_dvec tmp_ny;
+    struct blasfeo_dvec tmp_2ns;
+} ocp_nlp_cost_conl_workspace;
+
+//
+acados_size_t ocp_nlp_cost_conl_workspace_calculate_size(void *config, void *dims, void *opts);
+//
+size_t ocp_nlp_cost_conl_get_external_fun_workspace_requirement(void *config_, void *dims_, void *opts_, void *model_);
+//
+void ocp_nlp_cost_conl_set_external_fun_workspaces(void *config_, void *dims_, void *opts_, void *model_, void *workspace_);
+
+
+/************************************************
+ * functions
+ ************************************************/
+
+//
+void ocp_nlp_cost_conl_precompute(void *config_, void *dims_, void *model_, void *opts_, void *memory_, void *work_);
+//
+void ocp_nlp_cost_conl_config_initialize_default(void *config, int stage);
+//
+void ocp_nlp_cost_conl_initialize(void *config_, void *dims, void *model_, void *opts_, void *mem_, void *work_);
+//
+void ocp_nlp_cost_conl_update_qp_matrices(void *config_, void *dims, void *model_, void *opts_, void *memory_, void *work_);
+//
+void ocp_nlp_cost_conl_compute_fun(void *config_, void *dims, void *model_, void *opts_, void *memory_, void *work_);
+//
+void ocp_nlp_cost_conl_compute_jac_p(void *config_, void *dims, void *model_, void *opts_, void *memory_, void *work_);
+//
+void ocp_nlp_cost_conl_eval_grad_p(void *config_, void *dims, void *model_, void *opts_, void *memory_, void *work_, struct blasfeo_dvec *out);
+
+#ifdef __cplusplus
+} /* extern "C" */
+#endif
+
+#endif  // ACADOS_OCP_NLP_OCP_NLP_COST_CONL_H_
+/// @}
+/// @}
+/// @}

acados/ocp_nlp/ocp_nlp_cost_external.h

@@ -0,0 +1,204 @@
+/*
+ * Copyright (c) The acados authors.
+ *
+ * This file is part of acados.
+ *
+ * The 2-Clause BSD License
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.;
+ */
+
+
+#ifndef ACADOS_OCP_NLP_OCP_NLP_COST_EXTERNAL_H_
+#define ACADOS_OCP_NLP_OCP_NLP_COST_EXTERNAL_H_
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+// blasfeo
+#include "blasfeo_common.h"
+
+// acados
+#include "acados/ocp_nlp/ocp_nlp_cost_common.h"
+#include "acados/utils/external_function_generic.h"
+#include "acados/utils/types.h"
+
+/************************************************
+ * dims
+ ************************************************/
+
+typedef struct
+{
+    int nx;  // number of states
+    int nz;  // number of algebraic variables
+    int nu;  // number of inputs
+    int ns;  // number of slacks
+    int np; // number of parameters
+    int np_global; // number of global parameters
+} ocp_nlp_cost_external_dims;
+
+//
+acados_size_t ocp_nlp_cost_external_dims_calculate_size(void *config);
+//
+void *ocp_nlp_cost_external_dims_assign(void *config, void *raw_memory);
+//
+void ocp_nlp_cost_external_dims_set(void *config_, void *dims_, const char *field, int* value);
+//
+void ocp_nlp_cost_external_dims_get(void *config_, void *dims_, const char *field, int* value);
+
+/************************************************
+ * model
+ ************************************************/
+
+typedef struct
+{
+    external_function_generic *ext_cost_fun;  // function
+    external_function_generic *ext_cost_fun_jac_hess;  // function, gradient and hessian
+    external_function_generic *ext_cost_fun_jac;  // function, gradient
+    external_function_generic *ext_cost_hess_xu_p;  // jacobian of cost gradient wrt params
+    external_function_generic *ext_cost_grad_p; // gradient of the cost wrt paraams
+    struct blasfeo_dvec Z;
+    struct blasfeo_dvec z;
+    struct blasfeo_dmat numerical_hessian;  // custom hessian approximation
+    double scaling;
+} ocp_nlp_cost_external_model;
+
+//
+acados_size_t ocp_nlp_cost_external_model_calculate_size(void *config, void *dims);
+//
+void *ocp_nlp_cost_external_model_assign(void *config, void *dims, void *raw_memory);
+
+
+
+/************************************************
+ * options
+ ************************************************/
+
+typedef struct
+{
+    int use_numerical_hessian;  // > 0 indicating custom hessian is used instead of CasADi evaluation
+    int with_solution_sens_wrt_params;
+} ocp_nlp_cost_external_opts;
+
+//
+acados_size_t ocp_nlp_cost_external_opts_calculate_size(void *config, void *dims);
+//
+void *ocp_nlp_cost_external_opts_assign(void *config, void *dims, void *raw_memory);
+//
+void ocp_nlp_cost_external_opts_initialize_default(void *config, void *dims, void *opts);
+//
+void ocp_nlp_cost_external_opts_update(void *config, void *dims, void *opts);
+//
+void ocp_nlp_cost_external_opts_set(void *config, void *opts, const char *field, void *value);
+
+
+/************************************************
+ * memory
+ ************************************************/
+
+typedef struct
+{
+    struct blasfeo_dmat *jac_lag_stat_p_global;    // pointer to jacobian of stationarity condition wrt parameters
+    struct blasfeo_dvec grad;    // gradient of cost function
+    struct blasfeo_dvec *ux;     // pointer to ux in nlp_out
+    struct blasfeo_dmat *RSQrq;  // pointer to RSQrq in qp_in
+    struct blasfeo_dvec *Z;      // pointer to Z in qp_in
+    struct blasfeo_dvec *z_alg;         ///< pointer to z in sim_out
+    struct blasfeo_dmat *dzdux_tran;    ///< pointer to sensitivity of a wrt ux in sim_out
+    double fun;                         ///< value of the cost function
+} ocp_nlp_cost_external_memory;
+
+//
+acados_size_t ocp_nlp_cost_external_memory_calculate_size(void *config, void *dims, void *opts);
+//
+void *ocp_nlp_cost_external_memory_assign(void *config, void *dims, void *opts, void *raw_memory);
+//
+double *ocp_nlp_cost_external_memory_get_fun_ptr(void *memory_);
+//
+struct blasfeo_dvec *ocp_nlp_cost_external_memory_get_grad_ptr(void *memory_);
+//
+void ocp_nlp_cost_external_memory_set_RSQrq_ptr(struct blasfeo_dmat *RSQrq, void *memory);
+//
+void ocp_nlp_cost_ls_memory_set_Z_ptr(struct blasfeo_dvec *Z, void *memory);
+//
+void ocp_nlp_cost_external_memory_set_ux_ptr(struct blasfeo_dvec *ux, void *memory_);
+//
+void ocp_nlp_cost_external_memory_set_z_alg_ptr(struct blasfeo_dvec *z_alg, void *memory_);
+//
+void ocp_nlp_cost_external_memory_set_dzdux_tran_ptr(struct blasfeo_dmat *dzdux_tran, void *memory_);
+//
+void ocp_nlp_cost_external_memory_set_jac_lag_stat_p_global_ptr(struct blasfeo_dmat *jac_lag_stat_p_global, void *memory_);
+
+/************************************************
+ * workspace
+ ************************************************/
+
+typedef struct
+{
+    struct blasfeo_dmat cost_grad_params_jac;  // jacobian of gradient of cost function wrt parameters
+    struct blasfeo_dmat tmp_nunx_nunx;
+    struct blasfeo_dmat tmp_nz_nz;
+    struct blasfeo_dmat tmp_nz_nunx;
+    struct blasfeo_dvec tmp_nunxnz;
+    struct blasfeo_dvec tmp_2ns;  // temporary vector of dimension 2*ns
+} ocp_nlp_cost_external_workspace;
+
+//
+acados_size_t ocp_nlp_cost_external_workspace_calculate_size(void *config, void *dims, void *opts);
+//
+size_t ocp_nlp_cost_external_get_external_fun_workspace_requirement(void *config_, void *dims_, void *opts_, void *model_);
+//
+void ocp_nlp_cost_external_set_external_fun_workspaces(void *config_, void *dims_, void *opts_, void *model_, void *workspace_);
+
+
+/************************************************
+ * functions
+ ************************************************/
+
+//
+void ocp_nlp_cost_external_precompute(void *config_, void *dims_, void *model_, void *opts_, void *memory_, void *work_);
+//
+void ocp_nlp_cost_external_config_initialize_default(void *config, int stage);
+//
+void ocp_nlp_cost_external_initialize(void *config_, void *dims, void *model_,
+                                      void *opts_, void *mem_, void *work_);
+//
+void ocp_nlp_cost_external_update_qp_matrices(void *config_, void *dims, void *model_,
+                                               void *opts_, void *memory_, void *work_);
+//
+void ocp_nlp_cost_external_compute_fun(void *config_, void *dims, void *model_,
+                                       void *opts_, void *memory_, void *work_);
+//
+void ocp_nlp_cost_external_compute_jac_p(void *config_, void *dims, void *model_,
+                                       void *opts_, void *memory_, void *work_);
+
+void ocp_nlp_cost_external_compute_gradient(void *config_, void *dims, void *model_,
+                                       void *opts_, void *memory_, void *work_);
+
+void ocp_nlp_cost_external_eval_grad_p(void *config_, void *dims, void *model_, void *opts_, void *memory_, void *work_, struct blasfeo_dvec *out);
+#ifdef __cplusplus
+} /* extern "C" */
+#endif
+
+#endif  // ACADOS_OCP_NLP_OCP_NLP_COST_EXTERNAL_H_

acados/ocp_nlp/ocp_nlp_cost_ls.h

@@ -0,0 +1,256 @@
+/*
+ * Copyright (c) The acados authors.
+ *
+ * This file is part of acados.
+ *
+ * The 2-Clause BSD License
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.;
+ */
+
+
+/// \addtogroup ocp_nlp
+/// @{
+/// \addtogroup ocp_nlp_cost ocp_nlp_cost
+/// @{
+/// \addtogroup ocp_nlp_cost_ls ocp_nlp_cost_ls
+/// \brief This module implements linear-least squares costs of the form
+/// \f$\min_{x,u,z} \| V_x x + V_u u + V_z z - y_{\text{ref}}\|_W^2\f$.
+/// @{
+
+
+
+#ifndef ACADOS_OCP_NLP_OCP_NLP_COST_LS_H_
+#define ACADOS_OCP_NLP_OCP_NLP_COST_LS_H_
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include <math.h>
+
+// blasfeo
+#include "blasfeo_common.h"
+
+// acados
+#include "acados/ocp_nlp/ocp_nlp_cost_common.h"
+#include "acados/utils/external_function_generic.h"
+#include "acados/utils/types.h"
+
+
+////////////////////////////////////////////////////////////////////////////////
+//                                     dims                                   //
+////////////////////////////////////////////////////////////////////////////////
+
+typedef struct
+{
+    int nx;  // number of states
+    int nz;  // number of algebraic variables
+    int nu;  // number of inputs
+    int ny;  // number of outputs
+    int ns;  // number of slacks
+    int np_global;
+} ocp_nlp_cost_ls_dims;
+
+
+///  Calculate the size of the ocp_nlp_cost_ls_dims struct
+///
+///  \param[in] config_ structure containing configuration of ocp_nlp_cost
+///  module
+///  \param[out] []
+///  \return \c size of ocp_nlp_dims struct
+acados_size_t ocp_nlp_cost_ls_dims_calculate_size(void *config);
+
+
+///  Assign memory pointed to by raw_memory to ocp_nlp-cost_ls dims struct
+///
+///  \param[in] config structure containing configuration of ocp_nlp_cost
+///  module
+///  \param[in] raw_memory pointer to memory location
+///  \param[out] []
+///  \return dims
+void *ocp_nlp_cost_ls_dims_assign(void *config, void *raw_memory);
+//
+void ocp_nlp_cost_ls_dims_set(void *config_, void *dims_, const char *field, int* value);
+//
+void ocp_nlp_cost_ls_dims_get(void *config_, void *dims_, const char *field, int* value);
+
+
+////////////////////////////////////////////////////////////////////////////////
+//                                     model                                  //
+////////////////////////////////////////////////////////////////////////////////
+
+
+/// structure containing the data describing the linear least-square cost
+typedef struct
+{
+    // slack penalty has the form z^T * s + .5 * s^T * Z * s
+    struct blasfeo_dmat Cyt;            ///< output matrix: Cy * [u,x] = y; in transposed form
+    struct blasfeo_dmat Vz;             ///< Vz in ls cost Vx*x + Vu*u + Vz*z
+    struct blasfeo_dmat W;              ///< ls norm corresponding to this matrix
+    struct blasfeo_dvec y_ref;          ///< yref
+    struct blasfeo_dvec Z;              ///< diagonal Hessian of slacks as vector (lower and upper)
+    struct blasfeo_dvec z;              ///< gradient of slacks as vector (lower and upper)
+    double scaling;
+    double outer_hess_is_diag;
+    int W_changed;                      ///< flag indicating whether W has changed and needs to be refactorized
+    int Cyt_or_scaling_changed;         ///< flag indicating whether Cyt or scaling has changed and Hessian needs to be recomputed
+} ocp_nlp_cost_ls_model;
+
+//
+acados_size_t ocp_nlp_cost_ls_model_calculate_size(void *config, void *dims);
+//
+void *ocp_nlp_cost_ls_model_assign(void *config, void *dims, void *raw_memory);
+//
+int ocp_nlp_cost_ls_model_set(void *config_, void *dims_, void *model_,
+                              const char *field, void *value_);
+//
+int ocp_nlp_cost_ls_model_get(void *config_, void *dims_, void *model_,
+                              const char *field, void *value_);
+
+
+////////////////////////////////////////////////////////////////////////////////
+//                                   options                                  //
+////////////////////////////////////////////////////////////////////////////////
+
+
+
+typedef struct
+{
+    int compute_hess;
+} ocp_nlp_cost_ls_opts;
+
+//
+acados_size_t ocp_nlp_cost_ls_opts_calculate_size(void *config, void *dims);
+//
+void *ocp_nlp_cost_ls_opts_assign(void *config, void *dims, void *raw_memory);
+//
+void ocp_nlp_cost_ls_opts_initialize_default(void *config, void *dims, void *opts);
+//
+void ocp_nlp_cost_ls_opts_update(void *config, void *dims, void *opts);
+//
+void ocp_nlp_cost_ls_opts_set(void *config, void *opts, const char *field, void *value);
+
+
+
+////////////////////////////////////////////////////////////////////////////////
+//                                     memory                                 //
+////////////////////////////////////////////////////////////////////////////////
+
+
+
+/// structure containing the memory associated with cost_ls component
+/// of the ocp_nlp module
+typedef struct
+{
+    struct blasfeo_dmat hess;           ///< hessian of cost function
+    struct blasfeo_dmat W_chol;         ///< cholesky factor of weight matrix
+    struct blasfeo_dvec W_chol_diag;    ///< W_chol_diag
+    struct blasfeo_dvec res;            ///< ls residual r(x)
+    struct blasfeo_dvec grad;           ///< gradient of cost function
+    struct blasfeo_dvec *ux;            ///< pointer to ux in nlp_out
+    struct blasfeo_dvec *z_alg;         ///< pointer to z in sim_out
+    struct blasfeo_dmat *dzdux_tran;    ///< pointer to sensitivity of a wrt ux in sim_out
+    struct blasfeo_dmat *RSQrq;         ///< pointer to RSQrq in qp_in
+    struct blasfeo_dvec *Z;             ///< pointer to Z in qp_in
+    double fun;                         ///< value of the cost function
+} ocp_nlp_cost_ls_memory;
+
+//
+acados_size_t ocp_nlp_cost_ls_memory_calculate_size(void *config, void *dims, void *opts);
+//
+void *ocp_nlp_cost_ls_memory_assign(void *config, void *dims, void *opts, void *raw_memory);
+//
+double *ocp_nlp_cost_ls_memory_get_fun_ptr(void *memory_);
+//
+struct blasfeo_dvec *ocp_nlp_cost_ls_memory_get_grad_ptr(void *memory_);
+//
+void ocp_nlp_cost_ls_memory_set_RSQrq_ptr(struct blasfeo_dmat *RSQrq, void *memory);
+//
+void ocp_nlp_cost_ls_memory_set_Z_ptr(struct blasfeo_dvec *Z, void *memory);
+//
+void ocp_nlp_cost_ls_memory_set_ux_ptr(struct blasfeo_dvec *ux, void *memory_);
+//
+void ocp_nlp_cost_ls_memory_set_z_alg_ptr(struct blasfeo_dvec *z_alg, void *memory_);
+//
+void ocp_nlp_cost_ls_memory_set_dzdux_tran_ptr(struct blasfeo_dmat *dzdux_tran, void *memory_);
+
+
+
+////////////////////////////////////////////////////////////////////////////////
+//                                 workspace                                  //
+////////////////////////////////////////////////////////////////////////////////
+
+
+
+typedef struct
+{
+    struct blasfeo_dmat tmp_nv_ny;   // temporary matrix of dimensions nv, ny
+    struct blasfeo_dmat Cyt_tilde;   // updated Cyt (after z elimination)
+    struct blasfeo_dmat dzdux_tran;  // derivatives of z wrt u and x (tran)
+    struct blasfeo_dvec tmp_ny;      // temporary vector of dimension ny
+    struct blasfeo_dvec tmp_2ns;     // temporary vector of dimension ny
+    struct blasfeo_dvec tmp_nz;      // temporary vector of dimension nz
+    struct blasfeo_dvec y_ref_tilde; // updated y_ref (after z elimination)
+} ocp_nlp_cost_ls_workspace;
+
+//
+acados_size_t ocp_nlp_cost_ls_workspace_calculate_size(void *config, void *dims, void *opts);
+//
+size_t ocp_nlp_cost_ls_get_external_fun_workspace_requirement(void *config_, void *dims_, void *opts_, void *model_);
+//
+void ocp_nlp_cost_ls_set_external_fun_workspaces(void *config_, void *dims_, void *opts_, void *model_, void *workspace_);
+
+
+
+////////////////////////////////////////////////////////////////////////////////
+//                                 functions                                  //
+////////////////////////////////////////////////////////////////////////////////
+
+
+// computations that are done once when solver is created
+void ocp_nlp_cost_ls_precompute(void *config_, void *dims_, void *model_, void *opts_, void *memory_, void *work_);
+//
+void ocp_nlp_cost_ls_config_initialize_default(void *config, int stage);
+//
+void ocp_nlp_cost_ls_initialize(void *config_, void *dims, void *model_, void *opts_,
+                                void *mem_, void *work_);
+//
+void ocp_nlp_cost_ls_update_qp_matrices(void *config_, void *dims, void *model_,
+                                        void *opts_, void *memory_, void *work_);
+//
+void ocp_nlp_cost_ls_compute_fun(void *config_, void *dims, void *model_, void *opts_,
+                                 void *memory_, void *work_);
+//
+void ocp_nlp_cost_ls_compute_jac_p(void *config_, void *dims, void *model_, void *opts_, void *memory_, void *work_);
+//
+void ocp_nlp_cost_ls_eval_grad_p(void *config_, void *dims, void *model_, void *opts_, void *memory_, void *work_, struct blasfeo_dvec *out);
+
+#ifdef __cplusplus
+} /* extern "C" */
+#endif
+
+#endif  // ACADOS_OCP_NLP_OCP_NLP_COST_LS_H_
+/// @}
+/// @}
+/// @}

acados/ocp_nlp/ocp_nlp_cost_nls.h

@@ -0,0 +1,227 @@
+/*
+ * Copyright (c) The acados authors.
+ *
+ * This file is part of acados.
+ *
+ * The 2-Clause BSD License
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.;
+ */
+
+
+/// \addtogroup ocp_nlp
+/// @{
+/// \addtogroup ocp_nlp_cost ocp_nlp_cost
+/// @{
+/// \addtogroup ocp_nlp_cost_nls ocp_nlp_cost_nls
+/// \brief This module implements nonlinear-least squares costs of the form
+/// \f$\min_{x,u,z} \| y(x,u,z,p) - y_{\text{ref}} \|_W^2\f$,
+
+#ifndef ACADOS_OCP_NLP_OCP_NLP_COST_NLS_H_
+#define ACADOS_OCP_NLP_OCP_NLP_COST_NLS_H_
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+// blasfeo
+#include "blasfeo_common.h"
+
+// acados
+#include "acados/ocp_nlp/ocp_nlp_cost_common.h"
+#include "acados/utils/external_function_generic.h"
+#include "acados/utils/types.h"
+
+
+
+/************************************************
+ * dims
+ ************************************************/
+
+typedef struct
+{
+    int nx;  // number of states
+    int nz;  // number of algebraic variables
+    int nu;  // number of inputs
+    int ny;  // number of outputs
+    int ns;  // number of slacks
+} ocp_nlp_cost_nls_dims;
+
+//
+acados_size_t ocp_nlp_cost_nls_dims_calculate_size(void *config);
+//
+void *ocp_nlp_cost_nls_dims_assign(void *config, void *raw_memory);
+//
+void ocp_nlp_cost_nls_dims_set(void *config_, void *dims_, const char *field, int* value);
+//
+void ocp_nlp_cost_nls_dims_get(void *config_, void *dims_, const char *field, int* value);
+
+
+
+/************************************************
+ * model
+ ************************************************/
+
+typedef struct
+{
+    // nonliner function nls_y(x,u) replaces Cy * [x,u] in ls_cost
+    // slack penalty has the form z^T * s + .5 * s^T * Z * s
+    external_function_generic *nls_y_fun;  // evaluation of nls function
+    external_function_generic *nls_y_fun_jac;  // evaluation nls function and jacobian
+    external_function_generic *nls_y_hess;  // hessian*seeds of nls residuals
+    struct blasfeo_dmat W;                //
+    struct blasfeo_dvec y_ref;
+    struct blasfeo_dvec Z;              // diagonal Hessian of slacks as vector
+    struct blasfeo_dvec z;              // gradient of slacks as vector
+    double scaling;
+    double t; // time (always zero) to match signature of external function wrt cost integration
+    double outer_hess_is_diag;    // flag indicating if outer_hess_is_diag; Note: double for compatibility with CONL cost
+    int W_changed;                      ///< flag indicating whether W has changed and needs to be refactorized
+} ocp_nlp_cost_nls_model;
+
+//
+acados_size_t ocp_nlp_cost_nls_model_calculate_size(void *config, void *dims);
+//
+void *ocp_nlp_cost_nls_model_assign(void *config, void *dims, void *raw_memory);
+//
+int ocp_nlp_cost_nls_model_set(void *config_, void *dims_, void *model_, const char *field, void *value_);
+int ocp_nlp_cost_nls_model_get(void *config_, void *dims_, void *model_, const char *field, void *value_);
+
+
+
+/************************************************
+ * options
+ ************************************************/
+
+typedef struct
+{
+    bool gauss_newton_hess;  // gauss-newton hessian approximation
+    int integrator_cost; // > 0 indicating that cost is propagated within integrator instead of cost module, only add slack contributions
+} ocp_nlp_cost_nls_opts;
+
+//
+acados_size_t ocp_nlp_cost_nls_opts_calculate_size(void *config, void *dims);
+//
+void *ocp_nlp_cost_nls_opts_assign(void *config, void *dims, void *raw_memory);
+//
+void ocp_nlp_cost_nls_opts_initialize_default(void *config, void *dims, void *opts);
+//
+void ocp_nlp_cost_nls_opts_update(void *config, void *dims, void *opts);
+//
+void ocp_nlp_cost_nls_opts_set(void *config, void *opts, const char *field, void *value);
+
+
+
+/************************************************
+ * memory
+ ************************************************/
+
+typedef struct
+{
+    struct blasfeo_dmat W_chol;  // cholesky factor of weight matrix
+    struct blasfeo_dvec W_chol_diag;  // cholesky factor of weight matrix if the Hessian is diagonal
+    struct blasfeo_dmat Jt;      // jacobian of nls fun
+    struct blasfeo_dvec res;     // nls residual r(x)
+    struct blasfeo_dvec grad;    // gradient of cost function
+    struct blasfeo_dvec *ux;     // pointer to ux in nlp_out
+    struct blasfeo_dmat *RSQrq;  // pointer to RSQrq in qp_in
+    struct blasfeo_dvec *Z;      // pointer to Z in qp_in
+    struct blasfeo_dvec *z_alg;         ///< pointer to z in sim_out
+    struct blasfeo_dmat *dzdux_tran;    ///< pointer to sensitivity of a wrt ux in sim_out
+    double fun;                         ///< value of the cost function
+} ocp_nlp_cost_nls_memory;
+
+//
+acados_size_t ocp_nlp_cost_nls_memory_calculate_size(void *config, void *dims, void *opts);
+//
+void *ocp_nlp_cost_nls_memory_assign(void *config, void *dims, void *opts, void *raw_memory);
+//
+double *ocp_nlp_cost_nls_memory_get_fun_ptr(void *memory_);
+//
+struct blasfeo_dvec *ocp_nlp_cost_nls_memory_get_grad_ptr(void *memory_);
+//
+struct blasfeo_dmat *ocp_nlp_cost_nls_memory_get_W_chol_ptr(void *memory_);
+//
+struct blasfeo_dvec *ocp_nlp_cost_nls_memory_get_W_chol_diag_ptr(void *memory_);
+//
+void ocp_nlp_cost_nls_memory_set_RSQrq_ptr(struct blasfeo_dmat *RSQrq, void *memory);
+//
+void ocp_nlp_cost_nls_memory_set_Z_ptr(struct blasfeo_dvec *Z, void *memory);
+//
+void ocp_nlp_cost_nls_memory_set_ux_ptr(struct blasfeo_dvec *ux, void *memory_);
+//
+void ocp_nlp_cost_nls_memory_set_z_alg_ptr(struct blasfeo_dvec *z_alg, void *memory_);
+//
+void ocp_nlp_cost_nls_memory_set_dzdux_tran_ptr(struct blasfeo_dmat *dzdux_tran, void *memory_);
+
+/************************************************
+ * workspace
+ ************************************************/
+
+typedef struct
+{
+    struct blasfeo_dmat tmp_nv_ny;
+    struct blasfeo_dmat tmp_nv_nv;
+    struct blasfeo_dmat Vz;
+    struct blasfeo_dmat Cyt_tilde;
+    struct blasfeo_dvec tmp_ny;
+    struct blasfeo_dvec tmp_2ns;
+    struct blasfeo_dvec tmp_nz;
+} ocp_nlp_cost_nls_workspace;
+
+//
+acados_size_t ocp_nlp_cost_nls_workspace_calculate_size(void *config, void *dims, void *opts);
+//
+size_t ocp_nlp_cost_nls_get_external_fun_workspace_requirement(void *config_, void *dims_, void *opts_, void *model_);
+//
+void ocp_nlp_cost_nls_set_external_fun_workspaces(void *config_, void *dims_, void *opts_, void *model_, void *workspace_);
+
+
+/************************************************
+ * functions
+ ************************************************/
+
+//
+void ocp_nlp_cost_nls_precompute(void *config_, void *dims_, void *model_, void *opts_, void *memory_, void *work_);
+//
+void ocp_nlp_cost_nls_config_initialize_default(void *config, int stage);
+//
+void ocp_nlp_cost_nls_initialize(void *config_, void *dims, void *model_, void *opts_, void *mem_, void *work_);
+//
+void ocp_nlp_cost_nls_update_qp_matrices(void *config_, void *dims, void *model_, void *opts_, void *memory_, void *work_);
+//
+void ocp_nlp_cost_nls_compute_fun(void *config_, void *dims, void *model_, void *opts_,
+                                  void *memory_, void *work_);
+//
+void ocp_nlp_cost_nls_compute_jac_p(void *config_, void *dims, void *model_, void *opts_, void *memory_, void *work_);
+//
+void ocp_nlp_cost_nls_eval_grad_p(void *config_, void *dims, void *model_, void *opts_, void *memory_, void *work_, struct blasfeo_dvec *out);
+
+#ifdef __cplusplus
+} /* extern "C" */
+#endif
+
+#endif  // ACADOS_OCP_NLP_OCP_NLP_COST_NLS_H_
+/// @}
+/// @}
+/// @}

acados/ocp_nlp/ocp_nlp_ddp.h

@@ -0,0 +1,165 @@
+/*
+ * Copyright (c) The acados authors.
+ *
+ * This file is part of acados.
+ *
+ * The 2-Clause BSD License
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.;
+ */
+
+
+/// \addtogroup ocp_nlp
+/// @{
+/// \addtogroup ocp_nlp_solver
+/// @{
+/// \addtogroup ocp_nlp_ddp ocp_nlp_ddp
+/// @{
+
+#ifndef ACADOS_OCP_NLP_OCP_NLP_DDP_H_
+#define ACADOS_OCP_NLP_OCP_NLP_DDP_H_
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+// acados
+#include "acados/ocp_nlp/ocp_nlp_common.h"
+#include "acados/utils/types.h"
+
+
+
+/************************************************
+ * options
+ ************************************************/
+
+typedef struct
+{
+    ocp_nlp_opts *nlp_opts;
+    double tol_stat;     // exit tolerance on stationarity condition
+    double tol_eq;       // exit tolerance on equality constraints
+    double tol_ineq;     // exit tolerance on inequality constraints
+    double tol_comp;     // exit tolerance on complementarity condition
+    double tol_zero_res; // exit tolerance if objective function is 0 for least-squares problem
+    int ext_qp_res;      // compute external QP residuals (i.e. at SQP level) at each SQP iteration (for debugging)
+    bool warm_start_first_qp; // to set qp_warm_start in first iteration
+    bool warm_start_first_qp_from_nlp;
+    bool eval_residual_at_max_iter; // if convergence should be checked after last iterations or only throw max_iter reached
+} ocp_nlp_ddp_opts;
+
+//
+acados_size_t ocp_nlp_ddp_opts_calculate_size(void *config, void *dims);
+//
+void *ocp_nlp_ddp_opts_assign(void *config, void *dims, void *raw_memory);
+//
+void ocp_nlp_ddp_opts_initialize_default(void *config, void *dims, void *opts);
+//
+void ocp_nlp_ddp_opts_update(void *config, void *dims, void *opts);
+//
+void ocp_nlp_ddp_opts_set(void *config_, void *opts_, const char *field, void* value);
+//
+void ocp_nlp_ddp_opts_set_at_stage(void *config_, void *opts_, size_t stage, const char *field, void* value);
+
+
+/************************************************
+ * memory
+ ************************************************/
+
+typedef struct
+{
+    // nlp memory
+    ocp_nlp_memory *nlp_mem;
+    double alpha;
+
+    // statistics
+    double *stat;
+    int stat_m;
+    int stat_n;
+
+    // ddp specific memory
+    double *tmp_nu_times_nx;
+    struct blasfeo_dmat K_mat;
+
+    // regularization for Levenberg-Marquardt
+    double step_norm;
+
+} ocp_nlp_ddp_memory;
+
+//
+acados_size_t ocp_nlp_ddp_memory_calculate_size(void *config, void *dims, void *opts_, void *in_);
+//
+void *ocp_nlp_ddp_memory_assign(void *config, void *dims, void *opts_, void *in_, void *raw_memory);
+//
+void ocp_nlp_ddp_memory_reset_qp_solver(void *config_, void *dims_, void *nlp_in_, void *nlp_out_,
+    void *opts_, void *mem_, void *work_);
+
+
+/************************************************
+ * workspace
+ ************************************************/
+
+typedef struct
+{
+    ocp_nlp_workspace *nlp_work;
+
+    // qp residuals
+    ocp_qp_res *qp_res;
+    ocp_qp_res_ws *qp_res_ws;
+
+} ocp_nlp_ddp_workspace;
+
+//
+acados_size_t ocp_nlp_ddp_workspace_calculate_size(void *config, void *dims, void *opts_, void *in_);
+
+
+
+/************************************************
+ * functions
+ ************************************************/
+//
+int ocp_nlp_ddp(void *config, void *dims, void *nlp_in, void *nlp_out,
+                void *args, void *mem, void *work_);
+//
+void ocp_nlp_ddp_config_initialize_default(void *config_);
+//
+int ocp_nlp_ddp_precompute(void *config_, void *dims_, void *nlp_in_, void *nlp_out_,
+                void *opts_, void *mem_, void *work_);
+//
+void ocp_nlp_ddp_eval_lagr_grad_p(void *config_, void *dims_, void *nlp_in_, void *opts_, void *mem_, void *work_,
+                            const char *field, void *lagr_grad_wrt_params);
+//
+void ocp_nlp_ddp_get(void *config_, void *dims_, void *mem_, const char *field, void *return_value_);
+//
+void ocp_nlp_ddp_compute_trial_iterate(void *config_, void *dims_,
+            void *in_, void *out_, void *opts_, void *mem_,
+            void *work_, void *out_destination_, void *solver_mem,
+            double alpha, bool full_step_dual);
+
+#ifdef __cplusplus
+} /* extern "C" */
+#endif
+
+#endif  // ACADOS_OCP_NLP_OCP_NLP_DDP_H_
+/// @}
+/// @}
+/// @}

acados/ocp_nlp/ocp_nlp_dynamics_common.c

@@ -0,0 +1,72 @@
+/*
+ * Copyright (c) The acados authors.
+ *
+ * This file is part of acados.
+ *
+ * The 2-Clause BSD License
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.;
+ */
+
+
+#include "acados/ocp_nlp/ocp_nlp_dynamics_common.h"
+
+#include <assert.h>
+#include <stdlib.h>
+#include <string.h>
+
+// blasfeo
+#include "blasfeo_d_aux.h"
+#include "blasfeo_d_blas.h"
+// acados
+#include "acados/utils/mem.h"
+
+/************************************************
+ * config
+ ************************************************/
+
+acados_size_t ocp_nlp_dynamics_config_calculate_size()
+{
+    acados_size_t size = 0;
+
+    size += sizeof(ocp_nlp_dynamics_config);
+
+    size += sim_config_calculate_size();
+
+    return size;
+}
+
+
+
+ocp_nlp_dynamics_config *ocp_nlp_dynamics_config_assign(void *raw_memory)
+{
+    char *c_ptr = raw_memory;
+
+    ocp_nlp_dynamics_config *config = (ocp_nlp_dynamics_config *) c_ptr;
+    c_ptr += sizeof(ocp_nlp_dynamics_config);
+
+    config->sim_solver = sim_config_assign(c_ptr);
+    c_ptr += sim_config_calculate_size();
+
+    return config;
+}

acados/ocp_nlp/ocp_nlp_dynamics_common.h

@@ -0,0 +1,128 @@
+/*
+ * Copyright (c) The acados authors.
+ *
+ * This file is part of acados.
+ *
+ * The 2-Clause BSD License
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.;
+ */
+
+
+/// \ingroup ocp_nlp
+/// @{
+
+/// \defgroup ocp_nlp_dynamics ocp_nlp_dynamics
+/// @{
+
+#ifndef ACADOS_OCP_NLP_OCP_NLP_DYNAMICS_COMMON_H_
+#define ACADOS_OCP_NLP_OCP_NLP_DYNAMICS_COMMON_H_
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+
+
+// blasfeo
+#include "blasfeo_common.h"
+
+// acados
+#include "acados/sim/sim_common.h"
+#include "acados/utils/external_function_generic.h"
+#include "acados/utils/types.h"
+
+
+
+/************************************************
+ * config
+ ************************************************/
+
+typedef struct
+{
+    void (*config_initialize_default)(void *config, int stage);
+    sim_config *sim_solver;
+    /* dims */
+    acados_size_t (*dims_calculate_size)(void *config);
+    void *(*dims_assign)(void *config, void *raw_memory);
+    void (*dims_set)(void *config_, void *dims_, const char *field, int *value);
+    void (*dims_get)(void *config_, void *dims_, const char *field, int* value);
+    /* model */
+    acados_size_t (*model_calculate_size)(void *config, void *dims);
+    void *(*model_assign)(void *config, void *dims, void *raw_memory);
+    void (*model_set)(void *config_, void *dims_, void *model_, const char *field, void *value_);
+    /* opts */
+    acados_size_t (*opts_calculate_size)(void *config, void *dims);
+    void *(*opts_assign)(void *config, void *dims, void *raw_memory);
+    void (*opts_initialize_default)(void *config, void *dims, void *opts);
+    void (*opts_set)(void *config_, void *opts_, const char *field, void *value);
+    void (*opts_get)(void *config_, void *opts_, const char *field, void *value);
+    void (*opts_update)(void *config, void *dims, void *opts);
+    /* memory */
+    acados_size_t (*memory_calculate_size)(void *config, void *dims, void *opts);
+    void *(*memory_assign)(void *config, void *dims, void *opts, void *raw_memory);
+    // get shooting node gap x_next(x_n, u_n) - x_{n+1}
+    struct blasfeo_dvec *(*memory_get_fun_ptr)(void *memory_);
+    struct blasfeo_dvec *(*memory_get_adj_ptr)(void *memory_);
+    void (*memory_set_ux_ptr)(struct blasfeo_dvec *ux, void *memory_);
+    void (*memory_set_ux1_ptr)(struct blasfeo_dvec *ux1, void *memory_);
+    void (*memory_set_pi_ptr)(struct blasfeo_dvec *pi, void *memory_);
+    void (*memory_set_BAbt_ptr)(struct blasfeo_dmat *BAbt, void *memory_);
+    void (*memory_set_RSQrq_ptr)(struct blasfeo_dmat *RSQrq, void *memory_);
+    void (*memory_set_dzduxt_ptr)(struct blasfeo_dmat *mat, void *memory_);
+    void (*memory_set_sim_guess_ptr)(struct blasfeo_dvec *vec, bool *bool_ptr, void *memory_);
+    void (*memory_set_z_alg_ptr)(struct blasfeo_dvec *vec, void *memory_);
+    void (*memory_set_dyn_jac_p_global_ptr)(struct blasfeo_dmat *dyn_jac_p_global, void *memory);
+    void (*memory_set_jac_lag_stat_p_global_ptr)(struct blasfeo_dmat *jac_lag_stat_p_global, void *memory);
+    void (*memory_get)(void *config, void *dims, void *mem, const char *field, void* value);
+    void (*memory_set)(void *config, void *dims, void *mem, const char *field, void* value);
+    /* workspace */
+    acados_size_t (*workspace_calculate_size)(void *config, void *dims, void *opts);
+    void (*initialize)(void *config_, void *dims, void *model_, void *opts_, void *mem_, void *work_);
+    void (*update_qp_matrices)(void *config_, void *dims, void *model_, void *opts_, void *mem_, void *work_);
+    void (*compute_fun)(void *config_, void *dims, void *model_, void *opts_, void *mem_, void *work_);
+    void (*compute_jac_hess_p)(void *config_, void *dims, void *model_, void *opts, void *mem, void *work_);
+
+    acados_size_t (*get_external_fun_workspace_requirement)(void *config, void *dims, void *opts_, void *in);
+    void (*set_external_fun_workspaces)(void *config, void *dims, void *opts_, void *in, void *work_);
+
+    void (*compute_adj_p)(void *config, void *dims, void *model, void *opts, void *memory, struct blasfeo_dvec *out);
+    void (*compute_fun_and_adj)(void *config_, void *dims, void *model_, void *opts_, void *mem_, void *work_);
+    int (*precompute)(void *config_, void *dims, void *model_, void *opts_, void *mem_, void *work_);
+    int stage;
+} ocp_nlp_dynamics_config;
+
+//
+acados_size_t ocp_nlp_dynamics_config_calculate_size();
+//
+ocp_nlp_dynamics_config *ocp_nlp_dynamics_config_assign(void *raw_memory);
+
+
+
+#ifdef __cplusplus
+} /* extern "C" */
+#endif
+
+#endif  // ACADOS_OCP_NLP_OCP_NLP_DYNAMICS_COMMON_H_
+/// @}
+/// @}

acados/ocp_nlp/ocp_nlp_dynamics_cont.h

@@ -0,0 +1,210 @@
+/*
+ * Copyright (c) The acados authors.
+ *
+ * This file is part of acados.
+ *
+ * The 2-Clause BSD License
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.;
+ */
+
+
+/// \addtogroup ocp_nlp
+/// @{
+/// \addtogroup ocp_nlp_dynamics
+/// @{
+
+#ifndef ACADOS_OCP_NLP_OCP_NLP_DYNAMICS_CONT_H_
+#define ACADOS_OCP_NLP_OCP_NLP_DYNAMICS_CONT_H_
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+
+
+// blasfeo
+#include "blasfeo_common.h"
+
+// acados
+#include "acados/ocp_nlp/ocp_nlp_dynamics_common.h"
+#include "acados/utils/external_function_generic.h"
+#include "acados/utils/types.h"
+#include "acados_c/sim_interface.h"
+
+
+
+/************************************************
+ * dims
+ ************************************************/
+
+typedef struct
+{
+    void *sim;
+    int nx;   // number of states at the current stage
+    int nz;   // number of algebraic states at the current stage
+    int nu;   // number of inputs at the current stage
+    int nx1;  // number of states at the next stage
+    int nu1;  // number of inputes at the next stage
+} ocp_nlp_dynamics_cont_dims;
+
+//
+acados_size_t ocp_nlp_dynamics_cont_dims_calculate_size(void *config);
+//
+void *ocp_nlp_dynamics_cont_dims_assign(void *config, void *raw_memory);
+//
+void ocp_nlp_dynamics_cont_dims_set(void *config_, void *dims_, const char *field, int* value);
+
+/************************************************
+ * options
+ ************************************************/
+
+typedef struct
+{
+    void *sim_solver;
+    int compute_adj;
+    int compute_hess;
+} ocp_nlp_dynamics_cont_opts;
+
+//
+acados_size_t ocp_nlp_dynamics_cont_opts_calculate_size(void *config, void *dims);
+//
+void *ocp_nlp_dynamics_cont_opts_assign(void *config, void *dims, void *raw_memory);
+//
+void ocp_nlp_dynamics_cont_opts_initialize_default(void *config, void *dims, void *opts);
+//
+void ocp_nlp_dynamics_cont_opts_update(void *config, void *dims, void *opts);
+//
+void ocp_nlp_dynamics_cont_opts_set(void *config, void *opts, const char *field, void* value);
+
+
+
+/************************************************
+ * memory
+ ************************************************/
+
+typedef struct
+{
+    struct blasfeo_dvec fun;
+    struct blasfeo_dvec adj;
+    struct blasfeo_dvec *ux;            // pointer to ux in nlp_out at current stage
+    struct blasfeo_dvec *ux1;           // pointer to ux in nlp_out at next stage
+    struct blasfeo_dvec *pi;            // pointer to pi in nlp_out at current stage
+    struct blasfeo_dmat *BAbt;          // pointer to BAbt in qp_in
+    struct blasfeo_dmat *RSQrq;         // pointer to RSQrq in qp_in
+    struct blasfeo_dvec *z_alg;         // pointer to output z at t = 0
+    bool *set_sim_guess;                 // indicate if initialization for integrator is set from outside
+    struct blasfeo_dvec *sim_guess;     // initializations for integrator
+    // struct blasfeo_dvec *z;             // pointer to (input) z in nlp_out at current stage
+    struct blasfeo_dmat *dzduxt;        // pointer to dzdux transposed
+    void *sim_solver;                   // sim solver memory
+    acados_size_t workspace_size;
+    acados_size_t sim_workspace_size;
+
+} ocp_nlp_dynamics_cont_memory;
+
+//
+acados_size_t ocp_nlp_dynamics_cont_memory_calculate_size(void *config, void *dims, void *opts);
+//
+void *ocp_nlp_dynamics_cont_memory_assign(void *config, void *dims, void *opts, void *raw_memory);
+//
+struct blasfeo_dvec *ocp_nlp_dynamics_cont_memory_get_fun_ptr(void *memory);
+//
+struct blasfeo_dvec *ocp_nlp_dynamics_cont_memory_get_adj_ptr(void *memory);
+//
+void ocp_nlp_dynamics_cont_memory_set_ux_ptr(struct blasfeo_dvec *ux, void *memory);
+//
+void ocp_nlp_dynamics_cont_memory_set_ux1_ptr(struct blasfeo_dvec *ux1, void *memory);
+//
+void ocp_nlp_dynamics_cont_memory_set_pi_ptr(struct blasfeo_dvec *pi, void *memory);
+//
+void ocp_nlp_dynamics_cont_memory_set_BAbt_ptr(struct blasfeo_dmat *BAbt, void *memory);
+//
+void ocp_nlp_dynamics_cont_memory_get_params_lag_grad(void *config, void *dims, void *opts, void *memory, int index, struct blasfeo_dvec *out, int offset);
+
+
+
+/************************************************
+ * workspace
+ ************************************************/
+
+typedef struct
+{
+    struct blasfeo_dmat hess;
+    sim_in *sim_in;
+    sim_out *sim_out;
+    void *sim_solver;  // sim solver workspace
+} ocp_nlp_dynamics_cont_workspace;
+
+acados_size_t ocp_nlp_dynamics_cont_workspace_calculate_size(void *config, void *dims, void *opts);
+
+
+
+/************************************************
+ * model
+ ************************************************/
+
+typedef struct
+{
+    void *sim_model;
+    // double *state_transition; // TODO
+    double T;  // simulation time
+} ocp_nlp_dynamics_cont_model;
+
+//
+acados_size_t ocp_nlp_dynamics_cont_model_calculate_size(void *config, void *dims);
+//
+void *ocp_nlp_dynamics_cont_model_assign(void *config, void *dims, void *raw_memory);
+//
+void ocp_nlp_dynamics_cont_model_set(void *config_, void *dims_, void *model_, const char *field, void *value);
+
+
+
+/************************************************
+ * functions
+ ************************************************/
+
+//
+void ocp_nlp_dynamics_cont_config_initialize_default(void *config, int stage);
+//
+void ocp_nlp_dynamics_cont_initialize(void *config_, void *dims, void *model_, void *opts, void *mem, void *work_);
+//
+void ocp_nlp_dynamics_cont_update_qp_matrices(void *config_, void *dims, void *model_, void *opts, void *mem, void *work_);
+//
+void ocp_nlp_dynamics_cont_compute_fun(void *config_, void *dims, void *model_, void *opts, void *mem, void *work_);
+//
+void ocp_nlp_dynamics_cont_compute_fun_and_adj(void *config_, void *dims, void *model_, void *opts, void *mem, void *work_);
+//
+int ocp_nlp_dynamics_cont_precompute(void *config_, void *dims, void *model_, void *opts_, void *mem_, void *work_);
+//
+void ocp_nlp_dynamics_cont_compute_jac_hess_p(void *config_, void *dims, void *model_, void *opts, void *mem, void *work_);
+//
+void ocp_nlp_dynamics_cont_compute_adj_p(void* config_, void *dims_, void *model_, void *opts_, void *mem_, struct blasfeo_dvec *out);
+
+#ifdef __cplusplus
+} /* extern "C" */
+#endif
+
+#endif  // ACADOS_OCP_NLP_OCP_NLP_DYNAMICS_CONT_H_
+/// @}
+/// @}

acados/ocp_nlp/ocp_nlp_dynamics_disc.h

@@ -0,0 +1,197 @@
+/*
+ * Copyright (c) The acados authors.
+ *
+ * This file is part of acados.
+ *
+ * The 2-Clause BSD License
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.;
+ */
+
+
+/// \addtogroup ocp_nlp
+/// @{
+/// \addtogroup ocp_nlp_dynamics
+/// @{
+
+#ifndef ACADOS_OCP_NLP_OCP_NLP_DYNAMICS_DISC_H_
+#define ACADOS_OCP_NLP_OCP_NLP_DYNAMICS_DISC_H_
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+// blasfeo
+#include "blasfeo_common.h"
+
+// acados
+#include "acados/ocp_nlp/ocp_nlp_dynamics_common.h"
+#include "acados/utils/external_function_generic.h"
+#include "acados/utils/types.h"
+
+/************************************************
+ * dims
+ ************************************************/
+
+typedef struct
+{
+    int nx;   // number of states at the current stage
+    int nu;   // number of inputs at the current stage
+    int nx1;  // number of states at the next stage
+    int nu1;  // number of inputes at the next stage
+    int np;   // number of parameters
+    int np_global;   // number of global parameters
+
+} ocp_nlp_dynamics_disc_dims;
+
+//
+acados_size_t ocp_nlp_dynamics_disc_dims_calculate_size(void *config);
+//
+void *ocp_nlp_dynamics_disc_dims_assign(void *config, void *raw_memory);
+//
+void ocp_nlp_dynamics_disc_dims_set(void *config_, void *dims_, const char *dim, int* value);
+
+
+/************************************************
+ * options
+ ************************************************/
+
+typedef struct
+{
+    int compute_adj;
+    int compute_hess;
+    int cost_computation;
+    int with_solution_sens_wrt_params;
+} ocp_nlp_dynamics_disc_opts;
+
+//
+acados_size_t ocp_nlp_dynamics_disc_opts_calculate_size(void *config, void *dims);
+//
+void *ocp_nlp_dynamics_disc_opts_assign(void *config, void *dims, void *raw_memory);
+//
+void ocp_nlp_dynamics_disc_opts_initialize_default(void *config, void *dims, void *opts);
+//
+void ocp_nlp_dynamics_disc_opts_update(void *config, void *dims, void *opts);
+//
+int ocp_nlp_dynamics_disc_precompute(void *config_, void *dims, void *model_, void *opts_,
+                                        void *mem_, void *work_);
+
+
+/************************************************
+ * memory
+ ************************************************/
+
+typedef struct
+{
+    struct blasfeo_dmat *dyn_jac_p_global;  // pointer to jacobian of the dynamics wrt the parameters
+    struct blasfeo_dmat *jac_lag_stat_p_global;    // pointer to jacobian of stationarity condition wrt parameters
+    struct blasfeo_dvec fun;
+    struct blasfeo_dvec adj;
+    struct blasfeo_dvec *ux;     // pointer to ux in nlp_out at current stage
+    struct blasfeo_dvec *ux1;    // pointer to ux in nlp_out at next stage
+    struct blasfeo_dvec *pi;     // pointer to pi in nlp_out at current stage
+    struct blasfeo_dmat *BAbt;   // pointer to BAbt in qp_in
+    struct blasfeo_dmat *RSQrq;  // pointer to RSQrq in qp_in
+} ocp_nlp_dynamics_disc_memory;
+
+//
+acados_size_t ocp_nlp_dynamics_disc_memory_calculate_size(void *config, void *dims, void *opts);
+//
+void *ocp_nlp_dynamics_disc_memory_assign(void *config, void *dims, void *opts, void *raw_memory);
+//
+struct blasfeo_dvec *ocp_nlp_dynamics_disc_memory_get_fun_ptr(void *memory);
+//
+struct blasfeo_dvec *ocp_nlp_dynamics_disc_memory_get_adj_ptr(void *memory);
+//
+void ocp_nlp_dynamics_disc_memory_set_ux_ptr(struct blasfeo_dvec *ux, void *memory);
+//
+void ocp_nlp_dynamics_disc_memory_set_ux1_ptr(struct blasfeo_dvec *ux1, void *memory);
+//
+void ocp_nlp_dynamics_disc_memory_set_pi_ptr(struct blasfeo_dvec *pi, void *memory);
+//
+void ocp_nlp_dynamics_disc_memory_set_BAbt_ptr(struct blasfeo_dmat *BAbt, void *memory);
+//
+void ocp_nlp_dynamics_disc_memory_set_jac_lag_stat_p_global_ptr(struct blasfeo_dmat *jac_lag_stat_p_global, void *memory_);
+
+void ocp_nlp_dynamics_disc_memory_set_dyn_jac_p_global_ptr(struct blasfeo_dmat *dyn_jac_p_global, void *memory_);
+
+
+/************************************************
+ * workspace
+ ************************************************/
+
+typedef struct
+{
+    struct blasfeo_dmat tmp_nv_nv;
+} ocp_nlp_dynamics_disc_workspace;
+
+acados_size_t ocp_nlp_dynamics_disc_workspace_calculate_size(void *config, void *dims, void *opts);
+
+
+
+/************************************************
+ * model
+ ************************************************/
+
+typedef struct
+{
+    external_function_generic *disc_dyn_fun;
+    external_function_generic *disc_dyn_fun_jac;
+    external_function_generic *disc_dyn_fun_jac_hess;
+    external_function_generic *disc_dyn_phi_jac_p_hess_xu_p;
+    external_function_generic *disc_dyn_adj_p;
+} ocp_nlp_dynamics_disc_model;
+
+//
+acados_size_t ocp_nlp_dynamics_disc_model_calculate_size(void *config, void *dims);
+//
+void *ocp_nlp_dynamics_disc_model_assign(void *config, void *dims, void *raw_memory);
+//
+void ocp_nlp_dynamics_disc_model_set(void *config_, void *dims_, void *model_, const char *field, void *value);
+
+
+
+/************************************************
+ * functions
+ ************************************************/
+
+//
+void ocp_nlp_dynamics_disc_config_initialize_default(void *config, int stage);
+//
+void ocp_nlp_dynamics_disc_initialize(void *config_, void *dims, void *model_, void *opts, void *mem, void *work_);
+//
+void ocp_nlp_dynamics_disc_update_qp_matrices(void *config_, void *dims, void *model_, void *opts, void *mem, void *work_);
+//
+void ocp_nlp_dynamics_disc_compute_fun(void *config_, void *dims, void *model_, void *opts, void *mem, void *work_);
+//
+void ocp_nlp_dynamics_disc_compute_jac_hess_p(void *config_, void *dims, void *model_, void *opts, void *mem, void *work_);
+//
+void ocp_nlp_dynamics_disc_compute_adj_p(void* config_, void *dims_, void *model_, void *opts_, void *mem_, struct blasfeo_dvec *out);
+
+#ifdef __cplusplus
+} /* extern "C" */
+#endif
+
+#endif  // ACADOS_OCP_NLP_OCP_NLP_DYNAMICS_DISC_H_
+/// @}
+/// @}

acados/ocp_nlp/ocp_nlp_globalization_common.c

@@ -0,0 +1,148 @@
+/*
+ * Copyright (c) The acados authors.
+ *
+ * This file is part of acados.
+ *
+ * The 2-Clause BSD License
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.;
+ */
+
+
+#include "acados/ocp_nlp/ocp_nlp_globalization_common.h"
+
+#include <assert.h>
+#include <stdlib.h>
+#include <string.h>
+
+// blasfeo
+#include "blasfeo_d_aux.h"
+#include "blasfeo_d_blas.h"
+// acados
+#include "acados/utils/mem.h"
+
+/************************************************
+ * config
+ ************************************************/
+
+acados_size_t ocp_nlp_globalization_config_calculate_size()
+{
+    return sizeof(ocp_nlp_globalization_config);
+}
+
+
+ocp_nlp_globalization_config *ocp_nlp_globalization_config_assign(void *raw_memory)
+{
+    char *c_ptr = raw_memory;
+
+    ocp_nlp_globalization_config *config = (ocp_nlp_globalization_config *) c_ptr;
+    c_ptr += sizeof(ocp_nlp_globalization_config);
+
+    return config;
+}
+
+/************************************************
+ * dims
+ ************************************************/
+// NOTE: we use ocp_nlp_dims for the globalization module.
+
+/************************************************
+ * options
+ ************************************************/
+acados_size_t ocp_nlp_globalization_opts_calculate_size(void *config, void *dims)
+{
+    return sizeof(ocp_nlp_globalization_opts);
+}
+
+void ocp_nlp_globalization_opts_initialize_default(void *config_, void *dims_, void *opts_)
+{
+    ocp_nlp_globalization_opts *opts = opts_;
+
+    opts->use_SOC = 0;
+    opts->line_search_use_sufficient_descent = 0;
+    opts->full_step_dual = 0;
+    opts->alpha_min = 0.05;
+    opts->alpha_reduction = 0.7;
+    opts->eps_sufficient_descent = 1e-4; // Leineweber1999: MUSCOD-I eps_T = 1e-4 (p.89); Note: eps_T = 0.1 originally proposed by Powell 1978 (Leineweber 1999, p. 53)
+
+    return;
+}
+
+void *ocp_nlp_globalization_opts_assign(void *config_, void *dims_, void *raw_memory)
+{
+    // char *c_ptr = (char *) raw_memory;
+    // // align_char_to(8, &c_ptr);
+
+    // ocp_nlp_globalization_opts *opts = (ocp_nlp_globalization_opts *) c_ptr;
+    // c_ptr += sizeof(ocp_nlp_globalization_opts);
+
+    // assert((char *) raw_memory + ocp_nlp_globalization_opts_calculate_size(config_, dims_) >= c_ptr);
+
+    // return opts;
+    return raw_memory;
+}
+
+
+void ocp_nlp_globalization_opts_set(void *config_, void *opts_, const char *field, void* value)
+{
+    ocp_nlp_globalization_opts *opts = (ocp_nlp_globalization_opts *) opts_;
+
+    // nlp_globalization_opts
+    if (!strcmp(field, "alpha_reduction"))
+    {
+        double* alpha_reduction = (double *) value;
+        opts->alpha_reduction = *alpha_reduction;
+    }
+    else if (!strcmp(field, "alpha_min"))
+    {
+        double* alpha_min = (double *) value;
+        opts->alpha_min = *alpha_min;
+    }
+    else if (!strcmp(field, "eps_sufficient_descent"))
+    {
+        double* eps_sufficient_descent = (double *) value;
+        opts->eps_sufficient_descent = *eps_sufficient_descent;
+    }
+    else if (!strcmp(field, "full_step_dual"))
+    {
+        int* full_step_dual = (int *) value;
+        opts->full_step_dual = *full_step_dual;
+    }
+    else if (!strcmp(field, "line_search_use_sufficient_descent"))
+    {
+        int* line_search_use_sufficient_descent = (int *) value;
+        opts->line_search_use_sufficient_descent = *line_search_use_sufficient_descent;
+    }
+    else if (!strcmp(field, "use_SOC"))
+    {
+        int* use_SOC = (int *) value;
+        opts->use_SOC = *use_SOC;
+    }
+    else
+    {
+        printf("\nerror: ocp_nlp_opts_set: wrong field: %s\n", field);
+        exit(1);
+    }
+
+    return;
+}

acados/ocp_nlp/ocp_nlp_globalization_common.h

@@ -0,0 +1,115 @@
+/*
+ * Copyright (c) The acados authors.
+ *
+ * This file is part of acados.
+ *
+ * The 2-Clause BSD License
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.;
+ */
+
+
+
+/// \defgroup ocp_nlp ocp_nlp
+/// @{
+
+/// \defgroup ocp_nlp_globalization ocp_nlp_globalization
+/// @{
+
+#ifndef ACADOS_OCP_NLP_OCP_NLP_GLOBALIZATION_COMMON_H_
+#define ACADOS_OCP_NLP_OCP_NLP_GLOBALIZATION_COMMON_H_
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+// This would cause cyclic include is not possible due to cycle
+// #include "acados/ocp_nlp/ocp_nlp_common.h"
+#include "acados/utils/types.h"
+
+/************************************************
+ * config
+ ************************************************/
+
+typedef struct
+{
+    /* opts */
+    acados_size_t (*opts_calculate_size)(void *config, void *dims);
+    void *(*opts_assign)(void *config, void *dims, void *raw_memory);
+    void (*opts_initialize_default)(void *config, void *dims, void *opts);
+    void (*opts_set)(void *config, void *opts, const char *field, void* value);
+    /* memory */
+    acados_size_t (*memory_calculate_size)(void *config, void *dims);
+    void *(*memory_assign)(void *config, void *dims, void *raw_memory);
+    /* functions */
+    int (*find_acceptable_iterate)(void *nlp_config, void *nlp_dims, void *nlp_in, void *nlp_out, void *nlp_mem, void *solver_mem, void *nlp_work, void *nlp_opts, double *step_size);
+    void (*print_iteration_header)();
+    void (*print_iteration)(double objective_value,
+                            void *globalization_opts,
+                            void* globalization_mem);
+    int (*needs_objective_value)();
+    int (*needs_qp_objective_value)();
+    void (*initialize_memory)(void *config_,
+                            void *dims_,
+                            void *nlp_mem_,
+                            void *nlp_opts_);
+} ocp_nlp_globalization_config;
+
+//
+acados_size_t ocp_nlp_globalization_config_calculate_size();
+//
+ocp_nlp_globalization_config *ocp_nlp_globalization_config_assign(void *raw_memory);
+
+
+/************************************************
+ * options
+ ************************************************/
+typedef struct ocp_nlp_globalization_opts
+{
+    int use_SOC;
+    int line_search_use_sufficient_descent;
+    int full_step_dual;
+    double alpha_min;
+    double alpha_reduction;
+    double eps_sufficient_descent;
+} ocp_nlp_globalization_opts;
+
+//
+acados_size_t ocp_nlp_globalization_opts_calculate_size(void *config, void *dims);
+//
+void *ocp_nlp_globalization_opts_assign(void *config, void *dims, void *raw_memory);
+//
+void ocp_nlp_globalization_opts_initialize_default(void *config, void *dims, void *opts);
+//
+// void ocp_nlp_globalization_opts_update(void *config, void *dims, void *opts);
+//
+void ocp_nlp_globalization_opts_set(void *config_, void *opts_, const char *field, void* value);
+
+
+#ifdef __cplusplus
+} /* extern "C" */
+#endif
+
+#endif  // ACADOS_OCP_NLP_OCP_NLP_GLOBALIZATION_COMMON_H_
+/// @}
+/// @}

acados/ocp_nlp/ocp_nlp_globalization_fixed_step.c

@@ -0,0 +1,220 @@
+/*
+ * Copyright (c) The acados authors.
+ *
+ * This file is part of acados.
+ *
+ * The 2-Clause BSD License
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.;
+ */
+
+// external
+#include <assert.h>
+#include <math.h>
+#include <stdio.h>
+#include <string.h>
+#include <stdlib.h>
+#if defined(ACADOS_WITH_OPENMP)
+#include <omp.h>
+#endif
+
+// blasfeo
+#include "blasfeo_d_aux.h"
+#include "blasfeo_d_blas.h"
+// acados
+#include "acados/utils/mem.h"
+
+#include "acados/ocp_nlp/ocp_nlp_globalization_common.h"
+#include "acados/ocp_nlp/ocp_nlp_globalization_fixed_step.h"
+#include "acados/ocp_nlp/ocp_nlp_common.h"
+#include "acados/ocp_nlp/ocp_nlp_sqp.h"
+
+/************************************************
+ * options
+ ************************************************/
+
+acados_size_t ocp_nlp_globalization_fixed_step_opts_calculate_size(void *config_, void *dims_)
+{
+    ocp_nlp_dims *dims = dims_;
+    ocp_nlp_config *config = config_;
+
+    acados_size_t size = 0;
+
+    size += sizeof(ocp_nlp_globalization_fixed_step_opts);
+
+    size += ocp_nlp_globalization_opts_calculate_size(config, dims);
+
+    return size;
+}
+
+void ocp_nlp_globalization_fixed_step_opts_initialize_default(void *config_, void *dims_, void *opts_)
+{
+    ocp_nlp_dims *dims = dims_;
+    ocp_nlp_globalization_fixed_step_opts *opts = opts_;
+    ocp_nlp_globalization_opts *globalization_opts = opts->globalization_opts;
+    ocp_nlp_globalization_config *config = config_;
+
+    opts->step_length = 1.0;
+    ocp_nlp_globalization_opts_initialize_default(config, dims, globalization_opts);
+    return;
+}
+
+
+void ocp_nlp_globalization_fixed_step_opts_set(void *config_, void *opts_, const char *field, void* value)
+{
+    ocp_nlp_globalization_fixed_step_opts *opts = opts_;
+    ocp_nlp_globalization_config *config = config_;
+
+    if (!strcmp(field, "fixed_step_length"))
+    {
+        double* step_step = (double *) value;
+        if (*step_step < 0.0 || *step_step > 1.0)
+        {
+            printf("\nerror: ocp_nlp_globalization_fixed_step_opts_set: invalid value for field fixed_step_length, need double in [0,1], got %f.", *step_step);
+            exit(1);
+        }
+        opts->step_length = *step_step;
+    }
+    else
+    {
+        ocp_nlp_globalization_opts_set(config, opts->globalization_opts, field, value);
+    }
+    return;
+}
+
+void *ocp_nlp_globalization_fixed_step_opts_assign(void *config_, void *dims_, void *raw_memory)
+{
+    ocp_nlp_dims *dims = dims_;
+    ocp_nlp_globalization_config *config = config_;
+
+    char *c_ptr = (char *) raw_memory;
+
+    ocp_nlp_globalization_fixed_step_opts *opts = (ocp_nlp_globalization_fixed_step_opts *) c_ptr;
+    c_ptr += sizeof(ocp_nlp_globalization_fixed_step_opts);
+
+    opts->globalization_opts = ocp_nlp_globalization_opts_assign(config, dims, c_ptr);
+    c_ptr += ocp_nlp_globalization_opts_calculate_size(config, dims);
+
+    assert((char *) raw_memory + ocp_nlp_globalization_fixed_step_opts_calculate_size(config_, dims_) >=
+           c_ptr);
+
+    return opts;
+}
+
+/************************************************
+ * memory
+ ************************************************/
+
+acados_size_t ocp_nlp_globalization_fixed_step_memory_calculate_size(void *config_, void *dims_)
+{
+    acados_size_t size = 0;
+
+    size += sizeof(ocp_nlp_globalization_fixed_step_memory);
+
+    return size;
+}
+
+void *ocp_nlp_globalization_fixed_step_memory_assign(void *config_, void *dims_, void *raw_memory)
+{
+    char *c_ptr = (char *) raw_memory;
+
+    // initial align
+    align_char_to(8, &c_ptr);
+
+    ocp_nlp_globalization_fixed_step_memory *mem = (ocp_nlp_globalization_fixed_step_memory *) c_ptr;
+    c_ptr += sizeof(ocp_nlp_globalization_fixed_step_memory);
+
+    align_char_to(8, &c_ptr);
+
+    assert((char *) raw_memory + ocp_nlp_globalization_fixed_step_memory_calculate_size(config_, dims_) >= c_ptr);
+
+    return mem;
+}
+
+
+/************************************************
+ * fixed step functions
+ ************************************************/
+int ocp_nlp_globalization_fixed_step_find_acceptable_iterate(void *nlp_config_, void *nlp_dims_, void *nlp_in_, void *nlp_out_, void *nlp_mem_, void *solver_mem, void *nlp_work_, void *nlp_opts_, double *step_size)
+{
+    ocp_nlp_config *nlp_config = nlp_config_;
+    ocp_nlp_dims *nlp_dims = nlp_dims_;
+    ocp_nlp_in *nlp_in = nlp_in_;
+    ocp_nlp_out *nlp_out = nlp_out_;
+    ocp_nlp_memory *nlp_mem = nlp_mem_;
+    ocp_nlp_workspace *nlp_work = nlp_work_;
+    ocp_nlp_opts *nlp_opts = nlp_opts_;
+    ocp_nlp_globalization_fixed_step_opts *opts = nlp_opts->globalization;
+
+    nlp_config->step_update(nlp_config, nlp_dims, nlp_in, nlp_out, nlp_opts, nlp_mem, nlp_work, nlp_out, solver_mem, opts->step_length, opts->globalization_opts->full_step_dual);
+    *step_size = opts->step_length;
+
+    return ACADOS_SUCCESS;
+}
+
+void ocp_nlp_globalization_fixed_step_print_iteration_header()
+{
+    printf("%8s   ", "alpha");
+}
+
+void ocp_nlp_globalization_fixed_step_print_iteration(double objective_value,
+                                                void* nlp_opts_,
+                                                void* mem_)
+{
+    ocp_nlp_globalization_fixed_step_opts *opts = nlp_opts_;
+    printf("%8.2e    ", opts->step_length);
+}
+
+int ocp_nlp_globalization_fixed_step_needs_objective_value()
+{
+    return 0;
+}
+
+int ocp_nlp_globalization_fixed_step_needs_qp_objective_value()
+{
+    return 0;
+}
+
+void ocp_nlp_globalization_fixed_step_initialize_memory(void *config_, void *dims_, void *nlp_mem_, void *nlp_opts_)
+{
+    return;
+}
+
+void ocp_nlp_globalization_fixed_step_config_initialize_default(ocp_nlp_globalization_config *config)
+{
+    // opts
+    config->opts_calculate_size = &ocp_nlp_globalization_fixed_step_opts_calculate_size;
+    config->opts_assign = &ocp_nlp_globalization_fixed_step_opts_assign;
+    config->opts_initialize_default = &ocp_nlp_globalization_fixed_step_opts_initialize_default;
+    config->opts_set = &ocp_nlp_globalization_fixed_step_opts_set;
+    // memory
+    config->memory_calculate_size = &ocp_nlp_globalization_fixed_step_memory_calculate_size;
+    config->memory_assign = &ocp_nlp_globalization_fixed_step_memory_assign;
+    // functions
+    config->find_acceptable_iterate = &ocp_nlp_globalization_fixed_step_find_acceptable_iterate;
+    config->print_iteration_header = &ocp_nlp_globalization_fixed_step_print_iteration_header;
+    config->print_iteration = &ocp_nlp_globalization_fixed_step_print_iteration;
+    config->needs_objective_value = &ocp_nlp_globalization_fixed_step_needs_objective_value;
+    config->needs_qp_objective_value = &ocp_nlp_globalization_fixed_step_needs_qp_objective_value;
+    config->initialize_memory = &ocp_nlp_globalization_fixed_step_initialize_memory;
+}

acados/ocp_nlp/ocp_nlp_globalization_fixed_step.h

@@ -0,0 +1,110 @@
+/*
+ * Copyright (c) The acados authors.
+ *
+ * This file is part of acados.
+ *
+ * The 2-Clause BSD License
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.;
+ */
+
+
+/// \addtogroup ocp_nlp
+/// @{
+/// \addtogroup ocp_nlp_globalization
+/// @{
+
+#ifndef ACADOS_OCP_NLP_OCP_NLP_GLOBALIZATION_FIXED_STEP_H_
+#define ACADOS_OCP_NLP_OCP_NLP_GLOBALIZATION_FIXED_STEP_H_
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+// blasfeo
+#include "blasfeo_common.h"
+
+// acados
+#include "acados/ocp_nlp/ocp_nlp_globalization_common.h"
+#include "acados/ocp_nlp/ocp_nlp_common.h"
+#include "acados/utils/types.h"
+
+/************************************************
+ * options
+ ************************************************/
+typedef struct
+{
+    double step_length;
+    ocp_nlp_globalization_opts *globalization_opts;
+
+} ocp_nlp_globalization_fixed_step_opts;
+//
+acados_size_t ocp_nlp_globalization_fixed_step_opts_calculate_size(void *config, void *dims);
+//
+void *ocp_nlp_globalization_fixed_step_opts_assign(void *config, void *dims, void *raw_memory);
+//
+void ocp_nlp_globalization_fixed_step_opts_initialize_default(void *config, void *dims, void *opts);
+//
+void ocp_nlp_globalization_fixed_step_opts_set(void *config, void *opts, const char *field, void* value);
+
+/************************************************
+ * memory
+ ************************************************/
+typedef struct
+{
+    double alpha;  // dummy value, not used, just to have non-empty struct
+} ocp_nlp_globalization_fixed_step_memory;
+
+//
+acados_size_t ocp_nlp_globalization_fixed_step_memory_calculate_size(void *config, void *dims);
+//
+void *ocp_nlp_globalization_fixed_step_memory_assign(void *config, void *dims, void *raw_memory);
+//
+
+/************************************************
+ * functions
+ ************************************************/
+//
+int ocp_nlp_globalization_fixed_step_find_acceptable_iterate(void *nlp_config_, void *nlp_dims_, void *nlp_in_, void *nlp_out_, void *solver_mem, void *nlp_mem_, void *nlp_work_, void *nlp_opts_, double *step_size);
+//
+void ocp_nlp_globalization_fixed_step_print_iteration_header();
+//
+void ocp_nlp_globalization_fixed_step_print_iteration(double objective_value,
+                                                    void* nlp_opts_,
+                                                    void* mem_);
+//
+int ocp_nlp_globalization_fixed_step_needs_objective_value();
+//
+int ocp_nlp_globalization_fixed_step_needs_qp_objective_value();
+//
+void ocp_nlp_globalization_fixed_step_initialize_memory(void *config_, void *dims_, void *nlp_mem_, void *nlp_opts_);
+//
+void ocp_nlp_globalization_fixed_step_config_initialize_default(ocp_nlp_globalization_config *config);
+
+#ifdef __cplusplus
+} /* extern "C" */
+#endif
+
+#endif  // ACADOS_OCP_NLP_OCP_NLP_GLOBALIZATION_FIXED_STEP_H_
+/// @}
+/// @}

acados/ocp_nlp/ocp_nlp_globalization_funnel.c

@@ -0,0 +1,627 @@
+/*
+ * Copyright (c) The acados authors.
+ *
+ * This file is part of acados.
+ *
+ * The 2-Clause BSD License
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.;
+ */
+
+
+#include "acados/ocp_nlp/ocp_nlp_globalization_common.h"
+#include "acados/ocp_nlp/ocp_nlp_globalization_funnel.h"
+#include "acados/ocp_nlp/ocp_nlp_common.h"
+
+// external
+#include <assert.h>
+#include <math.h>
+#include <stdio.h>
+#include <string.h>
+#include <stdlib.h>
+#if defined(ACADOS_WITH_OPENMP)
+#include <omp.h>
+#endif
+
+// blasfeo
+#include "blasfeo_d_aux.h"
+#include "blasfeo_d_blas.h"
+// acados
+#include "acados/utils/mem.h"
+#include "acados/utils/print.h"
+
+
+/************************************************
+ * options
+ ************************************************/
+
+acados_size_t ocp_nlp_globalization_funnel_opts_calculate_size(void *config_, void *dims_)
+{
+    ocp_nlp_dims *dims = dims_;
+    ocp_nlp_config *config = config_;
+
+    acados_size_t size = 0;
+
+    size += sizeof(ocp_nlp_globalization_funnel_opts);
+
+    size += ocp_nlp_globalization_opts_calculate_size(config, dims);
+
+    return size;
+}
+
+void ocp_nlp_globalization_funnel_opts_initialize_default(void *config_, void *dims_, void *opts_)
+{
+    ocp_nlp_dims *dims = dims_;
+    ocp_nlp_globalization_funnel_opts *opts = opts_;
+    ocp_nlp_globalization_opts *globalization_opts = opts->globalization_opts;
+    ocp_nlp_globalization_config *config = config_;
+
+    ocp_nlp_globalization_opts_initialize_default(config, dims, globalization_opts);
+
+    // funnel method opts
+    opts->initialization_increase_factor = 15.0;
+    opts->initialization_upper_bound = 1.0;
+    opts->sufficient_decrease_factor = 0.9;
+    opts->kappa = 0.9;
+    opts->fraction_switching_condition = 1e-3;
+    opts->initial_penalty_parameter = 1.0;
+    opts->penalty_contraction = 5e-1;
+    opts->penalty_eta = 1e-6;
+    opts->type_switching_condition = false; // use ipopt/gould type of switching
+    opts->use_merit_fun_only = false;
+
+    return;
+}
+
+
+void ocp_nlp_globalization_funnel_opts_set(void *config_, void *opts_, const char *field, void* value)
+{
+    ocp_nlp_globalization_funnel_opts *opts = opts_;
+    ocp_nlp_globalization_config *config = config_;
+
+    if (!strcmp(field, "funnel_init_increase_factor"))
+    {
+        double* funnel_init_increase_factor = (double *) value;
+        if (*funnel_init_increase_factor <= 1.0)
+        {
+            printf("\nerror: ocp_nlp_globalization_funnel_opts_set: invalid value for funnel_init_increase_factor field, need double > 1, got %f.", *funnel_init_increase_factor);
+            exit(1);
+        }
+        opts->initialization_increase_factor = *funnel_init_increase_factor;
+    }
+    else if (!strcmp(field, "funnel_init_upper_bound"))
+    {
+        double* funnel_init_upper_bound = (double *) value;
+        if (*funnel_init_upper_bound <= 0.0)
+        {
+            printf("\nerror: ocp_nlp_globalization_funnel_opts_set: invalid value for funnel_init_upper_bound field, need double > 0, got %f.", *funnel_init_upper_bound);
+            exit(1);
+        }
+        opts->initialization_upper_bound = *funnel_init_upper_bound;
+    }
+    else if (!strcmp(field, "funnel_sufficient_decrease_factor"))
+    {
+        double* funnel_sufficient_decrease_factor = (double *) value;
+        if (*funnel_sufficient_decrease_factor <= 0.0 || *funnel_sufficient_decrease_factor >= 1.0)
+        {
+            printf("\nerror: ocp_nlp_globalization_funnel_opts_set: invalid value for funnel_sufficient_decrease_factor field, need double in (0,1), got %f.", *funnel_sufficient_decrease_factor);
+            exit(1);
+        }
+        opts->sufficient_decrease_factor = *funnel_sufficient_decrease_factor;
+    }
+    else if (!strcmp(field, "funnel_kappa"))
+    {
+        double* funnel_kappa = (double *) value;
+        if (*funnel_kappa <= 0.0 || *funnel_kappa >= 1.0)
+        {
+            printf("\nerror: ocp_nlp_globalization_funnel_opts_set: invalid value for funnel_kappa field, need double in (0,1), got %f.", *funnel_kappa);
+            exit(1);
+        }
+        opts->kappa = *funnel_kappa;
+    }
+    else if (!strcmp(field, "funnel_fraction_switching_condition"))
+    {
+        double* funnel_fraction_switching_condition = (double *) value;
+        if (*funnel_fraction_switching_condition <= 0.0 || *funnel_fraction_switching_condition >= 1.0)
+        {
+            printf("\nerror: ocp_nlp_globalization_funnel_opts_set: invalid value for funnel_fraction_switching_condition field, need double in (0,1), got %f.", *funnel_fraction_switching_condition);
+            exit(1);
+        }
+        opts->fraction_switching_condition = *funnel_fraction_switching_condition;
+    }
+    else if (!strcmp(field, "funnel_initial_penalty_parameter"))
+    {
+        double* funnel_initial_penalty_parameter = (double *) value;
+        if (*funnel_initial_penalty_parameter < 0.0 || *funnel_initial_penalty_parameter > 1.0)
+        {
+            printf("\nerror: ocp_nlp_globalization_funnel_opts_set: invalid value for funnel_initial_penalty_parameter field, need double in [0,1], got %f.", *funnel_initial_penalty_parameter);
+            exit(1);
+        }
+        opts->initial_penalty_parameter = *funnel_initial_penalty_parameter;
+    }
+    else if (!strcmp(field, "funnel_use_merit_fun_only"))
+    {
+        bool* use_merit_fun_only = (bool *) value;
+        opts->use_merit_fun_only = *use_merit_fun_only;
+    }
+    else
+    {
+        ocp_nlp_globalization_opts_set(config, opts->globalization_opts, field, value);
+    }
+    return;
+}
+
+void *ocp_nlp_globalization_funnel_opts_assign(void *config_, void *dims_, void *raw_memory)
+{
+    ocp_nlp_dims *dims = dims_;
+    ocp_nlp_globalization_config *config = config_;
+
+    char *c_ptr = (char *) raw_memory;
+
+    ocp_nlp_globalization_funnel_opts *opts = (ocp_nlp_globalization_funnel_opts *) c_ptr;
+    c_ptr += sizeof(ocp_nlp_globalization_funnel_opts);
+
+    opts->globalization_opts = ocp_nlp_globalization_opts_assign(config, dims, c_ptr);
+    c_ptr += ocp_nlp_globalization_opts_calculate_size(config, dims);
+
+    assert((char *) raw_memory + ocp_nlp_globalization_funnel_opts_calculate_size(config_, dims_) >=
+           c_ptr);
+
+    return opts;
+}
+
+/************************************************
+ * memory
+ ************************************************/
+
+acados_size_t ocp_nlp_globalization_funnel_memory_calculate_size(void *config_, void *dims_)
+{
+    acados_size_t size = 0;
+
+    size += sizeof(ocp_nlp_globalization_funnel_memory);
+
+    return size;
+}
+
+void *ocp_nlp_globalization_funnel_memory_assign(void *config_, void *dims_, void *raw_memory)
+{
+    char *c_ptr = (char *) raw_memory;
+
+    // initial align
+    align_char_to(8, &c_ptr);
+
+    ocp_nlp_globalization_funnel_memory *mem = (ocp_nlp_globalization_funnel_memory *) c_ptr;
+    c_ptr += sizeof(ocp_nlp_globalization_funnel_memory);
+
+    align_char_to(8, &c_ptr);
+
+    assert((char *) raw_memory + ocp_nlp_globalization_funnel_memory_calculate_size(config_, dims_) >= c_ptr);
+
+    return mem;
+}
+
+/************************************************
+ * funnel functions
+ ************************************************/
+
+void initialize_funnel_width(ocp_nlp_globalization_funnel_memory *mem, ocp_nlp_globalization_funnel_opts *opts, double initial_infeasibility)
+{
+    mem->funnel_width = fmax(opts->initialization_upper_bound,
+                            opts->initialization_increase_factor*initial_infeasibility);
+}
+
+void initialize_funnel_penalty_parameter(ocp_nlp_globalization_funnel_memory *mem, ocp_nlp_globalization_funnel_opts *opts)
+{
+    mem->penalty_parameter = opts->initial_penalty_parameter;
+}
+
+void update_funnel_penalty_parameter(ocp_nlp_globalization_funnel_memory *mem,
+                                     ocp_nlp_globalization_funnel_opts *opts,
+                                     ocp_nlp_opts *nlp_opts,
+                                     double pred_optimality,
+                                     double pred_infeasibility)
+{
+    print_debug_output("-- Objective Multiplier Update: \n", nlp_opts->print_level, 1);
+    print_debug_output_double("left hand side: ", mem->penalty_parameter * pred_optimality + pred_infeasibility, nlp_opts->print_level, 2);
+    print_debug_output_double("right hand side: ", opts->penalty_eta * pred_infeasibility, nlp_opts->print_level, 2);
+    //TODO(david): What do we do here to make it correct? We would like to avoid numerical noise
+    if (pred_optimality < 0 && pred_optimality > -1e-4)
+    {
+        pred_optimality = 0.0;
+    }
+    if (mem->penalty_parameter * pred_optimality + pred_infeasibility < opts->penalty_eta * pred_infeasibility)
+    {
+        mem->penalty_parameter = fmax(0.0, //objective multiplier should always be >= 0!
+                                        fmin(opts->penalty_contraction * mem->penalty_parameter,
+                                        ((1-opts->penalty_eta) * pred_infeasibility) / (-pred_optimality + 1e-9))
+                                     );
+    }
+    assert(mem->penalty_parameter >= 0.0);
+    // else: do not decrease penalty parameter
+}
+
+void decrease_funnel(ocp_nlp_globalization_funnel_memory *mem, ocp_nlp_globalization_funnel_opts *opts, double trial_infeasibility, double current_infeasibility)
+{
+    mem->funnel_width = (1-opts->kappa) * trial_infeasibility + opts->kappa * mem->funnel_width;
+}
+
+bool is_iterate_inside_of_funnel(ocp_nlp_globalization_funnel_memory *mem, ocp_nlp_globalization_funnel_opts *opts, double infeasibility)
+{
+    if (infeasibility <= mem->funnel_width)
+    {
+        return true;
+    }
+    else
+    {
+        return false;
+    }
+}
+
+bool is_funnel_sufficient_decrease_satisfied(ocp_nlp_globalization_funnel_memory *mem, ocp_nlp_globalization_funnel_opts *opts, double infeasibility)
+{
+    if (infeasibility <= opts->sufficient_decrease_factor* mem->funnel_width)
+    {
+        return true;
+    }
+    else
+    {
+        return false;
+    }
+}
+
+bool is_switching_condition_satisfied(ocp_nlp_globalization_funnel_opts *opts, double pred_optimality, double step_size, double pred_infeasibility)
+{
+    // if (step_size * pred_optimality >= opts->fraction_switching_condition * pred_infeasibility * pred_infeasibility)
+    if (step_size * pred_optimality >= opts->fraction_switching_condition * pred_infeasibility)
+    {
+        return true;
+    }
+    else
+    {
+        return false;
+    }
+}
+
+bool is_f_type_armijo_condition_satisfied(ocp_nlp_globalization_opts *globalization_opts,
+                                                    double negative_ared,
+                                                    double pred,
+                                                    double alpha)
+{
+    if (negative_ared <= fmin(globalization_opts->eps_sufficient_descent * alpha * fmax(pred, 0) + 1e-18, 0))
+    {
+        return true;
+    }
+    else
+    {
+        return false;
+    }
+}
+
+bool is_trial_iterate_acceptable_to_funnel(ocp_nlp_globalization_funnel_memory *mem,
+                                           ocp_nlp_opts *nlp_opts,
+                                                  double pred, double ared, double alpha,
+                                                  double current_infeasibility,
+                                                  double trial_infeasibility,
+                                                  double current_objective,
+                                                  double trial_objective,
+                                                  double current_merit,
+                                                  double trial_merit,
+                                                  double pred_merit,
+                                                  double pred_infeasibility)
+{
+    ocp_nlp_globalization_funnel_opts *opts = nlp_opts->globalization;
+    ocp_nlp_globalization_opts *globalization_opts = opts->globalization_opts;
+    bool accept_step = false;
+    print_debug_output_double("-- FUNNEL TEST with alpha: ", alpha, nlp_opts->print_level, 2);
+    print_debug_output_double("current objective", current_objective, nlp_opts->print_level, 2);
+    print_debug_output_double("current infeasibility", current_infeasibility, nlp_opts->print_level, 2);
+    print_debug_output_double("trial objective", trial_objective, nlp_opts->print_level, 2);
+    print_debug_output_double("trial infeasibility", trial_infeasibility, nlp_opts->print_level, 2);
+    print_debug_output_double("pred", pred, nlp_opts->print_level, 2);
+
+    if (opts->use_merit_fun_only) // We only check the penalty method but not the funnel!
+    {
+        mem->funnel_penalty_mode = true;
+    }
+
+    if (opts->use_merit_fun_only || is_iterate_inside_of_funnel(mem, opts, trial_infeasibility))
+    {
+        print_debug_output("Trial iterate is INSIDE of funnel\n", nlp_opts->print_level, 1);
+        if (!mem->funnel_penalty_mode)
+        {
+            print_debug_output("Penalty Mode not active!\n", nlp_opts->print_level, 1);
+            if (is_switching_condition_satisfied(opts, pred, alpha, pred_infeasibility))
+            {
+                print_debug_output("Switching condition IS satisfied!\n", nlp_opts->print_level, 1);
+                if (is_f_type_armijo_condition_satisfied(globalization_opts, -ared, pred, alpha))
+                {
+                    print_debug_output("f-type step: Armijo condition satisfied\n", nlp_opts->print_level, 1);
+                    accept_step = true;
+                    mem->funnel_iter_type = 'f';
+                }
+                else
+                {
+                    print_debug_output("f-type step: Armijo condition NOT satisfied\n", nlp_opts->print_level, 1);
+                }
+
+            }
+            else if (is_funnel_sufficient_decrease_satisfied(mem, opts, trial_infeasibility))
+            {
+                print_debug_output("Switching condition is NOT satisfied!\n", nlp_opts->print_level, 1);
+                print_debug_output("h-type step: funnel suff. decrease satisfied!\n", nlp_opts->print_level, 1);
+                accept_step = true;
+                mem->funnel_iter_type = 'h';
+                decrease_funnel(mem, opts, trial_infeasibility, current_infeasibility);
+            }
+            else
+            {
+                print_debug_output("Switching condition is NOT satisfied!\n", nlp_opts->print_level, 1);
+                print_debug_output("Entered penalty check!\n", nlp_opts->print_level, 1);
+                //TODO move to function and test more
+                if (trial_merit <= current_merit + globalization_opts->eps_sufficient_descent * alpha * pred_merit)
+                {
+                    print_debug_output("Penalty Function accepted\n", nlp_opts->print_level, 1);
+                    accept_step = true;
+                    mem->funnel_iter_type = 'b';
+                    mem->funnel_penalty_mode = true;
+                }
+            }
+        }
+        else
+        {
+            print_debug_output("Penalty mode active\n", nlp_opts->print_level,1);
+            if (trial_merit <= current_merit + globalization_opts->eps_sufficient_descent * alpha * pred_merit)
+            {
+                print_debug_output("p-type step: accepted iterate\n", nlp_opts->print_level, 1);
+                accept_step = true;
+                mem->funnel_iter_type = 'p';
+
+                if (is_funnel_sufficient_decrease_satisfied(mem, opts, trial_infeasibility))
+                {
+                    decrease_funnel(mem, opts, trial_infeasibility, current_infeasibility);
+                    mem->funnel_penalty_mode = false;
+                }
+            }
+        }
+    }
+    else
+    {
+        print_debug_output("Trial iterate is NOT INSIDE of funnel\n", nlp_opts->print_level, 1);
+    }
+    return accept_step;
+}
+
+int backtracking_line_search(ocp_nlp_config *config,
+                            ocp_nlp_dims *dims,
+                            ocp_nlp_in *nlp_in,
+                            ocp_nlp_out *nlp_out,
+                            ocp_nlp_memory *nlp_mem,
+                            void *solver_mem,
+                            ocp_nlp_workspace *nlp_work,
+                            ocp_nlp_opts *nlp_opts,
+                            double *step_size)
+{
+    print_debug_output("-- ENTERING FUNNEL GLOBALIZATION -- \n", nlp_opts->print_level, 1);
+    ocp_nlp_globalization_funnel_opts *opts = nlp_opts->globalization;
+    ocp_nlp_globalization_opts *globalization_opts = opts->globalization_opts;
+    ocp_nlp_globalization_funnel_memory *mem = nlp_mem->globalization;
+
+    int N = dims->N;
+    double pred_merit = 0.0; // Calculate this here
+    double pred_optimality = nlp_mem->predicted_optimality_reduction;
+    double pred_infeasibility = nlp_mem->predicted_infeasibility_reduction;
+    double alpha = 1.0;
+    double trial_cost;
+    double trial_infeasibility = 0.0;
+    double ared;
+    bool accept_step;
+    double current_infeasibility = mem->l1_infeasibility;
+    double current_cost = nlp_mem->cost_value;
+
+    // do the penalty parameter update here .... might be changed later
+    mem->penalty_parameter = nlp_mem->objective_multiplier;
+    print_debug_output_double("pred_optimality", pred_optimality, nlp_opts->print_level, 2);
+    print_debug_output_double("pred_infeasibility", pred_infeasibility, nlp_opts->print_level, 2);
+    update_funnel_penalty_parameter(mem, opts, nlp_opts, pred_optimality, pred_infeasibility);
+    double current_merit = mem->penalty_parameter*current_cost + current_infeasibility; // Shouldn't this be the update below??
+    nlp_mem->objective_multiplier = mem->penalty_parameter;
+
+    int i;
+
+    while (true)
+    {
+        // Calculate trial iterate: trial_iterate = current_iterate + alpha * direction
+        config->step_update(config, dims, nlp_in, nlp_out, nlp_opts, nlp_mem,
+                                     nlp_work, nlp_work->tmp_nlp_out, solver_mem, alpha, globalization_opts->full_step_dual);
+
+        ///////////////////////////////////////////////////////////////////////
+        // Evaluate cost function at trial iterate
+        // set evaluation point to tmp_nlp_out
+        ocp_nlp_set_primal_variable_pointers_in_submodules(config, dims, nlp_in, nlp_work->tmp_nlp_out, nlp_mem);
+        // compute trial dynamics value
+#if defined(ACADOS_WITH_OPENMP)
+    #pragma omp parallel for
+#endif
+        for (i=0; i<N; i++)
+        {
+            // dynamics: Note has to be first, because cost_integration might be used.
+            config->dynamics[i]->compute_fun(config->dynamics[i], dims->dynamics[i], nlp_in->dynamics[i],
+                                            nlp_opts->dynamics[i], nlp_mem->dynamics[i], nlp_work->dynamics[i]);
+        }
+        // compute trial objective function value
+#if defined(ACADOS_WITH_OPENMP)
+    #pragma omp parallel for
+#endif
+        for (i=0; i<=N; i++)
+        {
+            // cost
+            config->cost[i]->compute_fun(config->cost[i], dims->cost[i], nlp_in->cost[i], nlp_opts->cost[i],
+                                        nlp_mem->cost[i], nlp_work->cost[i]);
+        }
+#if defined(ACADOS_WITH_OPENMP)
+    #pragma omp parallel for
+#endif
+        for (i=0; i<=N; i++)
+        {
+            // constr
+            config->constraints[i]->compute_fun(config->constraints[i], dims->constraints[i],
+                                                nlp_in->constraints[i], nlp_opts->constraints[i],
+                                                nlp_mem->constraints[i], nlp_work->constraints[i]);
+        }
+        // reset evaluation point to SQP iterate
+        ocp_nlp_set_primal_variable_pointers_in_submodules(config, dims, nlp_in, nlp_out, nlp_mem);
+
+        double *tmp_fun;
+        // Calculate the trial objective and constraint violation
+        trial_cost = 0.0;
+        for(i=0; i<=N; i++)
+        {
+            tmp_fun = config->cost[i]->memory_get_fun_ptr(nlp_mem->cost[i]);
+            trial_cost += *tmp_fun;
+        }
+        trial_infeasibility = ocp_nlp_get_l1_infeasibility(config, dims, nlp_mem);
+
+        ///////////////////////////////////////////////////////////////////////
+        // Evaluate merit function at trial point
+        double trial_merit = mem->penalty_parameter*trial_cost + trial_infeasibility;
+        pred_merit = mem->penalty_parameter * pred_optimality + current_infeasibility;
+        ared = nlp_mem->cost_value - trial_cost;
+
+        // Funnel globalization
+        accept_step = is_trial_iterate_acceptable_to_funnel(mem, nlp_opts,
+                                                            pred_optimality, ared,
+                                                            alpha, current_infeasibility,
+                                                            trial_infeasibility, current_cost,
+                                                            trial_cost, current_merit, trial_merit,
+                                                            pred_merit, pred_infeasibility);
+
+        if (accept_step)
+        {
+            mem->alpha = alpha;
+            *step_size = alpha;
+            nlp_mem->cost_value = trial_cost;
+            mem->l1_infeasibility = trial_infeasibility;
+            return ACADOS_SUCCESS;
+        }
+
+        if (alpha < globalization_opts->alpha_min)
+        {
+            printf("Funnel Linesearch: Step size gets too small. alpha = %e < alpha_min = %e Should enter penalty phase. \n", alpha, globalization_opts->alpha_min);
+            return ACADOS_MINSTEP;
+        }
+
+        alpha *= globalization_opts->alpha_reduction;
+    }
+}
+
+
+int ocp_nlp_globalization_funnel_find_acceptable_iterate(void *nlp_config_, void *nlp_dims_, void *nlp_in_, void *nlp_out_, void *nlp_mem_, void *solver_mem, void *nlp_work_, void *nlp_opts_, double *step_size)
+{
+    ocp_nlp_config *nlp_config = nlp_config_;
+    ocp_nlp_dims *nlp_dims = nlp_dims_;
+    ocp_nlp_in *nlp_in = nlp_in_;
+    ocp_nlp_out *nlp_out = nlp_out_;
+    ocp_nlp_memory *nlp_mem = nlp_mem_;
+    ocp_nlp_workspace *nlp_work = nlp_work_;
+    ocp_nlp_opts *nlp_opts = nlp_opts_;
+
+    int linesearch_success;
+    linesearch_success = backtracking_line_search(nlp_config, nlp_dims, nlp_in, nlp_out, nlp_mem, solver_mem, nlp_work, nlp_opts, step_size);
+    // Copy new iterate to nlp_out
+    if (linesearch_success == ACADOS_SUCCESS)
+    {
+        // in case line search fails, we do not want to copy trial iterates!
+        copy_ocp_nlp_out(nlp_dims, nlp_work->tmp_nlp_out, nlp_out);
+    }
+    return linesearch_success;
+}
+
+/****************************************************
+Printing functions
+*****************************************************/
+
+void ocp_nlp_globalization_funnel_print_iteration_header()
+{
+    printf("%10s   %8s   %10s   %10s   %7s   ", "obj", "alpha", "funnel_w", "penalty", "it_type");
+}
+
+void ocp_nlp_globalization_funnel_print_iteration(double objective_value, void* nlp_opts_, void* mem_)
+{
+    ocp_nlp_globalization_funnel_memory* mem = (ocp_nlp_globalization_funnel_memory*) mem_;
+    printf("%10.4e   %8.2e   %10.4e   %10.4e   %7c   ",
+            objective_value,
+            mem->alpha,
+            mem->funnel_width,
+            mem->penalty_parameter,
+            mem->funnel_iter_type);
+}
+
+int ocp_nlp_globalization_funnel_needs_objective_value()
+{
+    return 1;
+}
+
+// QP objective value! Do not delete :D
+int ocp_nlp_globalization_funnel_needs_qp_objective_value()
+{
+    return 1;
+}
+
+// TODO(David): maybe rename to initialize
+void ocp_nlp_globalization_funnel_initialize_memory(void *config_, void *dims_, void *nlp_mem_, void *nlp_opts_)
+{
+    printf("Note: The funnel globalization is still under development.\n");
+    printf("If you encouter problems or bugs, please report to the acados developers!\n");
+
+    ocp_nlp_config* config = config_;
+    ocp_nlp_dims* dims = dims_;
+    ocp_nlp_memory *nlp_mem = (ocp_nlp_memory *) nlp_mem_;
+    ocp_nlp_opts *nlp_opts = (ocp_nlp_opts *) nlp_opts_;
+    ocp_nlp_globalization_funnel_opts *opts = nlp_opts->globalization;
+    ocp_nlp_globalization_funnel_memory *mem = nlp_mem->globalization;
+    mem->l1_infeasibility = ocp_nlp_get_l1_infeasibility(config, dims, nlp_mem);
+    initialize_funnel_width(mem, opts, mem->l1_infeasibility);
+    mem->funnel_iter_type = '-';
+    initialize_funnel_penalty_parameter(mem, opts);
+    mem->alpha = 1.0;
+    mem->penalty_parameter = nlp_mem->objective_multiplier;
+}
+
+void ocp_nlp_globalization_funnel_config_initialize_default(ocp_nlp_globalization_config *config)
+{
+    // opts
+    config->opts_calculate_size = &ocp_nlp_globalization_funnel_opts_calculate_size;
+    config->opts_assign = &ocp_nlp_globalization_funnel_opts_assign;
+    config->opts_initialize_default = &ocp_nlp_globalization_funnel_opts_initialize_default;
+    config->opts_set = &ocp_nlp_globalization_funnel_opts_set;
+    // memory
+    config->memory_calculate_size = &ocp_nlp_globalization_funnel_memory_calculate_size;
+    config->memory_assign = &ocp_nlp_globalization_funnel_memory_assign;
+    // functions
+    config->find_acceptable_iterate = &ocp_nlp_globalization_funnel_find_acceptable_iterate;
+    config->print_iteration_header = &ocp_nlp_globalization_funnel_print_iteration_header;
+    config->print_iteration = &ocp_nlp_globalization_funnel_print_iteration;
+    config->needs_objective_value = &ocp_nlp_globalization_funnel_needs_objective_value;
+    config->needs_qp_objective_value = &ocp_nlp_globalization_funnel_needs_qp_objective_value;
+    config->initialize_memory = &ocp_nlp_globalization_funnel_initialize_memory;
+}

acados/ocp_nlp/ocp_nlp_globalization_funnel.h

@@ -0,0 +1,171 @@
+/*
+ * Copyright (c) The acados authors.
+ *
+ * This file is part of acados.
+ *
+ * The 2-Clause BSD License
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.;
+ */
+
+
+/// \addtogroup ocp_nlp
+/// @{
+/// \addtogroup ocp_nlp_globalization
+/// @{
+
+#ifndef ACADOS_OCP_NLP_OCP_NLP_GLOBALIZATION_FUNNEL_H_
+#define ACADOS_OCP_NLP_OCP_NLP_GLOBALIZATION_FUNNEL_H_
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+// blasfeo
+#include "blasfeo_common.h"
+
+// acados
+#include "acados/ocp_nlp/ocp_nlp_globalization_common.h"
+#include "acados/ocp_nlp/ocp_nlp_common.h"
+#include "acados/utils/types.h"
+
+/************************************************
+ * options
+ ************************************************/
+
+typedef struct
+{
+    ocp_nlp_globalization_opts *globalization_opts;
+
+    // Funnel globalization related options
+    double initialization_increase_factor; // for multiplication with initial infeasibility
+    double initialization_upper_bound; // for initialization of initial funnel width
+    double sufficient_decrease_factor; // multiplication factor for funnel suff. decrease factor
+    double kappa; // parameter for reduction of funnel
+    double fraction_switching_condition; // parameter in switching condition
+    double initial_penalty_parameter; // initial penalty parameter for penalty phase
+    double penalty_eta; // fraction in penalty update
+    double penalty_contraction; // penalty contraction factor
+    bool type_switching_condition; // which type of switching condition do we use?
+    bool use_merit_fun_only;
+} ocp_nlp_globalization_funnel_opts;
+
+//
+acados_size_t ocp_nlp_globalization_funnel_opts_calculate_size(void *config, void *dims);
+//
+void *ocp_nlp_globalization_funnel_opts_assign(void *config, void *dims, void *raw_memory);
+//
+void ocp_nlp_globalization_funnel_opts_initialize_default(void *config, void *dims, void *opts);
+//
+void ocp_nlp_globalization_funnel_opts_set(void *config, void *opts, const char *field, void* value);
+
+
+/************************************************
+ * memory
+ ************************************************/
+
+typedef struct
+{
+    double funnel_width;
+    char funnel_iter_type;
+    bool funnel_penalty_mode;
+    double l1_infeasibility;
+    double penalty_parameter;
+    double alpha;
+
+} ocp_nlp_globalization_funnel_memory;
+//
+acados_size_t ocp_nlp_globalization_funnel_memory_calculate_size(void *config, void *dims);
+//
+void *ocp_nlp_globalization_funnel_memory_assign(void *config, void *dims, void *raw_memory);
+//
+/************************************************
+ * functions
+ ************************************************/
+//
+void initialize_funnel_width(ocp_nlp_globalization_funnel_memory *mem, ocp_nlp_globalization_funnel_opts *opts, double initial_infeasibility);
+//
+void update_funnel_penalty_parameter(ocp_nlp_globalization_funnel_memory *mem,
+                                     ocp_nlp_globalization_funnel_opts *opts,
+                                     ocp_nlp_opts *nlp_opts,
+                                     double pred_optimality,
+                                     double pred_infeasibility);
+//
+void decrease_funnel(ocp_nlp_globalization_funnel_memory *mem, ocp_nlp_globalization_funnel_opts *opts, double trial_infeasibility, double current_infeasibility);
+//
+bool is_iterate_inside_of_funnel(ocp_nlp_globalization_funnel_memory *mem, ocp_nlp_globalization_funnel_opts *opts, double infeasibility);
+//
+bool is_funnel_sufficient_decrease_satisfied(ocp_nlp_globalization_funnel_memory *mem, ocp_nlp_globalization_funnel_opts *opts, double infeasibility);
+//
+bool is_switching_condition_satisfied(ocp_nlp_globalization_funnel_opts *opts, double pred_optimality, double step_size, double pred_infeasibility);
+//
+bool is_f_type_armijo_condition_satisfied(ocp_nlp_globalization_opts *globalization_opts,
+                                        double negative_ared,
+                                        double pred,
+                                        double alpha);
+//
+bool is_trial_iterate_acceptable_to_funnel(ocp_nlp_globalization_funnel_memory *mem,
+                                            ocp_nlp_opts *nlp_opts,
+                                            double pred, double ared, double alpha,
+                                            double current_infeasibility,
+                                            double trial_infeasibility,
+                                            double current_objective,
+                                            double trial_objective,
+                                            double current_merit,
+                                            double trial_merit,
+                                            double pred_merit,
+                                            double pred_infeasibility);
+//
+int backtracking_line_search(ocp_nlp_config *config,
+                            ocp_nlp_dims *dims,
+                            ocp_nlp_in *nlp_in,
+                            ocp_nlp_out *nlp_out,
+                            ocp_nlp_memory *nlp_mem,
+                            void *solver_mem,
+                            ocp_nlp_workspace *nlp_work,
+                            ocp_nlp_opts *nlp_opts,
+                            double *step_size);
+//
+int ocp_nlp_globalization_funnel_find_acceptable_iterate(void *nlp_config_, void *nlp_dims_, void *nlp_in_, void *nlp_out_, void *nlp_mem_, void *solver_mem, void *nlp_work_, void *nlp_opts_, double *step_size);
+//
+void ocp_nlp_globalization_funnel_print_iteration_header();
+//
+void ocp_nlp_globalization_funnel_print_iteration(double objective_value,
+                                                    void* nlp_opts_,
+                                                    void* mem_);
+//
+int ocp_nlp_globalization_funnel_needs_objective_value();
+//
+int ocp_nlp_globalization_funnel_needs_qp_objective_value();
+//
+void ocp_nlp_globalization_funnel_initialize_memory(void *config_, void *dims_, void *nlp_mem_, void *nlp_opts_);
+//
+void ocp_nlp_globalization_funnel_config_initialize_default(ocp_nlp_globalization_config *config);
+
+#ifdef __cplusplus
+} /* extern "C" */
+#endif
+
+#endif  // ACADOS_OCP_NLP_OCP_NLP_GLOBALIZATION_FUNNEL_H_
+/// @}
+/// @}

acados/ocp_nlp/ocp_nlp_globalization_merit_backtracking.h

@@ -0,0 +1,135 @@
+/*
+ * Copyright (c) The acados authors.
+ *
+ * This file is part of acados.
+ *
+ * The 2-Clause BSD License
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.;
+ */
+
+
+/// \addtogroup ocp_nlp
+/// @{
+/// \addtogroup ocp_nlp_globalization
+/// @{
+
+#ifndef ACADOS_OCP_NLP_OCP_NLP_GLOBALIZATION_MERIT_BACKTRACKING_H_
+#define ACADOS_OCP_NLP_OCP_NLP_GLOBALIZATION_MERIT_BACKTRACKING_H_
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+// blasfeo
+#include "blasfeo_common.h"
+
+// acados
+#include "acados/ocp_nlp/ocp_nlp_globalization_common.h"
+#include "acados/ocp_nlp/ocp_nlp_common.h"
+#include "acados/utils/types.h"
+
+/************************************************
+ * options
+ ************************************************/
+// TODO: remove stufF!
+typedef struct
+{
+    ocp_nlp_globalization_opts *globalization_opts;
+
+} ocp_nlp_globalization_merit_backtracking_opts;
+
+//
+acados_size_t ocp_nlp_globalization_merit_backtracking_opts_calculate_size(void *config, void *dims);
+//
+void *ocp_nlp_globalization_merit_backtracking_opts_assign(void *config, void *dims, void *raw_memory);
+//
+void ocp_nlp_globalization_merit_backtracking_opts_initialize_default(void *config, void *dims, void *opts);
+//
+void ocp_nlp_globalization_merit_backtracking_opts_set(void *config, void *opts, const char *field, void* value);
+
+
+/************************************************
+ * memory
+ ************************************************/
+
+// TODO: remove stufF!
+typedef struct
+{
+    double step_norm;
+    double alpha;
+} ocp_nlp_globalization_merit_backtracking_memory;
+
+//
+acados_size_t ocp_nlp_globalization_merit_backtracking_memory_calculate_size(void *config, void *dims);
+//
+void *ocp_nlp_globalization_merit_backtracking_memory_assign(void *config, void *dims, void *raw_memory);
+//
+
+/************************************************
+ * functions
+ ************************************************/
+//
+double ocp_nlp_compute_merit_gradient(ocp_nlp_config *config, ocp_nlp_dims *dims,
+                                  ocp_nlp_in *in, ocp_nlp_out *out, ocp_nlp_opts *opts,
+                                  ocp_nlp_memory *mem, ocp_nlp_workspace *work);
+//
+int ocp_nlp_line_search(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_in *in,
+            ocp_nlp_out *out, ocp_nlp_opts *opts, ocp_nlp_memory *mem, ocp_nlp_workspace *work,
+            int sqp_iter, double *alpha_ref);
+//
+double ocp_nlp_evaluate_merit_fun(ocp_nlp_config *config, ocp_nlp_dims *dims, ocp_nlp_in *in,
+          ocp_nlp_out *out, ocp_nlp_opts *opts, ocp_nlp_memory *mem, ocp_nlp_workspace *work);
+//
+void merit_backtracking_initialize_weights(ocp_nlp_dims *dims, ocp_nlp_out *weight_merit_fun, ocp_qp_out *qp_out);
+//
+void merit_backtracking_update_weights(ocp_nlp_dims *dims, ocp_nlp_out *weight_merit_fun, ocp_qp_out *qp_out);
+//
+int ocp_nlp_globalization_merit_backtracking_find_acceptable_iterate(void *nlp_config_, void *nlp_dims_, void *nlp_in_, void *nlp_out_, void *nlp_mem_, void *solver_mem, void *nlp_work_, void *nlp_opts_, double *step_size);
+//
+int ocp_nlp_globalization_merit_backtracking_find_acceptable_iterate_for_ddp(void *nlp_config_, void *nlp_dims_, void *nlp_in_, void *nlp_out_, void *nlp_mem_, void *solver_mem, void *nlp_work_, void *nlp_opts_, double *step_size);
+//
+void ocp_nlp_globalization_merit_backtracking_print_iteration_header();
+//
+void ocp_nlp_globalization_merit_backtracking_print_iteration(double objective_value,
+                                                                void* nlp_opts_,
+                                                                void* mem_);
+//
+int ocp_nlp_globalization_merit_backtracking_needs_objective_value();
+//
+int ocp_nlp_globalization_merit_backtracking_needs_qp_objective_value();
+//
+int ocp_nlp_globalization_merit_backtracking_ddp_needs_qp_objective_value();
+//
+void ocp_nlp_globalization_merit_backtracking_initialize_memory(void *config_,
+    void *dims_, void *nlp_mem_, void *nlp_opts_);
+//
+void ocp_nlp_globalization_merit_backtracking_config_initialize_default(ocp_nlp_globalization_config *config);
+
+#ifdef __cplusplus
+} /* extern "C" */
+#endif
+
+#endif  // ACADOS_OCP_NLP_OCP_NLP_GLOBALIZATION_MERIT_BACKTRACKING_H_
+/// @}
+/// @}

acados/ocp_nlp/ocp_nlp_reg_common.c

@@ -0,0 +1,275 @@
+/*
+ * Copyright (c) The acados authors.
+ *
+ * This file is part of acados.
+ *
+ * The 2-Clause BSD License
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.;
+ */
+
+
+#include <assert.h>
+#include <stdlib.h>
+#include <stdio.h>
+#include <string.h>
+#include <math.h>
+
+#include "acados/utils/math.h"
+
+#include "acados/ocp_nlp/ocp_nlp_reg_common.h"
+
+
+
+/************************************************
+ * config
+ ************************************************/
+
+acados_size_t ocp_nlp_reg_config_calculate_size(void)
+{
+    return sizeof(ocp_nlp_reg_config);
+}
+
+
+
+void *ocp_nlp_reg_config_assign(void *raw_memory)
+{
+    return raw_memory;
+}
+
+
+
+/************************************************
+ * dims
+ ************************************************/
+
+acados_size_t ocp_nlp_reg_dims_calculate_size(int N)
+{
+    acados_size_t size = sizeof(ocp_nlp_reg_dims);
+
+    size += 5*(N+1)*sizeof(int); // nx nu nbu nbx ng
+
+    return size;
+}
+
+
+
+ocp_nlp_reg_dims *ocp_nlp_reg_dims_assign(int N, void *raw_memory)
+{
+    char *c_ptr = (char *) raw_memory;
+
+    // dims
+    ocp_nlp_reg_dims *dims = (ocp_nlp_reg_dims *) c_ptr;
+    c_ptr += sizeof(ocp_nlp_reg_dims);
+    // nx
+    dims->nx = (int *) c_ptr;
+    c_ptr += (N+1)*sizeof(int);
+    // nu
+    dims->nu = (int *) c_ptr;
+    c_ptr += (N+1)*sizeof(int);
+    // nbu
+    dims->nbu = (int *) c_ptr;
+    c_ptr += (N+1)*sizeof(int);
+    // nbx
+    dims->nbx = (int *) c_ptr;
+    c_ptr += (N+1)*sizeof(int);
+    // ng
+    dims->ng = (int *) c_ptr;
+    c_ptr += (N+1)*sizeof(int);
+
+    dims->N = N;
+
+    // initialize to zero by default
+    int ii;
+    // nx
+    for(ii=0; ii<=N; ii++)
+        dims->nx[ii] = 0;
+    // nu
+    for(ii=0; ii<=N; ii++)
+        dims->nu[ii] = 0;
+    // nbx
+    for(ii=0; ii<=N; ii++)
+        dims->nbx[ii] = 0;
+    // nbu
+    for(ii=0; ii<=N; ii++)
+        dims->nbu[ii] = 0;
+    // ng
+    for(ii=0; ii<=N; ii++)
+        dims->ng[ii] = 0;
+
+    assert((char *) raw_memory + ocp_nlp_reg_dims_calculate_size(N) >= c_ptr);
+
+    return dims;
+}
+
+
+
+void ocp_nlp_reg_dims_set(void *config_, ocp_nlp_reg_dims *dims, int stage, char *field, int* value)
+{
+
+    if (!strcmp(field, "nx"))
+    {
+        dims->nx[stage] = *value;
+    }
+    else if (!strcmp(field, "nu"))
+    {
+        dims->nu[stage] = *value;
+    }
+    else if (!strcmp(field, "nbu"))
+    {
+        dims->nbu[stage] = *value;
+    }
+    else if (!strcmp(field, "nbx"))
+    {
+        dims->nbx[stage] = *value;
+    }
+    else if (!strcmp(field, "ng"))
+    {
+        dims->ng[stage] = *value;
+    }
+    else
+    {
+        printf("\nerror: field %s not available in module ocp_nlp_reg_dims_set\n", field);
+        exit(1);
+    }
+
+    return;
+}
+
+
+
+/************************************************
+ * regularization help functions
+ ************************************************/
+
+// reconstruct A = V * d * V'
+void acados_reconstruct_A(int dim, double *A, double *V, double *d)
+{
+    int i, j, k;
+
+    for (i=0; i<dim; i++)
+    {
+        for (j=0; j<=i; j++)
+        {
+            A[i*dim+j] = 0.0;
+            for (k=0; k<dim; k++)
+                A[i*dim+j] += V[i*dim+k] * d[k] * V[j*dim+k];
+            A[j*dim+i] = A[i*dim+j];
+        }
+    }
+}
+
+
+
+// mirroring regularization
+void acados_mirror(int dim, double *A, double *V, double *d, double *e, double epsilon)
+{
+    int i;
+
+    acados_eigen_decomposition(dim, A, V, d, e);
+
+    // mirror
+    for (i = 0; i < dim; i++)
+    {
+        if (d[i] >= -epsilon && d[i] <= epsilon)
+            d[i] = epsilon;
+        else if (d[i] < 0)
+            d[i] = -d[i];
+    }
+
+    acados_reconstruct_A(dim, A, V, d);
+}
+
+void acados_mirror_adaptive_eps(int dim, double *A, double *V, double *d, double *e, double max_cond_block)
+{
+    int i;
+    acados_eigen_decomposition(dim, A, V, d, e);
+    double max_eig = 0.0;
+    double eps;
+
+    // compute max and min eigenvalues
+    for (i=0; i < dim; i++)
+    {
+        max_eig = MAX(max_eig, fabs(d[i]));
+    }
+    if (max_eig == 0.0)
+        eps = 1.0;
+    else
+        eps = max_eig/max_cond_block;
+
+    // mirror
+    for (i = 0; i < dim; i++)
+    {
+        if (d[i] >= -eps && d[i] <= eps)
+            d[i] = eps;
+        else if (d[i] < 0)
+            d[i] = -d[i];
+    }
+
+    acados_reconstruct_A(dim, A, V, d);
+}
+
+// projecting regularization
+void acados_project(int dim, double *A, double *V, double *d, double *e, double epsilon)
+{
+    int i;
+
+    acados_eigen_decomposition(dim, A, V, d, e);
+
+    // project
+    for (i = 0; i < dim; i++)
+    {
+        if (d[i] < epsilon)
+            d[i] = epsilon;
+    }
+
+    acados_reconstruct_A(dim, A, V, d);
+}
+
+
+void acados_project_adaptive_eps(int dim, double *A, double *V, double *d, double *e, double max_cond_block)
+{
+    int i;
+    acados_eigen_decomposition(dim, A, V, d, e);
+    double max_eig = 0.0;
+    double eps;
+
+    // compute max and min eigenvalues
+    for (i=0; i < dim; i++)
+    {
+        max_eig = MAX(max_eig, d[i]);
+    }
+    if (max_eig == 0.0)
+        eps = 1.0;
+    else
+        eps = max_eig/max_cond_block;
+
+    // project
+    for (i = 0; i < dim; i++)
+    {
+        if (d[i] < eps)
+            d[i] = eps;
+    }
+
+    acados_reconstruct_A(dim, A, V, d);
+}

acados/ocp_nlp/ocp_nlp_reg_common.h

@@ -0,0 +1,125 @@
+/*
+ * Copyright (c) The acados authors.
+ *
+ * This file is part of acados.
+ *
+ * The 2-Clause BSD License
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.;
+ */
+
+
+/// \ingroup ocp_nlp
+/// @{
+
+/// \defgroup ocp_nlp_reg ocp_nlp_reg
+/// @{
+
+#ifndef ACADOS_OCP_NLP_OCP_NLP_REG_COMMON_H_
+#define ACADOS_OCP_NLP_OCP_NLP_REG_COMMON_H_
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include "acados/ocp_qp/ocp_qp_common.h"
+
+
+
+/* dims */
+
+// same as qp_dims
+typedef struct
+{
+    int *nx;
+    int *nu;
+    int *nbu;
+    int *nbx;
+    int *ng;
+    int N;
+} ocp_nlp_reg_dims;
+
+//
+acados_size_t ocp_nlp_reg_dims_calculate_size(int N);
+//
+ocp_nlp_reg_dims *ocp_nlp_reg_dims_assign(int N, void *raw_memory);
+//
+void ocp_nlp_reg_dims_set(void *config_, ocp_nlp_reg_dims *dims, int stage, char *field, int* value);
+
+
+
+/* config */
+
+typedef struct
+{
+    /* dims */
+    acados_size_t (*dims_calculate_size)(int N);
+    ocp_nlp_reg_dims *(*dims_assign)(int N, void *raw_memory);
+    void (*dims_set)(void *config, ocp_nlp_reg_dims *dims, int stage, char *field, int *value);
+    /* opts */
+    acados_size_t (*opts_calculate_size)(void);
+    void *(*opts_assign)(void *raw_memory);
+    void (*opts_initialize_default)(void *config, ocp_nlp_reg_dims *dims, void *opts);
+    void (*opts_set)(void *config, void *opts, const char *field, void* value);
+    /* memory */
+    acados_size_t (*memory_calculate_size)(void *config, ocp_nlp_reg_dims *dims, void *opts);
+    void *(*memory_assign)(void *config, ocp_nlp_reg_dims *dims, void *opts, void *raw_memory);
+    void (*memory_set)(void *config, ocp_nlp_reg_dims *dims, void *memory, char *field, void* value);
+    void (*memory_set_RSQrq_ptr)(ocp_nlp_reg_dims *dims, struct blasfeo_dmat *mat, void *memory);
+    void (*memory_set_rq_ptr)(ocp_nlp_reg_dims *dims, struct blasfeo_dvec *vec, void *memory);
+    void (*memory_set_BAbt_ptr)(ocp_nlp_reg_dims *dims, struct blasfeo_dmat *mat, void *memory);
+    void (*memory_set_b_ptr)(ocp_nlp_reg_dims *dims, struct blasfeo_dvec *vec, void *memory);
+    void (*memory_set_idxb_ptr)(ocp_nlp_reg_dims *dims, int **idxb, void *memory);
+    void (*memory_set_DCt_ptr)(ocp_nlp_reg_dims *dims, struct blasfeo_dmat *mat, void *memory);
+    void (*memory_set_ux_ptr)(ocp_nlp_reg_dims *dims, struct blasfeo_dvec *vec, void *memory);
+    void (*memory_set_pi_ptr)(ocp_nlp_reg_dims *dims, struct blasfeo_dvec *vec, void *memory);
+    void (*memory_set_lam_ptr)(ocp_nlp_reg_dims *dims, struct blasfeo_dvec *vec, void *memory);
+    /* functions */
+    void (*regularize)(void *config, ocp_nlp_reg_dims *dims, void *opts, void *memory);
+    void (*regularize_lhs)(void *config, ocp_nlp_reg_dims *dims, void *opts, void *memory);
+    void (*regularize_rhs)(void *config, ocp_nlp_reg_dims *dims, void *opts, void *memory);
+    void (*correct_dual_sol)(void *config, ocp_nlp_reg_dims *dims, void *opts, void *memory);
+} ocp_nlp_reg_config;
+
+//
+acados_size_t ocp_nlp_reg_config_calculate_size(void);
+//
+void *ocp_nlp_reg_config_assign(void *raw_memory);
+
+
+
+/* regularization help functions */
+void acados_reconstruct_A(int dim, double *A, double *V, double *d);
+void acados_mirror(int dim, double *A, double *V, double *d, double *e, double epsilon);
+void acados_mirror_adaptive_eps(int dim, double *A, double *V, double *d, double *e, double max_cond_block);
+void acados_project(int dim, double *A, double *V, double *d, double *e, double epsilon);
+void acados_project_adaptive_eps(int dim, double *A, double *V, double *d, double *e, double max_cond_block);
+
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif  // ACADOS_OCP_NLP_OCP_NLP_REG_COMMON_H_
+/// @}
+/// @}

acados/ocp_nlp/ocp_nlp_reg_convexify.c

@@ -0,0 +1,965 @@
+/*
+ * Copyright (c) The acados authors.
+ *
+ * This file is part of acados.
+ *
+ * The 2-Clause BSD License
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.;
+ */
+
+
+#include "acados/ocp_nlp/ocp_nlp_reg_convexify.h"
+
+#include <stdlib.h>
+#include <stdio.h>
+#include <string.h>
+#include <math.h>
+#include <assert.h>
+
+#include "acados/utils/math.h"
+#include "acados/utils/mem.h"
+
+#include "blasfeo_d_aux.h"
+#include "blasfeo_d_blas.h"
+
+
+
+/************************************************
+ * opts
+ ************************************************/
+
+acados_size_t ocp_nlp_reg_convexify_opts_calculate_size(void)
+{
+    return sizeof(ocp_nlp_reg_convexify_opts);
+}
+
+
+
+void *ocp_nlp_reg_convexify_opts_assign(void *raw_memory)
+{
+    return raw_memory;
+}
+
+
+
+void ocp_nlp_reg_convexify_opts_initialize_default(void *config_, ocp_nlp_reg_dims *dims, void *opts_)
+{
+    ocp_nlp_reg_convexify_opts *opts = opts_;
+
+    opts->delta = 1e-4;
+    opts->epsilon = 1e-4;
+
+    return;
+}
+
+
+
+void ocp_nlp_reg_convexify_opts_set(void *config_, void *opts_, const char *field, void* value)
+{
+
+    ocp_nlp_reg_convexify_opts *opts = opts_;
+
+    if (!strcmp(field, "delta"))
+    {
+        double *d_ptr = value;
+        opts->delta = *d_ptr;
+    }
+    else if (!strcmp(field, "epsilon"))
+    {
+        double *d_ptr = value;
+        opts->epsilon = *d_ptr;
+    }
+    else
+    {
+        printf("\nerror: field %s not available in ocp_nlp_reg_convexify_opts_set\n", field);
+        exit(1);
+    }
+
+    return;
+}
+
+
+
+/************************************************
+ * memory
+ ************************************************/
+
+acados_size_t ocp_nlp_reg_convexify_calculate_memory_size(void *config_, ocp_nlp_reg_dims *dims, void *opts_)
+{
+
+    int N = dims->N;
+    int *nx = dims->nx;
+    int *nu = dims->nu;
+    int *nbx = dims->nbx;
+    int *nbu = dims->nbu;
+    int *ng = dims->ng;
+
+    int ii;
+
+    int nuM = nu[0];
+    for(ii=1; ii<=N; ii++)
+    {
+        nuM = nu[ii]>nuM ? nu[ii] : nuM;
+    }
+
+    int nxM = nx[0];
+    for(ii=1; ii<=N; ii++)
+    {
+        nxM = nx[ii]>nxM ? nx[ii] : nxM;
+    }
+
+    int nuxM = nu[0]+nx[0];
+    for(ii=1; ii<=N; ii++)
+    {
+        nuxM = nu[ii]+nx[ii]>nuxM ? nu[ii]+nx[ii] : nuxM;
+    }
+
+    int nbgM = nbu[0]+nbx[0]+ng[0];
+    for(ii=1; ii<=N; ii++)
+    {
+        nbgM = nbu[ii]+nbx[ii]+ng[ii]>nbgM ? nbu[ii]+nbx[ii]+ng[ii] : nbgM;
+    }
+
+    acados_size_t size = 0;
+
+    size += sizeof(ocp_nlp_reg_convexify_memory);
+
+    size += nuM*nuM*sizeof(double);     // R
+    size += nuxM*nuxM*sizeof(double);   // V
+    size += 2*nuxM*sizeof(double);      // d e
+    size += nuxM*nuxM*sizeof(double);   // reg_hess
+
+    size += (N+1)*sizeof(struct blasfeo_dmat); // original_RSQrq
+
+    size += 1 * 64;
+
+    size += 3*blasfeo_memsize_dmat(nxM, nxM);     // Q_tilde Q_bar delta_eye
+    size += blasfeo_memsize_dmat(nuxM, nxM);    // BAQ
+    size += blasfeo_memsize_dmat(nuM, nuM);     // L
+    size += blasfeo_memsize_dmat(nxM, nuM);     // St_copy
+
+    size += blasfeo_memsize_dvec(nuxM);     // tmp_nuxM
+    size += blasfeo_memsize_dvec(nbgM);     // tmp_nbgM
+
+    for (ii=0; ii<=N; ii++)
+    {
+        size += 2*blasfeo_memsize_dmat(nu[ii]+nx[ii]+1, nu[ii]+nx[ii]); // original_RSQrq
+    }
+    size += blasfeo_memsize_dmat(nuxM, nuxM); // tmp_RSQ
+
+//    size += 2*blasfeo_memsize_dvec(nxM); // grad b2
+
+    // giaf's
+    size += (2*(N+1)+N)*sizeof(struct blasfeo_dmat *); // RSQrq BAbt DCt
+    size += (3*(N+1)+2*N)*sizeof(struct blasfeo_dvec *); // rq b ux pi lam
+    size += (N+1)*sizeof(int *); // idxb
+
+    return size;
+}
+
+
+
+void *ocp_nlp_reg_convexify_assign_memory(void *config_, ocp_nlp_reg_dims *dims, void *opts_, void *raw_memory)
+{
+
+    int N = dims->N;
+    int *nx = dims->nx;
+    int *nu = dims->nu;
+    int *nbx = dims->nbx;
+    int *nbu = dims->nbu;
+    int *ng = dims->ng;
+
+    int ii;
+
+    int nuM = nu[0];
+    for(ii=1; ii<=N; ii++)
+    {
+        nuM = nu[ii]>nuM ? nu[ii] : nuM;
+    }
+
+    int nxM = nx[0];
+    for(ii=1; ii<=N; ii++)
+    {
+        nxM = nx[ii]>nxM ? nx[ii] : nxM;
+    }
+
+    int nuxM = nu[0]+nx[0];
+    for(ii=1; ii<=N; ii++)
+    {
+        nuxM = nu[ii]+nx[ii]>nuxM ? nu[ii]+nx[ii] : nuxM;
+    }
+
+    int nbgM = nbu[0]+nbx[0]+ng[0];
+    for(ii=1; ii<=N; ii++)
+    {
+        nbgM = nbu[ii]+nbx[ii]+ng[ii]>nbgM ? nbu[ii]+nbx[ii]+ng[ii] : nbgM;
+    }
+
+
+    char *c_ptr = (char *) raw_memory;
+
+    ocp_nlp_reg_convexify_memory *mem = (ocp_nlp_reg_convexify_memory *) c_ptr;
+    c_ptr += sizeof(ocp_nlp_reg_convexify_memory);
+
+    mem->R = (double *) c_ptr;
+    c_ptr += nuM*nuM*sizeof(double);
+
+    mem->V = (double *) c_ptr;
+    c_ptr += nuxM*nuxM*sizeof(double);
+
+    mem->d = (double *) c_ptr;
+    c_ptr += nuxM*sizeof(double);
+
+    mem->e = (double *) c_ptr;
+    c_ptr += nuxM*sizeof(double);
+
+    mem->reg_hess = (double *) c_ptr;
+    c_ptr += nuxM*nuxM*sizeof(double);
+
+    mem->original_RSQrq = (struct blasfeo_dmat *) c_ptr;
+    c_ptr += (N+1)*sizeof(struct blasfeo_dmat);
+
+    mem->RSQrq = (struct blasfeo_dmat **) c_ptr;
+    c_ptr += (N+1)*sizeof(struct blasfeo_dmat *);
+
+    mem->BAbt = (struct blasfeo_dmat **) c_ptr;
+    c_ptr += N*sizeof(struct blasfeo_dmat *);
+
+    mem->DCt = (struct blasfeo_dmat **) c_ptr;
+    c_ptr += (N+1)*sizeof(struct blasfeo_dmat *);
+
+    mem->rq = (struct blasfeo_dvec **) c_ptr;
+    c_ptr += (N+1)*sizeof(struct blasfeo_dvec *);
+
+    mem->b = (struct blasfeo_dvec **) c_ptr;
+    c_ptr += N*sizeof(struct blasfeo_dvec *);
+
+    mem->ux = (struct blasfeo_dvec **) c_ptr;
+    c_ptr += (N+1)*sizeof(struct blasfeo_dvec *);
+
+    mem->pi = (struct blasfeo_dvec **) c_ptr;
+    c_ptr += N*sizeof(struct blasfeo_dvec *);
+
+    mem->lam = (struct blasfeo_dvec **) c_ptr;
+    c_ptr += (N+1)*sizeof(struct blasfeo_dvec *);
+
+    mem->idxb = (int **) c_ptr;
+    c_ptr += (N+1)*sizeof(int *);
+
+    align_char_to(64, &c_ptr);
+
+    assign_and_advance_blasfeo_dmat_mem(nxM, nxM, &mem->Q_tilde, &c_ptr);
+    assign_and_advance_blasfeo_dmat_mem(nxM, nxM, &mem->Q_bar, &c_ptr);
+    assign_and_advance_blasfeo_dmat_mem(nuxM, nxM, &mem->BAQ, &c_ptr);
+    assign_and_advance_blasfeo_dmat_mem(nuM, nuM, &mem->L, &c_ptr);
+    assign_and_advance_blasfeo_dmat_mem(nxM, nxM, &mem->delta_eye, &c_ptr);
+    assign_and_advance_blasfeo_dmat_mem(nxM, nuM, &mem->St_copy, &c_ptr);
+
+    for (ii=0; ii<=N; ii++)
+        assign_and_advance_blasfeo_dmat_mem(nu[ii]+nx[ii]+1, nu[ii]+nx[ii], &mem->original_RSQrq[ii], &c_ptr);
+
+
+    assign_and_advance_blasfeo_dmat_mem(nuxM, nuxM, &mem->tmp_RSQ, &c_ptr);
+
+    assign_and_advance_blasfeo_dvec_mem(nuxM, &mem->tmp_nuxM, &c_ptr);
+    assign_and_advance_blasfeo_dvec_mem(nbgM, &mem->tmp_nbgM, &c_ptr);
+
+//    assign_and_advance_blasfeo_dvec_mem(nxM, &mem->grad, &c_ptr);
+//    assign_and_advance_blasfeo_dvec_mem(nxM, &mem->b2, &c_ptr);
+
+    assert((char *)mem + ocp_nlp_reg_convexify_calculate_memory_size(config_, dims, opts_) >= c_ptr);
+
+    return mem;
+}
+
+
+
+void ocp_nlp_reg_convexify_memory_set_RSQrq_ptr(ocp_nlp_reg_dims *dims, struct blasfeo_dmat *RSQrq, void *memory_)
+{
+    ocp_nlp_reg_convexify_memory *memory = memory_;
+
+    int ii;
+
+    int N = dims->N;
+
+    for(ii=0; ii<=N; ii++)
+    {
+        memory->RSQrq[ii] = RSQrq+ii;
+//        blasfeo_print_dmat(nu[ii]+nx[ii], nu[ii]+nx[ii], memory->RSQrq[ii], 0, 0);
+    }
+
+    return;
+}
+
+
+
+void ocp_nlp_reg_convexify_memory_set_rq_ptr(ocp_nlp_reg_dims *dims, struct blasfeo_dvec *rq, void *memory_)
+{
+    ocp_nlp_reg_convexify_memory *memory = memory_;
+
+    int ii;
+
+    int N = dims->N;
+    // int *nx = dims->nx;
+    // int *nu = dims->nu;
+
+    for(ii=0; ii<=N; ii++)
+    {
+        memory->rq[ii] = rq+ii;
+//        blasfeo_print_dvec(nu[ii]+nx[ii], memory->rq[ii], 0);
+    }
+
+    return;
+}
+
+
+
+void ocp_nlp_reg_convexify_memory_set_BAbt_ptr(ocp_nlp_reg_dims *dims, struct blasfeo_dmat *BAbt, void *memory_)
+{
+    ocp_nlp_reg_convexify_memory *memory = memory_;
+
+    int ii;
+
+    int N = dims->N;
+    // int *nx = dims->nx;
+    // int *nu = dims->nu;
+
+    for(ii=0; ii<N; ii++)
+    {
+        memory->BAbt[ii] = BAbt+ii;
+//        blasfeo_print_dmat(nu[ii]+nx[ii]+1, nx[ii+1], memory->BAbt[ii], 0, 0);
+    }
+
+    return;
+}
+
+
+
+void ocp_nlp_reg_convexify_memory_set_b_ptr(ocp_nlp_reg_dims *dims, struct blasfeo_dvec *b, void *memory_)
+{
+    ocp_nlp_reg_convexify_memory *memory = memory_;
+
+    int ii;
+
+    int N = dims->N;
+    // int *nx = dims->nx;
+    // int *nu = dims->nu;
+
+    for(ii=0; ii<N; ii++)
+    {
+        memory->b[ii] = b+ii;
+//        blasfeo_print_dvec(nx[ii=1], memory->b[ii], 0);
+    }
+
+    return;
+}
+
+
+
+void ocp_nlp_reg_convexify_memory_set_idxb_ptr(ocp_nlp_reg_dims *dims, int **idxb, void *memory_)
+{
+    ocp_nlp_reg_convexify_memory *memory = memory_;
+
+    int ii;
+
+    int N = dims->N;
+
+    for(ii=0; ii<=N; ii++)
+    {
+        memory->idxb[ii] = idxb[ii];
+    }
+
+    return;
+}
+
+
+
+void ocp_nlp_reg_convexify_memory_set_DCt_ptr(ocp_nlp_reg_dims *dims, struct blasfeo_dmat *DCt, void *memory_)
+{
+    ocp_nlp_reg_convexify_memory *memory = memory_;
+
+    int ii;
+
+    int N = dims->N;
+
+    for(ii=0; ii<=N; ii++)
+    {
+        memory->DCt[ii] = DCt+ii;
+//        blasfeo_print_dmat(nu[ii]+nx[ii]+1, ng[ii], memory->DCt[ii], 0, 0);
+    }
+
+    return;
+}
+
+
+
+void ocp_nlp_reg_convexify_memory_set_ux_ptr(ocp_nlp_reg_dims *dims, struct blasfeo_dvec *ux, void *memory_)
+{
+    ocp_nlp_reg_convexify_memory *memory = memory_;
+
+    int ii;
+
+    int N = dims->N;
+    // int *nx = dims->nx;
+    // int *nu = dims->nu;
+
+    for(ii=0; ii<=N; ii++)
+    {
+        memory->ux[ii] = ux+ii;
+//        blasfeo_print_dvec(nu[ii]+nx[ii], memory->ux[ii], 0);
+    }
+
+    return;
+}
+
+
+
+void ocp_nlp_reg_convexify_memory_set_pi_ptr(ocp_nlp_reg_dims *dims, struct blasfeo_dvec *pi, void *memory_)
+{
+    ocp_nlp_reg_convexify_memory *memory = memory_;
+
+    int ii;
+
+    int N = dims->N;
+    // int *nx = dims->nx;
+    // int *nu = dims->nu;
+
+    for(ii=0; ii<N; ii++)
+    {
+        memory->pi[ii] = pi+ii;
+//        blasfeo_print_dvec(nx[ii+1], memory->pi[ii], 0);
+    }
+
+    return;
+}
+
+
+
+void ocp_nlp_reg_convexify_memory_set_lam_ptr(ocp_nlp_reg_dims *dims, struct blasfeo_dvec *lam, void *memory_)
+{
+    ocp_nlp_reg_convexify_memory *memory = memory_;
+
+    int ii;
+
+    int N = dims->N;
+    // int *nbu = dims->nbu;
+    // int *nbx = dims->nbx;
+    // int *ng = dims->ng;
+
+    for(ii=0; ii<=N; ii++)
+    {
+        memory->lam[ii] = lam+ii;
+//        blasfeo_print_dvec(2*nbu[ii]+2*nbx[ii]+2*ng[ii], memory->lam[ii], 0);
+    }
+
+    return;
+}
+
+
+
+void ocp_nlp_reg_convexify_memory_set(void *config_, ocp_nlp_reg_dims *dims, void *memory_, char *field, void *value)
+{
+
+    if(!strcmp(field, "RSQrq_ptr"))
+    {
+        struct blasfeo_dmat *RSQrq = value;
+        ocp_nlp_reg_convexify_memory_set_RSQrq_ptr(dims, RSQrq, memory_);
+    }
+    else if(!strcmp(field, "rq_ptr"))
+    {
+        struct blasfeo_dvec *rq = value;
+        ocp_nlp_reg_convexify_memory_set_rq_ptr(dims, rq, memory_);
+    }
+    else if(!strcmp(field, "BAbt_ptr"))
+    {
+        struct blasfeo_dmat *BAbt = value;
+        ocp_nlp_reg_convexify_memory_set_BAbt_ptr(dims, BAbt, memory_);
+    }
+    else if(!strcmp(field, "b_ptr"))
+    {
+        struct blasfeo_dvec *b = value;
+        ocp_nlp_reg_convexify_memory_set_b_ptr(dims, b, memory_);
+    }
+    else if(!strcmp(field, "idxb_ptr"))
+    {
+        int **idxb = value;
+        ocp_nlp_reg_convexify_memory_set_idxb_ptr(dims, idxb, memory_);
+    }
+    else if(!strcmp(field, "DCt_ptr"))
+    {
+        struct blasfeo_dmat *DCt = value;
+        ocp_nlp_reg_convexify_memory_set_DCt_ptr(dims, DCt, memory_);
+    }
+    else if(!strcmp(field, "ux_ptr"))
+    {
+        struct blasfeo_dvec *ux = value;
+        ocp_nlp_reg_convexify_memory_set_ux_ptr(dims, ux, memory_);
+    }
+    else if(!strcmp(field, "pi_ptr"))
+    {
+        struct blasfeo_dvec *pi = value;
+        ocp_nlp_reg_convexify_memory_set_pi_ptr(dims, pi, memory_);
+    }
+    else if(!strcmp(field, "lam_ptr"))
+    {
+        struct blasfeo_dvec *lam = value;
+        ocp_nlp_reg_convexify_memory_set_lam_ptr(dims, lam, memory_);
+    }
+    else
+    {
+        printf("\nerror: field %s not available in ocp_nlp_reg_convexify_set\n", field);
+        exit(1);
+    }
+
+    return;
+}
+
+
+
+/************************************************
+ * functions
+ ************************************************/
+// Algorithm 6 from Verschueren2017
+// NOTE this only considers the case of (dynamics) equality constraints (no inequality constraints)
+// TODO inequality constraints case
+void ocp_nlp_reg_convexify_regularize(void *config, ocp_nlp_reg_dims *dims, void *opts_, void *mem_)
+{
+    ocp_nlp_reg_convexify_memory *mem = mem_;
+    ocp_nlp_reg_convexify_opts *opts = opts_;
+
+    int ii, jj;
+
+    int *nx = dims->nx;
+    int *nu = dims->nu;
+    int N = dims->N;
+
+    double delta = opts->delta;
+
+    blasfeo_drowin(nu[N]+nx[N], 1.0, mem->rq[N], 0, mem->RSQrq[N], nu[N]+nx[N], 0);
+    blasfeo_dgecp(nu[N]+nx[N]+1, nu[N]+nx[N], mem->RSQrq[N], 0, 0, &mem->original_RSQrq[N], 0, 0);
+
+    // TODO regularize R at last stage if needed !!!
+    // TODO fix for nu[N]>0 !!!!!!!!!!
+
+    // delta_eye = delta * eye(nx[N])
+    blasfeo_dgese(nx[N], nx[N], 0.0, &mem->delta_eye, 0, 0);
+    blasfeo_ddiare(nx[N], delta, &mem->delta_eye, 0, 0);
+    // Q_tilde = delta_eye
+    blasfeo_dgecp(nx[N], nx[N], &mem->delta_eye, 0, 0, &mem->Q_tilde, 0, 0);
+    // Q_bar = RSQ[N]
+    blasfeo_dgecp(nx[N], nx[N], mem->RSQrq[N], nu[N], nu[N], &mem->Q_bar, 0, 0);
+    // RSQ[N] = Q_tilde
+    blasfeo_dgecp(nx[N], nx[N], &mem->Q_tilde, 0, 0, mem->RSQrq[N], nu[N], nu[N]);
+    // Q_bar -= Q_tilde
+    blasfeo_dgead(nx[N], nx[N], -1.0, &mem->Q_tilde, 0, 0, &mem->Q_bar, 0, 0);
+    // make Q_bar symmetric
+    blasfeo_dtrtr_l(nx[N], &mem->Q_bar, 0, 0, &mem->Q_bar, 0, 0);
+
+    for (ii = N-1; ii >= 0; --ii)
+    {
+        // add b in BAbt, rq in RSQrq
+        blasfeo_drowin(nx[ii+1], 1.0, mem->b[ii], 0, mem->BAbt[ii], nu[ii]+nx[ii], 0);
+        blasfeo_drowin(nu[ii]+nx[ii], 1.0, mem->rq[ii], 0, mem->RSQrq[ii], nu[ii]+nx[ii], 0);
+        // backup RSQrq -> original_RSQrq
+        blasfeo_dgecp(nu[ii]+nx[ii]+1, nu[ii]+nx[ii], mem->RSQrq[ii], 0, 0, &mem->original_RSQrq[ii], 0, 0);
+
+        // printf("----------------\n");
+        // printf("--- stage %d ---\n", i);
+        // printf("----------------\n");
+
+        // printf("QSR\n");
+        // blasfeo_print_dmat(nx+nu+1, nx+nu, &work->qp_in->RSQrq[i], 0, 0);
+
+        // printf("Q_bar\n");
+        // blasfeo_print_dmat(nx, nx, &Q_bar, 0, 0);
+
+        // printf("BAbt\n");
+        // blasfeo_print_dmat(nx+nu, nx, &work->qp_in->BAbt[i], 0, 0);
+
+        // TODO implement using cholesky
+
+        // BAQ = BA * Q_bar
+        blasfeo_dgemm_nt(nu[ii]+nx[ii], nx[ii], nx[ii+1], 1.0, mem->BAbt[ii], 0, 0, &mem->Q_bar, 0, 0, 0.0, &mem->BAQ, 0, 0, &mem->BAQ, 0, 0);
+        // rank nx[ii+1] update to RSQrq with BAQ
+        blasfeo_dsyrk_ln_mn(nu[ii]+nx[ii]+1, nu[ii]+nx[ii], nx[ii+1], 1.0, mem->BAbt[ii], 0, 0, &mem->BAQ, 0, 0, 1.0, mem->RSQrq[ii], 0, 0, mem->RSQrq[ii], 0, 0);
+
+        blasfeo_drowex(nu[ii]+nx[ii], 1.0, mem->RSQrq[ii], nu[ii]+nx[ii], 0, mem->rq[ii], 0);
+
+        // printf("BAQ\n");
+        // blasfeo_print_dmat(nx+nu, nx, &BAQ, 0, 0);
+
+        blasfeo_unpack_dmat(nu[ii], nu[ii], mem->RSQrq[ii], 0, 0, mem->R, nu[ii]);
+        acados_eigen_decomposition(nu[ii], mem->R, mem->V, mem->d, mem->e);
+
+        bool needs_regularization = false;
+        for (jj = 0; jj < nu[ii]; jj++)
+            if (mem->d[jj] < 1e-10)
+                needs_regularization = true;
+
+        if (needs_regularization)
+        {
+            blasfeo_dgecp(nu[ii]+nx[ii], nu[ii]+nx[ii], mem->RSQrq[ii], 0, 0, &mem->tmp_RSQ, 0, 0);
+            // TODO project only nu instead ???????????
+            // TODO compute correction as a separate matrix, and apply to original_RSQrq too (TODO change this name then)
+//            acados_mirror(nu[ii]+nx[ii], mem->reg_hess, mem->V, mem->d, mem->e, 1e-4);
+            blasfeo_unpack_dmat(nu[ii]+nx[ii], nu[ii]+nx[ii], mem->RSQrq[ii], 0, 0, mem->reg_hess, nu[ii]+nx[ii]);
+            acados_project(nu[ii]+nx[ii], mem->reg_hess, mem->V, mem->d, mem->e, 1e-4);
+            blasfeo_pack_dmat(nu[ii]+nx[ii], nu[ii]+nx[ii], mem->reg_hess, nu[ii]+nx[ii], mem->RSQrq[ii], 0, 0);
+
+            blasfeo_dgead(nu[ii]+nx[ii], nu[ii]+nx[ii], -1.0, mem->RSQrq[ii], 0, 0, &mem->tmp_RSQ, 0, 0);
+            blasfeo_dgead(nu[ii]+nx[ii], nu[ii]+nx[ii], -1.0, &mem->tmp_RSQ, 0, 0, &mem->original_RSQrq[ii], 0, 0);
+        }
+
+        // printf("QSR_hat\n");
+        // blasfeo_print_dmat(nx+nu+1, nx+nu, &work->qp_in->RSQrq[i], 0, 0);
+
+
+        // backup Q
+        blasfeo_dgecp(nx[ii], nx[ii], mem->RSQrq[ii], nu[ii], nu[ii], &mem->Q_bar, 0, 0);
+
+        // R = L * L^T
+        blasfeo_dpotrf_l(nu[ii], mem->RSQrq[ii], 0, 0, &mem->L, 0, 0);
+        // Q = S^T * L^-T
+        blasfeo_dgecp(nx[ii], nu[ii], mem->RSQrq[ii], nu[ii], 0, &mem->St_copy, 0, 0);
+//        blasfeo_dtrsm_rltn(nx[ii], nu[ii], 1.0, &mem->L, 0, 0, &mem->St_copy, 0, 0, &mem->Q_tilde, 0, 0);
+        blasfeo_dtrsm_rltn(nx[ii], nu[ii], 1.0, &mem->L, 0, 0, &mem->St_copy, 0, 0, &mem->St_copy, 0, 0);
+
+        // Q = S^T * R^-1 * S + delta*I
+        blasfeo_dgese(nx[ii], nx[ii], 0.0, &mem->delta_eye, 0, 0);
+        blasfeo_ddiare(nx[ii], delta, &mem->delta_eye, 0, 0);
+//        blasfeo_dsyrk_ln(nx[ii], nx[ii], 1.0, &mem->Q_tilde, 0, 0, &mem->Q_tilde, 0, 0, 1.0, &mem->delta_eye, 0, 0, mem->RSQrq[ii], nu[ii], nu[ii]);
+        blasfeo_dsyrk_ln(nx[ii], nu[ii], 1.0, &mem->St_copy, 0, 0, &mem->St_copy, 0, 0, 1.0, &mem->delta_eye, 0, 0, mem->RSQrq[ii], nu[ii], nu[ii]);
+
+        // printf("H_tilde\n");
+        // blasfeo_print_dmat(nu+nx, nu+nx, &work->qp_in->RSQrq[i], 0, 0);
+
+        // make symmetric
+//        blasfeo_dtrtr_l(nx[ii], &mem->Q_bar, 0, 0, &mem->Q_bar, 0, 0);
+
+        // TODO take from b !!!!!!
+//        for (jj = 0; jj < nx[ii+1]; jj++)
+//            BLASFEO_DVECEL(&mem->b2, jj) = BLASFEO_DMATEL(mem->BAbt[ii], nu[ii]+nx[ii], jj);
+
+        // TODO nx stage is not consistent with above !!!!!!!
+//        blasfeo_dgemv_n(nx[ii+1], nx[ii+1], 1.0, &mem->Q_bar, 0, 0, &mem->b2, 0, 0.0, &mem->grad, 0, &mem->grad, 0);
+//        blasfeo_dgemv_n(nu[ii]+nx[ii], nx[ii+1], 1.0, mem->BAbt[ii], 0, 0, &mem->grad, 0, 0.0, &mem->b2, 0, &mem->b2, 0);
+
+//        for (jj = 0; jj < nu[ii]+nx[ii]; jj++)
+            // TODO maybe 'b' is a bad naming...
+//            BLASFEO_DMATEL(mem->RSQrq[ii], nu[ii]+nx[ii], jj) = BLASFEO_DMATEL(mem->RSQrq[ii], nu[ii]+nx[ii], jj) + BLASFEO_DVECEL(&mem->b2, jj);
+
+        blasfeo_dgead(nx[ii], nx[ii], -1.0, mem->RSQrq[ii], nu[ii], nu[ii], &mem->Q_bar, 0, 0);
+
+        // make symmetric
+        blasfeo_dtrtr_l(nx[ii], &mem->Q_bar, 0, 0, &mem->Q_bar, 0, 0);
+
+    }
+
+    return;
+}
+
+
+// TODO: implement proper split! Right now, we basically do everything twice.
+
+void ocp_nlp_reg_convexify_regularize_lhs(void *config, ocp_nlp_reg_dims *dims, void *opts_, void *mem_)
+{
+    ocp_nlp_reg_convexify_memory *mem = mem_;
+    ocp_nlp_reg_convexify_opts *opts = opts_;
+
+    int ii, jj;
+
+    int *nx = dims->nx;
+    int *nu = dims->nu;
+    int N = dims->N;
+
+
+    double delta = opts->delta;
+
+    // regularize() without updating mem->rq!
+    // blasfeo_drowin(nu[N]+nx[N], 1.0, mem->rq[N], 0, mem->RSQrq[N], nu[N]+nx[N], 0);
+
+    blasfeo_dgecp(nu[N]+nx[N]+1, nu[N]+nx[N], mem->RSQrq[N], 0, 0, &mem->original_RSQrq[N], 0, 0);
+
+    blasfeo_dgese(nx[N], nx[N], 0.0, &mem->delta_eye, 0, 0);
+    blasfeo_ddiare(nx[N], delta, &mem->delta_eye, 0, 0);
+    blasfeo_dgecp(nx[N], nx[N], &mem->delta_eye, 0, 0, &mem->Q_tilde, 0, 0);
+    blasfeo_dgecp(nx[N], nx[N], mem->RSQrq[N], nu[N], nu[N], &mem->Q_bar, 0, 0);
+    blasfeo_dgecp(nx[N], nx[N], &mem->Q_tilde, 0, 0, mem->RSQrq[N], nu[N], nu[N]);
+    blasfeo_dgead(nx[N], nx[N], -1.0, &mem->Q_tilde, 0, 0, &mem->Q_bar, 0, 0);
+    blasfeo_dtrtr_l(nx[N], &mem->Q_bar, 0, 0, &mem->Q_bar, 0, 0);
+
+    for (ii = N-1; ii >= 0; --ii)
+    {
+        blasfeo_drowin(nx[ii+1], 1.0, mem->b[ii], 0, mem->BAbt[ii], nu[ii]+nx[ii], 0);
+        // blasfeo_drowin(nu[ii]+nx[ii], 1.0, mem->rq[ii], 0, mem->RSQrq[ii], nu[ii]+nx[ii], 0);
+
+        blasfeo_dgecp(nu[ii]+nx[ii]+1, nu[ii]+nx[ii], mem->RSQrq[ii], 0, 0, &mem->original_RSQrq[ii], 0, 0);
+
+        blasfeo_dgemm_nt(nu[ii]+nx[ii], nx[ii], nx[ii+1], 1.0, mem->BAbt[ii], 0, 0, &mem->Q_bar, 0, 0, 0.0, &mem->BAQ, 0, 0, &mem->BAQ, 0, 0);
+        blasfeo_dsyrk_ln_mn(nu[ii]+nx[ii]+1, nu[ii]+nx[ii], nx[ii+1], 1.0, mem->BAbt[ii], 0, 0, &mem->BAQ, 0, 0, 1.0, mem->RSQrq[ii], 0, 0, mem->RSQrq[ii], 0, 0);
+
+        // blasfeo_drowex(nu[ii]+nx[ii], 1.0, mem->RSQrq[ii], nu[ii]+nx[ii], 0, mem->rq[ii], 0);
+
+        blasfeo_unpack_dmat(nu[ii], nu[ii], mem->RSQrq[ii], 0, 0, mem->R, nu[ii]);
+        acados_eigen_decomposition(nu[ii], mem->R, mem->V, mem->d, mem->e);
+
+        bool needs_regularization = false;
+        for (jj = 0; jj < nu[ii]; jj++)
+            if (mem->d[jj] < 1e-10)
+                needs_regularization = true;
+
+        if (needs_regularization)
+        {
+            blasfeo_dgecp(nu[ii]+nx[ii], nu[ii]+nx[ii], mem->RSQrq[ii], 0, 0, &mem->tmp_RSQ, 0, 0);
+
+            blasfeo_unpack_dmat(nu[ii]+nx[ii], nu[ii]+nx[ii], mem->RSQrq[ii], 0, 0, mem->reg_hess, nu[ii]+nx[ii]);
+            acados_project(nu[ii]+nx[ii], mem->reg_hess, mem->V, mem->d, mem->e, 1e-4);
+            blasfeo_pack_dmat(nu[ii]+nx[ii], nu[ii]+nx[ii], mem->reg_hess, nu[ii]+nx[ii], mem->RSQrq[ii], 0, 0);
+
+            blasfeo_dgead(nu[ii]+nx[ii], nu[ii]+nx[ii], -1.0, mem->RSQrq[ii], 0, 0, &mem->tmp_RSQ, 0, 0);
+            blasfeo_dgead(nu[ii]+nx[ii], nu[ii]+nx[ii], -1.0, &mem->tmp_RSQ, 0, 0, &mem->original_RSQrq[ii], 0, 0);
+        }
+
+        // backup Q
+        blasfeo_dgecp(nx[ii], nx[ii], mem->RSQrq[ii], nu[ii], nu[ii], &mem->Q_bar, 0, 0);
+
+        // R = L * L^T
+        blasfeo_dpotrf_l(nu[ii], mem->RSQrq[ii], 0, 0, &mem->L, 0, 0);
+        // Q = S^T * L^-T
+        blasfeo_dgecp(nx[ii], nu[ii], mem->RSQrq[ii], nu[ii], 0, &mem->St_copy, 0, 0);
+//        blasfeo_dtrsm_rltn(nx[ii], nu[ii], 1.0, &mem->L, 0, 0, &mem->St_copy, 0, 0, &mem->Q_tilde, 0, 0);
+        blasfeo_dtrsm_rltn(nx[ii], nu[ii], 1.0, &mem->L, 0, 0, &mem->St_copy, 0, 0, &mem->St_copy, 0, 0);
+
+        // Q = S^T * R^-1 * S + delta*I
+        blasfeo_dgese(nx[ii], nx[ii], 0.0, &mem->delta_eye, 0, 0);
+        blasfeo_ddiare(nx[ii], delta, &mem->delta_eye, 0, 0);
+//        blasfeo_dsyrk_ln(nx[ii], nx[ii], 1.0, &mem->Q_tilde, 0, 0, &mem->Q_tilde, 0, 0, 1.0, &mem->delta_eye, 0, 0, mem->RSQrq[ii], nu[ii], nu[ii]);
+        blasfeo_dsyrk_ln(nx[ii], nu[ii], 1.0, &mem->St_copy, 0, 0, &mem->St_copy, 0, 0, 1.0, &mem->delta_eye, 0, 0, mem->RSQrq[ii], nu[ii], nu[ii]);
+
+        blasfeo_dgead(nx[ii], nx[ii], -1.0, mem->RSQrq[ii], nu[ii], nu[ii], &mem->Q_bar, 0, 0);
+
+        // make symmetric
+        blasfeo_dtrtr_l(nx[ii], &mem->Q_bar, 0, 0, &mem->Q_bar, 0, 0);
+
+    }
+
+    return;
+}
+
+
+
+void ocp_nlp_reg_convexify_regularize_rhs(void *config, ocp_nlp_reg_dims *dims, void *opts_, void *mem_)
+{
+    ocp_nlp_reg_convexify_memory *mem = mem_;
+    ocp_nlp_reg_convexify_opts *opts = opts_;
+
+    int ii, jj;
+
+    int *nx = dims->nx;
+    int *nu = dims->nu;
+    int N = dims->N;
+
+
+    double delta = opts->delta;
+
+    // start from original qp and do it all from scratch
+    for (ii = 0; ii<=N; ii++)
+    {
+        blasfeo_dgecp(nu[ii]+nx[ii]+1, nu[ii]+nx[ii], &mem->original_RSQrq[ii], 0, 0, mem->RSQrq[ii], 0, 0);
+    }
+
+    blasfeo_drowin(nu[N]+nx[N], 1.0, mem->rq[N], 0, mem->RSQrq[N], nu[N]+nx[N], 0);
+    blasfeo_dgecp(nu[N]+nx[N]+1, nu[N]+nx[N], mem->RSQrq[N], 0, 0, &mem->original_RSQrq[N], 0, 0);
+
+    blasfeo_dgese(nx[N], nx[N], 0.0, &mem->delta_eye, 0, 0);
+    blasfeo_ddiare(nx[N], delta, &mem->delta_eye, 0, 0);
+    blasfeo_dgecp(nx[N], nx[N], &mem->delta_eye, 0, 0, &mem->Q_tilde, 0, 0);
+    blasfeo_dgecp(nx[N], nx[N], mem->RSQrq[N], nu[N], nu[N], &mem->Q_bar, 0, 0);
+    blasfeo_dgecp(nx[N], nx[N], &mem->Q_tilde, 0, 0, mem->RSQrq[N], nu[N], nu[N]);
+    blasfeo_dgead(nx[N], nx[N], -1.0, &mem->Q_tilde, 0, 0, &mem->Q_bar, 0, 0);
+    blasfeo_dtrtr_l(nx[N], &mem->Q_bar, 0, 0, &mem->Q_bar, 0, 0);
+
+    for (ii = N-1; ii >= 0; --ii)
+    {
+        blasfeo_drowin(nx[ii+1], 1.0, mem->b[ii], 0, mem->BAbt[ii], nu[ii]+nx[ii], 0);
+        blasfeo_drowin(nu[ii]+nx[ii], 1.0, mem->rq[ii], 0, mem->RSQrq[ii], nu[ii]+nx[ii], 0);
+
+        // blasfeo_dgecp(nu[ii]+nx[ii]+1, nu[ii]+nx[ii], mem->RSQrq[ii], 0, 0, &mem->original_RSQrq[ii], 0, 0);
+
+        blasfeo_dgemm_nt(nu[ii]+nx[ii], nx[ii], nx[ii+1], 1.0, mem->BAbt[ii], 0, 0, &mem->Q_bar, 0, 0, 0.0, &mem->BAQ, 0, 0, &mem->BAQ, 0, 0);
+        blasfeo_dsyrk_ln_mn(nu[ii]+nx[ii]+1, nu[ii]+nx[ii], nx[ii+1], 1.0, mem->BAbt[ii], 0, 0, &mem->BAQ, 0, 0, 1.0, mem->RSQrq[ii], 0, 0, mem->RSQrq[ii], 0, 0);
+
+        blasfeo_drowex(nu[ii]+nx[ii], 1.0, mem->RSQrq[ii], nu[ii]+nx[ii], 0, mem->rq[ii], 0);
+
+        blasfeo_unpack_dmat(nu[ii], nu[ii], mem->RSQrq[ii], 0, 0, mem->R, nu[ii]);
+        acados_eigen_decomposition(nu[ii], mem->R, mem->V, mem->d, mem->e);
+
+        bool needs_regularization = false;
+        for (jj = 0; jj < nu[ii]; jj++)
+            if (mem->d[jj] < 1e-10)
+                needs_regularization = true;
+
+        if (needs_regularization)
+        {
+            blasfeo_dgecp(nu[ii]+nx[ii], nu[ii]+nx[ii], mem->RSQrq[ii], 0, 0, &mem->tmp_RSQ, 0, 0);
+
+            blasfeo_unpack_dmat(nu[ii]+nx[ii], nu[ii]+nx[ii], mem->RSQrq[ii], 0, 0, mem->reg_hess, nu[ii]+nx[ii]);
+            acados_project(nu[ii]+nx[ii], mem->reg_hess, mem->V, mem->d, mem->e, 1e-4);
+            blasfeo_pack_dmat(nu[ii]+nx[ii], nu[ii]+nx[ii], mem->reg_hess, nu[ii]+nx[ii], mem->RSQrq[ii], 0, 0);
+
+            blasfeo_dgead(nu[ii]+nx[ii], nu[ii]+nx[ii], -1.0, mem->RSQrq[ii], 0, 0, &mem->tmp_RSQ, 0, 0);
+            blasfeo_dgead(nu[ii]+nx[ii], nu[ii]+nx[ii], -1.0, &mem->tmp_RSQ, 0, 0, &mem->original_RSQrq[ii], 0, 0);
+        }
+
+        // backup Q
+        blasfeo_dgecp(nx[ii], nx[ii], mem->RSQrq[ii], nu[ii], nu[ii], &mem->Q_bar, 0, 0);
+
+        // R = L * L^T
+        blasfeo_dpotrf_l(nu[ii], mem->RSQrq[ii], 0, 0, &mem->L, 0, 0);
+        // Q = S^T * L^-T
+        blasfeo_dgecp(nx[ii], nu[ii], mem->RSQrq[ii], nu[ii], 0, &mem->St_copy, 0, 0);
+//        blasfeo_dtrsm_rltn(nx[ii], nu[ii], 1.0, &mem->L, 0, 0, &mem->St_copy, 0, 0, &mem->Q_tilde, 0, 0);
+        blasfeo_dtrsm_rltn(nx[ii], nu[ii], 1.0, &mem->L, 0, 0, &mem->St_copy, 0, 0, &mem->St_copy, 0, 0);
+
+        // Q = S^T * R^-1 * S + delta*I
+        blasfeo_dgese(nx[ii], nx[ii], 0.0, &mem->delta_eye, 0, 0);
+        blasfeo_ddiare(nx[ii], delta, &mem->delta_eye, 0, 0);
+//        blasfeo_dsyrk_ln(nx[ii], nx[ii], 1.0, &mem->Q_tilde, 0, 0, &mem->Q_tilde, 0, 0, 1.0, &mem->delta_eye, 0, 0, mem->RSQrq[ii], nu[ii], nu[ii]);
+        blasfeo_dsyrk_ln(nx[ii], nu[ii], 1.0, &mem->St_copy, 0, 0, &mem->St_copy, 0, 0, 1.0, &mem->delta_eye, 0, 0, mem->RSQrq[ii], nu[ii], nu[ii]);
+
+        blasfeo_dgead(nx[ii], nx[ii], -1.0, mem->RSQrq[ii], nu[ii], nu[ii], &mem->Q_bar, 0, 0);
+
+        // make symmetric
+        blasfeo_dtrtr_l(nx[ii], &mem->Q_bar, 0, 0, &mem->Q_bar, 0, 0);
+
+    }
+}
+
+
+
+// TODO: check if inequality constraints case is implemented correctly
+void ocp_nlp_reg_convexify_correct_dual_sol(void *config, ocp_nlp_reg_dims *dims, void *opts_, void *mem_)
+{
+    ocp_nlp_reg_convexify_memory *mem = mem_;
+    // ocp_nlp_reg_convexify_opts *opts = opts_;
+
+    int ii;
+
+    int N = dims->N;
+    int *nx = dims->nx;
+    int *nu = dims->nu;
+    int *nbx = dims->nbx;
+    int *nbu = dims->nbu;
+    int *ng = dims->ng;
+
+//    printf("\nRSQrq reg\n");
+//    for(ii=0; ii<=N; ii++)
+//        blasfeo_print_dmat(nu[ii]+nx[ii]+1, nu[ii]+nx[ii], mem->RSQrq[ii], 0, 0);
+
+    // restore original hessian and gradient
+    for(ii=0; ii<=N; ii++)
+    {
+        blasfeo_dgecp(nu[ii]+nx[ii]+1, nu[ii]+nx[ii], &mem->original_RSQrq[ii], 0, 0, mem->RSQrq[ii], 0, 0);
+        blasfeo_drowex(nu[ii]+nx[ii], 1.0, &mem->original_RSQrq[ii], nu[ii]+nx[ii], 0, mem->rq[ii], 0);
+    }
+
+//    printf("\nRSQrq orig\n");
+//    for(ii=0; ii<=N; ii++)
+//        blasfeo_print_dmat(nu[ii]+nx[ii]+1, nu[ii]+nx[ii], mem->RSQrq[ii], 0, 0);
+
+
+
+    // compute multipliers of equality constraints
+
+#if 0
+    blasfeo_dgemv_n(nx[N], nu[N]+nx[N], 1.0, mem->RSQrq[N], nu[N], 0, mem->ux[N], 0, 1.0, mem->rq[N], nu[N], mem->pi[N-1], 0);
+
+    for(ii=1; ii<N; ii++)
+    {
+        ss = N-ii;
+        blasfeo_dgemv_n(nx[ss], nu[ss]+nx[ss], 1.0, mem->RSQrq[ss], nu[ss], 0, mem->ux[ss], 0, 1.0, mem->rq[ss], nu[ss], mem->pi[ss-1], 0);
+        blasfeo_dgemv_n(nx[ss], nx[ss+1], 1.0, mem->BAbt[ss], nu[ss], 0, mem->pi[ss], 0, 1.0, mem->pi[ss-1], 0, mem->pi[ss-1], 0);
+    }
+
+#else
+
+    // last stage
+    blasfeo_dveccp(nx[N], mem->rq[N], nu[N], &mem->tmp_nuxM, nu[N]);
+    blasfeo_daxpy(nbu[N]+nbx[N]+ng[N], -1.0, mem->lam[N], 0, mem->lam[N], nbu[N]+nbx[N]+ng[N], &mem->tmp_nbgM, 0);
+    blasfeo_dvecad_sp(nbu[N]+nbx[N], 1.0, &mem->tmp_nbgM, 0, mem->idxb[N], &mem->tmp_nuxM, 0);
+    // TODO avoid to multiply by R ???
+    blasfeo_dsymv_l(nu[N]+nx[N], 1.0, mem->RSQrq[N], 0, 0, mem->ux[N], 0, 1.0, &mem->tmp_nuxM, 0, &mem->tmp_nuxM, 0);
+    blasfeo_dgemv_n(nx[N], ng[N], 1.0, mem->DCt[N], nu[N], 0, &mem->tmp_nbgM, nbu[N]+nbx[N], 1.0, &mem->tmp_nuxM, nu[N], &mem->tmp_nuxM, nu[N]);
+    blasfeo_dveccp(nx[N], &mem->tmp_nuxM, nu[N], mem->pi[N-1], 0);
+
+    // middle stages
+    for(ii=0; ii<N-1; ii++)
+        {
+        blasfeo_dveccp(nx[N-1-ii], mem->rq[N-1-ii], nu[N-1-ii], &mem->tmp_nuxM, nu[N-1-ii]);
+        blasfeo_daxpy(nbu[N-1-ii]+nbx[N-1-ii]+ng[N-1-ii], -1.0, mem->lam[N-1-ii], 0, mem->lam[N-1-ii], nbu[N-1-ii]+nbx[N-1-ii]+ng[N-1-ii], &mem->tmp_nbgM, 0);
+        blasfeo_dvecad_sp(nbu[N-1-ii]+nbx[N-1-ii], 1.0, &mem->tmp_nbgM, 0, mem->idxb[N-1-ii], &mem->tmp_nuxM, 0);
+        // TODO avoid to multiply by R ???
+        blasfeo_dsymv_l(nu[N-1-ii]+nx[N-1-ii], 1.0, mem->RSQrq[N-1-ii], 0, 0, mem->ux[N-1-ii], 0, 1.0, &mem->tmp_nuxM, 0, &mem->tmp_nuxM, 0);
+        blasfeo_dgemv_n(nx[N-1-ii], nx[N-ii], 1.0, mem->BAbt[N-1-ii], nu[N-1-ii], 0, mem->pi[N-1-ii], 0, 1.0, &mem->tmp_nuxM, nu[N-1-ii], &mem->tmp_nuxM, nu[N-1-ii]);
+        blasfeo_dgemv_n(nx[N-1-ii], ng[N-1-ii], 1.0, mem->DCt[N-1-ii], nu[N-1-ii], 0, &mem->tmp_nbgM, nbu[N-1-ii]+nbx[N-1-ii], 1.0, &mem->tmp_nuxM, nu[N-1-ii], &mem->tmp_nuxM, nu[N-1-ii]);
+        blasfeo_dveccp(nx[N-1-ii], &mem->tmp_nuxM, nu[N-1-ii], mem->pi[N-2-ii], 0);
+        }
+#endif
+
+    return;
+}
+
+
+
+void ocp_nlp_reg_convexify_config_initialize_default(ocp_nlp_reg_config *config)
+{
+    // dims
+    config->dims_calculate_size = &ocp_nlp_reg_dims_calculate_size;
+    config->dims_assign = &ocp_nlp_reg_dims_assign;
+    config->dims_set = &ocp_nlp_reg_dims_set;
+    // opts
+    config->opts_calculate_size = &ocp_nlp_reg_convexify_opts_calculate_size;
+    config->opts_assign = &ocp_nlp_reg_convexify_opts_assign;
+    config->opts_initialize_default = &ocp_nlp_reg_convexify_opts_initialize_default;
+    config->opts_set = &ocp_nlp_reg_convexify_opts_set;
+    // memory
+    config->memory_calculate_size = &ocp_nlp_reg_convexify_calculate_memory_size;
+    config->memory_assign = &ocp_nlp_reg_convexify_assign_memory;
+    config->memory_set = &ocp_nlp_reg_convexify_memory_set;
+    config->memory_set_RSQrq_ptr = &ocp_nlp_reg_convexify_memory_set_RSQrq_ptr;
+    config->memory_set_rq_ptr = &ocp_nlp_reg_convexify_memory_set_rq_ptr;
+    config->memory_set_BAbt_ptr = &ocp_nlp_reg_convexify_memory_set_BAbt_ptr;
+    config->memory_set_b_ptr = &ocp_nlp_reg_convexify_memory_set_b_ptr;
+    config->memory_set_idxb_ptr = &ocp_nlp_reg_convexify_memory_set_idxb_ptr;
+    config->memory_set_DCt_ptr = &ocp_nlp_reg_convexify_memory_set_DCt_ptr;
+    config->memory_set_ux_ptr = &ocp_nlp_reg_convexify_memory_set_ux_ptr;
+    config->memory_set_pi_ptr = &ocp_nlp_reg_convexify_memory_set_pi_ptr;
+    config->memory_set_lam_ptr = &ocp_nlp_reg_convexify_memory_set_lam_ptr;
+    // functions
+    // TODO: fix split
+    config->regularize = &ocp_nlp_reg_convexify_regularize;
+    config->regularize_lhs = &ocp_nlp_reg_convexify_regularize_lhs;
+    config->regularize_rhs = &ocp_nlp_reg_convexify_regularize_rhs;
+    config->correct_dual_sol = &ocp_nlp_reg_convexify_correct_dual_sol;
+}

acados/ocp_nlp/ocp_nlp_reg_convexify.h

@@ -0,0 +1,146 @@
+/*
+ * Copyright (c) The acados authors.
+ *
+ * This file is part of acados.
+ *
+ * The 2-Clause BSD License
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.;
+ */
+
+
+/// \addtogroup ocp_nlp
+/// @{
+/// \addtogroup ocp_nlp_reg
+/// @{
+
+#ifndef ACADOS_OCP_NLP_OCP_NLP_REG_CONVEXIFY_H_
+#define ACADOS_OCP_NLP_OCP_NLP_REG_CONVEXIFY_H_
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+
+
+// blasfeo
+#include "blasfeo_common.h"
+
+// acados
+#include "acados/ocp_nlp/ocp_nlp_reg_common.h"
+
+
+
+/************************************************
+ * dims
+ ************************************************/
+
+// use the functions in ocp_nlp_reg_common
+
+/************************************************
+ * options
+ ************************************************/
+
+typedef struct
+{
+    double delta;
+    double epsilon;
+//    double gamma; // 0.0
+} ocp_nlp_reg_convexify_opts;
+
+//
+acados_size_t ocp_nlp_reg_convexify_opts_calculate_size(void);
+//
+void *ocp_nlp_reg_convexify_opts_assign(void *raw_memory);
+//
+void ocp_nlp_reg_convexify_opts_initialize_default(void *config_, ocp_nlp_reg_dims *dims, void *opts_);
+//
+void ocp_nlp_reg_convexify_opts_set(void *config_, void *opts_, const char *field, void* value);
+
+
+
+/************************************************
+ * memory
+ ************************************************/
+
+typedef struct {
+    double *R;
+    double *V; // TODO move to workspace
+    double *d; // TODO move to workspace
+    double *e; // TODO move to workspace
+    double *reg_hess; // TODO move to workspace
+
+    struct blasfeo_dmat Q_tilde;
+    struct blasfeo_dmat Q_bar;
+    struct blasfeo_dmat BAQ;
+    struct blasfeo_dmat L;
+    struct blasfeo_dmat delta_eye;
+    struct blasfeo_dmat St_copy;
+
+    struct blasfeo_dmat *original_RSQrq;
+    struct blasfeo_dmat tmp_RSQ;
+
+    struct blasfeo_dvec tmp_nuxM;
+    struct blasfeo_dvec tmp_nbgM;
+
+//    struct blasfeo_dvec grad;
+//    struct blasfeo_dvec b2;
+
+    // giaf's
+    struct blasfeo_dmat **RSQrq;  // pointer to RSQrq in qp_in
+    struct blasfeo_dvec **rq;  // pointer to rq in qp_in
+    struct blasfeo_dmat **BAbt;  // pointer to BAbt in qp_in
+    struct blasfeo_dvec **b;  // pointer to b in qp_in
+    struct blasfeo_dmat **DCt;  // pointer to DCt in qp_in
+    struct blasfeo_dvec **ux;  // pointer to ux in qp_out
+    struct blasfeo_dvec **pi;  // pointer to pi in qp_out
+    struct blasfeo_dvec **lam;  // pointer to lam in qp_out
+    int **idxb; // pointer to idxb in qp_in
+
+} ocp_nlp_reg_convexify_memory;
+
+//
+acados_size_t ocp_nlp_reg_convexify_calculate_memory_size(void *config, ocp_nlp_reg_dims *dims, void *opts);
+//
+void *ocp_nlp_reg_convexify_assign_memory(void *config, ocp_nlp_reg_dims *dims, void *opts, void *raw_memory);
+
+/************************************************
+ * workspace
+ ************************************************/
+
+ // TODO
+
+/************************************************
+ * functions
+ ************************************************/
+
+//
+void ocp_nlp_reg_convexify_config_initialize_default(ocp_nlp_reg_config *config);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif  // ACADOS_OCP_NLP_OCP_NLP_REG_CONVEXIFY_H_
+/// @}
+/// @}

acados/ocp_nlp/ocp_nlp_reg_mirror.c

@@ -0,0 +1,363 @@
+/*
+ * Copyright (c) The acados authors.
+ *
+ * This file is part of acados.
+ *
+ * The 2-Clause BSD License
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.;
+ */
+
+
+#include "acados/ocp_nlp/ocp_nlp_reg_mirror.h"
+
+#include <stdlib.h>
+#include <stdio.h>
+#include <string.h>
+#include <assert.h>
+
+#include "acados/ocp_nlp/ocp_nlp_reg_common.h"
+#include "acados/utils/math.h"
+
+#include "blasfeo_d_aux.h"
+#include "blasfeo_d_blas.h"
+
+
+
+/************************************************
+ * opts
+ ************************************************/
+
+acados_size_t ocp_nlp_reg_mirror_opts_calculate_size(void)
+{
+    return sizeof(ocp_nlp_reg_mirror_opts);
+}
+
+
+
+void *ocp_nlp_reg_mirror_opts_assign(void *raw_memory)
+{
+    return raw_memory;
+}
+
+
+
+void ocp_nlp_reg_mirror_opts_initialize_default(void *config_, ocp_nlp_reg_dims *dims, void *opts_)
+{
+    ocp_nlp_reg_mirror_opts *opts = opts_;
+
+    opts->epsilon = 1e-4;
+    opts->adaptive_eps = false;
+    opts->max_cond_block = 1e7;
+
+    return;
+}
+
+
+
+void ocp_nlp_reg_mirror_opts_set(void *config_, void *opts_, const char *field, void* value)
+{
+
+    ocp_nlp_reg_mirror_opts *opts = opts_;
+
+    if (!strcmp(field, "epsilon"))
+    {
+        double *d_ptr = value;
+        opts->epsilon = *d_ptr;
+    }
+    else if (!strcmp(field, "max_cond_block"))
+    {
+        double *d_ptr = value;
+        opts->max_cond_block = *d_ptr;
+    }
+    else if (!strcmp(field, "adaptive_eps"))
+    {
+        bool *b_ptr = value;
+        opts->adaptive_eps = *b_ptr;
+    }
+    else
+    {
+        printf("\nerror: field %s not available in ocp_nlp_reg_mirror_opts_set\n", field);
+        exit(1);
+    }
+
+    return;
+}
+
+
+
+/************************************************
+ * memory
+ ************************************************/
+
+acados_size_t ocp_nlp_reg_mirror_memory_calculate_size(void *config_, ocp_nlp_reg_dims *dims, void *opts_)
+{
+    int *nx = dims->nx;
+    int *nu = dims->nu;
+    int N = dims->N;
+
+    int ii;
+
+    int nuxM = nu[0]+nx[0];
+    for(ii=1; ii<=N; ii++)
+    {
+        nuxM = nu[ii]+nx[ii]>nuxM ? nu[ii]+nx[ii] : nuxM;
+    }
+
+    acados_size_t size = 0;
+
+    size += sizeof(ocp_nlp_reg_mirror_memory);
+
+    size += nuxM*nuxM*sizeof(double);  // reg_hess
+    size += nuxM*nuxM*sizeof(double);  // V
+    size += 2*nuxM*sizeof(double);     // d e
+    size += (N+1)*sizeof(struct blasfeo_dmat *); // RSQrq
+
+    return size;
+}
+
+
+
+void *ocp_nlp_reg_mirror_memory_assign(void *config_, ocp_nlp_reg_dims *dims, void *opts_, void *raw_memory)
+{
+    int *nx = dims->nx;
+    int *nu = dims->nu;
+    int N = dims->N;
+
+    int ii;
+
+    int nuxM = nu[0]+nx[0];
+    for(ii=1; ii<=N; ii++)
+    {
+        nuxM = nu[ii]+nx[ii]>nuxM ? nu[ii]+nx[ii] : nuxM;
+    }
+
+    char *c_ptr = (char *) raw_memory;
+
+    ocp_nlp_reg_mirror_memory *mem = (ocp_nlp_reg_mirror_memory *) c_ptr;
+    c_ptr += sizeof(ocp_nlp_reg_mirror_memory);
+
+    mem->reg_hess = (double *) c_ptr;
+    c_ptr += nuxM*nuxM*sizeof(double);  // reg_hess
+
+    mem->V = (double *) c_ptr;
+    c_ptr += nuxM*nuxM*sizeof(double);  // V
+
+    mem->d = (double *) c_ptr;
+    c_ptr += nuxM*sizeof(double); // d
+
+    mem->e = (double *) c_ptr;
+    c_ptr += nuxM*sizeof(double); // e
+
+    mem->RSQrq = (struct blasfeo_dmat **) c_ptr;
+    c_ptr += (N+1)*sizeof(struct blasfeo_dmat *); // RSQrq
+
+    assert((char *) mem + ocp_nlp_reg_mirror_memory_calculate_size(config_, dims, opts_) >= c_ptr);
+
+    return mem;
+}
+
+
+
+void ocp_nlp_reg_mirror_memory_set_RSQrq_ptr(ocp_nlp_reg_dims *dims, struct blasfeo_dmat *RSQrq, void *memory_)
+{
+    ocp_nlp_reg_mirror_memory *memory = memory_;
+
+    int ii;
+
+    int N = dims->N;
+    // int *nx = dims->nx;
+    // int *nu = dims->nu;
+
+    for(ii=0; ii<=N; ii++)
+    {
+        memory->RSQrq[ii] = RSQrq+ii;
+//        blasfeo_print_dmat(nu[ii]+nx[ii]+1, nu[ii]+nx[ii], memory->RSQrq[ii], 0, 0);
+    }
+
+    return;
+}
+
+
+
+void ocp_nlp_reg_mirror_memory_set_rq_ptr(ocp_nlp_reg_dims *dims, struct blasfeo_dvec *rq, void *memory_)
+{
+    return;
+}
+
+
+
+void ocp_nlp_reg_mirror_memory_set_BAbt_ptr(ocp_nlp_reg_dims *dims, struct blasfeo_dmat *BAbt, void *memory_)
+{
+    return;
+}
+
+
+
+void ocp_nlp_reg_mirror_memory_set_b_ptr(ocp_nlp_reg_dims *dims, struct blasfeo_dvec *b, void *memory_)
+{
+    return;
+}
+
+
+
+void ocp_nlp_reg_mirror_memory_set_idxb_ptr(ocp_nlp_reg_dims *dims, int **idxb, void *memory_)
+{
+    return;
+}
+
+
+
+void ocp_nlp_reg_mirror_memory_set_DCt_ptr(ocp_nlp_reg_dims *dims, struct blasfeo_dmat *DCt, void *memory_)
+{
+    return;
+}
+
+
+
+void ocp_nlp_reg_mirror_memory_set_ux_ptr(ocp_nlp_reg_dims *dims, struct blasfeo_dvec *ux, void *memory_)
+{
+    return;
+}
+
+
+
+void ocp_nlp_reg_mirror_memory_set_pi_ptr(ocp_nlp_reg_dims *dims, struct blasfeo_dvec *pi, void *memory_)
+{
+    return;
+}
+
+
+
+void ocp_nlp_reg_mirror_memory_set_lam_ptr(ocp_nlp_reg_dims *dims, struct blasfeo_dvec *lam, void *memory_)
+{
+    return;
+}
+
+
+
+void ocp_nlp_reg_mirror_memory_set(void *config_, ocp_nlp_reg_dims *dims, void *memory_, char *field, void *value)
+{
+
+    if(!strcmp(field, "RSQrq_ptr"))
+    {
+        struct blasfeo_dmat *RSQrq = value;
+        ocp_nlp_reg_mirror_memory_set_RSQrq_ptr(dims, RSQrq, memory_);
+    }
+    else
+    {
+        printf("\nerror: field %s not available in ocp_nlp_reg_mirror_set\n", field);
+        exit(1);
+    }
+
+    return;
+}
+
+
+
+/************************************************
+ * functions
+ ************************************************/
+
+void ocp_nlp_reg_mirror_regularize(void *config, ocp_nlp_reg_dims *dims, void *opts_, void *mem_)
+{
+    ocp_nlp_reg_mirror_memory *mem = (ocp_nlp_reg_mirror_memory *) mem_;
+    ocp_nlp_reg_mirror_opts *opts = opts_;
+
+    int ii;
+
+    int *nx = dims->nx;
+    int *nu = dims->nu;
+    // int N = dims->N;
+
+    for(ii=0; ii<=dims->N; ii++)
+    {
+        // make symmetric
+        blasfeo_dtrtr_l(nu[ii]+nx[ii], mem->RSQrq[ii], 0, 0, mem->RSQrq[ii], 0, 0);
+
+        // regularize
+        blasfeo_unpack_dmat(nu[ii]+nx[ii], nu[ii]+nx[ii], mem->RSQrq[ii], 0, 0, mem->reg_hess, nu[ii]+nx[ii]);
+        if (opts->adaptive_eps)
+        {
+            acados_mirror_adaptive_eps(nu[ii]+nx[ii], mem->reg_hess, mem->V, mem->d, mem->e, opts->max_cond_block);
+        }
+        else
+        {
+            acados_mirror(nu[ii]+nx[ii], mem->reg_hess, mem->V, mem->d, mem->e, opts->epsilon);
+        }
+        blasfeo_pack_dmat(nu[ii]+nx[ii], nu[ii]+nx[ii], mem->reg_hess, nu[ii]+nx[ii], mem->RSQrq[ii], 0, 0);
+    }
+}
+
+
+
+void ocp_nlp_reg_mirror_regularize_lhs(void *config, ocp_nlp_reg_dims *dims, void *opts_, void *mem_)
+{
+    ocp_nlp_reg_mirror_regularize(config, dims, opts_, mem_);
+}
+
+
+
+void ocp_nlp_reg_mirror_regularize_rhs(void *config, ocp_nlp_reg_dims *dims, void *opts_, void *mem_)
+{
+    return;
+}
+
+
+void ocp_nlp_reg_mirror_correct_dual_sol(void *config, ocp_nlp_reg_dims *dims, void *opts_, void *mem_)
+{
+    return;
+}
+
+
+
+void ocp_nlp_reg_mirror_config_initialize_default(ocp_nlp_reg_config *config)
+{
+    // dims
+    config->dims_calculate_size = &ocp_nlp_reg_dims_calculate_size;
+    config->dims_assign = &ocp_nlp_reg_dims_assign;
+    config->dims_set = &ocp_nlp_reg_dims_set;
+    // opts
+    config->opts_calculate_size = &ocp_nlp_reg_mirror_opts_calculate_size;
+    config->opts_assign = &ocp_nlp_reg_mirror_opts_assign;
+    config->opts_initialize_default = &ocp_nlp_reg_mirror_opts_initialize_default;
+    config->opts_set = &ocp_nlp_reg_mirror_opts_set;
+    // memory
+    config->memory_calculate_size = &ocp_nlp_reg_mirror_memory_calculate_size;
+    config->memory_assign = &ocp_nlp_reg_mirror_memory_assign;
+    config->memory_set = &ocp_nlp_reg_mirror_memory_set;
+    config->memory_set_RSQrq_ptr = &ocp_nlp_reg_mirror_memory_set_RSQrq_ptr;
+    config->memory_set_rq_ptr = &ocp_nlp_reg_mirror_memory_set_rq_ptr;
+    config->memory_set_BAbt_ptr = &ocp_nlp_reg_mirror_memory_set_BAbt_ptr;
+    config->memory_set_b_ptr = &ocp_nlp_reg_mirror_memory_set_b_ptr;
+    config->memory_set_idxb_ptr = &ocp_nlp_reg_mirror_memory_set_idxb_ptr;
+    config->memory_set_DCt_ptr = &ocp_nlp_reg_mirror_memory_set_DCt_ptr;
+    config->memory_set_ux_ptr = &ocp_nlp_reg_mirror_memory_set_ux_ptr;
+    config->memory_set_pi_ptr = &ocp_nlp_reg_mirror_memory_set_pi_ptr;
+    config->memory_set_lam_ptr = &ocp_nlp_reg_mirror_memory_set_lam_ptr;
+    // functions
+    config->regularize = &ocp_nlp_reg_mirror_regularize;
+    config->regularize_rhs = &ocp_nlp_reg_mirror_regularize_rhs;
+    config->regularize_lhs = &ocp_nlp_reg_mirror_regularize_lhs;
+    config->correct_dual_sol = &ocp_nlp_reg_mirror_correct_dual_sol;
+}

acados/ocp_nlp/ocp_nlp_reg_mirror.h

@@ -0,0 +1,123 @@
+/*
+ * Copyright (c) The acados authors.
+ *
+ * This file is part of acados.
+ *
+ * The 2-Clause BSD License
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.;
+ */
+
+
+/// \addtogroup ocp_nlp
+/// @{
+/// \addtogroup ocp_nlp_reg
+/// @{
+
+#ifndef ACADOS_OCP_NLP_OCP_NLP_REG_MIRROR_H_
+#define ACADOS_OCP_NLP_OCP_NLP_REG_MIRROR_H_
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+
+
+// blasfeo
+#include "blasfeo_common.h"
+
+// acados
+#include "acados/ocp_nlp/ocp_nlp_reg_common.h"
+
+
+
+/************************************************
+ * dims
+ ************************************************/
+
+// use the functions in ocp_nlp_reg_common
+
+/************************************************
+ * options
+ ************************************************/
+
+typedef struct
+{
+    double epsilon;
+    bool adaptive_eps;
+    double max_cond_block;
+} ocp_nlp_reg_mirror_opts;
+
+//
+acados_size_t ocp_nlp_reg_mirror_opts_calculate_size(void);
+//
+void *ocp_nlp_reg_mirror_opts_assign(void *raw_memory);
+//
+void ocp_nlp_reg_mirror_opts_initialize_default(void *config_, ocp_nlp_reg_dims *dims, void *opts_);
+//
+void ocp_nlp_reg_mirror_opts_set(void *config_, void *opts_, const char *field, void* value);
+
+
+
+/************************************************
+ * memory
+ ************************************************/
+
+typedef struct
+{
+    double *reg_hess; // TODO move to workspace
+    double *V; // TODO move to workspace
+    double *d; // TODO move to workspace
+    double *e; // TODO move to workspace
+
+    // giaf's
+    struct blasfeo_dmat **RSQrq;  // pointer to RSQrq in qp_in
+} ocp_nlp_reg_mirror_memory;
+
+//
+acados_size_t ocp_nlp_reg_mirror_memory_calculate_size(void *config, ocp_nlp_reg_dims *dims, void *opts);
+//
+void *ocp_nlp_reg_mirror_memory_assign(void *config, ocp_nlp_reg_dims *dims, void *opts, void *raw_memory);
+
+/************************************************
+ * workspace
+ ************************************************/
+
+ // TODO
+
+/************************************************
+ * functions
+ ************************************************/
+
+//
+void ocp_nlp_reg_mirror_config_initialize_default(ocp_nlp_reg_config *config);
+
+
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif  // ACADOS_OCP_NLP_OCP_NLP_REG_MIRROR_H_
+/// @}
+/// @}

acados/ocp_nlp/ocp_nlp_reg_noreg.c

@@ -0,0 +1,237 @@
+/*
+ * Copyright (c) The acados authors.
+ *
+ * This file is part of acados.
+ *
+ * The 2-Clause BSD License
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.;
+ */
+
+
+#include "acados/ocp_nlp/ocp_nlp_reg_noreg.h"
+
+#include <stdlib.h>
+#include <stdio.h>
+#include <string.h>
+#include <assert.h>
+
+#include "acados/ocp_nlp/ocp_nlp_reg_common.h"
+#include "acados/utils/math.h"
+
+#include "blasfeo_d_aux.h"
+#include "blasfeo_d_blas.h"
+
+
+
+/************************************************
+ * opts
+ ************************************************/
+
+acados_size_t ocp_nlp_reg_noreg_opts_calculate_size(void)
+{
+    acados_size_t size = 0;
+
+    return size;
+}
+
+
+
+void *ocp_nlp_reg_noreg_opts_assign(void *raw_memory)
+{
+    return raw_memory;
+}
+
+
+
+void ocp_nlp_reg_noreg_opts_initialize_default(void *config_, ocp_nlp_reg_dims *dims, void *opts_)
+{
+
+    return;
+}
+
+
+
+void ocp_nlp_reg_noreg_opts_set(void *config_, void *opts_, const char *field, void* value)
+{
+
+    printf("\nerror: field %s not available in ocp_nlp_reg_noreg_opts_set\n", field);
+    exit(1);
+
+}
+
+
+
+/************************************************
+ * memory
+ ************************************************/
+
+acados_size_t ocp_nlp_reg_noreg_memory_calculate_size(void *config_, ocp_nlp_reg_dims *dims, void *opts_)
+{
+    acados_size_t size = 0;
+
+    return size;
+}
+
+
+
+void *ocp_nlp_reg_noreg_memory_assign(void *config_, ocp_nlp_reg_dims *dims, void *opts_, void *raw_memory)
+{
+    return raw_memory;
+}
+
+
+
+void ocp_nlp_reg_noreg_memory_set_RSQrq_ptr(ocp_nlp_reg_dims *dims, struct blasfeo_dmat *RSQrq, void *memory_)
+{
+    return;
+}
+
+
+
+void ocp_nlp_reg_noreg_memory_set_rq_ptr(ocp_nlp_reg_dims *dims, struct blasfeo_dvec *rq, void *memory_)
+{
+    return;
+}
+
+
+
+void ocp_nlp_reg_noreg_memory_set_BAbt_ptr(ocp_nlp_reg_dims *dims, struct blasfeo_dmat *BAbt, void *memory_)
+{
+    return;
+}
+
+
+
+void ocp_nlp_reg_noreg_memory_set_b_ptr(ocp_nlp_reg_dims *dims, struct blasfeo_dvec *b, void *memory_)
+{
+    return;
+}
+
+
+
+void ocp_nlp_reg_noreg_memory_set_idxb_ptr(ocp_nlp_reg_dims *dims, int **idxb, void *memory_)
+{
+    return;
+}
+
+
+
+void ocp_nlp_reg_noreg_memory_set_DCt_ptr(ocp_nlp_reg_dims *dims, struct blasfeo_dmat *DCt, void *memory_)
+{
+    return;
+}
+
+
+
+void ocp_nlp_reg_noreg_memory_set_ux_ptr(ocp_nlp_reg_dims *dims, struct blasfeo_dvec *ux, void *memory_)
+{
+    return;
+}
+
+
+
+void ocp_nlp_reg_noreg_memory_set_pi_ptr(ocp_nlp_reg_dims *dims, struct blasfeo_dvec *pi, void *memory_)
+{
+    return;
+}
+
+
+
+void ocp_nlp_reg_noreg_memory_set_lam_ptr(ocp_nlp_reg_dims *dims, struct blasfeo_dvec *lam, void *memory_)
+{
+    return;
+}
+
+
+
+void ocp_nlp_reg_noreg_memory_set(void *config_, ocp_nlp_reg_dims *dims, void *memory_, char *field, void *value)
+{
+
+    printf("\nerror: field %s not available in ocp_nlp_reg_noreg_set\n", field);
+    exit(1);
+
+    return;
+}
+
+
+
+/************************************************
+ * functions
+ ************************************************/
+
+void ocp_nlp_reg_noreg_regularize(void *config, ocp_nlp_reg_dims *dims, void *opts_, void *mem_)
+{
+    return;
+}
+
+
+void ocp_nlp_reg_noreg_regularize_lhs(void *config, ocp_nlp_reg_dims *dims, void *opts_, void *mem_)
+{
+    return;
+}
+
+
+void ocp_nlp_reg_noreg_regularize_rhs(void *config, ocp_nlp_reg_dims *dims, void *opts_, void *mem_)
+{
+    return;
+}
+
+void ocp_nlp_reg_noreg_correct_dual_sol(void *config, ocp_nlp_reg_dims *dims, void *opts_, void *mem_)
+{
+    return;
+}
+
+
+
+void ocp_nlp_reg_noreg_config_initialize_default(ocp_nlp_reg_config *config)
+{
+    // dims
+    config->dims_calculate_size = &ocp_nlp_reg_dims_calculate_size;
+    config->dims_assign = &ocp_nlp_reg_dims_assign;
+    config->dims_set = &ocp_nlp_reg_dims_set;
+    // opts
+    config->opts_calculate_size = &ocp_nlp_reg_noreg_opts_calculate_size;
+    config->opts_assign = &ocp_nlp_reg_noreg_opts_assign;
+    config->opts_initialize_default = &ocp_nlp_reg_noreg_opts_initialize_default;
+    config->opts_set = &ocp_nlp_reg_noreg_opts_set;
+    // memory
+    config->memory_calculate_size = &ocp_nlp_reg_noreg_memory_calculate_size;
+    config->memory_assign = &ocp_nlp_reg_noreg_memory_assign;
+    config->memory_set = &ocp_nlp_reg_noreg_memory_set;
+    config->memory_set_RSQrq_ptr = &ocp_nlp_reg_noreg_memory_set_RSQrq_ptr;
+    config->memory_set_rq_ptr = &ocp_nlp_reg_noreg_memory_set_rq_ptr;
+    config->memory_set_BAbt_ptr = &ocp_nlp_reg_noreg_memory_set_BAbt_ptr;
+    config->memory_set_b_ptr = &ocp_nlp_reg_noreg_memory_set_b_ptr;
+    config->memory_set_idxb_ptr = &ocp_nlp_reg_noreg_memory_set_idxb_ptr;
+    config->memory_set_DCt_ptr = &ocp_nlp_reg_noreg_memory_set_DCt_ptr;
+    config->memory_set_ux_ptr = &ocp_nlp_reg_noreg_memory_set_ux_ptr;
+    config->memory_set_pi_ptr = &ocp_nlp_reg_noreg_memory_set_pi_ptr;
+    config->memory_set_lam_ptr = &ocp_nlp_reg_noreg_memory_set_lam_ptr;
+    // functions
+    config->regularize = &ocp_nlp_reg_noreg_regularize;
+    config->regularize_lhs = &ocp_nlp_reg_noreg_regularize_lhs;
+    config->regularize_rhs = &ocp_nlp_reg_noreg_regularize_rhs;
+    config->correct_dual_sol = &ocp_nlp_reg_noreg_correct_dual_sol;
+}
+

acados/ocp_nlp/ocp_nlp_reg_noreg.h

@@ -0,0 +1,106 @@
+/*
+ * Copyright (c) The acados authors.
+ *
+ * This file is part of acados.
+ *
+ * The 2-Clause BSD License
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.;
+ */
+
+
+/// \addtogroup ocp_nlp
+/// @{
+/// \addtogroup ocp_nlp_reg
+/// @{
+
+#ifndef ACADOS_OCP_NLP_OCP_NLP_REG_NOREG_H_
+#define ACADOS_OCP_NLP_OCP_NLP_REG_NOREG_H_
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+
+
+// blasfeo
+#include "blasfeo_common.h"
+
+// acados
+#include "acados/ocp_nlp/ocp_nlp_reg_common.h"
+
+
+
+/************************************************
+ * dims
+ ************************************************/
+
+// use the functions in ocp_nlp_reg_common
+
+
+/************************************************
+ * options
+ ************************************************/
+
+//
+acados_size_t ocp_nlp_reg_noreg_opts_calculate_size(void);
+//
+void *ocp_nlp_reg_noreg_opts_assign(void *raw_memory);
+//
+void ocp_nlp_reg_noreg_opts_initialize_default(void *config_, ocp_nlp_reg_dims *dims, void *opts_);
+//
+void ocp_nlp_reg_noreg_opts_set(void *config_, void *opts_, const char *field, void* value);
+
+
+
+/************************************************
+ * memory
+ ************************************************/
+//
+acados_size_t ocp_nlp_reg_noreg_memory_calculate_size(void *config, ocp_nlp_reg_dims *dims, void *opts);
+//
+void *ocp_nlp_reg_noreg_memory_assign(void *config, ocp_nlp_reg_dims *dims, void *opts, void *raw_memory);
+
+/************************************************
+ * workspace
+ ************************************************/
+
+// not needed
+
+/************************************************
+ * functions
+ ************************************************/
+
+//
+void ocp_nlp_reg_noreg_config_initialize_default(ocp_nlp_reg_config *config);
+
+
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif  // ACADOS_OCP_NLP_OCP_NLP_REG_NOREG_H_
+
+/// @}
+/// @}

acados/ocp_nlp/ocp_nlp_reg_project.c

@@ -0,0 +1,361 @@
+/*
+ * Copyright (c) The acados authors.
+ *
+ * This file is part of acados.
+ *
+ * The 2-Clause BSD License
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.;
+ */
+
+
+#include "acados/ocp_nlp/ocp_nlp_reg_project.h"
+
+#include <stdlib.h>
+#include <stdio.h>
+#include <string.h>
+#include <assert.h>
+
+#include "acados/ocp_nlp/ocp_nlp_reg_common.h"
+#include "acados/utils/math.h"
+
+#include "blasfeo_d_aux.h"
+#include "blasfeo_d_blas.h"
+
+
+
+/************************************************
+ * opts
+ ************************************************/
+
+acados_size_t ocp_nlp_reg_project_opts_calculate_size(void)
+{
+    return sizeof(ocp_nlp_reg_project_opts);
+}
+
+
+
+void *ocp_nlp_reg_project_opts_assign(void *raw_memory)
+{
+    return raw_memory;
+}
+
+
+
+void ocp_nlp_reg_project_opts_initialize_default(void *config_, ocp_nlp_reg_dims *dims, void *opts_)
+{
+    ocp_nlp_reg_project_opts *opts = opts_;
+
+    opts->epsilon = 1e-4;
+    opts->adaptive_eps = false;
+    opts->max_cond_block = 1e7;
+
+    return;
+}
+
+
+
+void ocp_nlp_reg_project_opts_set(void *config_, void *opts_, const char *field, void* value)
+{
+
+    ocp_nlp_reg_project_opts *opts = opts_;
+
+    if (!strcmp(field, "epsilon"))
+    {
+        double *d_ptr = value;
+        opts->epsilon = *d_ptr;
+    }
+    else if (!strcmp(field, "max_cond_block"))
+    {
+        double *d_ptr = value;
+        opts->max_cond_block = *d_ptr;
+    }
+    else if (!strcmp(field, "adaptive_eps"))
+    {
+        bool *b_ptr = value;
+        opts->adaptive_eps = *b_ptr;
+    }
+    else
+    {
+        printf("\nerror: field %s not available in ocp_nlp_reg_project_opts_set\n", field);
+        exit(1);
+    }
+
+    return;
+}
+
+
+
+/************************************************
+ * memory
+ ************************************************/
+
+acados_size_t ocp_nlp_reg_project_memory_calculate_size(void *config_, ocp_nlp_reg_dims *dims, void *opts_)
+{
+    int *nx = dims->nx;
+    int *nu = dims->nu;
+    int N = dims->N;
+
+    int ii;
+
+    int nuxM = nu[0]+nx[0];
+    for(ii=1; ii<=N; ii++)
+    {
+        nuxM = nu[ii]+nx[ii]>nuxM ? nu[ii]+nx[ii] : nuxM;
+    }
+
+    acados_size_t size = 0;
+
+    size += sizeof(ocp_nlp_reg_project_memory);
+
+    size += nuxM*nuxM*sizeof(double);  // reg_hess
+    size += nuxM*nuxM*sizeof(double);  // V
+    size += 2*nuxM*sizeof(double);     // d e
+    size += (N+1)*sizeof(struct blasfeo_dmat *); // RSQrq
+
+    return size;
+}
+
+
+
+void *ocp_nlp_reg_project_memory_assign(void *config_, ocp_nlp_reg_dims *dims, void *opts_, void *raw_memory)
+{
+    int *nx = dims->nx;
+    int *nu = dims->nu;
+    int N = dims->N;
+
+    int ii;
+
+    int nuxM = nu[0]+nx[0];
+    for(ii=1; ii<=N; ii++)
+    {
+        nuxM = nu[ii]+nx[ii]>nuxM ? nu[ii]+nx[ii] : nuxM;
+    }
+
+    char *c_ptr = (char *) raw_memory;
+
+    ocp_nlp_reg_project_memory *mem = (ocp_nlp_reg_project_memory *) c_ptr;
+    c_ptr += sizeof(ocp_nlp_reg_project_memory);
+
+    mem->reg_hess = (double *) c_ptr;
+    c_ptr += nuxM*nuxM*sizeof(double);  // reg_hess
+
+    mem->V = (double *) c_ptr;
+    c_ptr += nuxM*nuxM*sizeof(double);  // V
+
+    mem->d = (double *) c_ptr;
+    c_ptr += nuxM*sizeof(double); // d
+
+    mem->e = (double *) c_ptr;
+    c_ptr += nuxM*sizeof(double); // e
+
+    mem->RSQrq = (struct blasfeo_dmat **) c_ptr;
+    c_ptr += (N+1)*sizeof(struct blasfeo_dmat *); // RSQrq
+
+    assert((char *) mem + ocp_nlp_reg_project_memory_calculate_size(config_, dims, opts_) >= c_ptr);
+
+    return mem;
+}
+
+
+
+void ocp_nlp_reg_project_memory_set_RSQrq_ptr(ocp_nlp_reg_dims *dims, struct blasfeo_dmat *RSQrq, void *memory_)
+{
+    ocp_nlp_reg_project_memory *memory = memory_;
+
+    int ii;
+
+    int N = dims->N;
+    // int *nx = dims->nx;
+    // int *nu = dims->nu;
+
+    for(ii=0; ii<=N; ii++)
+    {
+        memory->RSQrq[ii] = RSQrq+ii;
+//        blasfeo_print_dmat(nu[ii]+nx[ii]+1, nu[ii]+nx[ii], memory->RSQrq[ii], 0, 0);
+    }
+
+    return;
+}
+
+
+
+void ocp_nlp_reg_project_memory_set_rq_ptr(ocp_nlp_reg_dims *dims, struct blasfeo_dvec *rq, void *memory_)
+{
+    return;
+}
+
+
+
+void ocp_nlp_reg_project_memory_set_BAbt_ptr(ocp_nlp_reg_dims *dims, struct blasfeo_dmat *BAbt, void *memory_)
+{
+    return;
+}
+
+
+
+void ocp_nlp_reg_project_memory_set_b_ptr(ocp_nlp_reg_dims *dims, struct blasfeo_dvec *b, void *memory_)
+{
+    return;
+}
+
+
+
+void ocp_nlp_reg_project_memory_set_idxb_ptr(ocp_nlp_reg_dims *dims, int **idxb, void *memory_)
+{
+    return;
+}
+
+
+
+void ocp_nlp_reg_project_memory_set_DCt_ptr(ocp_nlp_reg_dims *dims, struct blasfeo_dmat *DCt, void *memory_)
+{
+    return;
+}
+
+
+
+void ocp_nlp_reg_project_memory_set_ux_ptr(ocp_nlp_reg_dims *dims, struct blasfeo_dvec *ux, void *memory_)
+{
+    return;
+}
+
+
+
+void ocp_nlp_reg_project_memory_set_pi_ptr(ocp_nlp_reg_dims *dims, struct blasfeo_dvec *pi, void *memory_)
+{
+    return;
+}
+
+
+
+void ocp_nlp_reg_project_memory_set_lam_ptr(ocp_nlp_reg_dims *dims, struct blasfeo_dvec *lam, void *memory_)
+{
+    return;
+}
+
+
+
+void ocp_nlp_reg_project_memory_set(void *config_, ocp_nlp_reg_dims *dims, void *memory_, char *field, void *value)
+{
+
+    if(!strcmp(field, "RSQrq_ptr"))
+    {
+        struct blasfeo_dmat *RSQrq = value;
+        ocp_nlp_reg_project_memory_set_RSQrq_ptr(dims, RSQrq, memory_);
+    }
+    else
+    {
+        printf("\nerror: field %s not available in ocp_nlp_reg_project_set\n", field);
+        exit(1);
+    }
+
+    return;
+}
+
+
+
+/************************************************
+ * functions
+ ************************************************/
+
+void ocp_nlp_reg_project_regularize(void *config, ocp_nlp_reg_dims *dims, void *opts_, void *mem_)
+{
+    ocp_nlp_reg_project_memory *mem = (ocp_nlp_reg_project_memory *) mem_;
+    ocp_nlp_reg_project_opts *opts = opts_;
+
+    int ii;
+
+    int *nx = dims->nx;
+    int *nu = dims->nu;
+
+    for(ii=0; ii<=dims->N; ii++)
+    {
+        // make symmetric
+        blasfeo_dtrtr_l(nu[ii]+nx[ii], mem->RSQrq[ii], 0, 0, mem->RSQrq[ii], 0, 0);
+
+        // regularize
+        blasfeo_unpack_dmat(nu[ii]+nx[ii], nu[ii]+nx[ii], mem->RSQrq[ii], 0, 0, mem->reg_hess, nu[ii]+nx[ii]);
+        if (opts->adaptive_eps)
+        {
+            acados_project_adaptive_eps(nu[ii]+nx[ii], mem->reg_hess, mem->V, mem->d, mem->e, opts->max_cond_block);
+        }
+        else
+        {
+            acados_project(nu[ii]+nx[ii], mem->reg_hess, mem->V, mem->d, mem->e, opts->epsilon);
+        }
+        blasfeo_pack_dmat(nu[ii]+nx[ii], nu[ii]+nx[ii], mem->reg_hess, nu[ii]+nx[ii], mem->RSQrq[ii], 0, 0);
+    }
+}
+
+
+void ocp_nlp_reg_project_regularize_lhs(void *config, ocp_nlp_reg_dims *dims, void *opts_, void *mem_)
+{
+    ocp_nlp_reg_project_regularize(config, dims, opts_, mem_);
+}
+
+
+void ocp_nlp_reg_project_regularize_rhs(void *config, ocp_nlp_reg_dims *dims, void *opts_, void *mem_)
+{
+    return;
+}
+
+
+void ocp_nlp_reg_project_correct_dual_sol(void *config, ocp_nlp_reg_dims *dims, void *opts_, void *mem_)
+{
+    return;
+}
+
+
+
+void ocp_nlp_reg_project_config_initialize_default(ocp_nlp_reg_config *config)
+{
+    // dims
+    config->dims_calculate_size = &ocp_nlp_reg_dims_calculate_size;
+    config->dims_assign = &ocp_nlp_reg_dims_assign;
+    config->dims_set = &ocp_nlp_reg_dims_set;
+    // opts
+    config->opts_calculate_size = &ocp_nlp_reg_project_opts_calculate_size;
+    config->opts_assign = &ocp_nlp_reg_project_opts_assign;
+    config->opts_initialize_default = &ocp_nlp_reg_project_opts_initialize_default;
+    config->opts_set = &ocp_nlp_reg_project_opts_set;
+    // memory
+    config->memory_calculate_size = &ocp_nlp_reg_project_memory_calculate_size;
+    config->memory_assign = &ocp_nlp_reg_project_memory_assign;
+    config->memory_set = &ocp_nlp_reg_project_memory_set;
+    config->memory_set_RSQrq_ptr = &ocp_nlp_reg_project_memory_set_RSQrq_ptr;
+    config->memory_set_rq_ptr = &ocp_nlp_reg_project_memory_set_rq_ptr;
+    config->memory_set_BAbt_ptr = &ocp_nlp_reg_project_memory_set_BAbt_ptr;
+    config->memory_set_b_ptr = &ocp_nlp_reg_project_memory_set_b_ptr;
+    config->memory_set_idxb_ptr = &ocp_nlp_reg_project_memory_set_idxb_ptr;
+    config->memory_set_DCt_ptr = &ocp_nlp_reg_project_memory_set_DCt_ptr;
+    config->memory_set_ux_ptr = &ocp_nlp_reg_project_memory_set_ux_ptr;
+    config->memory_set_pi_ptr = &ocp_nlp_reg_project_memory_set_pi_ptr;
+    config->memory_set_lam_ptr = &ocp_nlp_reg_project_memory_set_lam_ptr;
+    // functions
+    config->regularize = &ocp_nlp_reg_project_regularize;
+    config->regularize_rhs = &ocp_nlp_reg_project_regularize_rhs;
+    config->regularize_lhs = &ocp_nlp_reg_project_regularize_lhs;
+    config->correct_dual_sol = &ocp_nlp_reg_project_correct_dual_sol;
+}
+

acados/ocp_nlp/ocp_nlp_reg_project.h

@@ -0,0 +1,123 @@
+/*
+ * Copyright (c) The acados authors.
+ *
+ * This file is part of acados.
+ *
+ * The 2-Clause BSD License
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.;
+ */
+
+
+/// \addtogroup ocp_nlp
+/// @{
+/// \addtogroup ocp_nlp_reg
+/// @{
+
+#ifndef ACADOS_OCP_NLP_OCP_NLP_REG_PROJECT_H_
+#define ACADOS_OCP_NLP_OCP_NLP_REG_PROJECT_H_
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+
+
+// blasfeo
+#include "blasfeo_common.h"
+
+// acados
+#include "acados/ocp_nlp/ocp_nlp_reg_common.h"
+
+
+
+/************************************************
+ * dims
+ ************************************************/
+
+// use the functions in ocp_nlp_reg_common
+
+/************************************************
+ * options
+ ************************************************/
+
+typedef struct
+{
+    double epsilon;
+    bool adaptive_eps;
+    double max_cond_block;
+} ocp_nlp_reg_project_opts;
+
+//
+acados_size_t ocp_nlp_reg_project_opts_calculate_size(void);
+//
+void *ocp_nlp_reg_project_opts_assign(void *raw_memory);
+//
+void ocp_nlp_reg_project_opts_initialize_default(void *config_, ocp_nlp_reg_dims *dims, void *opts_);
+//
+void ocp_nlp_reg_project_opts_set(void *config_, void *opts_, const char *field, void* value);
+
+
+
+/************************************************
+ * memory
+ ************************************************/
+
+typedef struct
+{
+    double *reg_hess; // TODO move to workspace
+    double *V; // TODO move to workspace
+    double *d; // TODO move to workspace
+    double *e; // TODO move to workspace
+
+    // giaf's
+    struct blasfeo_dmat **RSQrq;  // pointer to RSQrq in qp_in
+} ocp_nlp_reg_project_memory;
+
+//
+acados_size_t ocp_nlp_reg_project_memory_calculate_size(void *config, ocp_nlp_reg_dims *dims, void *opts);
+//
+void *ocp_nlp_reg_project_memory_assign(void *config, ocp_nlp_reg_dims *dims, void *opts, void *raw_memory);
+
+/************************************************
+ * workspace
+ ************************************************/
+
+ // TODO
+
+/************************************************
+ * functions
+ ************************************************/
+
+//
+void ocp_nlp_reg_project_config_initialize_default(ocp_nlp_reg_config *config);
+
+
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif  // ACADOS_OCP_NLP_OCP_NLP_REG_PROJECT_H_
+/// @}
+/// @}

acados/ocp_nlp/ocp_nlp_reg_project_reduc_hess.c

@@ -0,0 +1,574 @@
+/*
+ * Copyright (c) The acados authors.
+ *
+ * This file is part of acados.
+ *
+ * The 2-Clause BSD License
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.;
+ */
+
+
+#include "acados/ocp_nlp/ocp_nlp_reg_project_reduc_hess.h"
+
+#include <stdlib.h>
+#include <stdio.h>
+#include <string.h>
+#include <assert.h>
+
+#include "acados/ocp_nlp/ocp_nlp_reg_common.h"
+#include "acados/utils/math.h"
+#include "acados/utils/mem.h"
+
+#include "blasfeo_d_aux.h"
+#include "blasfeo_d_blas.h"
+
+
+
+/************************************************
+ * opts
+ ************************************************/
+
+acados_size_t ocp_nlp_reg_project_reduc_hess_opts_calculate_size(void)
+{
+    return sizeof(ocp_nlp_reg_project_reduc_hess_opts);
+}
+
+
+
+void *ocp_nlp_reg_project_reduc_hess_opts_assign(void *raw_memory)
+{
+    return raw_memory;
+}
+
+
+
+void ocp_nlp_reg_project_reduc_hess_opts_initialize_default(void *config_, ocp_nlp_reg_dims *dims, void *opts_)
+{
+    ocp_nlp_reg_project_reduc_hess_opts *opts = opts_;
+
+    opts->thr_eig = 1e-12;
+    opts->min_eig = 1e-4;
+    opts->min_pivot = 1e-12;
+    opts->pivoting = 1;
+
+    return;
+}
+
+
+
+void ocp_nlp_reg_project_reduc_hess_opts_set(void *config_, void *opts_, const char *field, void* value)
+{
+
+    ocp_nlp_reg_project_reduc_hess_opts *opts = opts_;
+
+    if (!strcmp(field, "thr_eig"))
+    {
+        double *d_ptr = value;
+        opts->thr_eig = *d_ptr;
+    }
+    else if (!strcmp(field, "min_eig"))
+    {
+        double *d_ptr = value;
+        opts->min_eig = *d_ptr;
+    }
+    else if (!strcmp(field, "min_pivot"))
+    {
+        double *d_ptr = value;
+        opts->min_pivot = *d_ptr;
+    }
+    else if (!strcmp(field, "pivoting"))
+    {
+        int *i_ptr = value;
+        opts->pivoting = *i_ptr;
+    }
+    else
+    {
+        printf("\nerror: field %s not available in ocp_nlp_reg_project_reduc_hess_opts_set\n", field);
+        exit(1);
+    }
+
+    return;
+}
+
+
+
+/************************************************
+ * memory
+ ************************************************/
+
+acados_size_t ocp_nlp_reg_project_reduc_hess_memory_calculate_size(void *config_, ocp_nlp_reg_dims *dims, void *opts_)
+{
+    int *nx = dims->nx;
+    int *nu = dims->nu;
+    int N = dims->N;
+
+    int ii;
+
+    int nuxM = nu[0]+nx[0];
+    int nuM = nu[0];
+    int nxM = nx[0];
+    for(ii=1; ii<=N; ii++)
+    {
+        nuxM = nu[ii]+nx[ii]>nuxM ? nu[ii]+nx[ii] : nuxM;
+        nuM = nu[ii]>nuM ? nu[ii] : nuM;
+        nxM = nx[ii]>nxM ? nx[ii] : nxM;
+    }
+
+    acados_size_t size = 0;
+
+    size += sizeof(ocp_nlp_reg_project_reduc_hess_memory);
+
+    size += nuxM*nuxM*sizeof(double);  // reg_hess
+    size += nuxM*nuxM*sizeof(double);  // V
+    size += 2*nuxM*sizeof(double);     // d e
+    size += (N+1)*sizeof(struct blasfeo_dmat *); // RSQrq
+    size += N*sizeof(struct blasfeo_dmat *); // BAbt
+
+    size += 1 * 64;
+
+    size += blasfeo_memsize_dmat(nuxM, nuxM);     // L
+    size += blasfeo_memsize_dmat(nuxM, nuxM);     // L2
+    size += blasfeo_memsize_dmat(nuxM, nuxM);     // L3
+    size += blasfeo_memsize_dmat(nxM, nuM);     // Ls
+    size += blasfeo_memsize_dmat(nxM, nxM);     // P
+    size += blasfeo_memsize_dmat(nuxM, nxM);     // AL
+
+    return size;
+}
+
+
+
+void *ocp_nlp_reg_project_reduc_hess_memory_assign(void *config_, ocp_nlp_reg_dims *dims, void *opts_, void *raw_memory)
+{
+    int *nx = dims->nx;
+    int *nu = dims->nu;
+    int N = dims->N;
+
+    int ii;
+
+    int nuxM = nu[0]+nx[0];
+    int nuM = nu[0];
+    int nxM = nx[0];
+    for(ii=1; ii<=N; ii++)
+    {
+        nuxM = nu[ii]+nx[ii]>nuxM ? nu[ii]+nx[ii] : nuxM;
+        nuM = nu[ii]>nuM ? nu[ii] : nuM;
+        nxM = nx[ii]>nxM ? nx[ii] : nxM;
+    }
+
+    char *c_ptr = (char *) raw_memory;
+
+    ocp_nlp_reg_project_reduc_hess_memory *mem = (ocp_nlp_reg_project_reduc_hess_memory *) c_ptr;
+    c_ptr += sizeof(ocp_nlp_reg_project_reduc_hess_memory);
+
+    mem->reg_hess = (double *) c_ptr;
+    c_ptr += nuxM*nuxM*sizeof(double);  // reg_hess
+
+    mem->V = (double *) c_ptr;
+    c_ptr += nuxM*nuxM*sizeof(double);  // V
+
+    mem->d = (double *) c_ptr;
+    c_ptr += nuxM*sizeof(double); // d
+
+    mem->e = (double *) c_ptr;
+    c_ptr += nuxM*sizeof(double); // e
+
+    mem->RSQrq = (struct blasfeo_dmat **) c_ptr;
+    c_ptr += (N+1)*sizeof(struct blasfeo_dmat *); // RSQrq
+
+    mem->BAbt = (struct blasfeo_dmat **) c_ptr;
+    c_ptr += N*sizeof(struct blasfeo_dmat *); // BAbt
+
+    align_char_to(64, &c_ptr);
+
+    assign_and_advance_blasfeo_dmat_mem(nuxM, nuxM, &mem->L, &c_ptr);
+    assign_and_advance_blasfeo_dmat_mem(nuxM, nuxM, &mem->L2, &c_ptr);
+    assign_and_advance_blasfeo_dmat_mem(nuxM, nuxM, &mem->L3, &c_ptr);
+    assign_and_advance_blasfeo_dmat_mem(nxM, nuM, &mem->Ls, &c_ptr);
+    assign_and_advance_blasfeo_dmat_mem(nxM, nxM, &mem->P, &c_ptr);
+    assign_and_advance_blasfeo_dmat_mem(nuxM, nxM, &mem->AL, &c_ptr);
+
+    assert((char *) mem + ocp_nlp_reg_project_reduc_hess_memory_calculate_size(config_, dims, opts_) >= c_ptr);
+
+    return mem;
+}
+
+
+
+void ocp_nlp_reg_project_reduc_hess_memory_set_RSQrq_ptr(ocp_nlp_reg_dims *dims, struct blasfeo_dmat *RSQrq, void *memory_)
+{
+    ocp_nlp_reg_project_reduc_hess_memory *memory = memory_;
+
+    int ii;
+
+    int N = dims->N;
+
+    for(ii=0; ii<=N; ii++)
+    {
+        memory->RSQrq[ii] = RSQrq+ii;
+    }
+
+    return;
+}
+
+
+
+void ocp_nlp_reg_project_reduc_hess_memory_set_rq_ptr(ocp_nlp_reg_dims *dims, struct blasfeo_dvec *rq, void *memory_)
+{
+    return;
+}
+
+
+
+void ocp_nlp_reg_project_reduc_hess_memory_set_BAbt_ptr(ocp_nlp_reg_dims *dims, struct blasfeo_dmat *BAbt, void *memory_)
+{
+    ocp_nlp_reg_project_reduc_hess_memory *memory = memory_;
+
+    int ii;
+
+    int N = dims->N;
+
+    for(ii=0; ii<N; ii++)
+    {
+        memory->BAbt[ii] = BAbt+ii;
+    }
+
+    return;
+}
+
+
+
+void ocp_nlp_reg_project_reduc_hess_memory_set_b_ptr(ocp_nlp_reg_dims *dims, struct blasfeo_dvec *b, void *memory_)
+{
+    return;
+}
+
+
+
+void ocp_nlp_reg_project_reduc_hess_memory_set_idxb_ptr(ocp_nlp_reg_dims *dims, int **idxb, void *memory_)
+{
+    return;
+}
+
+
+
+void ocp_nlp_reg_project_reduc_hess_memory_set_DCt_ptr(ocp_nlp_reg_dims *dims, struct blasfeo_dmat *DCt, void *memory_)
+{
+    return;
+}
+
+
+
+void ocp_nlp_reg_project_reduc_hess_memory_set_ux_ptr(ocp_nlp_reg_dims *dims, struct blasfeo_dvec *ux, void *memory_)
+{
+    return;
+}
+
+
+
+void ocp_nlp_reg_project_reduc_hess_memory_set_pi_ptr(ocp_nlp_reg_dims *dims, struct blasfeo_dvec *pi, void *memory_)
+{
+    return;
+}
+
+
+
+void ocp_nlp_reg_project_reduc_hess_memory_set_lam_ptr(ocp_nlp_reg_dims *dims, struct blasfeo_dvec *pi, void *memory_)
+{
+    return;
+}
+
+
+
+void ocp_nlp_reg_project_reduc_hess_memory_set(void *config_, ocp_nlp_reg_dims *dims, void *memory_, char *field, void *value)
+{
+
+    if(!strcmp(field, "RSQrq_ptr"))
+    {
+        struct blasfeo_dmat *RSQrq = value;
+        ocp_nlp_reg_project_reduc_hess_memory_set_RSQrq_ptr(dims, RSQrq, memory_);
+    }
+    else if(!strcmp(field, "BAbt_ptr"))
+    {
+        struct blasfeo_dmat *BAbt = value;
+        ocp_nlp_reg_project_reduc_hess_memory_set_BAbt_ptr(dims, BAbt, memory_);
+    }
+    else
+    {
+        printf("\nerror: field %s not available in ocp_nlp_reg_project_reduc_hess_set\n", field);
+        exit(1);
+    }
+
+    return;
+}
+
+
+
+/************************************************
+ * functions
+ ************************************************/
+
+void ocp_nlp_reg_project_reduc_hess_regularize(void *config, ocp_nlp_reg_dims *dims, void *opts_, void *mem_)
+{
+    ocp_nlp_reg_project_reduc_hess_memory *mem = (ocp_nlp_reg_project_reduc_hess_memory *) mem_;
+    ocp_nlp_reg_project_reduc_hess_opts *opts = opts_;
+
+    int ii, jj, kk, ll, ss, idx;
+
+    int *nx = dims->nx;
+    int *nu = dims->nu;
+    int N = dims->N;
+
+    struct blasfeo_dmat *L = &mem->L;
+    struct blasfeo_dmat *L2 = &mem->L2;
+    struct blasfeo_dmat *L3 = &mem->L3;
+    struct blasfeo_dmat *Ls = &mem->Ls;
+    struct blasfeo_dmat *P = &mem->P;
+    struct blasfeo_dmat *AL = &mem->AL;
+
+    int do_reg = 0;
+    double pivot, tmp_el;
+
+
+    // last + middle stages
+    for(ii=-1; ii<N-1; ii++)
+    {
+
+        ss = N-ii-1;
+        // last stage
+        if(ss==N)
+        {
+            blasfeo_dtrcp_l(nu[ss]+nx[ss], mem->RSQrq[ss], 0, 0, L, 0, 0);
+        }
+        // middle stages
+        else
+        {
+            blasfeo_dgemm_nt(nu[ss]+nx[ss], nx[ss+1], nx[ss+1], 1.0, mem->BAbt[ss], 0, 0, P, 0, 0, 0.0, AL, 0, 0, AL, 0, 0); // TODO symm
+            blasfeo_dsyrk_ln(nu[ss]+nx[ss], nx[ss+1], 1.0, AL, 0, 0, mem->BAbt[ss], 0, 0, 1.0, mem->RSQrq[ss], 0, 0, L, 0, 0);
+        }
+        blasfeo_dtrtr_l(nu[ss]+nx[ss], L, 0, 0, L, 0, 0); // necessary ???
+
+        // backup L in L3
+        blasfeo_dgese(nu[ss]+nx[ss], nu[ss]+nx[ss], 0.0, L3, 0, 0);
+        blasfeo_dgecp(nu[ss]+nx[ss], nu[ss], L, 0, 0, L3, 0, 0);
+
+        // project L_R
+        blasfeo_unpack_dmat(nu[ss], nu[ss], L, 0, 0, mem->reg_hess, nu[ss]);
+        acados_eigen_decomposition(nu[ss], mem->reg_hess, mem->V, mem->d, mem->e);
+        do_reg = 0;
+        for(jj=0; jj<nu[ss]; jj++)
+        {
+            if(mem->d[jj]<opts->thr_eig)
+            {
+                mem->e[jj] = opts->min_eig - mem->d[jj];
+                do_reg = 1;
+            }
+            else
+            {
+                mem->e[jj] = 0.0;
+            }
+        }
+        if(do_reg)
+        {
+            acados_reconstruct_A(nu[ss], mem->reg_hess, mem->V, mem->e);
+            blasfeo_dgese(nu[ss]+nx[ss], nu[ss]+nx[ss], 0.0, L2, 0, 0);
+            blasfeo_pack_dmat(nu[ss], nu[ss], mem->reg_hess, nu[ss], L2, 0, 0);
+
+            // apply reg to R
+            blasfeo_dgead(nu[ss], nu[ss], 1.0, L2, 0, 0, mem->RSQrq[ss], 0, 0);
+            // apply reg to L
+            blasfeo_dgead(nu[ss], nu[ss], 1.0, L2, 0, 0, L, 0, 0);
+        }
+
+        // compute reg_schur
+        blasfeo_dgecp(nu[ss]+nx[ss], nu[ss], L, 0, 0, L2, 0, 0);
+        blasfeo_dpotrf_l_mn(nu[ss]+nx[ss], nu[ss], L2, 0, 0, L2, 0, 0);
+        blasfeo_dgecp(nx[ss], nu[ss], L2, nu[ss], 0, Ls, 0, 0);
+        blasfeo_dsyrk_ln_mn(nx[ss], nx[ss], nu[ss], -1.0, Ls, 0, 0, Ls, 0, 0, 0.0, L2, nu[ss], nu[ss], L2, nu[ss], nu[ss]);
+
+        // compute true_schur
+        if(do_reg)
+        {
+            for(jj=0; jj<nu[ss]; jj++)
+            {
+                if(opts->pivoting)
+                {
+                    // find pivot element
+                    pivot = BLASFEO_DMATEL(L3, jj, jj);
+                    idx = jj;
+                    for(kk=jj+1; kk<nu[ss]; kk++)
+                    {
+                        tmp_el = BLASFEO_DMATEL(L3, kk, kk);
+                        if((tmp_el<pivot) & (tmp_el>-pivot))
+                        {
+                            pivot = BLASFEO_DMATEL(L3, kk, kk);
+                            idx = kk;
+                        }
+                    }
+                    // symmetric permute
+                    if(idx!=jj)
+                    {
+                        // top triangle
+                        for(kk=jj; kk<idx; kk++)
+                        {
+                            tmp_el = BLASFEO_DMATEL(L3, kk, jj);
+                            BLASFEO_DMATEL(L3, kk, jj) = BLASFEO_DMATEL(L3, idx-jj, idx-kk);
+                            BLASFEO_DMATEL(L3, idx-jj, idx-kk) = tmp_el;
+                        }
+                        // bottom rectangle
+                        for(kk=idx+1; kk<nu[ss]+nx[ss]; kk++)
+                            {
+                            tmp_el = BLASFEO_DMATEL(L3, kk, jj);
+                            BLASFEO_DMATEL(L3, kk, jj) = BLASFEO_DMATEL(L3, kk, idx);
+                            BLASFEO_DMATEL(L3, kk, idx) = tmp_el;
+                        }
+                    }
+                }
+
+                pivot = BLASFEO_DMATEL(L3, jj, jj);
+                if ((pivot<opts->min_pivot) & (pivot>-opts->min_pivot))
+                {
+                    if(pivot<0.0)
+                        pivot = opts->min_pivot;
+                    else
+                        pivot = - opts->min_pivot;
+                }
+                pivot = 1.0/pivot;
+                for(kk=jj+1; kk<nu[ss]+nx[ss]; kk++)
+                {
+                    tmp_el = pivot * BLASFEO_DMATEL(L3, kk, jj);
+                    for(ll=kk; ll<nu[ss]+nx[ss]; ll++)
+                    {
+                        BLASFEO_DMATEL(L3, ll, kk) -= BLASFEO_DMATEL(L3, ll, jj) * tmp_el;
+                    }
+                }
+            }
+        }
+
+        // apply schur to P
+        blasfeo_dgecp(nx[ss], nx[ss], L, nu[ss], nu[ss], P, 0, 0);
+        if(do_reg)
+//        if(0)
+        {
+            // P
+            blasfeo_dgead(nx[ss], nx[ss], 1.0, L3, nu[ss], nu[ss], P, 0, 0);
+            // Q
+            blasfeo_dgead(nx[ss], nx[ss], -1.0, L2, nu[ss], nu[ss], mem->RSQrq[ss], nu[ss], nu[ss]);
+            blasfeo_dgead(nx[ss], nx[ss],  1.0, L3, nu[ss], nu[ss], mem->RSQrq[ss], nu[ss], nu[ss]);
+        }
+        else
+        {
+            // P
+            blasfeo_dgead(nx[ss], nx[ss], 1.0, L2, nu[ss], nu[ss], P, 0, 0);
+        }
+        blasfeo_dtrtr_l(nx[ss], P, 0, 0, P, 0, 0);
+
+    }
+
+
+    // first stage: factorize P in L too
+    ss = 0;
+    blasfeo_dgemm_nt(nu[ss]+nx[ss], nx[ss+1], nx[ss+1], 1.0, mem->BAbt[ss], 0, 0, P, 0, 0, 0.0, AL, 0, 0, AL, 0, 0); // TODO symm
+    blasfeo_dsyrk_ln(nu[ss]+nx[ss], nx[ss+1], 1.0, AL, 0, 0, mem->BAbt[ss], 0, 0, 1.0, mem->RSQrq[ss], 0, 0, L, 0, 0);
+    blasfeo_dtrtr_l(nu[ss]+nx[ss], L, 0, 0, L, 0, 0); // necessary ???
+    blasfeo_unpack_dmat(nu[ss]+nx[ss], nu[ss]+nx[ss], L, 0, 0, mem->reg_hess, nu[ss]+nx[ss]);
+    acados_eigen_decomposition(nu[ss]+nx[ss], mem->reg_hess, mem->V, mem->d, mem->e);
+    for(jj=0; jj<nu[ss]+nx[ss]; jj++)
+    {
+        if(mem->d[jj]<opts->thr_eig)
+            mem->e[jj] = opts->min_eig - mem->d[jj];
+        else
+            mem->e[jj] = 0.0;
+    }
+    acados_reconstruct_A(nu[ss]+nx[ss], mem->reg_hess, mem->V, mem->e);
+    blasfeo_pack_dmat(nu[ss]+nx[ss], nu[ss]+nx[ss], mem->reg_hess, nu[ss]+nx[ss], L2, 0, 0);
+    blasfeo_dgead(nu[ss]+nx[ss], nu[ss]+nx[ss], 1.0, L2, 0, 0, mem->RSQrq[ss], 0, 0);
+
+
+//    printf("\nhessian after\n");
+//    for(ii=0; ii<=N; ii++)
+//    {
+//        printf("\nii = %d\n", ii);
+//        blasfeo_print_dmat(nu[ii]+nx[ii], nu[ii]+nx[ii], mem->RSQrq[ii], 0, 0);
+//        blasfeo_unpack_dmat(nu[ii]+nx[ii], nu[ii]+nx[ii], mem->RSQrq[ii], 0, 0, mem->reg_hess, nu[ii]+nx[ii]);
+//        acados_eigen_decomposition(nu[ii]+nx[ii], mem->reg_hess, mem->V, mem->d, mem->e);
+//        d_print_mat(1, nu[ii]+nx[ii], mem->d, 1);
+//    }
+//    exit(1);
+
+    return;
+}
+
+
+void ocp_nlp_reg_project_reduc_hess_regularize_lhs(void *config, ocp_nlp_reg_dims *dims, void *opts_, void *mem_)
+{
+    ocp_nlp_reg_project_reduc_hess_regularize(config, dims, opts_, mem_);
+}
+
+void ocp_nlp_reg_project_reduc_hess_regularize_rhs(void *config, ocp_nlp_reg_dims *dims, void *opts_, void *mem_)
+{
+    return;
+}
+
+
+
+
+void ocp_nlp_reg_project_reduc_hess_correct_dual_sol(void *config, ocp_nlp_reg_dims *dims, void *opts_, void *mem_)
+{
+    return;
+}
+
+
+
+void ocp_nlp_reg_project_reduc_hess_config_initialize_default(ocp_nlp_reg_config *config)
+{
+    // dims
+    config->dims_calculate_size = &ocp_nlp_reg_dims_calculate_size;
+    config->dims_assign = &ocp_nlp_reg_dims_assign;
+    config->dims_set = &ocp_nlp_reg_dims_set;
+    // opts
+    config->opts_calculate_size = &ocp_nlp_reg_project_reduc_hess_opts_calculate_size;
+    config->opts_assign = &ocp_nlp_reg_project_reduc_hess_opts_assign;
+    config->opts_initialize_default = &ocp_nlp_reg_project_reduc_hess_opts_initialize_default;
+    config->opts_set = &ocp_nlp_reg_project_reduc_hess_opts_set;
+    // memory
+    config->memory_calculate_size = &ocp_nlp_reg_project_reduc_hess_memory_calculate_size;
+    config->memory_assign = &ocp_nlp_reg_project_reduc_hess_memory_assign;
+    config->memory_set = &ocp_nlp_reg_project_reduc_hess_memory_set;
+    config->memory_set_RSQrq_ptr = &ocp_nlp_reg_project_reduc_hess_memory_set_RSQrq_ptr;
+    config->memory_set_rq_ptr = &ocp_nlp_reg_project_reduc_hess_memory_set_rq_ptr;
+    config->memory_set_BAbt_ptr = &ocp_nlp_reg_project_reduc_hess_memory_set_BAbt_ptr;
+    config->memory_set_b_ptr = &ocp_nlp_reg_project_reduc_hess_memory_set_b_ptr;
+    config->memory_set_idxb_ptr = &ocp_nlp_reg_project_reduc_hess_memory_set_idxb_ptr;
+    config->memory_set_DCt_ptr = &ocp_nlp_reg_project_reduc_hess_memory_set_DCt_ptr;
+    config->memory_set_ux_ptr = &ocp_nlp_reg_project_reduc_hess_memory_set_ux_ptr;
+    config->memory_set_pi_ptr = &ocp_nlp_reg_project_reduc_hess_memory_set_pi_ptr;
+    config->memory_set_lam_ptr = &ocp_nlp_reg_project_reduc_hess_memory_set_lam_ptr;
+    // functions
+    config->regularize = &ocp_nlp_reg_project_reduc_hess_regularize;
+    config->regularize_rhs = &ocp_nlp_reg_project_reduc_hess_regularize_rhs;
+    config->regularize_lhs = &ocp_nlp_reg_project_reduc_hess_regularize_lhs;
+    config->correct_dual_sol = &ocp_nlp_reg_project_reduc_hess_correct_dual_sol;
+}
+

acados/ocp_nlp/ocp_nlp_reg_project_reduc_hess.h

@@ -0,0 +1,132 @@
+/*
+ * Copyright (c) The acados authors.
+ *
+ * This file is part of acados.
+ *
+ * The 2-Clause BSD License
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.;
+ */
+
+
+/// \addtogroup ocp_nlp
+/// @{
+/// \addtogroup ocp_nlp_reg
+/// @{
+
+#ifndef ACADOS_OCP_NLP_OCP_NLP_REG_PROJECT_REDUC_HESS_H_
+#define ACADOS_OCP_NLP_OCP_NLP_REG_PROJECT_REDUC_HESS_H_
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+
+
+// blasfeo
+#include "blasfeo_common.h"
+
+// acados
+#include "acados/ocp_nlp/ocp_nlp_reg_common.h"
+
+
+
+/************************************************
+ * dims
+ ************************************************/
+
+// use the functions in ocp_nlp_reg_common
+
+/************************************************
+ * options
+ ************************************************/
+
+typedef struct
+{
+    double thr_eig;
+    double min_eig;
+    double min_pivot;
+    int pivoting;
+} ocp_nlp_reg_project_reduc_hess_opts;
+
+//
+acados_size_t ocp_nlp_reg_project_reduc_hess_opts_calculate_size(void);
+//
+void *ocp_nlp_reg_project_reduc_hess_opts_assign(void *raw_memory);
+//
+void ocp_nlp_reg_project_reduc_hess_opts_initialize_default(void *config_, ocp_nlp_reg_dims *dims, void *opts_);
+//
+void ocp_nlp_reg_project_reduc_hess_opts_set(void *config_, void *opts_, const char *field, void* value);
+
+
+
+/************************************************
+ * memory
+ ************************************************/
+
+typedef struct
+{
+    double *reg_hess; // TODO move to workspace
+    double *V; // TODO move to workspace
+    double *d; // TODO move to workspace
+    double *e; // TODO move to workspace
+
+    // giaf's
+    struct blasfeo_dmat L; // TODO move to workspace
+    struct blasfeo_dmat L2; // TODO move to workspace
+    struct blasfeo_dmat L3; // TODO move to workspace
+    struct blasfeo_dmat Ls; // TODO move to workspace
+    struct blasfeo_dmat P; // TODO move to workspace
+    struct blasfeo_dmat AL; // TODO move to workspace
+
+    struct blasfeo_dmat **RSQrq;  // pointer to RSQrq in qp_in
+    struct blasfeo_dmat **BAbt;  // pointer to RSQrq in qp_in
+} ocp_nlp_reg_project_reduc_hess_memory;
+
+//
+acados_size_t ocp_nlp_reg_project_reduc_hess_memory_calculate_size(void *config, ocp_nlp_reg_dims *dims, void *opts);
+//
+void *ocp_nlp_reg_project_reduc_hess_memory_assign(void *config, ocp_nlp_reg_dims *dims, void *opts, void *raw_memory);
+
+/************************************************
+ * workspace
+ ************************************************/
+
+ // TODO
+
+/************************************************
+ * functions
+ ************************************************/
+
+//
+void ocp_nlp_reg_project_reduc_hess_config_initialize_default(ocp_nlp_reg_config *config);
+
+
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif  // ACADOS_OCP_NLP_OCP_NLP_REG_PROJECT_REDUC_HESS_H_
+/// @}
+/// @}

acados/ocp_nlp/ocp_nlp_sqp.h

@@ -0,0 +1,166 @@
+/*
+ * Copyright (c) The acados authors.
+ *
+ * This file is part of acados.
+ *
+ * The 2-Clause BSD License
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.;
+ */
+
+
+/// \addtogroup ocp_nlp
+/// @{
+/// \addtogroup ocp_nlp_solver
+/// @{
+/// \addtogroup ocp_nlp_sqp ocp_nlp_sqp
+/// @{
+
+#ifndef ACADOS_OCP_NLP_OCP_NLP_SQP_H_
+#define ACADOS_OCP_NLP_OCP_NLP_SQP_H_
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+// acados
+#include "acados/ocp_nlp/ocp_nlp_common.h"
+#include "acados/utils/types.h"
+
+
+
+/************************************************
+ * options
+ ************************************************/
+
+typedef struct
+{
+    ocp_nlp_opts *nlp_opts;
+    double tol_stat;     // exit tolerance on stationarity condition
+    double tol_eq;       // exit tolerance on equality constraints
+    double tol_ineq;     // exit tolerance on inequality constraints
+    double tol_comp;     // exit tolerance on complementarity condition
+    double tol_unbounded; // exit threshold when objective function seems to be unbounded
+    double tol_min_step_norm; // exit tolerance for small step
+    int ext_qp_res;      // compute external QP residuals (i.e. at SQP level) at each SQP iteration (for debugging)
+    int log_primal_step_norm; // compute and log the max norm of the primal steps
+    bool warm_start_first_qp; // to set qp_warm_start in first iteration
+    bool warm_start_first_qp_from_nlp;  // if True first QP will be initialized using values from NLP iterate, otherwise from previous QP solution.
+    bool eval_residual_at_max_iter; // if convergence should be checked after last iterations or only throw max_iter reached
+    double timeout_max_time; // maximum time the solve may require before timeout is triggered. No timeout if 0.
+    ocp_nlp_timeout_heuristic_t timeout_heuristic; // type of heuristic used to predict solve time of next QP
+} ocp_nlp_sqp_opts;
+
+//
+acados_size_t ocp_nlp_sqp_opts_calculate_size(void *config, void *dims);
+//
+void *ocp_nlp_sqp_opts_assign(void *config, void *dims, void *raw_memory);
+//
+void ocp_nlp_sqp_opts_initialize_default(void *config, void *dims, void *opts);
+//
+void ocp_nlp_sqp_opts_update(void *config, void *dims, void *opts);
+//
+void ocp_nlp_sqp_opts_set(void *config_, void *opts_, const char *field, void* value);
+//
+void ocp_nlp_sqp_opts_set_at_stage(void *config_, void *opts_, size_t stage, const char *field, void* value);
+
+
+
+/************************************************
+ * memory
+ ************************************************/
+
+typedef struct
+{
+    // nlp memory
+    ocp_nlp_memory *nlp_mem;
+
+    double alpha;
+    double *primal_step_norm;
+
+    // statistics
+    double *stat;
+    int stat_m;
+    int stat_n;
+
+    double step_norm;
+    double timeout_estimated_per_iteration_time;
+
+} ocp_nlp_sqp_memory;
+
+//
+acados_size_t ocp_nlp_sqp_memory_calculate_size(void *config, void *dims, void *opts_, void *in_);
+//
+void *ocp_nlp_sqp_memory_assign(void *config, void *dims, void *opts_, void *in_, void *raw_memory);
+//
+void ocp_nlp_sqp_memory_reset_qp_solver(void *config_, void *dims_, void *nlp_in_, void *nlp_out_,
+    void *opts_, void *mem_, void *work_);
+
+
+/************************************************
+ * workspace
+ ************************************************/
+
+typedef struct
+{
+    ocp_nlp_workspace *nlp_work;
+} ocp_nlp_sqp_workspace;
+
+//
+acados_size_t ocp_nlp_sqp_workspace_calculate_size(void *config, void *dims, void *opts_, void *in_);
+
+
+
+/************************************************
+ * functions
+ ************************************************/
+
+//
+int ocp_nlp_sqp(void *config, void *dims, void *nlp_in, void *nlp_out,
+                void *args, void *mem, void *work_);
+//
+void ocp_nlp_sqp_config_initialize_default(void *config_);
+//
+int ocp_nlp_sqp_precompute(void *config_, void *dims_, void *nlp_in_, void *nlp_out_,
+                void *opts_, void *mem_, void *work_);
+//
+void ocp_nlp_sqp_eval_lagr_grad_p(void *config_, void *dims_, void *nlp_in_, void *opts_, void *mem_, void *work_,
+                            const char *field, void *grad_p);
+
+
+void ocp_nlp_sqp_eval_solution_sens_adj_p(void *config_, void *dims_,
+                        void *opts_, void *mem_, void *work_, void *sens_nlp_out,
+                        const char *field, int stage, void *grad_p);
+
+//
+void ocp_nlp_sqp_get(void *config_, void *dims_, void *mem_, const char *field, void *return_value_);
+//
+
+#ifdef __cplusplus
+} /* extern "C" */
+#endif
+
+#endif  // ACADOS_OCP_NLP_OCP_NLP_SQP_H_
+/// @}
+/// @}
+/// @}

acados/ocp_nlp/ocp_nlp_sqp_rti.h

@@ -0,0 +1,179 @@
+/*
+ * Copyright (c) The acados authors.
+ *
+ * This file is part of acados.
+ *
+ * The 2-Clause BSD License
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.;
+ */
+
+
+/// \addtogroup ocp_nlp
+/// @{
+/// \addtogroup ocp_nlp_solver
+/// @{
+/// \addtogroup ocp_nlp_sqp_rti ocp_nlp_sqp_rti
+/// @{
+
+#ifndef ACADOS_OCP_NLP_OCP_NLP_SQP_RTI_H_
+#define ACADOS_OCP_NLP_OCP_NLP_SQP_RTI_H_
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+// acados
+#include "acados/ocp_nlp/ocp_nlp_common.h"
+#include "acados/utils/types.h"
+
+
+
+/************************************************
+ * options
+ ************************************************/
+
+typedef enum
+{
+    PREPARATION_AND_FEEDBACK, // = 0,
+    PREPARATION, // = 1,
+    FEEDBACK, // = 2,
+} rti_phase_t;
+
+typedef enum
+{
+    SHIFT_ADVANCE, // = 0,
+    SIMULATE_ADVANCE, // = 1,
+    NO_ADVANCE, // = 2,
+} as_rti_advancement_strategy_t;
+
+typedef enum
+{
+    LEVEL_A, // 0
+    LEVEL_B, // 1
+    LEVEL_C, // 2
+    LEVEL_D, // 3
+    STANDARD_RTI, // 4
+} as_rti_level_t;
+
+typedef struct
+{
+    ocp_nlp_opts *nlp_opts;
+    int compute_dual_sol;
+    int ext_qp_res;           // compute external QP residuals (i.e. at SQP level) at each SQP iteration (for debugging)
+    bool warm_start_first_qp; // to set qp_warm_start in first iteration
+    bool warm_start_first_qp_from_nlp;
+    rti_phase_t rti_phase;
+    as_rti_level_t as_rti_level;
+    as_rti_advancement_strategy_t as_rti_advancement_strategy;
+    int as_rti_iter;
+    int rti_log_residuals;
+    int rti_log_only_available_residuals;
+
+} ocp_nlp_sqp_rti_opts;
+
+//
+acados_size_t ocp_nlp_sqp_rti_opts_calculate_size(void *config_, void *dims_);
+//
+void *ocp_nlp_sqp_rti_opts_assign(void *config_, void *dims_, void *raw_memory);
+//
+void ocp_nlp_sqp_rti_opts_initialize_default(void *config_, void *dims_, void *opts_);
+//
+void ocp_nlp_sqp_rti_opts_update(void *config_, void *dims_, void *opts_);
+//
+void ocp_nlp_sqp_rti_opts_set(void *config_, void *opts_, const char *field, void* value);
+//
+void ocp_nlp_sqp_rti_opts_set_at_stage(void *config_, void *opts_, size_t stage,
+    const char *field, void* value);
+
+
+
+/************************************************
+ * memory
+ ************************************************/
+
+typedef struct
+{
+    // nlp memory
+    ocp_nlp_memory *nlp_mem;
+
+    // statistics
+    double *stat;
+    int stat_m;
+    int stat_n;
+
+    bool is_first_call;
+
+} ocp_nlp_sqp_rti_memory;
+
+//
+acados_size_t ocp_nlp_sqp_rti_memory_calculate_size(void *config_, void *dims_, void *opts_, void *in);
+//
+void *ocp_nlp_sqp_rti_memory_assign(void *config_, void *dims_, void *opts_, void *in_,
+    void *raw_memory);
+
+
+
+/************************************************
+ * workspace
+ ************************************************/
+
+typedef struct
+{
+    ocp_nlp_workspace *nlp_work;
+
+    // qp residuals
+    ocp_qp_res *qp_res;
+    ocp_qp_res_ws *qp_res_ws;
+
+} ocp_nlp_sqp_rti_workspace;
+
+//
+acados_size_t ocp_nlp_sqp_rti_workspace_calculate_size(void *config_, void *dims_, void *opts_, void *nlp_in);
+
+
+
+/************************************************
+ * functions
+ ************************************************/
+//
+int ocp_nlp_sqp_rti(void *config_, void *dims_, void *nlp_in_, void *nlp_out_,
+    void *opts_, void *mem_, void *work_);
+//
+void ocp_nlp_sqp_rti_config_initialize_default(void *config_);
+//
+int ocp_nlp_sqp_rti_precompute(void *config_, void *dims_,
+    void *nlp_in_, void *nlp_out_, void *opts_, void *mem_, void *work_);
+//
+void ocp_nlp_sqp_rti_eval_lagr_grad_p(void *config_, void *dims_, void *nlp_in_, void *opts_,
+    void *mem_, void *work_, const char *field, void *grad_p);
+
+
+#ifdef __cplusplus
+} /* extern "C" */
+#endif
+
+#endif  // ACADOS_OCP_NLP_OCP_NLP_SQP_RTI_H_
+/// @}
+/// @}
+/// @}

acados/ocp_nlp/ocp_nlp_sqp_with_feasible_qp.h

@@ -0,0 +1,195 @@
+/*
+ * Copyright (c) The acados authors.
+ *
+ * This file is part of acados.
+ *
+ * The 2-Clause BSD License
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright notice,
+ * this list of conditions and the following disclaimer.
+ *
+ * 2. Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.;
+ */
+
+
+/// \addtogroup ocp_nlp
+/// @{
+/// \addtogroup ocp_nlp_solver
+/// @{
+/// \addtogroup ocp_nlp_sqp_wfqp ocp_nlp_sqp_wfqp
+/// @{
+
+#ifndef ACADOS_OCP_NLP_OCP_NLP_SQP_WITH_FEASIBLE_QP_H_
+#define ACADOS_OCP_NLP_OCP_NLP_SQP_WITH_FEASIBLE_QP_H_
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+// acados
+#include "acados/ocp_nlp/ocp_nlp_common.h"
+#include "acados/utils/types.h"
+
+/************************************************
+ * options
+ ************************************************/
+
+typedef struct
+{
+    ocp_nlp_opts *nlp_opts;
+    double tol_stat;
+    double tol_eq;
+    double tol_ineq;
+    double tol_comp;
+    double tol_unbounded; // exit threshold when objective function seems to be unbounded
+    double tol_min_step_norm; // exit tolerance for small step
+    int max_iter;
+    int log_primal_step_norm; // compute and log the max norm of the primal steps
+    bool log_pi_norm_inf; // compute and log the max norm of the pi multipliers
+    bool log_lam_norm_inf; // compute and log the max norm of the lam multipliers
+    bool warm_start_first_qp;
+    bool warm_start_first_qp_from_nlp;
+    bool eval_residual_at_max_iter; // if convergence should be checked after last iterations or only throw max_iter reached
+
+    bool use_constraint_hessian_in_feas_qp; // Either use exact Hessian or identity matrix in feasibility QP
+    bool use_QP_l1_inf_from_slacks; // True: sums up the slack variable values from qp_out; False: compute manually; Should give the same result.
+    int search_direction_mode; // determines how the QPs should be solved
+    int watchdog_zero_slacks_max; // number of consecutive BYRD_OMOJOKUN iterations with zero slacks before switching back to NOMINAL_QP
+    bool allow_direction_mode_switch_to_nominal; // if true, mode can switch from Byrd-Omojokun to nominal mode
+    double feasibility_qp_hessian_scalar; // multiplication factor of feasibility QP Hessian
+} ocp_nlp_sqp_wfqp_opts;
+
+
+//
+acados_size_t ocp_nlp_sqp_wfqp_opts_calculate_size(void *config, void *dims);
+//
+void *ocp_nlp_sqp_wfqp_opts_assign(void *config, void *dims, void *raw_memory);
+//
+void ocp_nlp_sqp_wfqp_opts_initialize_default(void *config, void *dims, void *opts);
+//
+void ocp_nlp_sqp_wfqp_opts_update(void *config, void *dims, void *opts);
+//
+void ocp_nlp_sqp_wfqp_opts_set(void *config_, void *opts_, const char *field, void* value);
+//
+void ocp_nlp_sqp_wfqp_opts_set_at_stage(void *config_, void *opts_, size_t stage, const char *field, void* value);
+
+
+/************************************************
+ * memory
+ ************************************************/
+
+typedef struct
+{
+    // nlp memory
+    ocp_nlp_memory *nlp_mem;
+
+    double alpha;
+    double *primal_step_norm;
+
+    int *nns;  // number of non-slacked constraints in NLP
+    int **idxns;  // indices of non-slacked constraints in NLP
+
+    // statistics
+    double *stat;
+        // res_stat, res_eq, res_ineq, res_comp, qp_stat_N1, qp_iter_N1, qp_stat_F, qp_iter_F, qp_stat_N2, qp_iter_N2, alpha
+    int stat_m;
+    int stat_n;
+
+    double step_norm;
+    double norm_inf_pi;
+    double norm_inf_lam;
+
+    struct blasfeo_dvec *Z_cost_module;  // Z values from cost module
+    struct blasfeo_dmat *RSQ_cost;
+    struct blasfeo_dmat *RSQ_constr;
+
+    double pred_l1_inf_QP;
+    double l1_infeasibility;
+    int search_direction_mode; // either NOMINAL_QP or BYRD_OMOJOKUN
+    char* search_direction_type; // for output logging
+    int watchdog_zero_slacks_counter; // counts number of consecutive BYRD_OMOJOKUN iter with slack sum == 0
+    int absolute_nns; // sum of all nns[i]
+
+    int qps_solved_in_sqp_iter;
+
+    // QP solver with always feasible QPs
+    ocp_qp_xcond_solver relaxed_qp_solver;
+    ocp_qp_xcond_solver_memory *relaxed_qp_solver_mem;
+    ocp_qp_xcond_solver_workspace *relaxed_qp_solver_work;
+    // qp in & out
+    ocp_qp_in *relaxed_qp_in;
+    ocp_qp_out *relaxed_qp_out;
+
+} ocp_nlp_sqp_wfqp_memory;
+
+//
+acados_size_t ocp_nlp_sqp_wfqp_memory_calculate_size(void *config, void *dims, void *opts_, void *in_);
+//
+void *ocp_nlp_sqp_wfqp_memory_assign(void *config, void *dims, void *opts_, void *in_, void *raw_memory);
+//
+void ocp_nlp_sqp_wfqp_memory_reset_qp_solver(void *config_, void *dims_, void *nlp_in_, void *nlp_out_,
+    void *opts_, void *mem_, void *work_);
+//
+void set_relaxed_qp_in_matrix_pointers(ocp_nlp_sqp_wfqp_memory *mem, ocp_qp_in *qp_in);
+
+/************************************************
+ * workspace
+ ************************************************/
+
+typedef struct
+{
+    ocp_nlp_workspace *nlp_work;
+} ocp_nlp_sqp_wfqp_workspace;
+
+//
+acados_size_t ocp_nlp_sqp_wfqp_workspace_calculate_size(void *config, void *dims, void *opts_, void *in_);
+
+
+
+/************************************************
+ * functions
+ ************************************************/
+//
+int ocp_nlp_sqp_wfqp(void *config, void *dims, void *nlp_in, void *nlp_out,
+                void *args, void *mem, void *work_);
+//
+void ocp_nlp_sqp_wfqp_config_initialize_default(void *config_);
+//
+void ocp_nlp_sqp_wfqp_config_initialize_default_feasible_qp(void *config_);
+//
+int ocp_nlp_sqp_wfqp_precompute(void *config_, void *dims_, void *nlp_in_, void *nlp_out_,
+                void *opts_, void *mem_, void *work_);
+//
+void ocp_nlp_sqp_wfqp_eval_lagr_grad_p(void *config_, void *dims_, void *nlp_in_, void *opts_, void *mem_, void *work_,
+                            const char *field, void *grad_p);
+//
+void ocp_nlp_sqp_wfqp_get(void *config_, void *dims_, void *mem_, const char *field, void *return_value_);
+//
+double ocp_nlp_sqp_wfqp_compute_qp_objective_value(ocp_nlp_dims *dims, ocp_qp_in *qp_in, ocp_qp_out *qp_out,
+                ocp_nlp_workspace *nlp_work);
+
+#ifdef __cplusplus
+} /* extern "C" */
+#endif
+
+#endif  // ACADOS_OCP_NLP_OCP_NLP_SQP_WITH_FEASIBLE_QP_H_
+/// @}
+/// @}
+/// @}