diff --git a/FindKDL.cmake b/FindKDL.cmake
index 0425105aab04459ac2a649f2424b063813610537..44e132d4af529c02e2416b2a7628b6f4028e216b 100644
--- a/FindKDL.cmake
+++ b/FindKDL.cmake
@@ -1,60 +1,15 @@
# Locate KDL install directory
-# This module defines
-# KDL_INSTALL where to find include, lib, bin, etc.
-# KDL_FOUND, is set to true
+MESSAGE(STATUS "Detecting KDL: using orocos_kdl-config.cmake to locate files.")
-INCLUDE (${PROJECT_SOURCE_DIR}/FindPkgConfig.cmake)
+FIND_PATH(KDL_DIR orocos_kdl-config.cmake /usr/local/share/orocos_kdl)
-MESSAGE( STATUS ${CMAKE_PKGCONFIG_EXECUTABLE} )
+INCLUDE (${KDL_DIR}/orocos_kdl-config.cmake)
-IF ( CMAKE_PKGCONFIG_EXECUTABLE )
-
- MESSAGE( STATUS "Detecting KDL" )
-
- SET(ENV{PKG_CONFIG_PATH} "${KDL_INSTALL}/lib/pkgconfig/")
- MESSAGE( "Looking for KDL in: ${KDL_INSTALL}")
- PKGCONFIG( "orocos-kdl >= 0.99" KDL_FOUND KDL_INCLUDE_DIRS KDL_DEFINES KDL_LINK_DIRS KDL_LIBS )
-
- IF( KDL_FOUND )
- MESSAGE(" Includes in: ${KDL_INCLUDE_DIRS}")
- MESSAGE(" Libraries in: ${KDL_LINK_DIRS}")
- MESSAGE(" Libraries: ${KDL_LIBS}")
- MESSAGE(" Defines: ${KDL_DEFINES}")
-
- INCLUDE_DIRECTORIES( ${KDL_INCLUDE_DIRS} )
- LINK_DIRECTORIES( ${KDL_LINK_DIRS})
- #OROCOS_PKGCONFIG_INCPATH("${KDLTK_INCLUDE_DIRS}")
- #OROCOS_PKGCONFIG_LIBS("${KDL_LIBS}")
- #OROCOS_PKGCONFIG_LIBPATH("${KDL_LINK_DIRS}")
-
-# SET(ENV{PKG_CONFIG_PATH} "${KDL_INSTALL}/lib/pkgconfig/:${OROCOS_INSTALL}/lib/pkgconfig")
-# MESSAGE( "Looking for KDL Toolkit in: ${PKG_CONFIG_PATH}")
-# PKGCONFIG( "orocos-kdltk-${OROCOS_TARGET} >= 0.99" KDLTK_FOUND KDLTK_INCLUDE_DIRS KDLTK_DEFINES KDLTK_LINK_DIRS KDLTK_LIBS )
-# IF(KDLTK_FOUND)
-# INCLUDE_DIRECTORIES( ${KDLTK_INCLUDE_DIRS} )
-# LINK_DIRECTORIES( ${KDLTK_LINK_DIRS})
-# OROCOS_PKGCONFIG_INCPATH("${KDLTK_INCLUDE_DIRS}")
-# OROCOS_PKGCONFIG_LIBPATH("${KDLTK_LINK_DIRS}")
-# OROCOS_PKGCONFIG_LIBS("${KDLTK_LIBS}")
-# IF(CORBA_ENABLED)
-# SET(ENV{PKG_CONFIG_PATH} "${KDL_INSTALL}/lib/pkgconfig/:${OROCOS_INSTALL}/lib/pkgconfig")
-# MESSAGE("Looking for KDL Toolkit CORBA extension in ${PKG_CONFIG_PATH}")
-# PKGCONFIG( "orocos-kdltk-corba-${OROCOS_TARGET} >= 0.99" KDLTKCORBA_FOUND KDLTKCORBA_INCLUDE_DIRS KDLTKCORBA_DEFINES KDLTKCORBA_LINK_DIRS KDLTKCORBA_LIBS )
-# IF(KDLTKCORBA_FOUND)
-# INCLUDE_DIRECTORIES( ${KDLTKCORBA_INCLUDE_DIRS} )
-# LINK_DIRECTORIES( ${KDLTKCORBA_LINK_DIRS})
-# OROCOS_PKGCONFIG_INCPATH("${KDLTKCORBA_INCLUDE_DIRS}")
-# OROCOS_PKGCONFIG_LIBPATH("${KDLTKCORBA_LINK_DIRS}")
-# OROCOS_PKGCONFIG_LIBS("${KDLTKCORBA_LIBS}")
-# ENDIF ( KDLTKCORBA_FOUND )
-# ENDIF(CORBA_ENABLED)
-# ENDIF ( KDLTK_FOUND )
- ENDIF ( KDL_FOUND )
-
-ELSE ( CMAKE_PKGCONFIG_EXECUTABLE )
-
- # Can't find pkg-config -- have to search manually
- MESSAGE( FATAL_ERROR "Can't find KDL without pkgconfig !")
-
-ENDIF ( CMAKE_PKGCONFIG_EXECUTABLE )
\ No newline at end of file
+IF (orocos_kdl_INCLUDE_DIRS AND orocos_kdl_LIBRARIES)
+ SET(KDL_FOUND TRUE)
+ MESSAGE(STATUS " Libraries: ${orocos_kdl_LIBRARIES}")
+ MESSAGE(STATUS " Defines: ${orocos_kdl_INCLUDE_DIRS}")
+ELSE (orocos_kdl_INCLUDE_DIRS AND orocos_kdl_LIBRARIES)
+ SET(KDL_FOUND FALSE)
+ENDIF (orocos_kdl_INCLUDE_DIRS AND orocos_kdl_LIBRARIES)
\ No newline at end of file
diff --git a/FindPkgConfig.cmake b/FindPkgConfig.cmake
deleted file mode 100644
index c0e1f3b12a1eece09c05b9c315cf5abd14b3ba0a..0000000000000000000000000000000000000000
--- a/FindPkgConfig.cmake
+++ /dev/null
@@ -1,133 +0,0 @@
-## FindPkgConfig.cmake
-## by Albert Strasheim <http://students . ee . sun . ac . za/~albert/>
-## and Alex Brooks (a.brooks at acfr . usyd . edu . au)
-##
-## This module finds packages using pkg-config, which retrieves
-## information about packages from special metadata files.
-##
-## See http://www . freedesktop . org/Software/pkgconfig/
-##
-## -------------------------------------------------------------------
-##
-## Usage:
-##
-## INCLUDE( ${CMAKE_ROOT}/Modules/FindPkgConfig.cmake)
-##
-## IF ( CMAKE_PKGCONFIG_EXECUTABLE )
-##
-## # Find all the librtk stuff with pkg-config
-## PKGCONFIG( "librtk >= 2.0" HAVE_RTK RTK_INCLUDE_DIRS RTK_DEFINES RTK_LINK_DIRS RTK_LIBS )
-##
-## ELSE ( CMAKE_PKGCONFIG_EXECUTABLE )
-##
-## # Can't find pkg-config -- have to find librtk somehow else
-##
-## ENDIF ( CMAKE_PKGCONFIG_EXECUTABLE )
-##
-##
-## Notes:
-##
-## You can set the PKG_CONFIG_PATH environment variable to tell
-## pkg-config where to search for .pc files. See pkg-config(1) for
-## more information.
-##
-#
-# FIXME: IF(WIN32) pkg-config --msvc-syntax ENDIF(WIN32) ???
-#
-# FIXME: Parsing of pkg-config output is specific to gnu-style flags
-#
-
-FIND_PROGRAM(CMAKE_PKGCONFIG_EXECUTABLE pkg-config)
-MARK_AS_ADVANCED(CMAKE_PKGCONFIG_EXECUTABLE)
-
-########################################
-
-MACRO(PKGCONFIG_PARSE_FLAGS FLAGS INCLUDES DEFINES)
-
- #MESSAGE("DEBUG: FLAGS: ${FLAGS}")
-
- STRING(REGEX MATCHALL " -I[^ ]*" ${INCLUDES} "${FLAGS}")
- STRING(REGEX REPLACE " -I" "" ${INCLUDES} "${${INCLUDES}}")
- #MESSAGE("DEBUG: INCLUDES: ${${INCLUDES}}")
-
- STRING(REGEX REPLACE " -I[^ ]*" "" ${DEFINES} "${FLAGS}")
- #MESSAGE("DEBUG: DEFINES: ${${DEFINES}}")
-
-ENDMACRO(PKGCONFIG_PARSE_FLAGS)
-
-########################################
-
-MACRO(PKGCONFIG_PARSE_LIBS LIBS LINKDIRS LINKLIBS)
-
- #MESSAGE("DEBUG: LIBS: ${LIBS}")
-
- STRING(REGEX MATCHALL " -L[^ ]*" ${LINKDIRS} "${LIBS}")
- STRING(REGEX REPLACE " -L" "" ${LINKDIRS} "${${LINKDIRS}}")
- #MESSAGE("DEBUG: LINKDIRS: ${${LINKDIRS}}")
-
- STRING(REGEX MATCHALL " -l[^ ]*" ${LINKLIBS} "${LIBS}")
- STRING(REGEX REPLACE " -l" "" ${LINKLIBS} "${${LINKLIBS}}")
- #MESSAGE("DEBUG: LINKLIBS: ${${LINKLIBS}}")
-
-ENDMACRO(PKGCONFIG_PARSE_LIBS)
-
-########################################
-
-MACRO(PKGCONFIG LIBRARY FOUND INCLUDE_DIRS DEFINES LINKDIRS LINKLIBS)
-
- SET(${FOUND} 0)
-
-# alexm: why print it twice? once here, and once when it's found/not found
-# MESSAGE("-- Looking for ${LIBRARY}")
-
- IF(CMAKE_PKGCONFIG_EXECUTABLE)
- # MESSAGE("DEBUG: pkg-config executable found")
-
- EXEC_PROGRAM(${CMAKE_PKGCONFIG_EXECUTABLE}
- ARGS "'${LIBRARY}'"
- OUTPUT_VARIABLE PKGCONFIG_OUTPUT
- RETURN_VALUE PKGCONFIG_RETURN)
-
- IF(NOT PKGCONFIG_RETURN)
-
- # set C_FLAGS and CXX_FLAGS
- EXEC_PROGRAM(${CMAKE_PKGCONFIG_EXECUTABLE}
- ARGS "--cflags '${LIBRARY}'"
- OUTPUT_VARIABLE CMAKE_PKGCONFIG_C_FLAGS)
-
- #SET(CMAKE_PKGCONFIG_CXX_FLAGS "${CMAKE_PKGCONFIG_C_FLAGS}")
- PKGCONFIG_PARSE_FLAGS(" ${CMAKE_PKGCONFIG_C_FLAGS}" ${INCLUDE_DIRS} ${DEFINES} )
-
- # set LIBRARIES
- EXEC_PROGRAM(${CMAKE_PKGCONFIG_EXECUTABLE}
- ARGS "--libs '${LIBRARY}'"
- OUTPUT_VARIABLE CMAKE_PKGCONFIG_LIBRARIES)
- PKGCONFIG_PARSE_LIBS (" ${CMAKE_PKGCONFIG_LIBRARIES}" ${LINKDIRS} ${LINKLIBS} )
-
- SET(${FOUND} 1)
- MESSAGE("-- Looking for ${LIBRARY} -- found")
-
- ELSE(NOT PKGCONFIG_RETURN)
- MESSAGE("-- Looking for ${LIBRARY} -- not found")
-
- SET(CMAKE_PKGCONFIG_C_FLAGS "")
- SET(CMAKE_PKGCONFIG_CXX_FLAGS "")
- SET(CMAKE_PKGCONFIG_LIBRARIES "")
- SET(${INCLUDE_DIRS} "")
- SET(${DEFINES} "")
- SET(${LINKDIRS} "")
- SET(${LINKLIBS} "")
-
- ENDIF(NOT PKGCONFIG_RETURN)
-
- ELSE(CMAKE_PKGCONFIG_EXECUTABLE)
- MESSAGE("-- pkg-config executable NOT FOUND")
- ENDIF(CMAKE_PKGCONFIG_EXECUTABLE)
-
- #MESSAGE("Have ${LIBRARY} : ${${FOUND}}")
- #MESSAGE("${LIBRARY} include dirs: ${${INCLUDE_DIRS}}")
- #MESSAGE("${LIBRARY} defines : ${${DEFINES}}")
- #MESSAGE("${LIBRARY} link dirs : ${${LINKDIRS}}")
- #MESSAGE("${LIBRARY} link libs : ${${LINKLIBS}}")
-
-ENDMACRO(PKGCONFIG)
\ No newline at end of file
diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt
index 7a2edb89f3446e50158708d6039428b8151fe472..57e7569115b14b1922d46e89b67960729e3d44b2 100644
--- a/src/CMakeLists.txt
+++ b/src/CMakeLists.txt
@@ -23,12 +23,12 @@ FIND_PACKAGE(Eigen3 REQUIRED)
SET(CMAKE_BUILD_TYPE release)
# add the necessary include directories
-INCLUDE_DIRECTORIES(. ${EIGEN3_INCLUDE_DIR} ${Boost_INCLUDE_DIRS} ${KDL_INCLUDE_DIRS})
+INCLUDE_DIRECTORIES(. ${EIGEN3_INCLUDE_DIR} ${Boost_INCLUDE_DIRS} ${orocos_kdl_INCLUDE_DIRS})
# create the shared library
ADD_LIBRARY(uam_task_ctrl SHARED ${sources} ${sources_tasks})
-TARGET_LINK_LIBRARIES(uam_task_ctrl ${Boost_LIBRARIES} ${KDL_LIBS})
+TARGET_LINK_LIBRARIES(uam_task_ctrl ${Boost_LIBRARIES} ${orocos_kdl_LIBRARIES})
# link necessary libraries
INSTALL(TARGETS uam_task_ctrl