diff --git a/FindKDL.cmake b/FindKDL.cmake
index 4ebbb6735d84a9dfe778e864d20608db062c58b8..0425105aab04459ac2a649f2424b063813610537 100644
--- a/FindKDL.cmake
+++ b/FindKDL.cmake
@@ -1,15 +1,60 @@
# Locate KDL install directory
-FIND_PATH(KDL_DIR orocos_kdl-config.cmake /usr/local/share/orocos_kdl)
+# This module defines
+# KDL_INSTALL where to find include, lib, bin, etc.
+# KDL_FOUND, is set to true
-INCLUDE (${KDL_DIR}/orocos_kdl-config.cmake)
+INCLUDE (${PROJECT_SOURCE_DIR}/FindPkgConfig.cmake)
-MESSAGE(STATUS "Detecting KDL: using orocos_kdl-config.cmake to locate files.")
+MESSAGE( STATUS ${CMAKE_PKGCONFIG_EXECUTABLE} )
-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)
+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
diff --git a/FindPkgConfig.cmake b/FindPkgConfig.cmake
new file mode 100644
index 0000000000000000000000000000000000000000..1b4dd35f73fae3a8921c137ae56654fa0885a498
--- /dev/null
+++ b/FindPkgConfig.cmake
@@ -0,0 +1,133 @@
+## 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 57e7569115b14b1922d46e89b67960729e3d44b2..656d92db42f44dffd12990d0874e52f06153cae2 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} ${orocos_kdl_INCLUDE_DIRS})
+INCLUDE_DIRECTORIES(. ${EIGEN3_INCLUDE_DIR} ${Boost_INCLUDE_DIRS} ${KDL_INCLUDE_DIRS})
# create the shared library
ADD_LIBRARY(uam_task_ctrl SHARED ${sources} ${sources_tasks})
-TARGET_LINK_LIBRARIES(uam_task_ctrl ${Boost_LIBRARIES} ${orocos_kdl_LIBRARIES})
+TARGET_LINK_LIBRARIES(uam_task_ctrl ${Boost_LIBRARIES} ${KDL_LIBS})
# link necessary libraries
INSTALL(TARGETS uam_task_ctrl
@@ -40,5 +40,7 @@ INSTALL(FILES ${headers} DESTINATION include/iridrivers/uam)
INSTALL(FILES ${headers_tasks} DESTINATION include/iridrivers/uam/tasks)
INSTALL(FILES ../Finduam_task_ctrl.cmake DESTINATION ${CMAKE_ROOT}/Modules/)
+INSTALL(FILES ../FindKDL.cmake DESTINATION ${CMAKE_ROOT}/Modules/)
+INSTALL(FILES ../FindPkgConfig.cmake DESTINATION ${CMAKE_ROOT}/Modules/)
ADD_SUBDIRECTORY(examples)