diff --git a/CMakeLists.txt b/CMakeLists.txt
index 693d6525453f49b7ff0473b16fe73f666fb26a60..4dc7ee9cd8109d7fab9c8d4b84bcb9c51dcf85ee 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -44,25 +44,30 @@ ENDIF(USE_GLPK)
 
 # Main Library
 SET(${PROJECT_NAME}_HEADERS
+  include/${CUSTOM_HEADER_DIR}/common_solve_methods.hh
+  include/${CUSTOM_HEADER_DIR}/common_solve_methods.inl
+  include/${CUSTOM_HEADER_DIR}/cost/costfunction_definition.hh
   include/${CUSTOM_HEADER_DIR}/data.hh
-  include/${CUSTOM_HEADER_DIR}/utils.hh
-  include/${CUSTOM_HEADER_DIR}/flags.hh
   include/${CUSTOM_HEADER_DIR}/definitions.hh
+  include/${CUSTOM_HEADER_DIR}/flags.hh
   include/${CUSTOM_HEADER_DIR}/local_config.hh
-  include/${CUSTOM_HEADER_DIR}/solve.hh
   include/${CUSTOM_HEADER_DIR}/solve_end_effector.hh
-  include/${CUSTOM_HEADER_DIR}/common_solve_methods.hh
-  include/${CUSTOM_HEADER_DIR}/common_solve_methods.inl
-  include/${CUSTOM_HEADER_DIR}/cost/costfunction_definition.hh
-  include/${CUSTOM_HEADER_DIR}/waypoints/waypoints_definition.hh
+  include/${CUSTOM_HEADER_DIR}/solve.hh
+  include/${CUSTOM_HEADER_DIR}/solver/eiquadprog-fast.hpp  # TODO: use stack-of-task/eiquadprog
+  include/${CUSTOM_HEADER_DIR}/solver/glpk-wrapper.hpp
+  include/${CUSTOM_HEADER_DIR}/solver/solver-abstract.hpp
+  include/${CUSTOM_HEADER_DIR}/utils.hh
   include/${CUSTOM_HEADER_DIR}/waypoints/waypoints_c0_dc0_c1.hh
-  include/${CUSTOM_HEADER_DIR}/waypoints/waypoints_c0_dc0_dc1.hh
   include/${CUSTOM_HEADER_DIR}/waypoints/waypoints_c0_dc0_dc1_c1.hh
+  include/${CUSTOM_HEADER_DIR}/waypoints/waypoints_c0_dc0_dc1.hh
   include/${CUSTOM_HEADER_DIR}/waypoints/waypoints_c0_dc0_ddc0_c1.hh
   include/${CUSTOM_HEADER_DIR}/waypoints/waypoints_c0_dc0_ddc0_dc1_c1.hh
   include/${CUSTOM_HEADER_DIR}/waypoints/waypoints_c0_dc0_ddc0_ddc1_dc1_c1.hh
-  include/${CUSTOM_HEADER_DIR}/solver/solver-abstract.hpp
-  include/${CUSTOM_HEADER_DIR}/solver/eiquadprog-fast.hpp  # TODO: use stack-of-task/eiquadprog
+  include/${CUSTOM_HEADER_DIR}/waypoints/waypoints_c0_dc0_ddc0.hh
+  include/${CUSTOM_HEADER_DIR}/waypoints/waypoints_c0_dc0_ddc0_j0_j1_ddc1_dc1_c1.hh
+  include/${CUSTOM_HEADER_DIR}/waypoints/waypoints_c0_dc0_ddc0_j0_x3_j1_ddc1_dc1_c1.hh
+  include/${CUSTOM_HEADER_DIR}/waypoints/waypoints_c0_dc0_ddc0_j0_x5_j1_ddc1_dc1_c1.hh
+  include/${CUSTOM_HEADER_DIR}/waypoints/waypoints_definition.hh
   )
 
 SET(${PROJECT_NAME}_SOURCES
diff --git a/cmake b/cmake
index 32015cb28d977b592227675665d17d11531ef418..f4997a81cebfa2dfb69733bc088c55688965dfe8 160000
--- a/cmake
+++ b/cmake
@@ -1 +1 @@
-Subproject commit 32015cb28d977b592227675665d17d11531ef418
+Subproject commit f4997a81cebfa2dfb69733bc088c55688965dfe8
diff --git a/include/hpp/bezier-com-traj/CMakeLists.txt b/include/hpp/bezier-com-traj/CMakeLists.txt
deleted file mode 100644
index 9c480336f4c3d2346a9be9d31a8b5f6e861cca7f..0000000000000000000000000000000000000000
--- a/include/hpp/bezier-com-traj/CMakeLists.txt
+++ /dev/null
@@ -1,20 +0,0 @@
-SET(${PROJECT_NAME}_HEADERS
-  data.hh
-  utils.hh
-  flags.hh
-  definitions.hh
-  local_config.hh
-  solve.hh
-  solve_end_effector.hh
-  common_solve_methods.hh
-  common_solve_methods.inl
-  )
-
-INSTALL(FILES
-  ${${PROJECT_NAME}_HEADERS}
-  DESTINATION include/${CUSTOM_HEADER_DIR}
-  )
-
-ADD_SUBDIRECTORY(cost)
-ADD_SUBDIRECTORY(waypoints)
-ADD_SUBDIRECTORY(solver)
diff --git a/include/hpp/bezier-com-traj/cost/CMakeLists.txt b/include/hpp/bezier-com-traj/cost/CMakeLists.txt
deleted file mode 100644
index 46a90d5717e249f3b662d70fc0700d943ddd88ea..0000000000000000000000000000000000000000
--- a/include/hpp/bezier-com-traj/cost/CMakeLists.txt
+++ /dev/null
@@ -1,8 +0,0 @@
-SET(${PROJECT_NAME}_COST_HEADERS
-  costfunction_definition.hh
-  )
-
-INSTALL(FILES
-  ${${PROJECT_NAME}_COST_HEADERS}
-  DESTINATION include/${CUSTOM_HEADER_DIR}/cost
-  )
diff --git a/include/hpp/bezier-com-traj/solver/CMakeLists.txt b/include/hpp/bezier-com-traj/solver/CMakeLists.txt
deleted file mode 100644
index c65945565a717e60c1e9e849c4dfa03ada7179ad..0000000000000000000000000000000000000000
--- a/include/hpp/bezier-com-traj/solver/CMakeLists.txt
+++ /dev/null
@@ -1,15 +0,0 @@
-SET(${PROJECT_NAME}_SOLVER_HEADERS
-  solver-abstract.hpp
-  eiquadprog-fast.hpp
-  )
-
-IF(USE_GLPK)
-  SET(${PROJECT_NAME}_SOLVER_HEADERS ${${PROJECT_NAME}_SOLVER_HEADERS}
-    glpk-wrapper.hpp
-    )
-ENDIF(USE_GLPK)
-
-INSTALL(FILES
-  ${${PROJECT_NAME}_SOLVER_HEADERS}
-  DESTINATION include/${CUSTOM_HEADER_DIR}/solver
-  )
diff --git a/include/hpp/bezier-com-traj/waypoints/CMakeLists.txt b/include/hpp/bezier-com-traj/waypoints/CMakeLists.txt
deleted file mode 100644
index 58d7a2adaaa97f09985d1300b29c988e9d9840e6..0000000000000000000000000000000000000000
--- a/include/hpp/bezier-com-traj/waypoints/CMakeLists.txt
+++ /dev/null
@@ -1,14 +0,0 @@
-SET(${PROJECT_NAME}_WAYPOINTS_HEADERS
-  waypoints_definition.hh
-  waypoints_c0_dc0_c1.hh
-  waypoints_c0_dc0_dc1.hh
-  waypoints_c0_dc0_dc1_c1.hh
-  waypoints_c0_dc0_ddc0_c1.hh
-  waypoints_c0_dc0_ddc0_dc1_c1.hh
-  waypoints_c0_dc0_ddc0_ddc1_dc1_c1.hh
-  )
-
-INSTALL(FILES
-  ${${PROJECT_NAME}_WAYPOINTS_HEADERS}
-  DESTINATION include/${CUSTOM_HEADER_DIR}/waypoints
-  )
diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt
deleted file mode 100644
index a4ffd0e2f433f2eda7ed6142c9e0b157f2bc6799..0000000000000000000000000000000000000000
--- a/src/CMakeLists.txt
+++ /dev/null
@@ -1,29 +0,0 @@
-SET(LIBRARY_NAME ${PROJECT_NAME})
-
-SET(${LIBRARY_NAME}_SOURCES
-  common_solve_methods.cpp
-  costfunction_definition.cpp
-  solver-abstract.cpp
-  eiquadprog-fast.cpp
-  computeCOMTraj.cpp
-  solve_0_step.cpp
-  utils.cpp
-  waypoints_definition.cpp
-  )
-
-
-if (USE_GLPK)
-  SET(${LIBRARY_NAME}_SOURCES ${${LIBRARY_NAME}_SOURCES}
-    glpk-wrapper.cpp
-    )
-endif(USE_GLPK)
-
-ADD_LIBRARY(${LIBRARY_NAME} SHARED ${${LIBRARY_NAME}_SOURCES})
-TARGET_LINK_LIBRARIES(${LIBRARY_NAME} ndcurves::ndcurves
-  hpp-centroidal-dynamics::hpp-centroidal-dynamics)
-
-if (USE_GLPK)
-  TARGET_LINK_LIBRARIES(${LIBRARY_NAME} ${glpk_LIBRARY})
-endif(USE_GLPK)
-
-INSTALL(TARGETS ${LIBRARY_NAME} DESTINATION lib)
diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt
index 9ff9ef09e6020f2dc9ef28228c6a09d5862d6604..d56cf9053811e8349e6b32e5223bd2d25965d006 100644
--- a/tests/CMakeLists.txt
+++ b/tests/CMakeLists.txt
@@ -6,6 +6,6 @@ SET(${PROJECT_NAME}_TESTS
   )
 
 FOREACH(test ${${PROJECT_NAME}_TESTS})
-  ADD_UNIT_TEST("${test}" "test-${test}")
+  ADD_UNIT_TEST("${test}" "test-${test}.cpp")
   TARGET_LINK_LIBRARIES(${test} ${PROJECT_NAME})
 ENDFOREACH(test)