diff --git a/CMakeLists.txt b/CMakeLists.txt
index 8a21bce..adad792 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -1,55 +1,107 @@
-cmake_minimum_required(VERSION 3.0.2)
+cmake_minimum_required(VERSION 3.8)
 project(sas_robot_driver_franka)
 
 ## Compile as C++11, supported in ROS Kinetic and newer
 # add_compile_options(-std=c++11)
 #add_compile_options(-std=c++11)
-add_compile_options(-Werror=return-type)
-
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+    add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+if(POLICY CMP0148)
+    cmake_policy(SET CMP0148 NEW)
+endif()
 
 #Add custom (non compiling) targets so launch scripts and python files show up in QT Creator's project view.
 file(GLOB_RECURSE EXTRA_FILES */*)
 #add_custom_target(${PROJECT_NAME}_OTHER_FILES ALL WORKING_DIRECTORY ${PROJECT_SOURCE_DIR} SOURCES ${EXTRA_FILES})
 add_custom_target(cfg ALL WORKING_DIRECTORY ${PROJECT_SOURCE_DIR} SOURCES ${EXTRA_FILES})
 
-## Find catkin macros and libraries
-## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
-## is used, also find other catkin packages
-find_package(catkin REQUIRED COMPONENTS
-        roscpp
-        rospy
-        std_msgs
-        tf2_ros
-        tf2
-        sas_common
-        sas_clock
-        sas_robot_driver
-        sas_patient_side_manager
-        message_generation
-        pybind11_catkin
+
+find_package(pybind11 REQUIRED)
+find_package(ament_cmake REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(geometry_msgs REQUIRED)
+find_package(std_msgs REQUIRED)
+find_package(sas_common REQUIRED)
+find_package(sas_core REQUIRED)
+find_package(sas_conversions REQUIRED)
+find_package(Eigen3 REQUIRED)
+find_package(tf2_ros REQUIRED)
+find_package(tf2 REQUIRED)
+find_package(sas_robot_driver REQUIRED)
+find_package(rosidl_default_generators REQUIRED)
+
+
+
+rosidl_generate_interfaces(${PROJECT_NAME}
+        "msg/GripperState.msg"
+        "srv/Move.srv"
+        "srv/Grasp.srv"
+        DEPENDENCIES std_msgs
 )
 
+ament_target_dependencies(${PROJECT_NAME}
+        rclcpp geometry_msgs std_msgs sas_common sas_core sas_conversions
+        tf2_ros tf2 sas_robot_driver)
 
-add_service_files(
-        DIRECTORY srv
-        FILES
-        Move.srv
-        Grasp.srv
+
+target_include_directories(${PROJECT_NAME}
+        PUBLIC
+        $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+        $<INSTALL_INTERFACE:include>)
+
+ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET)
+ament_export_dependencies(Eigen3 rclcpp geometry_msgs std_msgs sas_common sas_core sas_conversions
+        tf2_ros tf2 sas_robot_driver)
+
+
+add_library(qros_robot_dynamics_provider src/robot_dynamics/qros_robot_dynamics_provider.cpp)
+target_link_libraries(qros_robot_dynamics_provider
+        -ldqrobotics
+        Eigen3::Eigen
 )
 
-add_message_files(
-        DIRECTORY msg
-        FILES
-        GripperState.msg
+#####################################################################################
+ament_python_install_package(${PROJECT_NAME})
+# python binding
+include_directories(
+        include/robot_dynamic
 )
-
-catkin_python_setup()
-
-generate_messages(
-        DEPENDENCIES
-        std_msgs
-
+pybind_add_module(_qros_robot_dynamic SHARED
+        src/robot_dynamics/qros_robot_dynamic_py.cpp src/robot_dynamics/qros_robot_dynamics_interface.cpp src/robot_dynamics/qros_robot_dynamics_provider.cpp
 )
+target_compile_definitions(_qros_robot_dynamic PRIVATE BUILD_PYBIND)
+# https://github.com/pybind/pybind11/issues/387
+target_link_libraries(_qros_robot_dynamic PRIVATE ${catkin_LIBRARIES} -ldqrobotics)
+
+
+
+
+
+
+
+
+#/////
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
 
 
 catkin_package(
diff --git a/CMakeLists_catkin.txt b/CMakeLists_catkin.txt
new file mode 100644
index 0000000..8a21bce
--- /dev/null
+++ b/CMakeLists_catkin.txt
@@ -0,0 +1,239 @@
+cmake_minimum_required(VERSION 3.0.2)
+project(sas_robot_driver_franka)
+
+## Compile as C++11, supported in ROS Kinetic and newer
+# add_compile_options(-std=c++11)
+#add_compile_options(-std=c++11)
+add_compile_options(-Werror=return-type)
+
+
+#Add custom (non compiling) targets so launch scripts and python files show up in QT Creator's project view.
+file(GLOB_RECURSE EXTRA_FILES */*)
+#add_custom_target(${PROJECT_NAME}_OTHER_FILES ALL WORKING_DIRECTORY ${PROJECT_SOURCE_DIR} SOURCES ${EXTRA_FILES})
+add_custom_target(cfg ALL WORKING_DIRECTORY ${PROJECT_SOURCE_DIR} SOURCES ${EXTRA_FILES})
+
+## Find catkin macros and libraries
+## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
+## is used, also find other catkin packages
+find_package(catkin REQUIRED COMPONENTS
+        roscpp
+        rospy
+        std_msgs
+        tf2_ros
+        tf2
+        sas_common
+        sas_clock
+        sas_robot_driver
+        sas_patient_side_manager
+        message_generation
+        pybind11_catkin
+)
+
+
+add_service_files(
+        DIRECTORY srv
+        FILES
+        Move.srv
+        Grasp.srv
+)
+
+add_message_files(
+        DIRECTORY msg
+        FILES
+        GripperState.msg
+)
+
+catkin_python_setup()
+
+generate_messages(
+        DEPENDENCIES
+        std_msgs
+
+)
+
+
+catkin_package(
+        INCLUDE_DIRS include
+        CATKIN_DEPENDS roscpp rospy sas_common sas_clock sas_robot_driver tf2_ros tf2 pybind11_catkin message_runtime std_msgs
+)
+
+find_package(Franka REQUIRED)
+find_package(Eigen3 REQUIRED)
+find_package(yaml-cpp REQUIRED)
+include_directories(${EIGEN3_INCLUDE_DIR})
+
+# To correctly find and link with QT
+set(CMAKE_PREFIX_PATH $ENV{QT_PATH})
+set(CMAKE_AUTOMOC ON)
+set(CMAKE_AUTORCC ON)
+set(CMAKE_AUTOUIC ON)
+if (CMAKE_VERSION VERSION_LESS "3.7.0")
+    set(CMAKE_INCLUDE_CURRENT_DIR ON)
+endif ()
+find_package(Qt5 COMPONENTS Widgets REQUIRED)
+
+find_package(QT NAMES Qt6 Qt5 REQUIRED COMPONENTS Widgets)
+find_package(Qt${QT_VERSION_MAJOR} REQUIRED COMPONENTS Widgets)
+
+
+add_library(MotionGenerator src/generator/motion_generator.cpp)
+target_link_libraries(MotionGenerator Franka::Franka)
+
+
+add_library(ConstraintsManager constraints_manager/src/constraints_manager.cpp)
+
+add_library(QuadraticProgramMotionGenerator src/generator/quadratic_program_motion_generator.cpp)
+target_link_libraries(QuadraticProgramMotionGenerator
+        qpOASES
+        dqrobotics
+        ConstraintsManager)
+
+add_library(CustomMotionGeneration src/generator/custom_motion_generation.cpp)
+target_link_libraries(CustomMotionGeneration
+        qpOASES
+        dqrobotics
+        ConstraintsManager)
+
+add_library(robot_interface_franka src/joint/robot_interface_franka.cpp)
+target_link_libraries(robot_interface_franka Franka::Franka
+        dqrobotics
+        MotionGenerator
+        ConstraintsManager
+        QuadraticProgramMotionGenerator
+        CustomMotionGeneration)
+
+add_library(robot_interface_hand src/hand/robot_interface_hand.cpp)
+target_link_libraries(robot_interface_hand Franka::Franka
+        dqrobotics)
+
+
+############
+## Build ###
+############
+
+## Specify additional locations of header files
+## Your package locations should be listed before other locations
+
+
+include_directories(
+        include
+        include/generator
+        src/
+        src/robot_dynamics
+        src/hand
+        src/joint
+        ${catkin_INCLUDE_DIRS}
+        constraints_manager/include
+)
+
+
+add_library(qros_robot_dynamics_provider src/robot_dynamics/qros_robot_dynamics_provider.cpp)
+target_link_libraries(qros_robot_dynamics_provider
+        ${catkin_LIBRARIES}
+        dqrobotics)
+
+add_library(qros_robot_dynamics_interface src/robot_dynamics/qros_robot_dynamics_interface.cpp)
+target_link_libraries(qros_robot_dynamics_interface
+        ${catkin_LIBRARIES}
+        dqrobotics)
+
+
+add_library(sas_robot_driver_franka src/joint/sas_robot_driver_franka.cpp)
+target_link_libraries(sas_robot_driver_franka
+        qros_robot_dynamics_provider
+        dqrobotics
+        dqrobotics-interface-json11
+        robot_interface_franka)
+
+add_library(qros_effector_driver_franka_hand src/hand/qros_effector_driver_franka_hand.cpp)
+target_link_libraries(qros_effector_driver_franka_hand
+        dqrobotics
+#        robot_interface_hand
+        Franka::Franka)
+
+
+add_library(sas_robot_driver_coppelia src/coppelia/sas_robot_driver_coppelia.cpp)
+target_link_libraries(sas_robot_driver_coppelia
+        dqrobotics
+        dqrobotics-interface-vrep)
+
+add_dependencies(sas_robot_driver_franka ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+add_dependencies(sas_robot_driver_coppelia ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+add_dependencies(qros_effector_driver_franka_hand ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+
+add_executable(sas_robot_driver_coppelia_node src/sas_robot_driver_coppelia_node.cpp)
+target_link_libraries(sas_robot_driver_coppelia_node
+        sas_robot_driver_coppelia
+        ${catkin_LIBRARIES})
+
+add_executable(sas_robot_driver_franka_node src/sas_robot_driver_franka_node.cpp)
+target_link_libraries(sas_robot_driver_franka_node
+        sas_robot_driver_franka
+        ${catkin_LIBRARIES})
+
+
+add_executable(sas_robot_driver_franka_hand_node src/sas_robot_driver_franka_hand_node.cpp)
+target_link_libraries(sas_robot_driver_franka_hand_node
+        qros_effector_driver_franka_hand
+        ${catkin_LIBRARIES})
+
+
+add_executable(JuankaEmika
+        qt/configuration_window/main.cpp
+        qt/configuration_window/mainwindow.cpp
+        qt/configuration_window/mainwindow.ui
+)
+
+target_link_libraries(JuankaEmika PRIVATE Qt${QT_VERSION_MAJOR}::Widgets
+        dqrobotics
+        ${catkin_LIBRARIES}
+        robot_interface_franka
+)
+
+
+#####################################################################################
+# python binding
+include_directories(
+        include/robot_dynamic
+)
+pybind_add_module(_qros_robot_dynamic SHARED
+        src/robot_dynamics/qros_robot_dynamic_py.cpp src/robot_dynamics/qros_robot_dynamics_interface.cpp src/robot_dynamics/qros_robot_dynamics_provider.cpp
+)
+target_compile_definitions(_qros_robot_dynamic PRIVATE BUILD_PYBIND)
+# https://github.com/pybind/pybind11/issues/387
+target_link_libraries(_qros_robot_dynamic PRIVATE ${catkin_LIBRARIES} -ldqrobotics)
+
+
+if (QT_VERSION_MAJOR EQUAL 6)
+    qt_finalize_executable(JuankaEmika)
+endif ()
+
+install(TARGETS ${PROJECT_NAME}
+        DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+)
+
+install(TARGETS sas_robot_driver_franka_node
+        DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+)
+
+install(TARGETS sas_robot_driver_coppelia_node
+        DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+)
+
+install(TARGETS sas_robot_driver_franka_hand_node
+        DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+)
+
+## Mark cpp header files for installation
+install(DIRECTORY include/${PROJECT_NAME}/
+        DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+        FILES_MATCHING PATTERN "*.h"
+        #   PATTERN ".svn" EXCLUDE
+)
+
+
+install(TARGETS _qros_robot_dynamic
+        LIBRARY DESTINATION ${PYTHON_INSTALL_DIR}
+)
+
diff --git a/__init__.py b/__init__.py
new file mode 100644
index 0000000..e69de29
diff --git a/package.xml b/package.xml
index 0808ae8..5463bb0 100644
--- a/package.xml
+++ b/package.xml
@@ -3,92 +3,36 @@
   <name>sas_robot_driver_franka</name>
   <version>0.0.0</version>
   <description>The sas_driver_franka package</description>
-
-  <!-- One maintainer tag required, multiple allowed, one person per tag -->
-  <!-- Example:  -->
-  <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
   <maintainer email="moonshot@todo.todo">moonshot</maintainer>
 
+  <license>LGPLv3</license>
+  <buildtool_depend>ament_cmake</buildtool_depend>
+  <buildtool_depend>rosidl_default_generators</buildtool_depend>
 
-  <!-- One license tag required, multiple allowed, one license per tag -->
-  <!-- Commonly used license strings: -->
-  <!--   BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
-  <license>TODO</license>
+  <depend>roscpp</depend>
+  <depend>rospy</depend>
+  <depend>tf2_ros</depend>
+  <depend>tf2</depend>
+  <depend>std_msgs</depend>
+  <depend>sas_clock</depend>
+  <depend>sas_robot_driver</depend>
+  <depend>sas_common</depend>
+  <depend>pybind11_catkin</depend>
+  <member_of_group>rosidl_interface_packages</member_of_group>
+  <!--  <depend>rosidl_default_generator</depend>-->
 
+  <test_depend>roscpp</test_depend>
+  <test_depend>rospy</test_depend>
+  <test_depend>tf2_ros</test_depend>
+  <test_depend>tf2</test_depend>
+  <test_depend>std_msgs</test_depend>
+  <test_depend>sas_clock</test_depend>
+  <test_depend>sas_robot_driver</test_depend>
+  <test_depend>sas_common</test_depend>
+  <!--  <test_depend>message_runtime</test_depend>-->
+  <exec_depend>rosidl_default_runtime</exec_depend>
 
-  <!-- Url tags are optional, but multiple are allowed, one per tag -->
-  <!-- Optional attribute type can be: website, bugtracker, or repository -->
-  <!-- Example: -->
-  <!-- <url type="website">http://wiki.ros.org/sas_driver_franka</url> -->
-
-
-  <!-- Author tags are optional, multiple are allowed, one per tag -->
-  <!-- Authors do not have to be maintainers, but could be -->
-  <!-- Example: -->
-  <!-- <author email="jane.doe@example.com">Jane Doe</author> -->
-
-
-  <!-- The *depend tags are used to specify dependencies -->
-  <!-- Dependencies can be catkin packages or system dependencies -->
-  <!-- Examples: -->
-  <!-- Use depend as a shortcut for packages that are both build and exec dependencies -->
-  <!--   <depend>roscpp</depend> -->
-  <!--   Note that this is equivalent to the following: -->
-  <!--   <build_depend>roscpp</build_depend> -->
-  <!--   <exec_depend>roscpp</exec_depend> -->
-  <!-- Use build_depend for packages you need at compile time: -->
-  <!--   <build_depend>message_generation</build_depend> -->
-  <!-- Use build_export_depend for packages you need in order to build against this package: -->
-  <!--   <build_export_depend>message_generation</build_export_depend> -->
-  <!-- Use buildtool_depend for build tool packages: -->
-  <!--   <buildtool_depend>catkin</buildtool_depend> -->
-  <!-- Use exec_depend for packages you need at runtime: -->
-  <!--   <exec_depend>message_runtime</exec_depend> -->
-  <!-- Use test_depend for packages you need only for testing: -->
-  <!--   <test_depend>gtest</test_depend> -->
-  <!-- Use doc_depend for packages you need only for building documentation: -->
-  <!--   <doc_depend>doxygen</doc_depend> -->
-  <buildtool_depend>catkin</buildtool_depend>
-
-  <build_depend>roscpp</build_depend>
-  <build_depend>rospy</build_depend>
-  <build_depend>tf2_ros</build_depend>
-  <build_depend>tf2</build_depend>
-  <build_depend>std_msgs</build_depend>
-  <build_depend>sas_clock</build_depend>
-  <build_depend>sas_robot_driver</build_depend>
-  <build_depend>sas_common</build_depend>
-  <build_depend>sas_patient_side_manager</build_depend>
-  <build_depend>pybind11_catkin</build_depend>
-  <build_depend>message_generation</build_depend>
-
-  <build_export_depend>roscpp</build_export_depend>
-  <build_export_depend>rospy</build_export_depend>
-  <build_export_depend>tf2_ros</build_export_depend>
-  <build_export_depend>tf2</build_export_depend>
-  <build_export_depend>std_msgs</build_export_depend>
-  <build_export_depend>sas_clock</build_export_depend>
-  <build_export_depend>sas_robot_driver</build_export_depend>
-  <build_export_depend>sas_common</build_export_depend>
-  <build_export_depend>sas_patient_side_manager</build_export_depend>
-  <build_export_depend>pybind11_catkin</build_export_depend>
-
-
-  <exec_depend>roscpp</exec_depend>
-  <exec_depend>rospy</exec_depend>
-  <exec_depend>tf2_ros</exec_depend>
-  <exec_depend>tf2</exec_depend>
-  <exec_depend>std_msgs</exec_depend>
-  <exec_depend>sas_clock</exec_depend>
-  <exec_depend>sas_robot_driver</exec_depend>
-  <exec_depend>sas_common</exec_depend>
-  <exec_depend>sas_patient_side_manager</exec_depend>
-  <exec_depend>message_runtime</exec_depend>
-
-
-  <!-- The export tag contains other, unspecified, tags -->
   <export>
-    <!-- Other tools can request additional information be placed here -->
-
+    <build_type>ament_cmake</build_type>
   </export>
-</package>
+</package>
\ No newline at end of file
diff --git a/src/sas_robot_driver_franka/__init__.py b/sas_robot_driver_franka/__init__.py
similarity index 100%
rename from src/sas_robot_driver_franka/__init__.py
rename to sas_robot_driver_franka/__init__.py
diff --git a/src/sas_robot_driver_franka/franka_gripper_interface.py b/sas_robot_driver_franka/franka_gripper_interface.py
similarity index 100%
rename from src/sas_robot_driver_franka/franka_gripper_interface.py
rename to sas_robot_driver_franka/franka_gripper_interface.py
diff --git a/setup.py b/setup.py
deleted file mode 100644
index 1d7a146..0000000
--- a/setup.py
+++ /dev/null
@@ -1,12 +0,0 @@
-# ! DO NOT MANUALLY INVOKE THIS setup.py, USE CATKIN INSTEAD
-
-from distutils.core import setup
-from catkin_pkg.python_setup import generate_distutils_setup
-
-# fetch values from package.xml
-setup_args = generate_distutils_setup(
-    packages=['sas_robot_driver_franka'],
-    package_dir={'': 'src'},
-)
-
-setup(**setup_args)