Provide sparse implementation for collocation classes, namely:

• sparse composite differentiation matrix
• sparse constraint jacobian
• sparse cost hessian

in nmpf.hpp do not rectify augmented state after 2* pi

Implement a CG-based linear solver in the QP solver.

Scaling state and control and control variables has no effect, as the setting happens after the ode and cost are collocated.

The solvers should support Eigen::SparseMatrix types.
This should give a significant speedup, since for example the KKT system in the QP solver is very sparse (0.95).

When instantiating an Eigen::ConjugateGradient object with fixed-size matrices, an eigen_assert fails here at construction.

One can easily reproduce the problem when building with assertions enabled ("Debug" mode, NDEBUG not defined):

#include <Eigen/Dense>
#include <Eigen/IterativeLinearSolvers>

int main()
{
    Eigen::ConjugateGradient<Eigen::MatrixXd> cg2; // works fine
    Eigen::ConjugateGradient<Eigen::Matrix3d> cg1; // fails
}

Add support for the gcc-5 (and above) compilers.

Eigen defines a limit on the maximum size of a dense matrix with static dimensions.
This limits the maximal problem size for the QP and SQP solvers, because of the dense KKT matrix and Hessian.

The limit is defined by the EIGEN_STACK_ALLOCATION_LIMIT macro.

Here is an example error message

[...]/polympc/benchmark/qp_bench.cpp:314:10: error:
      non-const lvalue reference to type 'Mat' (aka 'Matrix<double, N, N>') cannot
      bind to a temporary of type 'Mat *' (aka 'Matrix<double, N, N> *')
    Mat &A = new Mat();
         ^   ~~~~~~~~~
In file included from [...]/polympc/benchmark/qp_bench.cpp:9:
In file included from /usr/local/include/eigen3/Eigen/IterativeLinearSolvers:11:
In file included from /usr/local/include/eigen3/Eigen/SparseCore:11:
In file included from /usr/local/include/eigen3/Eigen/Core:450:
/usr/local/include/eigen3/Eigen/src/Core/DenseStorage.h:33:3: error: static_assert
      failed "OBJECT_ALLOCATED_ON_STACK_IS_TOO_BIG"
  EIGEN_STATIC_ASSERT(Size * sizeof(T) <= EIGEN_STACK_ALLOCATION_LIMIT, OBJECT_ALLOCATED_ON_STACK_IS_TOO_BIG);
  ^~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
/usr/local/include/eigen3/Eigen/src/Core/util/StaticAssert.h:33:40: note: expanded
      from macro 'EIGEN_STATIC_ASSERT'
    #define EIGEN_STATIC_ASSERT(X,MSG) static_assert(X,#MSG);
                                       ^             ~
/usr/local/include/eigen3/Eigen/src/Core/DenseStorage.h:110:5: note: in
      instantiation of function template specialization
      'Eigen::internal::check_static_allocation_size<double, 160000>' requested
      here
    check_static_allocation_size<T,Size>();
    ^
/usr/local/include/eigen3/Eigen/src/Core/DenseStorage.h:187:23: note: in
      instantiation of member function 'Eigen::internal::plain_array<double,
      160000, 0, 16>::plain_array' requested here
    EIGEN_DEVICE_FUNC DenseStorage() {
                      ^
/usr/local/include/eigen3/Eigen/src/Core/PlainObjectBase.h:484:45: note: in
      instantiation of member function 'Eigen::DenseStorage<double, 160000, 400,
      400, 0>::DenseStorage' requested here
    EIGEN_STRONG_INLINE PlainObjectBase() : m_storage()
                                            ^
/usr/local/include/eigen3/Eigen/src/Core/Matrix.h:259:36: note: in instantiation of
      member function 'Eigen::PlainObjectBase<Eigen::Matrix<double, 400, 400, 0,
      400, 400> >::PlainObjectBase' requested here
    EIGEN_STRONG_INLINE Matrix() : Base()
                                   ^
[...]/polympc/benchmark/qp_bench.cpp:314:18: note:
      in instantiation of member function 'Eigen::Matrix<double, 400, 400, 0, 400,
      400>::Matrix' requested here
    Mat &A = new Mat();

Wrappers for external solvers should be added for validation and comparison purposes.

Suggested QP solvers for the SQP strategy:

• HPIPM
• qpOASES
• OSQP

The examples kite_control_test, kite_model_test, kite_sode_test, legendre_test run into an error because IPOPT doesn't find the HSL dynamic library libhsl.dylib.

We need to document how to install those dependencies.

The most significant memory cost of the dense QP solver come form storing the KKT matrix and its decomposition.
Eigen allows for in-place LDLT decomposition that reuses the coefficient matrix and to save memory.
The in-place decomposition requires a reference to the coefficient matrix at construction, which is not available at that point.
However, construction can be delayed using a placement new, which is illustrated below.

using linear_solver_t = Eigen::LDLT<Eigen::Ref<kkt_mat_t>>;
char linear_solver_buf[sizeof(linear_solver_t)]; // enforce some alignment somehow?
linear_solver_t* linear_solver;
//...
p = (void*) linear_solver_buf;
linear_solver = new(p) linear_solver_t(kkt_mat);

Note: Another option would be to construct the linear solver as a local object on the stack.

Eigen implementation of the Chebyshev class is missing due to lack of tests. Add the implementation and replicate control examples using Eigen interface.

I noticed that the resulting hessian estimation form the damped BFGS implementation is in some rare cases indefinite.

There is a test reproducing this issue in bfgs_test.cpp

I will suspend this test for now, as I have no clue how to fix this issue.

In the CMakeLists some options are redundant.
For example the optimization -O3 on this line which is overridden by the flags from CMAKE_BUILD_TYPE.

I propose to try to stick to Modern CMake practises.

We should add a documentation website for this project.
MkDocs is a good option which is easy to set up and works with GitHub Pages.

However, it might be challenging to set it up for a branch other than master.

in nmpf.hpp add path_parameters to PathError function input, as they are part of the residual

PathError = casadi::Function("PathError", {aug_state}, {residual});