diff --git a/CMakeLists.txt b/CMakeLists.txt
index 2185d2cc3a7ead117712b3c0ba01f57eb76ee6dc..e0dc5818cf0dd8b70d73c1bd7caa370765f4af99 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -1,10 +1,12 @@
 # RobotAPI
+
 cmake_minimum_required(VERSION 3.10.2)
 
 if(POLICY CMP0043)
     cmake_policy(SET CMP0043 OLD)
 endif()
 
+
 find_package("ArmarXCore" REQUIRED
     PATHS "$ENV{HOME}/armarx/Core/build"
 )
@@ -21,6 +23,13 @@ find_package(DMP QUIET)
 find_package(OpenCV QUIET)
 find_package(IVT COMPONENTS ivt ivtopencv QUIET)
 find_package(manif QUIET)
+#/common/homes/students/ulxvc_koch/Workspaces/deps/rbdl/share/FindRBDL.cmake
+# SET(RBDL_INCLUDE_DIR "${CMAKE_CURRENT_SOURCE_DIR}/../../deps/rbdl/include/rbdl")
+# SET(RBDL_LIBRARY "rbdl")
+# list(APPEND CMAKE_MODULE_PATH "${CMAKE_CURRENT_SOURCE_DIR}/etc/cmake")
+find_package(RBDL REQUIRED) 
+link_libraries(${RBDL_LIBRARY})
+
 
 add_subdirectory(source)
 
diff --git a/etc/cmake/FindRBDL.cmake b/etc/cmake/FindRBDL.cmake
new file mode 100644
index 0000000000000000000000000000000000000000..70e3196f8f9c8a1ac6ab261506341f24efdd28dd
--- /dev/null
+++ b/etc/cmake/FindRBDL.cmake
@@ -0,0 +1,294 @@
+SET (RBDL_FOUND FALSE)
+SET (RBDL_LUAMODEL_FOUND FALSE)
+SET (RBDL_URDFREADER_FOUND FALSE)
+SET (RBDL_GEOMETRY_FOUND FALSE)
+SET (RBDL_MUSCLE_FOUND FALSE)
+
+
+UNSET( RBDL_INCLUDE_DIR              CACHE)
+UNSET( RBDL_LIBRARY                  CACHE)
+UNSET( RBDL_LUAMODEL_INCLUDE_DIR     CACHE)
+UNSET( RBDL_LUAMODEL_LIBRARY         CACHE)
+UNSET( RBDL_URDFREADER_INCLUDE_DIR   CACHE)
+UNSET( RBDL_URDFREADER_LIBRARY       CACHE)
+UNSET( RBDL_MUSCLE_INCLUDE_DIR       CACHE)
+UNSET( RBDL_MUSCLE_LIBRARY           CACHE)
+UNSET( RBDL_GEOMETRY_INCLUDE_DIR     CACHE)
+UNSET( RBDL_GEOMETRY_LIBRARY         CACHE)
+
+#SET(CUSTOM_RBDL_PATH  ${CMAKE_CURRENT_SOURCE_DIR}/../../../../deps/rbdl FORCE)
+SET(CUSTOM_RBDL_PATH FALSE)
+
+
+# FIND_PATH(RBDL_INCLUDE_DIR NAMES rbdl.h
+#   PATHS
+#   ${RBDL_INCLUDE_DIR}/../../../../deps/rbdl/include/rbdl
+#   /common/homes/students/ulxvc_koch/Workspaces/deps/rbdl/include/rbdl
+#   $ENV{RBDL_INCLUDE_DIR}/include/
+#   $ENV{RBDL_INCLUDE_DIR}/include/rbdl/
+#   ${RBDL_INCLUDE_DIR}/include/
+#   ${RBDL_INCLUDE_DIR}/include/rbdl/
+#   ENV CPATH
+#   /usr/include/rbdl/
+#   /usr/local/include/rbdl/
+#   /opt/local/include/rbdl/
+#   NO_DEFAULT_PATH
+  
+# )
+
+# FIND_LIBRARY(RBDL_LIBRARY NAMES librbdl.so
+#   PATHS
+#   $ENV{RBDL_INCLUDE_DIR}/lib
+#   ${RBDL_INCLUDE_DIR}/lib
+#   ENV LD_LIBRARY_PATH
+#   ENV LIBRARY_PATH
+#   /usr/lib
+#   /usr/local/lib
+#   /opt/local/lib
+#   NO_DEFAULT_PATH
+# )
+
+
+IF(CUSTOM_RBDL_PATH)
+
+  FIND_PATH (RBDL_INCLUDE_DIR rbdl/rbdl.h
+    PATHS
+    ${CUSTOM_RBDL_PATH}/include
+    NO_DEFAULT_PATH
+    )
+
+  FIND_LIBRARY (RBDL_LIBRARY rbdl
+    PATHS
+    ${CUSTOM_RBDL_PATH}/lib
+    NO_DEFAULT_PATH
+    )
+
+  FIND_PATH (RBDL_LUAMODEL_INCLUDE_DIR rbdl/addons/luamodel/luamodel.h
+    PATHS
+    ${CUSTOM_RBDL_PATH}/include
+    NO_DEFAULT_PATH
+    )
+
+  FIND_LIBRARY (RBDL_LUAMODEL_LIBRARY rbdl_luamodel
+    PATHS
+    ${CUSTOM_RBDL_PATH}/lib
+    NO_DEFAULT_PATH
+    )
+
+  FIND_PATH (RBDL_URDFREADER_INCLUDE_DIR rbdl/addons/urdfreader/urdfreader.h
+    PATHS
+    ${CUSTOM_RBDL_PATH}/include
+    NO_DEFAULT_PATH
+    )
+
+
+  FIND_LIBRARY (RBDL_URDFREADER_LIBRARY rbdl_urdfreader
+    PATHS
+    ${CUSTOM_RBDL_PATH}/lib
+    NO_DEFAULT_PATH
+    )
+
+
+  FIND_PATH (RBDL_MUSCLE_INCLUDE_DIR rbdl/addons/muscle/muscle.h
+    PATHS
+    ${CUSTOM_RBDL_PATH}/include
+    NO_DEFAULT_PATH
+    )
+
+
+  FIND_LIBRARY (RBDL_MUSCLE_LIBRARY rbdl_muscle
+    PATHS
+    ${CUSTOM_RBDL_PATH}/lib
+    NO_DEFAULT_PATH
+    )
+
+  FIND_PATH (RBDL_GEOMETRY_INCLUDE_DIR rbdl/addons/geometry/geometry.h
+    PATHS
+    ${CUSTOM_RBDL_PATH}/include
+    NO_DEFAULT_PATH
+    )
+
+  FIND_LIBRARY (RBDL_GEOMETRY_LIBRARY rbdl_geometry
+    PATHS
+    ${CUSTOM_RBDL_PATH}/lib
+    NO_DEFAULT_PATH
+    )
+
+ELSE(CUSTOM_RBDL_PATH)
+  FIND_PATH (RBDL_INCLUDE_DIR rbdl/rbdl.h
+    HINTS
+    ${CMAKE_CURRENT_SOURCE_DIR}/../../../../deps/rbdl/include
+    ${CMAKE_CURRENT_SOURCE_DIR}/../../deps/rbdl/include
+    /common/homes/students/ulxvc_koch/Workspaces/deps/rbdl/include
+    $ENV{HOME}/local/include
+    $ENV{RBDL_PATH}/src
+    $ENV{RBDL_PATH}/include
+    $ENV{RBDL_INCLUDE_PATH}
+    /usr/local/include
+    /usr/include
+    )
+
+  FIND_LIBRARY (RBDL_LIBRARY NAMES librbdl.so librbdl librbdl.so.3.2.0 rbdl
+    PATHS
+    $ENV{HOME}/local/lib
+    $ENV{HOME}/local/lib/x86_64-linux-gnu
+    $ENV{RBDL_PATH}/lib
+    ${CMAKE_CURRENT_SOURCE_DIR}/../../../../deps/rbdl/build/install/lib
+    ${CMAKE_CURRENT_SOURCE_DIR}/../../deps/rbdl/build/install/lib
+    /common/homes/students/ulxvc_koch/Workspaces/deps/rbdl/build/install/lib
+    $ENV{RBDL_LIBRARY_PATH}
+    /usr/local/lib
+    /usr/local/lib/x86_64-linux-gnu
+    /usr/lib
+    /usr/lib/x86_64-linux-gnu
+    )
+
+  FIND_PATH (RBDL_LUAMODEL_INCLUDE_DIR rbdl/addons/luamodel/luamodel.h
+    HINTS
+    $ENV{HOME}/local/include
+    $ENV{RBDL_PATH}/src
+    $ENV{RBDL_PATH}/include
+    $ENV{RBDL_INCLUDE_PATH}
+    /usr/local/include
+    /usr/include
+    )
+
+  FIND_LIBRARY (RBDL_LUAMODEL_LIBRARY rbdl_luamodel
+    PATHS
+    $ENV{HOME}/local/lib
+    $ENV{HOME}/local/lib/x86_64-linux-gnu
+    $ENV{RBDL_PATH}
+    $ENV{RBDL_LIBRARY_PATH}
+    /usr/local/lib
+    /usr/local/lib/x86_64-linux-gnu
+    /usr/lib
+    /usr/lib/x86_64-linux-gnu
+    )
+
+  FIND_PATH (RBDL_URDFREADER_INCLUDE_DIR rbdl/addons/urdfreader/urdfreader.h
+    HINTS
+    $ENV{HOME}/local/include
+    $ENV{RBDL_PATH}/src
+    $ENV{RBDL_PATH}/include
+    $ENV{RBDL_INCLUDE_PATH}
+    /usr/local/include
+    /usr/include
+    )
+
+  FIND_LIBRARY (RBDL_URDFREADER_LIBRARY rbdl_urdfreader
+    PATHS
+    $ENV{HOME}/local/lib
+    $ENV{HOME}/local/lib/x86_64-linux-gnu
+    $ENV{RBDL_PATH}
+    $ENV{RBDL_LIBRARY_PATH}
+    /usr/local/lib
+    /usr/local/lib/x86_64-linux-gnu
+    /usr/lib
+    /usr/lib/x86_64-linux-gnu
+    )
+
+
+  FIND_PATH (RBDL_MUSCLE_INCLUDE_DIR rbdl/addons/muscle/muscle.h
+    HINTS
+    $ENV{HOME}/local/include
+    $ENV{RBDL_PATH}/src
+    $ENV{RBDL_PATH}/include
+    $ENV{RBDL_INCLUDE_PATH}
+    /usr/local/include
+    /usr/include
+    )
+
+  FIND_LIBRARY (RBDL_MUSCLE_LIBRARY rbdl_muscle
+    PATHS
+    $ENV{HOME}/local/lib
+    $ENV{HOME}/local/lib/x86_64-linux-gnu
+    $ENV{RBDL_PATH}
+    $ENV{RBDL_LIBRARY_PATH}
+    /usr/local/lib
+    /usr/local/lib/x86_64-linux-gnu
+    /usr/lib
+    /usr/lib/x86_64-linux-gnu
+    )
+
+  FIND_PATH (RBDL_GEOMETRY_INCLUDE_DIR rbdl/addons/geometry/geometry.h
+    HINTS
+    $ENV{HOME}/local/include
+    $ENV{RBDL_PATH}/src
+    $ENV{RBDL_PATH}/include
+    $ENV{RBDL_INCLUDE_PATH}
+    /usr/local/include
+    /usr/include
+    )
+
+  FIND_LIBRARY (RBDL_GEOMETRY_LIBRARY rbdl_geometry
+    PATHS
+    $ENV{HOME}/local/lib
+    $ENV{HOME}/local/lib/x86_64-linux-gnu
+    $ENV{RBDL_PATH}
+    $ENV{RBDL_LIBRARY_PATH}
+    /usr/local/lib
+    /usr/local/lib/x86_64-linux-gnu
+    /usr/lib
+    /usr/lib/x86_64-linux-gnu
+    )
+ENDIF(CUSTOM_RBDL_PATH)
+
+IF (RBDL_INCLUDE_DIR AND RBDL_LIBRARY)
+  SET (RBDL_FOUND TRUE)
+ELSE(RBDL_INCLUDE_DIR AND RBDL_LIBRARY)
+  IF(RBDL_FIND_REQUIRED)
+    MESSAGE (SEND_ERROR " Could not find RBDL.")
+    MESSAGE (SEND_ERROR " Try setting CUSTOM_RBDL_PATH in FindRBDL.cmake force CMake to use the desired directory.")
+  ELSE(RBDL_FIND_REQUIRED)
+    MESSAGE (STATUS " Could not find RBDL.")
+    MESSAGE (STATUS " Try setting CUSTOM_RBDL_PATH in FindRBDL.cmake force CMake to use the desired directory.")
+  ENDIF(RBDL_FIND_REQUIRED)
+ENDIF (RBDL_INCLUDE_DIR AND RBDL_LIBRARY)
+
+
+IF (RBDL_LUAMODEL_LIBRARY AND RBDL_LUAMODEL_INCLUDE_DIR)
+  SET (RBDL_LUAMODEL_FOUND TRUE)
+ENDIF (RBDL_LUAMODEL_LIBRARY AND RBDL_LUAMODEL_INCLUDE_DIR)
+
+IF (RBDL_URDFREADER_LIBRARY AND RBDL_URDFREADER_INCLUDE_DIR)
+  SET (RBDL_URDFREADER_FOUND TRUE)
+ENDIF (RBDL_URDFREADER_LIBRARY AND RBDL_URDFREADER_INCLUDE_DIR)
+
+IF (RBDL_MUSCLE_LIBRARY AND RBDL_MUSCLE_INCLUDE_DIR)
+  SET (RBDL_MUSCLE_FOUND TRUE)
+ENDIF (RBDL_MUSCLE_LIBRARY AND RBDL_MUSCLE_INCLUDE_DIR)
+
+IF (RBDL_GEOMETRY_LIBRARY AND RBDL_GEOMETRY_INCLUDE_DIR)
+  SET (RBDL_GEOMETRY_FOUND TRUE)
+ENDIF (RBDL_GEOMETRY_LIBRARY AND RBDL_GEOMETRY_INCLUDE_DIR)
+
+
+IF (RBDL_FOUND)
+   IF (NOT RBDL_FIND_QUIETLY)
+      MESSAGE(STATUS "Found RBDL: ${RBDL_LIBRARY}")
+   ENDIF (NOT RBDL_FIND_QUIETLY)
+
+   foreach ( COMPONENT ${RBDL_FIND_COMPONENTS} )
+     IF (RBDL_${COMPONENT}_FOUND)
+       IF (NOT RBDL_FIND_QUIETLY)
+         MESSAGE(STATUS "Found RBDL ${COMPONENT}: ${RBDL_${COMPONENT}_LIBRARY}")
+       ENDIF (NOT RBDL_FIND_QUIETLY)
+     ELSE (RBDL_${COMPONENT}_FOUND)
+       MESSAGE(ERROR " Could not find RBDL ${COMPONENT}")
+     ENDIF (RBDL_${COMPONENT}_FOUND)
+   endforeach ( COMPONENT )
+
+ENDIF (RBDL_FOUND)
+
+MARK_AS_ADVANCED (
+  RBDL_INCLUDE_DIR
+  RBDL_LIBRARY
+  RBDL_LUAMODEL_INCLUDE_DIR
+  RBDL_LUAMODEL_LIBRARY
+  RBDL_URDFREADER_INCLUDE_DIR
+  RBDL_URDFREADER_LIBRARY
+  RBDL_MUSCLE_INCLUDE_DIR
+  RBDL_MUSCLE_LIBRARY
+  RBDL_GEOMETRY_INCLUDE_DIR
+  RBDL_GEOMETRY_LIBRARY
+  )
diff --git a/source/RobotAPI/libraries/armem_mps/CMakeLists.txt b/source/RobotAPI/libraries/armem_mps/CMakeLists.txt
index 54482355002f280e19dbe768a6b754798f17830d..e51a8534780cbc9d8102ee4a23a57f9cf4daac4c 100644
--- a/source/RobotAPI/libraries/armem_mps/CMakeLists.txt
+++ b/source/RobotAPI/libraries/armem_mps/CMakeLists.txt
@@ -1,8 +1,25 @@
-set(LIB_NAME       armem_mps)
+set(LIB_NAME armem_mps)
 
 armarx_component_set_name("${LIB_NAME}")
 armarx_set_target("Library: ${LIB_NAME}")
 find_package(DMP QUIET) # needs to be changed to new MP package
+# find_package(RBDL REQUIRED) # needs to be changed to new MP package
+
+
+# FIND_LIBRARY (RBDL_LIBRARY NAMES librbdl librbdl.so librbdl.so.3.2.0 rbdl
+# PATHS
+# $ENV{HOME}/local/lib<
+# $ENV{HOME}/local/lib/x86_64-linux-gnu
+# $ENV{RBDL_PATH}/lib
+# ${CMAKE_CURRENT_SOURCE_DIR}/../../../../deps/rbdl/build
+# ${CMAKE_CURRENT_SOURCE_DIR}/../../deps/rbdl/build
+# /common/homes/students/ulxvc_koch/Workspaces/deps/rbdl/build
+# $ENV{RBDL_LIBRARY_PATH}
+# /usr/local/lib
+# /usr/local/lib/x86_64-linux-gnu
+# /usr/lib
+# /usr/lib/x86_64-linux-gnu
+# )
 
 armarx_build_if(DMP_FOUND "DMP not available")
 
@@ -16,9 +33,11 @@ armarx_add_library(
         RobotAPI::armem
         RobotAPI::PriorKnowledge::Motions
         RobotAPI::armem_motions
+        
 
         VirtualRobot
         ${DMP_LIBRARIES}
+        # ${RBDL_LIBRARY}
 
     SOURCES  
         aron_conversions.cpp
@@ -31,5 +50,5 @@ armarx_add_library(
 
 
 add_library(RobotAPI::armem_mps ALIAS armem_mps)
-
+target_link_libraries(armem_mps LINK_PUBLIC rbdl)
 add_subdirectory(server)
diff --git a/source/RobotAPI/libraries/armem_mps/server/CMakeLists.txt b/source/RobotAPI/libraries/armem_mps/server/CMakeLists.txt
index c4a5854ce895a7415a6ba21cdd06b8b4b03eb6c8..d9d355412ab0b1db42ee00964cda884b2061f756 100644
--- a/source/RobotAPI/libraries/armem_mps/server/CMakeLists.txt
+++ b/source/RobotAPI/libraries/armem_mps/server/CMakeLists.txt
@@ -8,6 +8,21 @@ armarx_set_target("Library: ${LIB_NAME}")
 
 armarx_build_if(DMP_FOUND "DMP not available")
 
+# FIND_LIBRARY (RBDL_LIBRARY NAMES librbdl librbdl.so librbdl.so.3.2.0 rbdl
+# PATHS
+# $ENV{HOME}/local/lib<
+# $ENV{HOME}/local/lib/x86_64-linux-gnu
+# $ENV{RBDL_PATH}/lib
+# ${CMAKE_CURRENT_SOURCE_DIR}/../../../../deps/rbdl/build
+# ${CMAKE_CURRENT_SOURCE_DIR}/../../deps/rbdl/build
+# /common/homes/students/ulxvc_koch/Workspaces/deps/rbdl/build
+# $ENV{RBDL_LIBRARY_PATH}
+# /usr/local/lib
+# /usr/local/lib/x86_64-linux-gnu
+# /usr/lib
+# /usr/lib/x86_64-linux-gnu
+# )
+
 armarx_add_library(
     LIBS     
         ArmarXCoreInterfaces
@@ -22,6 +37,7 @@ armarx_add_library(
 
         VirtualRobot
         ${DMP_LIBRARIES}
+        # ${RBDL_LIBRARY}
 
     SOURCES  
         MotionPrimitives/motionprimitives.cpp