diff --git a/.travis.yml b/.travis.yml
index bc6af268e..7040f6dd3 100644
--- a/.travis.yml
+++ b/.travis.yml
@@ -49,6 +49,13 @@ install:
   - sudo make install
   - cd ../../..
 
+  # Install color-debug
+  - cd
+  - git clone https://github.com/roboticslab-uc3m/color-debug
+  - mkdir -p color-debug/build && cd "$_"
+  - cmake ..
+  - sudo make install
+
   #-- (for AmorCartesianControl) Install AMOR API
   - git clone git@github.com:roboticslab-uc3m/amor-api
   - cd amor-api && mkdir build && cd build
diff --git a/CMakeLists.txt b/CMakeLists.txt
index 25600de8c..11ad4ad28 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -63,26 +63,11 @@ if(ENABLE_coverage)
     endif()
 endif()
 
-# Superbuild phase - include color-debug.
-include(FindOrBuildPackage)
-find_or_build_package(COLOR_DEBUG)
-
-# find_or_build_package() doesn't bring into scope COLOR_DEBUG_INCLUDE_DIRS
-# nor other config variables. If a system copy is found, we have to call the
-# find_package() command and proceed as usual.
-if(USE_SYSTEM_COLOR_DEBUG)
-    find_package(COLOR_DEBUG REQUIRED)
-    include_directories(${COLOR_DEBUG_INCLUDE_DIRS})
-    add_library(COLOR_DEBUG UNKNOWN IMPORTED)
-endif()
-
 # Store list of exported targets in global scope.
 set_property(GLOBAL PROPERTY ROBOTICSLAB_KINEMATICS_DYNAMICS_TARGETS)
 
-# Create targets if specific requirements are satisfied.
-include(CMakeDependentOption)
-
-# Find YARP (main dependency).
+# Find project's hard dependencies.
+find_package(COLOR_DEBUG REQUIRED)
 find_package(YARP REQUIRED)
 
 # Load YARP modules.
@@ -95,6 +80,9 @@ endif()
 # Configure installation paths for YARP resources.
 yarp_configure_external_installation(roboticslab-kinematics-dynamics WITH_PLUGINS)
 
+# Create targets if specific requirements are satisfied.
+include(CMakeDependentOption)
+
 # Dependency-related tweaks.
 set(_target_kdl_version 1.4)
 
diff --git a/cmake/ycm-modules/BuildCOLOR_DEBUG.cmake b/cmake/ycm-modules/BuildCOLOR_DEBUG.cmake
deleted file mode 100644
index 87c7b0ad7..000000000
--- a/cmake/ycm-modules/BuildCOLOR_DEBUG.cmake
+++ /dev/null
@@ -1,20 +0,0 @@
-include(YCMEPHelper)
-
-ycm_ep_helper(COLOR_DEBUG TYPE GIT
-              STYLE GITHUB
-              REPOSITORY roboticslab-uc3m/color-debug.git
-              TAG master)
-
-# Include path to ColorDebug.hpp.
-ExternalProject_Get_Property(COLOR_DEBUG INSTALL_DIR)
-include_directories(${INSTALL_DIR}/${CMAKE_INSTALL_INCLUDEDIR})
-
-# CMake has not downloaded color-debug yet (this happens on build step).
-if(NOT COLOR_DEBUG_FOUND)
-    message(STATUS "Build COLOR_DEBUG target and configure project again to make advanced CD options available on UI.")
-    # Fails on first configure due to the lack of a cache file.
-    #execute_process(COMMAND ${CMAKE_COMMAND} --build . --target COLOR_DEBUG WORKING_DIRECTORY ${CMAKE_BINARY_DIR})
-else()
-    # Load COLOR_DEBUGConfig.cmake, which in turn includes ColorDebugOptions.cmake.
-    find_package(COLOR_DEBUG QUIET)
-endif()
diff --git a/doc/kinematics-dynamics-install.md b/doc/kinematics-dynamics-install.md
index 970a19c80..64637860e 100644
--- a/doc/kinematics-dynamics-install.md
+++ b/doc/kinematics-dynamics-install.md
@@ -5,10 +5,9 @@ First install the dependencies:
 - [Install CMake](https://github.com/roboticslab-uc3m/installation-guides/blob/master/install-cmake.md)
 - [Install YARP](https://github.com/roboticslab-uc3m/installation-guides/blob/master/install-yarp.md)
 - [Install KDL](https://github.com/roboticslab-uc3m/installation-guides/blob/master/install-kdl.md)
+- [Install color-debug](https://github.com/roboticslab-uc3m/color-debug)
 
-Additionally, this project depends on YCM to download and build external packages. Although this process is intended to run automatically during the CMake configuration phase, you may still want to install YCM and said packages by yourself. In that respect, refer to [Install YCM](https://github.com/roboticslab-uc3m/installation-guides/blob/master/install-ycm.md) and to the installation guides of any package listed below:
-
-- [color-debug](https://github.com/roboticslab-uc3m/color-debug)
+Additionally, this project depends on YCM to download and use additional CMake modules. Although this process is intended to run automatically during the CMake configuration phase, you may still want to install YCM by yourself. In that respect, refer to [Install YCM](https://github.com/roboticslab-uc3m/installation-guides/blob/master/install-ycm.md).
 
 Only for `testBasicCartesianControl` and `streamingDeviceController`, we use `FakeControlboard` and `ProximitySensorsClient` from [yarp-devices](https://github.com/roboticslab-uc3m/yarp-devices), respectively:
 
diff --git a/libraries/KdlVectorConverterLib/CMakeLists.txt b/libraries/KdlVectorConverterLib/CMakeLists.txt
index 274c7a4a1..04027cb0e 100644
--- a/libraries/KdlVectorConverterLib/CMakeLists.txt
+++ b/libraries/KdlVectorConverterLib/CMakeLists.txt
@@ -13,11 +13,10 @@ if(ENABLE_KdlVectorConverterLib)
     add_library(KdlVectorConverterLib KdlVectorConverter.cpp
                                       KdlVectorConverter.hpp)
 
-    add_dependencies(KdlVectorConverterLib COLOR_DEBUG)
-
     set_target_properties(KdlVectorConverterLib PROPERTIES PUBLIC_HEADER KdlVectorConverter.hpp)
 
-    target_link_libraries(KdlVectorConverterLib PUBLIC ${orocos_kdl_LIBRARIES})
+    target_link_libraries(KdlVectorConverterLib PUBLIC ${orocos_kdl_LIBRARIES}
+                                                       PRIVATE ROBOTICSLAB::ColorDebug)
 
     target_include_directories(KdlVectorConverterLib PUBLIC ${orocos_kdl_INCLUDE_DIRS}
                                                             $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}>
diff --git a/libraries/KdlVectorConverterLib/KdlVectorConverter.cpp b/libraries/KdlVectorConverterLib/KdlVectorConverter.cpp
index 97ee8e569..cd93be859 100644
--- a/libraries/KdlVectorConverterLib/KdlVectorConverter.cpp
+++ b/libraries/KdlVectorConverterLib/KdlVectorConverter.cpp
@@ -2,7 +2,7 @@
 
 #include "KdlVectorConverter.hpp"
 
-#include <ColorDebug.hpp>
+#include <ColorDebug.h>
 
 // -----------------------------------------------------------------------------
 
diff --git a/libraries/KinematicRepresentationLib/CMakeLists.txt b/libraries/KinematicRepresentationLib/CMakeLists.txt
index d9e4173d8..18657bacc 100644
--- a/libraries/KinematicRepresentationLib/CMakeLists.txt
+++ b/libraries/KinematicRepresentationLib/CMakeLists.txt
@@ -12,11 +12,10 @@ if(ENABLE_KinematicRepresentationLib)
     add_library(KinematicRepresentationLib KinematicRepresentation.hpp
                                            KinematicRepresentation.cpp)
 
-    add_dependencies(KinematicRepresentationLib COLOR_DEBUG)
-
     set_target_properties(KinematicRepresentationLib PROPERTIES PUBLIC_HEADER KinematicRepresentation.hpp)
 
-    target_link_libraries(KinematicRepresentationLib PRIVATE ${orocos_kdl_LIBRARIES})
+    target_link_libraries(KinematicRepresentationLib PRIVATE ${orocos_kdl_LIBRARIES}
+                                                             ROBOTICSLAB::ColorDebug)
 
     target_include_directories(KinematicRepresentationLib PUBLIC $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}>
                                                                  $<INSTALL_INTERFACE:${CMAKE_INSTALL_INCLUDEDIR}>
diff --git a/libraries/KinematicRepresentationLib/KinematicRepresentation.cpp b/libraries/KinematicRepresentationLib/KinematicRepresentation.cpp
index 271637fb9..50c39cf76 100644
--- a/libraries/KinematicRepresentationLib/KinematicRepresentation.cpp
+++ b/libraries/KinematicRepresentationLib/KinematicRepresentation.cpp
@@ -6,7 +6,7 @@
 
 #include <kdl/frames.hpp>
 
-#include <ColorDebug.hpp>
+#include <ColorDebug.h>
 
 namespace roboticslab
 {
diff --git a/libraries/TrajectoryLib/CMakeLists.txt b/libraries/TrajectoryLib/CMakeLists.txt
index 100355786..3e795d026 100644
--- a/libraries/TrajectoryLib/CMakeLists.txt
+++ b/libraries/TrajectoryLib/CMakeLists.txt
@@ -15,16 +15,18 @@ if(ENABLE_TrajectoryLib)
                                 KdlTrajectory.hpp)
 
     # Set up our library.
-    add_library(TrajectoryLib ${_trayectory_lib_sources})
-
-    add_dependencies(TrajectoryLib COLOR_DEBUG)
+    add_library(TrajectoryLib ITrajectory.hpp
+                              ICartesianTrajectory.hpp
+                              KdlTrajectory.cpp
+                              KdlTrajectory.hpp)
 
     set_property(TARGET TrajectoryLib PROPERTY PUBLIC_HEADER ICartesianTrajectory.hpp
                                                              ITrajectory.hpp
                                                              KdlTrajectory.hpp)
 
     target_link_libraries(TrajectoryLib PUBLIC ${orocos_kdl_LIBRARIES}
-                                        PRIVATE KdlVectorConverterLib)
+                                        PRIVATE KdlVectorConverterLib
+                                                ROBOTICSLAB::ColorDebug)
 
     target_include_directories(TrajectoryLib PUBLIC ${orocos_kdl_INCLUDE_DIRS}
                                                     $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}>
diff --git a/libraries/TrajectoryLib/KdlTrajectory.cpp b/libraries/TrajectoryLib/KdlTrajectory.cpp
index 9caf89db3..5f31e820b 100644
--- a/libraries/TrajectoryLib/KdlTrajectory.cpp
+++ b/libraries/TrajectoryLib/KdlTrajectory.cpp
@@ -6,7 +6,7 @@
 #include <kdl/velocityprofile_trap.hpp>
 #include <kdl/path_line.hpp>
 
-#include <ColorDebug.hpp>
+#include <ColorDebug.h>
 
 #include "KdlVectorConverter.hpp"
 
diff --git a/libraries/YarpPlugins/AmorCartesianControl/AmorCartesianControl.cpp b/libraries/YarpPlugins/AmorCartesianControl/AmorCartesianControl.cpp
index c76a020cb..19a8fd42c 100644
--- a/libraries/YarpPlugins/AmorCartesianControl/AmorCartesianControl.cpp
+++ b/libraries/YarpPlugins/AmorCartesianControl/AmorCartesianControl.cpp
@@ -4,7 +4,7 @@
 
 #include <cmath>
 
-#include <ColorDebug.hpp>
+#include <ColorDebug.h>
 
 // -----------------------------------------------------------------------------
 
diff --git a/libraries/YarpPlugins/AmorCartesianControl/CMakeLists.txt b/libraries/YarpPlugins/AmorCartesianControl/CMakeLists.txt
index a1a83904c..d57d023bb 100644
--- a/libraries/YarpPlugins/AmorCartesianControl/CMakeLists.txt
+++ b/libraries/YarpPlugins/AmorCartesianControl/CMakeLists.txt
@@ -23,11 +23,10 @@ if(NOT SKIP_AmorCartesianControl)
                                          DeviceDriverImpl.cpp
                                          ICartesianControlImpl.cpp)
 
-    add_dependencies(AmorCartesianControl COLOR_DEBUG)
-
     target_link_libraries(AmorCartesianControl YARP::YARP_OS
                                                YARP::YARP_dev
                                                AMOR::amor_api
+                                               ROBOTICSLAB::ColorDebug
                                                KinematicRepresentationLib
                                                KinematicsDynamicsInterfaces)
 
diff --git a/libraries/YarpPlugins/AmorCartesianControl/DeviceDriverImpl.cpp b/libraries/YarpPlugins/AmorCartesianControl/DeviceDriverImpl.cpp
index a7d8b14e2..621a4109a 100644
--- a/libraries/YarpPlugins/AmorCartesianControl/DeviceDriverImpl.cpp
+++ b/libraries/YarpPlugins/AmorCartesianControl/DeviceDriverImpl.cpp
@@ -8,7 +8,7 @@
 #include <yarp/os/Property.h>
 #include <yarp/os/Value.h>
 
-#include <ColorDebug.hpp>
+#include <ColorDebug.h>
 
 #include "KinematicRepresentation.hpp"
 
diff --git a/libraries/YarpPlugins/AmorCartesianControl/ICartesianControlImpl.cpp b/libraries/YarpPlugins/AmorCartesianControl/ICartesianControlImpl.cpp
index 455f52edb..95e629f34 100644
--- a/libraries/YarpPlugins/AmorCartesianControl/ICartesianControlImpl.cpp
+++ b/libraries/YarpPlugins/AmorCartesianControl/ICartesianControlImpl.cpp
@@ -5,7 +5,7 @@
 #include <yarp/os/Time.h>
 #include <yarp/os/Vocab.h>
 
-#include <ColorDebug.hpp>
+#include <ColorDebug.h>
 
 #include "KinematicRepresentation.hpp"
 
diff --git a/libraries/YarpPlugins/AsibotSolver/AsibotConfiguration.cpp b/libraries/YarpPlugins/AsibotSolver/AsibotConfiguration.cpp
index ceba8e839..e73056f17 100644
--- a/libraries/YarpPlugins/AsibotSolver/AsibotConfiguration.cpp
+++ b/libraries/YarpPlugins/AsibotSolver/AsibotConfiguration.cpp
@@ -5,7 +5,7 @@
 #include <cmath>
 #include <sstream>
 
-#include <ColorDebug.hpp>
+#include <ColorDebug.h>
 
 using namespace roboticslab;
 
diff --git a/libraries/YarpPlugins/AsibotSolver/AsibotConfigurationLeastOverallAngularDisplacement.cpp b/libraries/YarpPlugins/AsibotSolver/AsibotConfigurationLeastOverallAngularDisplacement.cpp
index 441c7bbbf..3d2b9fe61 100644
--- a/libraries/YarpPlugins/AsibotSolver/AsibotConfigurationLeastOverallAngularDisplacement.cpp
+++ b/libraries/YarpPlugins/AsibotSolver/AsibotConfigurationLeastOverallAngularDisplacement.cpp
@@ -7,7 +7,7 @@
 #include <numeric>
 #include <utility>
 
-#include <ColorDebug.hpp>
+#include <ColorDebug.h>
 
 using namespace roboticslab;
 
diff --git a/libraries/YarpPlugins/AsibotSolver/CMakeLists.txt b/libraries/YarpPlugins/AsibotSolver/CMakeLists.txt
index 749c5e769..8479cc049 100644
--- a/libraries/YarpPlugins/AsibotSolver/CMakeLists.txt
+++ b/libraries/YarpPlugins/AsibotSolver/CMakeLists.txt
@@ -21,12 +21,11 @@ if(NOT SKIP_AsibotSolver)
                                  AsibotConfiguration.cpp
                                  AsibotConfigurationLeastOverallAngularDisplacement.cpp)
 
-    add_dependencies(AsibotSolver COLOR_DEBUG)
-
     target_link_libraries(AsibotSolver YARP::YARP_OS
                                        YARP::YARP_dev
                                        YARP::YARP_sig
                                        YARP::YARP_math
+                                       ROBOTICSLAB::ColorDebug
                                        KinematicRepresentationLib
                                        KinematicsDynamicsInterfaces)
 
diff --git a/libraries/YarpPlugins/AsibotSolver/DeviceDriverImpl.cpp b/libraries/YarpPlugins/AsibotSolver/DeviceDriverImpl.cpp
index 629c76424..01738cbf0 100644
--- a/libraries/YarpPlugins/AsibotSolver/DeviceDriverImpl.cpp
+++ b/libraries/YarpPlugins/AsibotSolver/DeviceDriverImpl.cpp
@@ -13,7 +13,7 @@
 
 #include <yarp/math/Math.h>
 
-#include <ColorDebug.hpp>
+#include <ColorDebug.h>
 
 // ------------------- DeviceDriver Related ------------------------------------
 
diff --git a/libraries/YarpPlugins/AsibotSolver/ICartesianSolverImpl.cpp b/libraries/YarpPlugins/AsibotSolver/ICartesianSolverImpl.cpp
index 4b8aa2919..3e12497fe 100644
--- a/libraries/YarpPlugins/AsibotSolver/ICartesianSolverImpl.cpp
+++ b/libraries/YarpPlugins/AsibotSolver/ICartesianSolverImpl.cpp
@@ -8,7 +8,7 @@
 #include <yarp/math/Math.h>
 #include <yarp/math/SVD.h>
 
-#include <ColorDebug.hpp>
+#include <ColorDebug.h>
 
 #include "KinematicRepresentation.hpp"
 
diff --git a/libraries/YarpPlugins/AsibotSolver/TrajGen.hpp b/libraries/YarpPlugins/AsibotSolver/TrajGen.hpp
index b41de5c97..e31d82a78 100644
--- a/libraries/YarpPlugins/AsibotSolver/TrajGen.hpp
+++ b/libraries/YarpPlugins/AsibotSolver/TrajGen.hpp
@@ -3,7 +3,7 @@
 #ifndef __TRAJ_GEN_HPP__
 #define __TRAJ_GEN_HPP__
 
-#include <ColorDebug.hpp>
+#include <ColorDebug.h>
 
 /**
  * @ingroup kinematics-dynamics-libraries
diff --git a/libraries/YarpPlugins/BasicCartesianControl/CMakeLists.txt b/libraries/YarpPlugins/BasicCartesianControl/CMakeLists.txt
index b37da9dea..fe94f5f8a 100644
--- a/libraries/YarpPlugins/BasicCartesianControl/CMakeLists.txt
+++ b/libraries/YarpPlugins/BasicCartesianControl/CMakeLists.txt
@@ -20,10 +20,9 @@ yarp_add_plugin(BasicCartesianControl BasicCartesianControl.hpp
                                       ICartesianControlImpl.cpp
                                       RateThreadImpl.cpp)
 
-add_dependencies(BasicCartesianControl COLOR_DEBUG)
-
 target_link_libraries(BasicCartesianControl YARP::YARP_OS
                                             YARP::YARP_dev
+                                            ROBOTICSLAB::ColorDebug
                                             TrajectoryLib
                                             KinematicsDynamicsInterfaces)
 
diff --git a/libraries/YarpPlugins/BasicCartesianControl/DeviceDriverImpl.cpp b/libraries/YarpPlugins/BasicCartesianControl/DeviceDriverImpl.cpp
index a6a415fa4..41dbae72e 100644
--- a/libraries/YarpPlugins/BasicCartesianControl/DeviceDriverImpl.cpp
+++ b/libraries/YarpPlugins/BasicCartesianControl/DeviceDriverImpl.cpp
@@ -2,7 +2,7 @@
 
 #include "BasicCartesianControl.hpp"
 
-#include <ColorDebug.hpp>
+#include <ColorDebug.h>
 
 // ------------------- DeviceDriver Related ------------------------------------
 
diff --git a/libraries/YarpPlugins/BasicCartesianControl/ICartesianControlImpl.cpp b/libraries/YarpPlugins/BasicCartesianControl/ICartesianControlImpl.cpp
index 6b82dc479..d0f8b14f5 100644
--- a/libraries/YarpPlugins/BasicCartesianControl/ICartesianControlImpl.cpp
+++ b/libraries/YarpPlugins/BasicCartesianControl/ICartesianControlImpl.cpp
@@ -9,7 +9,7 @@
 
 #include <yarp/os/Vocab.h>
 
-#include <ColorDebug.hpp>
+#include <ColorDebug.h>
 
 #include "KdlTrajectory.hpp"
 
diff --git a/libraries/YarpPlugins/BasicCartesianControl/RateThreadImpl.cpp b/libraries/YarpPlugins/BasicCartesianControl/RateThreadImpl.cpp
index 88625a654..b1242758d 100644
--- a/libraries/YarpPlugins/BasicCartesianControl/RateThreadImpl.cpp
+++ b/libraries/YarpPlugins/BasicCartesianControl/RateThreadImpl.cpp
@@ -4,7 +4,7 @@
 
 #include <cmath>  //-- abs
 #include <yarp/os/Time.h>
-#include <ColorDebug.hpp>
+#include <ColorDebug.h>
 
 // ------------------- RateThread Related ------------------------------------
 
diff --git a/libraries/YarpPlugins/BasicTwoLimbCartesianControl/CMakeLists.txt b/libraries/YarpPlugins/BasicTwoLimbCartesianControl/CMakeLists.txt
index da2e7ec6d..b7920d255 100644
--- a/libraries/YarpPlugins/BasicTwoLimbCartesianControl/CMakeLists.txt
+++ b/libraries/YarpPlugins/BasicTwoLimbCartesianControl/CMakeLists.txt
@@ -20,10 +20,9 @@ yarp_add_plugin(BasicTwoLimbCartesianControl BasicTwoLimbCartesianControl.hpp
                                              ICartesianControlImpl.cpp
                                              RateThreadImpl.cpp)
 
-add_dependencies(BasicTwoLimbCartesianControl COLOR_DEBUG)
-
 target_link_libraries(BasicTwoLimbCartesianControl YARP::YARP_OS
                                                    YARP::YARP_dev
+                                                   ROBOTICSLAB::ColorDebug
                                                    TrajectoryLib
                                                    Gait)
 
diff --git a/libraries/YarpPlugins/BasicTwoLimbCartesianControl/DeviceDriverImpl.cpp b/libraries/YarpPlugins/BasicTwoLimbCartesianControl/DeviceDriverImpl.cpp
index 8c4eb73e3..46253da7d 100644
--- a/libraries/YarpPlugins/BasicTwoLimbCartesianControl/DeviceDriverImpl.cpp
+++ b/libraries/YarpPlugins/BasicTwoLimbCartesianControl/DeviceDriverImpl.cpp
@@ -2,7 +2,7 @@
 
 #include "BasicTwoLimbCartesianControl.hpp"
 
-#include <ColorDebug.hpp>
+#include <ColorDebug.h>
 
 // ------------------- DeviceDriver Related ------------------------------------
 
diff --git a/libraries/YarpPlugins/BasicTwoLimbCartesianControl/ICartesianControlImpl.cpp b/libraries/YarpPlugins/BasicTwoLimbCartesianControl/ICartesianControlImpl.cpp
index 87d43bbbf..cd1a84913 100644
--- a/libraries/YarpPlugins/BasicTwoLimbCartesianControl/ICartesianControlImpl.cpp
+++ b/libraries/YarpPlugins/BasicTwoLimbCartesianControl/ICartesianControlImpl.cpp
@@ -2,7 +2,7 @@
 
 #include "BasicTwoLimbCartesianControl.hpp"
 
-#include <ColorDebug.hpp>
+#include <ColorDebug.h>
 
 // ------------------- ICartesianControl Related ------------------------------------
 
diff --git a/libraries/YarpPlugins/BasicTwoLimbCartesianControl/RateThreadImpl.cpp b/libraries/YarpPlugins/BasicTwoLimbCartesianControl/RateThreadImpl.cpp
index e3236f589..b26196ff0 100644
--- a/libraries/YarpPlugins/BasicTwoLimbCartesianControl/RateThreadImpl.cpp
+++ b/libraries/YarpPlugins/BasicTwoLimbCartesianControl/RateThreadImpl.cpp
@@ -2,7 +2,7 @@
 
 #include "BasicTwoLimbCartesianControl.hpp"
 
-#include <ColorDebug.hpp>
+#include <ColorDebug.h>
 
 // ------------------- RateThread Related ------------------------------------
 
diff --git a/libraries/YarpPlugins/CartesianControlClient/CMakeLists.txt b/libraries/YarpPlugins/CartesianControlClient/CMakeLists.txt
index 6acf695fa..f19217eb3 100644
--- a/libraries/YarpPlugins/CartesianControlClient/CMakeLists.txt
+++ b/libraries/YarpPlugins/CartesianControlClient/CMakeLists.txt
@@ -19,10 +19,9 @@ yarp_add_plugin(CartesianControlClient CartesianControlClient.hpp
                                        ICartesianControlImpl.cpp
                                        FkStreamResponder.cpp)
 
-add_dependencies(CartesianControlClient COLOR_DEBUG)
-
 target_link_libraries(CartesianControlClient YARP::YARP_OS
                                              YARP::YARP_dev
+                                             ROBOTICSLAB::ColorDebug
                                              KinematicsDynamicsInterfaces)
 
 yarp_install(TARGETS CartesianControlClient
diff --git a/libraries/YarpPlugins/CartesianControlClient/DeviceDriverImpl.cpp b/libraries/YarpPlugins/CartesianControlClient/DeviceDriverImpl.cpp
index 32e0cf91a..dfe6c9493 100644
--- a/libraries/YarpPlugins/CartesianControlClient/DeviceDriverImpl.cpp
+++ b/libraries/YarpPlugins/CartesianControlClient/DeviceDriverImpl.cpp
@@ -7,7 +7,7 @@
 #include <yarp/os/Network.h>
 #include <yarp/os/Time.h>
 
-#include <ColorDebug.hpp>
+#include <ColorDebug.h>
 
 // ------------------- DeviceDriver Related ------------------------------------
 
diff --git a/libraries/YarpPlugins/CartesianControlClient/ICartesianControlImpl.cpp b/libraries/YarpPlugins/CartesianControlClient/ICartesianControlImpl.cpp
index bf3fb541c..c526079f7 100644
--- a/libraries/YarpPlugins/CartesianControlClient/ICartesianControlImpl.cpp
+++ b/libraries/YarpPlugins/CartesianControlClient/ICartesianControlImpl.cpp
@@ -4,7 +4,7 @@
 
 #include <yarp/os/Time.h>
 
-#include <ColorDebug.hpp>
+#include <ColorDebug.h>
 
 // -----------------------------------------------------------------------------
 
diff --git a/libraries/YarpPlugins/CartesianControlServer/CMakeLists.txt b/libraries/YarpPlugins/CartesianControlServer/CMakeLists.txt
index 64138ff13..1c152e681 100644
--- a/libraries/YarpPlugins/CartesianControlServer/CMakeLists.txt
+++ b/libraries/YarpPlugins/CartesianControlServer/CMakeLists.txt
@@ -21,10 +21,9 @@ yarp_add_plugin(CartesianControlServer CartesianControlServer.hpp
                                        RpcResponder.cpp
                                        StreamResponder.cpp)
 
-add_dependencies(CartesianControlServer COLOR_DEBUG)
-
 target_link_libraries(CartesianControlServer YARP::YARP_OS
                                              YARP::YARP_dev
+                                             ROBOTICSLAB::ColorDebug
                                              KinematicRepresentationLib
                                              KinematicsDynamicsInterfaces)
 
diff --git a/libraries/YarpPlugins/CartesianControlServer/DeviceDriverImpl.cpp b/libraries/YarpPlugins/CartesianControlServer/DeviceDriverImpl.cpp
index cab2e7421..6a785ee35 100644
--- a/libraries/YarpPlugins/CartesianControlServer/DeviceDriverImpl.cpp
+++ b/libraries/YarpPlugins/CartesianControlServer/DeviceDriverImpl.cpp
@@ -4,7 +4,7 @@
 
 #include <string>
 
-#include <ColorDebug.hpp>
+#include <ColorDebug.h>
 
 // ------------------- DeviceDriver Related ------------------------------------
 
diff --git a/libraries/YarpPlugins/CartesianControlServer/RpcResponder.cpp b/libraries/YarpPlugins/CartesianControlServer/RpcResponder.cpp
index 81609102a..ba6141df0 100644
--- a/libraries/YarpPlugins/CartesianControlServer/RpcResponder.cpp
+++ b/libraries/YarpPlugins/CartesianControlServer/RpcResponder.cpp
@@ -2,7 +2,7 @@
 
 #include "CartesianControlServer.hpp"
 
-#include <ColorDebug.hpp>
+#include <ColorDebug.h>
 
 // -----------------------------------------------------------------------------
 
diff --git a/libraries/YarpPlugins/CartesianControlServer/StreamResponder.cpp b/libraries/YarpPlugins/CartesianControlServer/StreamResponder.cpp
index e845ffcb9..a278ebbc8 100644
--- a/libraries/YarpPlugins/CartesianControlServer/StreamResponder.cpp
+++ b/libraries/YarpPlugins/CartesianControlServer/StreamResponder.cpp
@@ -4,7 +4,7 @@
 
 #include <vector>
 
-#include <ColorDebug.hpp>
+#include <ColorDebug.h>
 
 // ------------------- StreamResponder Related ------------------------------------
 
diff --git a/libraries/YarpPlugins/KdlSolver/CMakeLists.txt b/libraries/YarpPlugins/KdlSolver/CMakeLists.txt
index 8c9d1433d..baec3a7cd 100644
--- a/libraries/YarpPlugins/KdlSolver/CMakeLists.txt
+++ b/libraries/YarpPlugins/KdlSolver/CMakeLists.txt
@@ -29,12 +29,11 @@ if(NOT SKIP_KdlSolver)
                               DeviceDriverImpl.cpp
                               ICartesianSolverImpl.cpp)
 
-    add_dependencies(KdlSolver COLOR_DEBUG)
-
     target_link_libraries(KdlSolver YARP::YARP_OS
                                     YARP::YARP_dev
                                     YARP::YARP_sig
                                     ${orocos_kdl_LIBRARIES}
+                                    ROBOTICSLAB::ColorDebug
                                     KdlVectorConverterLib
                                     KinematicRepresentationLib
                                     KinematicsDynamicsInterfaces)
diff --git a/libraries/YarpPlugins/KdlSolver/DeviceDriverImpl.cpp b/libraries/YarpPlugins/KdlSolver/DeviceDriverImpl.cpp
index 67dd72f0d..e751de0c4 100644
--- a/libraries/YarpPlugins/KdlSolver/DeviceDriverImpl.cpp
+++ b/libraries/YarpPlugins/KdlSolver/DeviceDriverImpl.cpp
@@ -6,7 +6,7 @@
 #include <kdl/rigidbodyinertia.hpp>
 #include <kdl/rotationalinertia.hpp>
 
-#include <ColorDebug.hpp>
+#include <ColorDebug.h>
 
 #include "KinematicRepresentation.hpp"
 
diff --git a/libraries/YarpPlugins/KdlSolver/ICartesianSolverImpl.cpp b/libraries/YarpPlugins/KdlSolver/ICartesianSolverImpl.cpp
index 0e6469196..6942f03b7 100644
--- a/libraries/YarpPlugins/KdlSolver/ICartesianSolverImpl.cpp
+++ b/libraries/YarpPlugins/KdlSolver/ICartesianSolverImpl.cpp
@@ -15,7 +15,7 @@
     #include <kdl/chainiksolverpos_nr_jl.hpp>
 #endif //_USE_LMA_
 
-#include <ColorDebug.hpp>
+#include <ColorDebug.h>
 
 #include "KdlVectorConverter.hpp"
 #include "KinematicRepresentation.hpp"
diff --git a/libraries/YarpPlugins/TwoLimbCartesianControlServer/CMakeLists.txt b/libraries/YarpPlugins/TwoLimbCartesianControlServer/CMakeLists.txt
index 46f9cd021..840c6b8c3 100644
--- a/libraries/YarpPlugins/TwoLimbCartesianControlServer/CMakeLists.txt
+++ b/libraries/YarpPlugins/TwoLimbCartesianControlServer/CMakeLists.txt
@@ -18,10 +18,9 @@ yarp_add_plugin(TwoLimbCartesianControlServer TwoLimbCartesianControlServer.hpp
                                               TwoLimbCartesianControlServer.cpp
                                               DeviceDriverImpl.cpp)
 
-add_dependencies(TwoLimbCartesianControlServer COLOR_DEBUG)
-
 target_link_libraries(TwoLimbCartesianControlServer YARP::YARP_OS
-                                                    YARP::YARP_dev)
+                                                    YARP::YARP_dev
+                                                    ROBOTICSLAB::ColorDebug)
 
 yarp_install(TARGETS TwoLimbCartesianControlServer
              COMPONENT runtime
diff --git a/libraries/YarpPlugins/TwoLimbCartesianControlServer/DeviceDriverImpl.cpp b/libraries/YarpPlugins/TwoLimbCartesianControlServer/DeviceDriverImpl.cpp
index 9ad98a83e..cbe8f361c 100644
--- a/libraries/YarpPlugins/TwoLimbCartesianControlServer/DeviceDriverImpl.cpp
+++ b/libraries/YarpPlugins/TwoLimbCartesianControlServer/DeviceDriverImpl.cpp
@@ -2,7 +2,7 @@
 
 #include "TwoLimbCartesianControlServer.hpp"
 
-#include <ColorDebug.hpp>
+#include <ColorDebug.h>
 
 // ------------------- DeviceDriver Related ------------------------------------
 
diff --git a/libraries/YarpPlugins/TwoLimbCartesianControlServer/TwoLimbCartesianControlServer.cpp b/libraries/YarpPlugins/TwoLimbCartesianControlServer/TwoLimbCartesianControlServer.cpp
index 1cfbd6fec..b43604067 100644
--- a/libraries/YarpPlugins/TwoLimbCartesianControlServer/TwoLimbCartesianControlServer.cpp
+++ b/libraries/YarpPlugins/TwoLimbCartesianControlServer/TwoLimbCartesianControlServer.cpp
@@ -2,7 +2,7 @@
 
 #include "TwoLimbCartesianControlServer.hpp"
 
-#include <ColorDebug.hpp>
+#include <ColorDebug.h>
 
 // ------------------- TwoLimbCartesianControlServer Related ------------------------------------
 
diff --git a/programs/keyboardController/CMakeLists.txt b/programs/keyboardController/CMakeLists.txt
index 95f23030d..052dd4257 100644
--- a/programs/keyboardController/CMakeLists.txt
+++ b/programs/keyboardController/CMakeLists.txt
@@ -13,11 +13,10 @@ add_executable(keyboardController main.cpp
                                   KeyboardController.hpp
                                   KeyboardController.cpp)
 
-add_dependencies(keyboardController COLOR_DEBUG)
-
 target_link_libraries(keyboardController YARP::YARP_OS
                                          YARP::YARP_init
                                          YARP::YARP_dev
+                                         ROBOTICSLAB::ColorDebug
                                          KinematicRepresentationLib
                                          KinematicsDynamicsInterfaces)
 
diff --git a/programs/keyboardController/KeyboardController.cpp b/programs/keyboardController/KeyboardController.cpp
index 17a786476..d60077087 100644
--- a/programs/keyboardController/KeyboardController.cpp
+++ b/programs/keyboardController/KeyboardController.cpp
@@ -16,7 +16,7 @@
 #include <yarp/os/Value.h>
 #include <yarp/os/Property.h>
 
-#include <ColorDebug.hpp>
+#include <ColorDebug.h>
 
 namespace
 {
diff --git a/programs/keyboardController/main.cpp b/programs/keyboardController/main.cpp
index fe94c5027..cf0a4ec58 100644
--- a/programs/keyboardController/main.cpp
+++ b/programs/keyboardController/main.cpp
@@ -3,7 +3,7 @@
 #include <yarp/os/ResourceFinder.h>
 #include <yarp/os/Network.h>
 
-#include <ColorDebug.hpp>
+#include <ColorDebug.h>
 
 #include "KeyboardController.hpp"
 
diff --git a/programs/streamingDeviceController/CMakeLists.txt b/programs/streamingDeviceController/CMakeLists.txt
index 839cfa0df..e9d2bdaa7 100644
--- a/programs/streamingDeviceController/CMakeLists.txt
+++ b/programs/streamingDeviceController/CMakeLists.txt
@@ -28,13 +28,12 @@ if(ENABLE_streamingDeviceController)
                                              StreamingDeviceController.hpp
                                              StreamingDeviceController.cpp)
 
-    add_dependencies(streamingDeviceController COLOR_DEBUG)
-
     target_link_libraries(streamingDeviceController ${orocos_kdl_LIBRARIES}
                                                     YARP::YARP_OS
                                                     YARP::YARP_init
                                                     YARP::YARP_dev
                                                     YARP::YARP_sig
+                                                    ROBOTICSLAB::ColorDebug
                                                     KdlVectorConverterLib
                                                     KinematicsDynamicsInterfaces)
 
diff --git a/programs/streamingDeviceController/LeapMotionSensorDevice.cpp b/programs/streamingDeviceController/LeapMotionSensorDevice.cpp
index 762cf1594..352bcde80 100644
--- a/programs/streamingDeviceController/LeapMotionSensorDevice.cpp
+++ b/programs/streamingDeviceController/LeapMotionSensorDevice.cpp
@@ -9,7 +9,7 @@
 
 #include <KdlVectorConverter.hpp>
 
-#include <ColorDebug.hpp>
+#include <ColorDebug.h>
 
 namespace
 {
diff --git a/programs/streamingDeviceController/SpnavSensorDevice.cpp b/programs/streamingDeviceController/SpnavSensorDevice.cpp
index 18e0c0a6c..e4633e41a 100644
--- a/programs/streamingDeviceController/SpnavSensorDevice.cpp
+++ b/programs/streamingDeviceController/SpnavSensorDevice.cpp
@@ -2,7 +2,7 @@
 
 #include <yarp/sig/Vector.h>
 
-#include <ColorDebug.hpp>
+#include <ColorDebug.h>
 
 bool roboticslab::SpnavSensorDevice::acquireInterfaces()
 {
diff --git a/programs/streamingDeviceController/StreamingDevice.cpp b/programs/streamingDeviceController/StreamingDevice.cpp
index 05cddf20d..ffccbfa1d 100644
--- a/programs/streamingDeviceController/StreamingDevice.cpp
+++ b/programs/streamingDeviceController/StreamingDevice.cpp
@@ -4,7 +4,7 @@
 
 #include <yarp/os/Property.h>
 
-#include <ColorDebug.hpp>
+#include <ColorDebug.h>
 
 #include "SpnavSensorDevice.hpp"
 #include "LeapMotionSensorDevice.hpp"
diff --git a/programs/streamingDeviceController/StreamingDeviceController.cpp b/programs/streamingDeviceController/StreamingDeviceController.cpp
index 5525d96ae..c00f8e073 100644
--- a/programs/streamingDeviceController/StreamingDeviceController.cpp
+++ b/programs/streamingDeviceController/StreamingDeviceController.cpp
@@ -8,7 +8,7 @@
 
 #include <yarp/sig/Vector.h>
 
-#include <ColorDebug.hpp>
+#include <ColorDebug.h>
 
 using namespace roboticslab;
 
diff --git a/programs/streamingDeviceController/WiimoteSensorDevice.cpp b/programs/streamingDeviceController/WiimoteSensorDevice.cpp
index c4110aa9f..9461f67c8 100644
--- a/programs/streamingDeviceController/WiimoteSensorDevice.cpp
+++ b/programs/streamingDeviceController/WiimoteSensorDevice.cpp
@@ -5,7 +5,7 @@
 
 #include <yarp/sig/Vector.h>
 
-#include <ColorDebug.hpp>
+#include <ColorDebug.h>
 
 bool roboticslab::WiimoteSensorDevice::acquireInterfaces()
 {
diff --git a/programs/streamingDeviceController/main.cpp b/programs/streamingDeviceController/main.cpp
index 73e4dd507..e98e23fcd 100644
--- a/programs/streamingDeviceController/main.cpp
+++ b/programs/streamingDeviceController/main.cpp
@@ -3,7 +3,7 @@
 #include <yarp/os/ResourceFinder.h>
 #include <yarp/os/Network.h>
 
-#include <ColorDebug.hpp>
+#include <ColorDebug.h>
 
 #include "StreamingDeviceController.hpp"
 
diff --git a/programs/transCoords/CMakeLists.txt b/programs/transCoords/CMakeLists.txt
index 1b8f45b02..d134831d1 100644
--- a/programs/transCoords/CMakeLists.txt
+++ b/programs/transCoords/CMakeLists.txt
@@ -18,12 +18,11 @@ if(ENABLE_transCoords)
                                TransCoords.cpp
                                TransCoords.hpp)
 
-    add_dependencies(transCoords COLOR_DEBUG)
-
     target_link_libraries(transCoords YARP::YARP_OS
                                       YARP::YARP_init
                                       YARP::YARP_dev
                                       ${orocos_kdl_LIBRARIES}
+                                      ROBOTICSLAB::ColorDebug
                                       KdlVectorConverterLib
                                       KinematicRepresentationLib
                                       KinematicsDynamicsInterfaces)
diff --git a/programs/transCoords/TransCoords.cpp b/programs/transCoords/TransCoords.cpp
index 4ceaac298..5a920daaa 100644
--- a/programs/transCoords/TransCoords.cpp
+++ b/programs/transCoords/TransCoords.cpp
@@ -7,7 +7,7 @@
 #include <yarp/os/Property.h>
 #include <yarp/os/Value.h>
 
-#include <ColorDebug.hpp>
+#include <ColorDebug.h>
 
 #include "KdlVectorConverter.hpp"
 
diff --git a/programs/transCoords/main.cpp b/programs/transCoords/main.cpp
index 7a6d17691..5b2551e3a 100644
--- a/programs/transCoords/main.cpp
+++ b/programs/transCoords/main.cpp
@@ -1,7 +1,7 @@
 #include <yarp/os/ResourceFinder.h>
 #include <yarp/os/Network.h>
 
-#include <ColorDebug.hpp>
+#include <ColorDebug.h>
 
 #include "TransCoords.hpp"
 
diff --git a/tests/CMakeLists.txt b/tests/CMakeLists.txt
index 66e828916..85f762866 100644
--- a/tests/CMakeLists.txt
+++ b/tests/CMakeLists.txt
@@ -18,19 +18,17 @@ if(ENABLE_tests)
 
     add_executable(testKinRepresentation testKinRepresentation.cpp)
 
-    add_dependencies(testKinRepresentation COLOR_DEBUG)
-
     target_link_libraries(testKinRepresentation KinematicRepresentationLib
+                                                ROBOTICSLAB::ColorDebug
                                                 gtest_main)
 
     # testKdlSolver
 
     add_executable(testKdlSolver testKdlSolver.cpp)
 
-    add_dependencies(testKdlSolver COLOR_DEBUG)
-
     target_link_libraries(testKdlSolver YARP::YARP_OS
                                         YARP::YARP_dev
+                                        ROBOTICSLAB::ColorDebug
                                         KinematicsDynamicsInterfaces
                                         gtest_main)
 
@@ -38,10 +36,9 @@ if(ENABLE_tests)
 
     add_executable(testKdlSolverFromFile testKdlSolverFromFile.cpp)
 
-    add_dependencies(testKdlSolverFromFile COLOR_DEBUG)
-
     target_link_libraries(testKdlSolverFromFile YARP::YARP_OS
                                                 YARP::YARP_dev
+                                                ROBOTICSLAB::ColorDebug
                                                 KinematicsDynamicsInterfaces
                                                 gtest_main)
 
@@ -49,10 +46,9 @@ if(ENABLE_tests)
 
     add_executable(testAsibotSolverFromFile testAsibotSolverFromFile.cpp)
 
-    add_dependencies(testAsibotSolverFromFile COLOR_DEBUG)
-
     target_link_libraries(testAsibotSolverFromFile YARP::YARP_OS
                                                    YARP::YARP_dev
+                                                   ROBOTICSLAB::ColorDebug
                                                    KinematicRepresentationLib
                                                    KinematicsDynamicsInterfaces
                                                    gtest_main)
@@ -61,19 +57,17 @@ if(ENABLE_tests)
 
     add_executable(testKdlTrajectory testKdlTrajectory.cpp)
 
-    add_dependencies(testKdlTrajectory COLOR_DEBUG)
-
     target_link_libraries(testKdlTrajectory TrajectoryLib
+                                            ROBOTICSLAB::ColorDebug
                                             gtest_main)
 
     # testBasicCartesianControl
 
     add_executable(testBasicCartesianControl testBasicCartesianControl.cpp)
 
-    add_dependencies(testBasicCartesianControl COLOR_DEBUG)
-
     target_link_libraries(testBasicCartesianControl YARP::YARP_OS
                                                     YARP::YARP_dev
+                                                    ROBOTICSLAB::ColorDebug
                                                     KinematicsDynamicsInterfaces
                                                     gtest_main)
 
diff --git a/tests/testAsibotSolverFromFile.cpp b/tests/testAsibotSolverFromFile.cpp
index 5f6df1bcd..368ca0bdc 100644
--- a/tests/testAsibotSolverFromFile.cpp
+++ b/tests/testAsibotSolverFromFile.cpp
@@ -7,7 +7,7 @@
 #include <yarp/dev/Drivers.h>
 #include <yarp/dev/PolyDriver.h>
 
-#include <ColorDebug.hpp>
+#include <ColorDebug.h>
 
 #include "ICartesianSolver.h"
 #include "KinematicRepresentation.hpp"
diff --git a/tests/testBasicCartesianControl.cpp b/tests/testBasicCartesianControl.cpp
index b572db3f5..fa6263e8a 100644
--- a/tests/testBasicCartesianControl.cpp
+++ b/tests/testBasicCartesianControl.cpp
@@ -8,7 +8,7 @@
 #include <yarp/dev/Drivers.h>
 #include <yarp/dev/PolyDriver.h>
 
-#include <ColorDebug.hpp>
+#include <ColorDebug.h>
 
 #include "ICartesianControl.h"
 
diff --git a/tests/testKdlSolver.cpp b/tests/testKdlSolver.cpp
index 92167057d..273735627 100644
--- a/tests/testKdlSolver.cpp
+++ b/tests/testKdlSolver.cpp
@@ -7,7 +7,7 @@
 #include <yarp/dev/Drivers.h>
 #include <yarp/dev/PolyDriver.h>
 
-#include <ColorDebug.hpp>
+#include <ColorDebug.h>
 
 #include "ICartesianSolver.h"
 
diff --git a/tests/testKdlSolverFromFile.cpp b/tests/testKdlSolverFromFile.cpp
index 473024739..795f2d071 100644
--- a/tests/testKdlSolverFromFile.cpp
+++ b/tests/testKdlSolverFromFile.cpp
@@ -7,7 +7,7 @@
 #include <yarp/dev/Drivers.h>
 #include <yarp/dev/PolyDriver.h>
 
-#include <ColorDebug.hpp>
+#include <ColorDebug.h>
 
 #include "ICartesianSolver.h"