diff --git a/CMakeLists.txt b/CMakeLists.txt
index e4a67b1bb2ab83919808446a47eab3863bf7fe50..31941341c070b7dc743f0ff07a7bfd2fa466c433 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -25,15 +25,13 @@ SET(PROJECT_DESCRIPTION "Classes for manipulation planning.")
 SET(CXX_DISABLE_WERROR TRUE)
 INCLUDE(cmake/hpp.cmake)
 INCLUDE(cmake/boost.cmake)
+INCLUDE(cmake/test.cmake)
 
 COMPUTE_PROJECT_ARGS(PROJECT_ARGS LANGUAGES CXX)
 PROJECT(${PROJECT_NAME} ${PROJECT_ARGS})
 
 LIST(APPEND PKG_CONFIG_ADDITIONAL_VARIABLES cmake_plugin)
 
-# Activate test using UR5 if requested
-SET (TEST_UR5 FALSE CACHE BOOL "Activate tests using ur5")
-
 # Ask Doxygen to create a tree view in html documentation
 SET(DOXYGEN_TREEVIEW "NO" CACHE STRING "Set to YES to generate a tree view in the html documentation")
 
@@ -53,8 +51,8 @@ ADD_REQUIRED_DEPENDENCY("hpp-statistics >= 1.1")
 IF (HPP_MANIPULATION_HAS_WHOLEBODY_STEP)
    ADD_REQUIRED_DEPENDENCY("hpp-wholebody-step >= 4")
 ENDIF ()
-IF (TEST_UR5)
-  ADD_REQUIRED_DEPENDENCY("hpp_universal_robot")
+IF (BUILD_TESTING)
+  ADD_REQUIRED_DEPENDENCY("example-robot-data >= 3")
 ENDIF ()
 
 CONFIG_FILES (doc/main.hh
@@ -103,9 +101,13 @@ SET (${PROJECT_NAME}_HEADERS
   include/hpp/manipulation/steering-method/end-effector-trajectory.hh
   )
 
+SET(LIBRARY_NAME ${PROJECT_NAME})
+
 ADD_SUBDIRECTORY(src)
 ADD_SUBDIRECTORY(plugins)
-ADD_SUBDIRECTORY(tests)
+IF(BUILD_TESTING)
+  ADD_SUBDIRECTORY(tests)
+ENDIF()
 
 # Add dependency toward hpp-manipulation library in pkg-config file.
 PKG_CONFIG_APPEND_LIBS("hpp-manipulation")
diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt
index 19c8702c7a5ffe6f9b887ceb0f7d50b8ce02a0ab..3687f805e48433b89982dfd05545b443317cb49e 100644
--- a/src/CMakeLists.txt
+++ b/src/CMakeLists.txt
@@ -17,8 +17,6 @@
 # hpp-manipulation  If not, see
 # <http://www.gnu.org/licenses/>.
 
-SET(LIBRARY_NAME ${PROJECT_NAME})
-
 SET(SOURCES
   handle.cc
   manipulation-planner.cc
diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt
index 16fb181e673ef53388d98ffa23070056e2edc08b..5da633d06a7e31f73950256351269ab3d73be861 100644
--- a/tests/CMakeLists.txt
+++ b/tests/CMakeLists.txt
@@ -20,32 +20,7 @@ INCLUDE_DIRECTORIES(${Boost_INCLUDE_DIRS})
 # Make Boost.Test generates the main function in test cases.
 ADD_DEFINITIONS(-DBOOST_TEST_DYN_LINK -DBOOST_TEST_MAIN)
 
-# ADD_TESTCASE(NAME)
-# ------------------------
-#
-# Define a test named `NAME'.
-#
-# This macro will create a binary from `NAME.cc', link it against
-# Boost and add it to the test suite.
-#
-MACRO(ADD_TESTCASE NAME GENERATED)
-  IF (${GENERATED} STREQUAL TRUE)
-    ADD_EXECUTABLE(${NAME} ${CMAKE_CURRENT_BINARY_DIR}/${NAME}.cc)
-  ELSE()
-    ADD_EXECUTABLE(${NAME} ${NAME}.cc)
-  ENDIF()
-  ADD_TEST(${NAME} ${RUNTIME_OUTPUT_DIRECTORY}/${NAME})
-
-  PKG_CONFIG_USE_DEPENDENCY (${NAME} hpp-constraints)
-  # Link against Boost and project library.
-  TARGET_LINK_LIBRARIES(${NAME}
-    ${PROJECT_NAME}
-    ${Boost_LIBRARIES}
-    )
-
-ENDMACRO(ADD_TESTCASE)
+ADD_UNIT_TEST(test-constraintgraph test-constraintgraph.cc)
+PKG_CONFIG_USE_DEPENDENCY(test-constraintgraph hpp-core)
+TARGET_LINK_LIBRARIES(test-constraintgraph ${LIBRARY_NAME})
 
-IF (TEST_UR5)
-  ADD_TESTCASE (test-constraintgraph FALSE)
-  SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -DTEST_UR5")
-ENDIF ()
diff --git a/tests/test-constraintgraph.cc b/tests/test-constraintgraph.cc
index eb51e13e8c8bf1766c58b1a59f7b895e8338ed38..f7d39f90900cbd050e0ffd136c7e3ff36cef15ec 100644
--- a/tests/test-constraintgraph.cc
+++ b/tests/test-constraintgraph.cc
@@ -14,7 +14,6 @@
 // received a copy of the GNU Lesser General Public License along with
 // hpp-manipulation. If not, see <http://www.gnu.org/licenses/>.
 
-#include <hpp/util/pointer.hh>
 #include <hpp/pinocchio/urdf/util.hh>
 #include <hpp/pinocchio/liegroup-element.hh>
 
@@ -70,15 +69,11 @@ namespace hpp_test {
   void initialize (bool ur5)
   {
     robot = hpp::manipulation::Device::create ("test-robot");
-    hpp::manipulation::ProblemPtr_t problem
-      (new hpp::manipulation::Problem (robot));
+    hpp::manipulation::ProblemPtr_t problem(hpp::manipulation::Problem::create
+                                            (robot));
     if (ur5) {
-#ifdef TEST_UR5
       hpp::pinocchio::urdf::loadUrdfModel (robot, "anchor", "ur_description",
                                            "ur5_joint_limited_robot");
-#else // TEST_UR5
-      BOOST_ERROR ("Set TEST_UR5 in cmake to activate this.");
-#endif // TEST_UR5
     }
     SteeringMethodPtr_t sm
       (hpp::manipulation::steeringMethod::Graph::create (*problem));
@@ -89,7 +84,7 @@ namespace hpp_test {
     components.push_back(graph_);
     graph_->maxIterations (20);
     graph_->errorThreshold (1e-4);
-    ns = graph_->createStateSelector("node-selector"); components.push_back(ns);
+    ns = graph_->createStateSelector("node-selector");
     n1 = ns->createState ("node 1"); components.push_back(n1);
     n2 = ns->createState ("node 2"); components.push_back(n2);
     e11 = n1->linkTo ("edge 11", n1); components.push_back(e11);