Merging latest changes of the devel branch

This PR contains mainly changes in the SolverFDDP, which it wasn't finish in the root of the cpp_devel branch. Additionally, there is an extra solver unit-test which is important to develop in c++.