[message interface] combining driver and interface package for better future management

bug fix minor dereference bug
added python interface additional type hint and misc
2025-11-06 09:21:22 +09:00 · 2025-07-07 16:43:41 +09:00 · 2025-07-05 18:09:59 +09:00 · 2025-07-05 18:02:49 +09:00 · 2025-07-05 18:02:13 +09:00 · 2025-07-04 23:08:23 +09:00
82 changed files with 3396 additions and 1501 deletions
--- a/.gitmodules
+++ b/.gitmodules
@@ -1,3 +0,0 @@
-[submodule "constraints_manager"]
-	path = constraints_manager
-	url = https://github.com/juanjqo/constraints_manager.git
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -1,185 +0,0 @@
-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
-  sas_common
-  sas_clock
-  sas_robot_driver
-  sas_patient_side_manager
-)
-
-catkin_package(
-  INCLUDE_DIRS include
-  CATKIN_DEPENDS roscpp rospy sas_common sas_clock sas_robot_driver
-)
-
-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/dynamic_interface
-    src/hand
-    src/joint
-    ${catkin_INCLUDE_DIRS}
-    constraints_manager/include
-    )
-
-
-add_library(sas_robot_dynamic_provider src/dynamic_interface/sas_robot_dynamic_provider.cpp)
-target_link_libraries(sas_robot_dynamic_provider dqrobotics)
-
-
-add_library(sas_robot_driver_franka src/joint/sas_robot_driver_franka.cpp)
-target_link_libraries(sas_robot_driver_franka
-                    sas_robot_dynamic_provider
-                    dqrobotics
-                    robot_interface_franka)
-
-add_library(sas_robot_driver_franka_hand src/hand/sas_robot_driver_franka_hand.cpp)
-target_link_libraries(sas_robot_driver_franka_hand
-        dqrobotics
-        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_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
-        sas_robot_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
-    )
-
-
-
-
-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
-)                        
--- a/README.md
+++ b/README.md
@@ -1,5 +0,0 @@
-![](https://img.shields.io/github/license/juanjqo/sas_robot_driver_franka)![](https://img.shields.io/github/contributors/juanjqo/sas_robot_driver_franka)![](https://img.shields.io/github/last-commit/juanjqo/sas_robot_driver_franka)![](https://img.shields.io/github/last-commit/juanjqo/sas_robot_driver_franka/master)![](https://img.shields.io/badge/status-experimental-red)
-# sas_robot_driver_franka
-
-
-![ezgif com-video-to-gif (1)](https://github.com/SmartArmStack/sas_robot_driver/assets/23158313/b4e1efa7-8d93-4a67-ab87-a74c41d8f4bc)
--- a/cfg/sas_robot_driver_franka_hand_1.yaml
+++ b/cfg/sas_robot_driver_franka_hand_1.yaml
@@ -1,7 +0,0 @@
-"robot_ip_address": "172.16.0.2"
-"robot_mode": "PositionControl"
-"robot_speed": 20.0
-"thread_sampling_time_nsec": 40000000
-"q_min": [0.00]
-"q_max": [0.04]
-
--- a/1
+++ b/1
--- a/package.xml
+++ b/package.xml
@@ -1,84 +0,0 @@
-<?xml version="1.0"?>
-<package format="2">
-  <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>
-
-
-  <!-- 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>
-
-
-  <!-- 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>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_export_depend>roscpp</build_export_depend>
-  <build_export_depend>rospy</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>
-
-
-  <exec_depend>roscpp</exec_depend>
-  <exec_depend>rospy</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>
-
-
-  <!-- The export tag contains other, unspecified, tags -->
-  <export>
-    <!-- Other tools can request additional information be placed here -->
-
-  </export>
-</package>
--- a/sas_robot_driver_franka/.gitignore
+++ b/sas_robot_driver_franka/.gitignore
--- a/sas_robot_driver_franka/CMakeLists.txt
+++ b/sas_robot_driver_franka/CMakeLists.txt
@@ -0,0 +1,299 @@
+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)
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+    add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+
+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(std_srvs REQUIRED)
+find_package(sas_common REQUIRED)
+find_package(sas_core REQUIRED)
+find_package(sas_msgs 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(Franka REQUIRED)
+find_package(pinocchio REQUIRED)
+find_package(sas_robot_driver_franka_interfaces REQUIRED)  # Add this line
+
+## 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)
+
+##### CPP LIBRARY initial #####
+include_directories(
+        include
+        constraints_manager/include
+)
+install(
+        DIRECTORY include/
+        DESTINATION 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 sas_robot_driver_franka_interfaces
+        tf2_ros tf2 sas_robot_driver)
+
+
+##### CPP LIBRARY initial end  #####
+
+##### CPP LIBRARY ROBOT_DYNAMICS #####
+
+add_library(${PROJECT_NAME}_robot_dynamics SHARED
+        src/robot_dynamics/qros_robot_dynamics_client.cpp
+        src/robot_dynamics/qros_robot_dynamics_server.cpp
+)
+
+ament_target_dependencies(${PROJECT_NAME}_robot_dynamics
+        rclcpp geometry_msgs std_msgs
+        sas_common sas_core sas_conversions
+        tf2_ros tf2 sas_robot_driver)
+
+
+target_include_directories(${PROJECT_NAME}_robot_dynamics
+        PUBLIC
+        $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+        $<INSTALL_INTERFACE:include>)
+
+
+target_link_libraries(${PROJECT_NAME}_robot_dynamics
+        -ldqrobotics
+        Eigen3::Eigen
+)
+
+install(
+        TARGETS ${PROJECT_NAME}_robot_dynamics # ${PROJECT_NAME}
+        EXPORT export_${PROJECT_NAME} #export_${PROJECT_NAME}
+        LIBRARY DESTINATION lib
+        #ARCHIVE DESTINATION lib
+        RUNTIME DESTINATION bin
+        INCLUDES DESTINATION include
+)
+
+##END## CPP LIBRARY ROBOT_DYNAMICS #####
+
+##### CPP LIBRARY Motion Generation  #####
+
+add_library(MotionGenerator src/generator/motion_generator.cpp)
+target_link_libraries(MotionGenerator Franka::Franka pinocchio::pinocchio Eigen3::Eigen)
+
+add_library(ConstraintsManager constraints_manager/src/constraints_manager.cpp)
+target_link_libraries(ConstraintsManager Eigen3::Eigen)
+install(
+        DIRECTORY constraints_manager/include/
+        DESTINATION include/constraints_manager
+)
+target_include_directories(ConstraintsManager
+        PUBLIC
+        $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/constraints_manager/include>
+        $<INSTALL_INTERFACE:include/constraints_manager>)
+
+
+
+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)
+
+##### CPP LIBRARY Motion Generation end  #####
+
+
+##### CPP LIBRARY franka low level interface  #####
+
+add_library(robot_interface_franka src/joint/robot_interface_franka.cpp)
+target_link_libraries(robot_interface_franka Franka::Franka
+        pinocchio::pinocchio
+        dqrobotics
+        MotionGenerator
+        ConstraintsManager
+        QuadraticProgramMotionGenerator
+        CustomMotionGeneration)
+
+add_library(robot_interface_hand src/hand/robot_interface_hand.cpp)
+target_link_libraries(robot_interface_hand Franka::Franka
+        pinocchio::pinocchio
+        dqrobotics)
+
+
+##### CPP LIBRARY franka low level interface end  #####
+
+##### SAS Franka Robot Driver #####
+add_library(sas_robot_driver_franka src/joint/sas_robot_driver_franka.cpp)
+target_link_libraries(sas_robot_driver_franka
+        ${PROJECT_NAME}_robot_dynamics
+        dqrobotics
+        dqrobotics-interface-json11
+        robot_interface_franka)
+
+##### SAS Franka Robot Driver end  #####
+##### qros hand effector driver #####
+
+add_library(qros_effector_driver_franka_hand src/hand/qros_effector_driver_franka_hand.cpp)
+
+ament_target_dependencies(qros_effector_driver_franka_hand
+        rclcpp sas_common sas_core std_srvs
+        sas_robot_driver_franka_interfaces
+)
+
+#ament_target_dependencies(${PROJECT_NAME}_robot_dynamics
+#        rclcpp geometry_msgs std_msgs
+#        sas_common sas_core sas_conversions
+#        tf2_ros tf2 sas_robot_driver)
+
+target_link_libraries(qros_effector_driver_franka_hand
+        dqrobotics
+        Franka::Franka
+        pinocchio::pinocchio
+)
+
+#target_include_directories(qros_effector_driver_franka_hand
+#        PUBLIC
+#        $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include/interface>
+#        $<INSTALL_INTERFACE:include/interface>)
+
+##### qros hand effector driver end  #####
+
+
+##### SAS Franka Robot Driver Node #####
+
+add_executable(sas_robot_driver_franka_node src/sas_robot_driver_franka_node.cpp)
+ament_target_dependencies(sas_robot_driver_franka_node
+        rclcpp sas_common sas_core
+        sas_robot_driver)
+target_link_libraries(sas_robot_driver_franka_node sas_robot_driver_franka)
+
+install(TARGETS
+        sas_robot_driver_franka_node
+        DESTINATION lib/${PROJECT_NAME})
+
+
+##### SAS Franka Robot Hand Driver Node #####
+add_executable(sas_robot_driver_franka_hand_node src/sas_robot_driver_franka_hand_node.cpp)
+ament_target_dependencies(sas_robot_driver_franka_node
+        rclcpp sas_common sas_core)
+target_link_libraries(sas_robot_driver_franka_hand_node
+        qros_effector_driver_franka_hand)
+
+install(TARGETS
+        sas_robot_driver_franka_hand_node
+        DESTINATION lib/${PROJECT_NAME})
+
+##### Coppelia Mirror Node #####
+
+add_executable(sas_robot_driver_franka_coppelia_node
+    src/sas_robot_driver_coppelia_node.cpp
+    src/coppelia/sas_robot_driver_coppelia.cpp
+)
+ament_target_dependencies(sas_robot_driver_franka_coppelia_node
+        rclcpp sas_common sas_core sas_conversions sas_robot_driver
+)
+
+target_link_libraries(sas_robot_driver_franka_coppelia_node
+        dqrobotics
+        dqrobotics-interface-json11
+        dqrobotics-interface-vrep
+)
+install(TARGETS
+        sas_robot_driver_franka_coppelia_node
+        DESTINATION lib/${PROJECT_NAME})
+
+##### JuanEmika
+
+add_executable(JuankaEmika
+        qt/configuration_window/main.cpp
+        qt/configuration_window/mainwindow.cpp
+        qt/configuration_window/mainwindow.ui
+)
+ament_target_dependencies(sas_robot_driver_franka_node
+        rclcpp sas_common sas_core
+        sas_robot_driver)
+
+target_link_libraries(JuankaEmika PRIVATE Qt${QT_VERSION_MAJOR}::Widgets
+        dqrobotics
+        robot_interface_franka
+)
+
+if (QT_VERSION_MAJOR EQUAL 6)
+    qt_finalize_executable(JuankaEmika)
+endif ()
+
+install(TARGETS
+        JuankaEmika
+        DESTINATION lib/${PROJECT_NAME})
+
+
+##### PYBIND11 LIBRARY ROBOT_DYNAMICS #####
+ament_python_install_package(${PROJECT_NAME})
+
+pybind11_add_module(_qros_franka_robot_dynamics_py SHARED
+        src/robot_dynamics/qros_robot_dynamics_py.cpp
+        src/robot_dynamics/qros_robot_dynamics_client.cpp
+        src/robot_dynamics/qros_robot_dynamics_server.cpp
+)
+
+target_include_directories(_qros_franka_robot_dynamics_py
+        PUBLIC
+        $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include/robot_dynamics>
+        $<INSTALL_INTERFACE:include/robot_dynamics>)
+
+target_compile_definitions(_qros_franka_robot_dynamics_py PRIVATE IS_SAS_PYTHON_BUILD)
+# https://github.com/pybind/pybind11/issues/387
+target_link_libraries(_qros_franka_robot_dynamics_py PRIVATE ${PROJECT_NAME}_robot_dynamics -ldqrobotics)
+
+install(TARGETS _qros_franka_robot_dynamics_py
+        DESTINATION "${PYTHON_INSTALL_DIR}/${PROJECT_NAME}"
+)
+##END## PYBIND11 LIBRARY ROBOT_DYNAMICS #####
+
+
+install(
+        DIRECTORY include/
+        DESTINATION include
+)
+
+
+
+
+##### LAUNCH FILES #####
+
+install(DIRECTORY
+        launch
+        DESTINATION share/${PROJECT_NAME}/
+)
+
+##END## LAUNCH FILES #####
+
+ament_package()
+
+
+
+
+
+
+
--- a/sas_robot_driver_franka/LICENSE
+++ b/sas_robot_driver_franka/LICENSE
--- a/sas_robot_driver_franka/README.md
+++ b/sas_robot_driver_franka/README.md
@@ -0,0 +1,84 @@
+![](https://img.shields.io/github/license/qlin960618/sas_robot_driver_franka)![](https://img.shields.io/github/contributors/qlin960618/sas_robot_driver_franka)![](https://img.shields.io/github/last-commit/qlin960618/sas_robot_driver_franka)![](https://img.shields.io/github/last-commit/qlin960618/sas_robot_driver_franka/ros2_jazzy)![](https://img.shields.io/badge/status-experimental-red)
+# sas_robot_driver_franka
+![ezgif com-video-to-gif (1)](https://github.com/SmartArmStack/sas_robot_driver/assets/23158313/b4e1efa7-8d93-4a67-ab87-a74c41d8f4bc)
+Robot driver for the Franka Research 3 Robot. 
+
+## Original Contributors
+- [Juan José Quiroz Omaña](https://github.com/juanjqo/sas_robot_driver_franka)
+
+## dependencies
+SAS: 
+- sas_common
+- sas_core
+- sas_msgs
+- sas_conversions
+- sas_robot_driver
+- [sas_robot_driver_franka_interfaces](https://www.github.com/qlin960618/sas_robot_driver_franka_interfaces)
+
+Misc: 
+- geometry_msgs
+- std_msgs
+- std_srvs
+- tf2_ros
+- tf2
+
+Franka Related
+
+- Franka (libfranka)
+- pinocchio
+
+## Components
+
+### sas_robot_driver_franka
+Kernel-space real-time robot driver for the SmartArm compatible control
+
+#### node parameters:
+```yaml
+"robot_ip_address": "172.16.0.x"  # robot ip address
+"thread_sampling_time_sec": 0.004  # control sampling time (1.0/loop_rate)
+"q_min": [-2.3093, -1.5133, -2.4937, -2.7478, -2.4800, 0.8521, -2.6895]  # joint limit minimum, if not define will obtain from [ROBOT_JSON]
+"q_max": [2.3093, 1.5133, 2.4937, -0.4461, 2.4800, 4.2094, 2.6895]  # joint limit maximum, if not define will obtain from [ROBOT_JSON]
+"force_upper_limits_scaling_factor": 4.0  # force and torque error limit scaling factor from default, (if any of the below is not defined)
+"upper_torque_threshold": [40.0, 40.0, 40.0, 40.0, 40.0, 40.0, 40.0]  # joint torque safety threshold, [j1, j2, ..., j7]
+"upper_force_threshold": [40.0, 40.0, 40.0, 40.0, 40.0, 40.0]  # end-effector frame safety force threshold [x, y, z, rx, ry, rz] 
+"robot_mode": "VelocityControl"  # control mode [VelocityControl/PositionControl]: currently PositionControl can be unstable 
+"robot_parameter_file_path": os.environ["ROBOT_3_JSON_PATH"]  # Robot definition JSON path
+```
+
+### sas_robot_driver_franka_hand
+Franka Hand driver for basic control of gripping functionality.
+
+#### special dependencies
+- sas_robot_driver_franka_interfaces
+  - <sas_robot_driver_franka_interfaces/srv/grasp.hpp>
+  - <sas_robot_driver_franka_interfaces/srv/move.hpp>
+  - <sas_robot_driver_franka_interfaces/msg/gripper_state.hpp>
+
+#### node parameters:
+```yaml
+"robot_ip_address": "172.16.0.x"  # robot ip address
+"thread_sampling_time_sec": 0.02  # control sampling time, (1.0/status update rate)
+"default_force": 2.0  # default gripping force (overridable from service call)
+"default_speed": 0.07  # default gripping speed (overridable from service call)
+"default_epsilon_inner": 0.007  # default grip call position epsilon (overridable from service call)
+"default_epsilon_outer": 0.007  # default grip call position epsilon (overridable from service call)
+```
+
+
+### sas_robot_driver_coppelia_node
+Digital twin mirroring node for driver node to CoppeliaSim
+
+
+#### node parameters:
+```yaml
+"thread_sampling_time_sec": 0.008  # control sampling time (1.0/loop_rate)
+"vrep_ip": os.environ["VREP_IP"]  # ip of the vrep computer
+"vrep_port": 20012  # vrep port
+"vrep_dynamically_enabled": True  # if vrep scene is dynamically enabled
+"using_real_robot": True  # if node should read-only from real robot driver, False for simulation mode
+"vrep_joint_names": ["Franka_joint1#1", "Franka_joint2#1", "Franka_joint3#1", "Franka_joint4#1",
+                     "Franka_joint5#1", "Franka_joint6#1", "Franka_joint7#1"]  # coppelia scene joint names
+"robot_topic_prefix": "/arm3"  # robot driver namespace
+"robot_mode": "PositionControl"  # only use when in simulation node, aka using_real_robot==False, for simulating velocity control mode. (TODO: currently VelocityControl is not stable)
+"robot_parameter_file_path": os.environ["ROBOT_3_JSON_PATH"]  # Robot definition JSON path
+```
--- a/sas_robot_driver_franka/cfg/sas_operator_side_receiver.yaml
+++ b/sas_robot_driver_franka/cfg/sas_operator_side_receiver.yaml
--- a/sas_robot_driver_franka/cfg/sas_patient_side_manager.yaml
+++ b/sas_robot_driver_franka/cfg/sas_patient_side_manager.yaml
--- a/sas_robot_driver_franka/cfg/sas_robot_driver_coppelia_franka_1.yaml
+++ b/sas_robot_driver_franka/cfg/sas_robot_driver_coppelia_franka_1.yaml
--- a/sas_robot_driver_franka/cfg/sas_robot_driver_franka_1.yaml
+++ b/sas_robot_driver_franka/cfg/sas_robot_driver_franka_1.yaml
--- a/sas_robot_driver_franka/cfg/sas_robot_driver_franka_hand_1.yaml
+++ b/sas_robot_driver_franka/cfg/sas_robot_driver_franka_hand_1.yaml
@@ -0,0 +1,3 @@
+"robot_ip_address": "172.16.0.2"
+"thread_sampling_time_nsec": 20000000  # 20ms , 50Hz
+
--- a/sas_robot_driver_franka/cfg/sas_robot_driver_mirror_coppelia_franka_1.yaml
+++ b/sas_robot_driver_franka/cfg/sas_robot_driver_mirror_coppelia_franka_1.yaml
--- a/sas_robot_driver_franka/constraints_manager/LICENSE
+++ b/sas_robot_driver_franka/constraints_manager/LICENSE
@@ -0,0 +1,165 @@
+                   GNU LESSER GENERAL PUBLIC LICENSE
+                       Version 3, 29 June 2007
+
+ Copyright (C) 2007 Free Software Foundation, Inc. <https://fsf.org/>
+ Everyone is permitted to copy and distribute verbatim copies
+ of this license document, but changing it is not allowed.
+
+
+  This version of the GNU Lesser General Public License incorporates
+the terms and conditions of version 3 of the GNU General Public
+License, supplemented by the additional permissions listed below.
+
+  0. Additional Definitions.
+
+  As used herein, "this License" refers to version 3 of the GNU Lesser
+General Public License, and the "GNU GPL" refers to version 3 of the GNU
+General Public License.
+
+  "The Library" refers to a covered work governed by this License,
+other than an Application or a Combined Work as defined below.
+
+  An "Application" is any work that makes use of an interface provided
+by the Library, but which is not otherwise based on the Library.
+Defining a subclass of a class defined by the Library is deemed a mode
+of using an interface provided by the Library.
+
+  A "Combined Work" is a work produced by combining or linking an
+Application with the Library.  The particular version of the Library
+with which the Combined Work was made is also called the "Linked
+Version".
+
+  The "Minimal Corresponding Source" for a Combined Work means the
+Corresponding Source for the Combined Work, excluding any source code
+for portions of the Combined Work that, considered in isolation, are
+based on the Application, and not on the Linked Version.
+
+  The "Corresponding Application Code" for a Combined Work means the
+object code and/or source code for the Application, including any data
+and utility programs needed for reproducing the Combined Work from the
+Application, but excluding the System Libraries of the Combined Work.
+
+  1. Exception to Section 3 of the GNU GPL.
+
+  You may convey a covered work under sections 3 and 4 of this License
+without being bound by section 3 of the GNU GPL.
+
+  2. Conveying Modified Versions.
+
+  If you modify a copy of the Library, and, in your modifications, a
+facility refers to a function or data to be supplied by an Application
+that uses the facility (other than as an argument passed when the
+facility is invoked), then you may convey a copy of the modified
+version:
+
+   a) under this License, provided that you make a good faith effort to
+   ensure that, in the event an Application does not supply the
+   function or data, the facility still operates, and performs
+   whatever part of its purpose remains meaningful, or
+
+   b) under the GNU GPL, with none of the additional permissions of
+   this License applicable to that copy.
+
+  3. Object Code Incorporating Material from Library Header Files.
+
+  The object code form of an Application may incorporate material from
+a header file that is part of the Library.  You may convey such object
+code under terms of your choice, provided that, if the incorporated
+material is not limited to numerical parameters, data structure
+layouts and accessors, or small macros, inline functions and templates
+(ten or fewer lines in length), you do both of the following:
+
+   a) Give prominent notice with each copy of the object code that the
+   Library is used in it and that the Library and its use are
+   covered by this License.
+
+   b) Accompany the object code with a copy of the GNU GPL and this license
+   document.
+
+  4. Combined Works.
+
+  You may convey a Combined Work under terms of your choice that,
+taken together, effectively do not restrict modification of the
+portions of the Library contained in the Combined Work and reverse
+engineering for debugging such modifications, if you also do each of
+the following:
+
+   a) Give prominent notice with each copy of the Combined Work that
+   the Library is used in it and that the Library and its use are
+   covered by this License.
+
+   b) Accompany the Combined Work with a copy of the GNU GPL and this license
+   document.
+
+   c) For a Combined Work that displays copyright notices during
+   execution, include the copyright notice for the Library among
+   these notices, as well as a reference directing the user to the
+   copies of the GNU GPL and this license document.
+
+   d) Do one of the following:
+
+       0) Convey the Minimal Corresponding Source under the terms of this
+       License, and the Corresponding Application Code in a form
+       suitable for, and under terms that permit, the user to
+       recombine or relink the Application with a modified version of
+       the Linked Version to produce a modified Combined Work, in the
+       manner specified by section 6 of the GNU GPL for conveying
+       Corresponding Source.
+
+       1) Use a suitable shared library mechanism for linking with the
+       Library.  A suitable mechanism is one that (a) uses at run time
+       a copy of the Library already present on the user's computer
+       system, and (b) will operate properly with a modified version
+       of the Library that is interface-compatible with the Linked
+       Version.
+
+   e) Provide Installation Information, but only if you would otherwise
+   be required to provide such information under section 6 of the
+   GNU GPL, and only to the extent that such information is
+   necessary to install and execute a modified version of the
+   Combined Work produced by recombining or relinking the
+   Application with a modified version of the Linked Version. (If
+   you use option 4d0, the Installation Information must accompany
+   the Minimal Corresponding Source and Corresponding Application
+   Code. If you use option 4d1, you must provide the Installation
+   Information in the manner specified by section 6 of the GNU GPL
+   for conveying Corresponding Source.)
+
+  5. Combined Libraries.
+
+  You may place library facilities that are a work based on the
+Library side by side in a single library together with other library
+facilities that are not Applications and are not covered by this
+License, and convey such a combined library under terms of your
+choice, if you do both of the following:
+
+   a) Accompany the combined library with a copy of the same work based
+   on the Library, uncombined with any other library facilities,
+   conveyed under the terms of this License.
+
+   b) Give prominent notice with the combined library that part of it
+   is a work based on the Library, and explaining where to find the
+   accompanying uncombined form of the same work.
+
+  6. Revised Versions of the GNU Lesser General Public License.
+
+  The Free Software Foundation may publish revised and/or new versions
+of the GNU Lesser General Public License from time to time. Such new
+versions will be similar in spirit to the present version, but may
+differ in detail to address new problems or concerns.
+
+  Each version is given a distinguishing version number. If the
+Library as you received it specifies that a certain numbered version
+of the GNU Lesser General Public License "or any later version"
+applies to it, you have the option of following the terms and
+conditions either of that published version or of any later version
+published by the Free Software Foundation. If the Library as you
+received it does not specify a version number of the GNU Lesser
+General Public License, you may choose any version of the GNU Lesser
+General Public License ever published by the Free Software Foundation.
+
+  If the Library as you received it specifies that a proxy can decide
+whether future versions of the GNU Lesser General Public License shall
+apply, that proxy's public statement of acceptance of any version is
+permanent authorization for you to choose that version for the
+Library.
--- a/sas_robot_driver_franka/constraints_manager/README.md
+++ b/sas_robot_driver_franka/constraints_manager/README.md
@@ -0,0 +1,2 @@
+![](https://img.shields.io/github/license/juanjqo/constraints_manager)![](https://img.shields.io/github/contributors/juanjqo/constraints_manager)![](https://img.shields.io/github/last-commit/juanjqo/constraints_manager/main)![](https://img.shields.io/github/commit-activity/t/juanjqo/constraints_manager/main)
+# constraints_manager
--- a/sas_robot_driver_franka/constraints_manager/constraints_manager_example/CMakeLists.txt
+++ b/sas_robot_driver_franka/constraints_manager/constraints_manager_example/CMakeLists.txt
@@ -0,0 +1,22 @@
+cmake_minimum_required(VERSION 3.5)
+
+project(constraints_manager_example LANGUAGES CXX)
+
+set(CMAKE_CXX_STANDARD 17)
+set(CMAKE_CXX_STANDARD_REQUIRED ON)
+
+FIND_PACKAGE(Eigen3 REQUIRED)
+INCLUDE_DIRECTORIES(${EIGEN3_INCLUDE_DIR})
+ADD_COMPILE_OPTIONS(-Werror=return-type -Wall -Wextra -Wmissing-declarations -Wredundant-decls -Woverloaded-virtual)
+
+include_directories(
+    ${CMAKE_CURRENT_SOURCE_DIR}/../include
+
+    )
+
+
+add_library(ConstraintsManager ${CMAKE_CURRENT_SOURCE_DIR}/../src/constraints_manager.cpp)
+add_executable(constraints_manager_example main.cpp)
+target_link_libraries(constraints_manager_example
+                      ConstraintsManager)
+
--- a/sas_robot_driver_franka/constraints_manager/constraints_manager_example/main.cpp
+++ b/sas_robot_driver_franka/constraints_manager/constraints_manager_example/main.cpp
@@ -0,0 +1,26 @@
+#include <iostream>
+#include "constraints_manager.h"
+
+using namespace Eigen;
+
+int main()
+{
+    auto manager = ConstraintsManager(3);
+    auto A1 = MatrixXd::Zero(3,3);
+    auto b1 = VectorXd::Zero(3);
+    auto A2 = MatrixXd::Ones(1,3);
+    auto b2 = VectorXd::Ones(1);
+
+    manager.add_inequality_constraint(A1, b1);
+    manager.add_inequality_constraint(A2, b2);
+
+    MatrixXd A;
+    VectorXd b;
+
+    std::tie(A,b) = manager.get_inequality_constraints();
+    std::cout<<"A: "<<std::endl;
+    std::cout<<A<<std::endl;
+    std::cout<<"b: "<<std::endl;
+    std::cout<<b<<std::endl;
+
+}
--- a/sas_robot_driver_franka/constraints_manager/include/constraints_manager.h
+++ b/sas_robot_driver_franka/constraints_manager/include/constraints_manager.h
@@ -0,0 +1,67 @@
+/*
+# Copyright (c) 2023 Juan Jose Quiroz Omana
+#
+#    constraints_manager is free software: you can redistribute it and/or modify
+#    it under the terms of the GNU Lesser General Public License as published by
+#    the Free Software Foundation, either version 3 of the License, or
+#    (at your option) any later version.
+#
+#    constraints_manager is distributed in the hope that it will be useful,
+#    but WITHOUT ANY WARRANTY; without even the implied warranty of
+#    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+#    GNU Lesser General Public License for more details.
+#
+#    You should have received a copy of the GNU Lesser General Public License
+#    along with constraints_manager.  If not, see <https://www.gnu.org/licenses/>.
+#
+# ################################################################
+#
+#   Author: Juan Jose Quiroz Omana, email: juanjqogm@gmail.com
+#
+# ################################################################*/
+
+#pragma once
+#include <Eigen/Dense>
+#include <vector>
+
+
+using namespace Eigen;
+
+class ConstraintsManager
+{
+protected:
+    int dim_configuration_;
+
+    VectorXd q_dot_min_ = VectorXd::Zero(0);
+    VectorXd q_dot_max_ = VectorXd::Zero(0);
+    VectorXd q_min_ = VectorXd::Zero(0);
+    VectorXd q_max_ = VectorXd::Zero(0);
+
+    MatrixXd equality_constraint_matrix_ = MatrixXd::Zero(0,0);
+    VectorXd equality_constraint_vector_ = VectorXd::Zero(0);
+    MatrixXd inequality_constraint_matrix_ = MatrixXd::Zero(0,0);
+    VectorXd inequality_constraint_vector_ = VectorXd::Zero(0);
+
+    MatrixXd _raw_add_matrix_constraint(const MatrixXd& A0, const MatrixXd& A);
+    VectorXd _raw_add_vector_constraint(const VectorXd& b0, const VectorXd& b);
+    void _check_matrix_and_vector_sizes(const MatrixXd& A, const VectorXd& b);
+
+    void _check_vectors_size(const VectorXd& q1, const VectorXd& q2, const std::string &msg);
+    void _check_vector_initialization(const VectorXd& q, const std::string &msg);
+    MatrixXd _create_matrix(const MatrixXd& A);
+
+public:
+    ConstraintsManager() = delete;
+    ConstraintsManager(const int& dim_configuration);
+
+    void add_equality_constraint(const MatrixXd& A, const VectorXd& b);
+    void add_inequality_constraint(const MatrixXd& A, const VectorXd& b);
+
+    std::tuple<MatrixXd, VectorXd> get_equality_constraints();
+    std::tuple<MatrixXd, VectorXd> get_inequality_constraints();
+
+    void set_joint_position_limits(const VectorXd& q_lower_bound, const VectorXd& q_upper_bound);
+    void set_joint_velocity_limits(const VectorXd& q_dot_lower_bound, const VectorXd& q_dot_upper_bound);
+};
+
+
--- a/sas_robot_driver_franka/constraints_manager/src/constraints_manager.cpp
+++ b/sas_robot_driver_franka/constraints_manager/src/constraints_manager.cpp
@@ -0,0 +1,259 @@
+/*
+# Copyright (c) 2023 Juan Jose Quiroz Omana
+#
+#    constraints_manager is free software: you can redistribute it and/or modify
+#    it under the terms of the GNU Lesser General Public License as published by
+#    the Free Software Foundation, either version 3 of the License, or
+#    (at your option) any later version.
+#
+#    constraints_manager is distributed in the hope that it will be useful,
+#    but WITHOUT ANY WARRANTY; without even the implied warranty of
+#    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+#    GNU Lesser General Public License for more details.
+#
+#    You should have received a copy of the GNU Lesser General Public License
+#    along with constraints_manager.  If not, see <https://www.gnu.org/licenses/>.
+#
+# ################################################################
+#
+#   Author: Juan Jose Quiroz Omana, email: juanjqogm@gmail.com
+#
+# ################################################################*/
+
+
+#include "constraints_manager.h"
+
+/**
+ * @brief Constructor
+ * @param int dim_configuration Dimension of the robot configuration
+ */
+ConstraintsManager::ConstraintsManager(const int& dim_configuration):dim_configuration_(dim_configuration)
+{
+
+}
+
+/**
+ * @brief The method _create_matrix returns a matrix containing the matrix A
+ *        and taking into acount the dimension of the robot configuration. If matrix A has lower columns than
+ *        dim_configuration then _create_matrix completes the correct size with zeros.
+ * @param MatrixXd A Input matrix
+ * @return
+ */
+MatrixXd ConstraintsManager::_create_matrix(const MatrixXd& A)
+{
+    MatrixXd constraint_matrix = MatrixXd::Zero(A.rows(), dim_configuration_);
+    constraint_matrix.block(0,0, A.rows(), A.cols()) = A;
+    return constraint_matrix;
+}
+
+
+/**
+ * @brief The method _check_matrix_and_vector_sizes(A,b) check the if the rows of Matrix A
+ * has the same dimension of Vector b.
+ * @param MatrixXd A
+ * @param VectorXd b
+ */
+void ConstraintsManager::_check_matrix_and_vector_sizes(const MatrixXd& A, const VectorXd& b)
+{
+    int m = A.rows();
+    int n = A.cols();
+    int nb = b.size();
+    if (m != nb)
+    {
+        throw std::runtime_error(std::string("Incompatible sizes. The rows of Matrix A must have the same dimension of Vector b. ")
+                               + std::string("But you used A ")+ std::to_string(m)+ std::string("x")+ std::to_string(n)
+                               + std::string(" and b ")+ std::to_string(nb) + std::string("x1"));
+    }
+    if (n != dim_configuration_)
+    {
+        throw std::runtime_error(std::string("Incompatible sizes. The cols of Matrix A must have dimension ") + std::to_string(dim_configuration_)
+                                 + std::string(". But you used A ")+ std::to_string(m)+ std::string("x")+ std::to_string(n)
+                                 + std::string(" and b ")+ std::to_string(nb) + std::string("x1"));
+    }
+
+}
+
+/**
+ * @brief ConstraintsManager::_check_vectors_size() checks the dimensions of two vectors.
+ * @param VectorXd q1 First vector to be checked.
+ * @param VectorXd q2 Second vector to be checked.
+ * @param std::string msg Desired error message if vectors q1 and q2 have different size.
+ */
+void ConstraintsManager::_check_vectors_size(const VectorXd& q1, const VectorXd& q2, const std::string &msg)
+{
+    if (q1.size() != q2.size() )
+    {
+       throw std::runtime_error(msg);
+    }
+}
+
+/**
+ * @brief ConstraintsManager::_check_vector_initialization()
+ * @param VectorXd q
+ * @param std::string msg
+ */
+void ConstraintsManager::_check_vector_initialization(const VectorXd& q, const std::string &msg)
+{
+    if (q.size() == 0)
+    {
+        throw std::runtime_error(msg);
+    }
+}
+
+
+/**
+ * @brief The method _raw_add_matrix_constraint(A0, A) returns a constraint_matrix, which is given as
+ *         constraint_matrix = [A0
+ *                              A]
+ * @param MatrixXd A0
+ * @param MatrixXd A
+ * @return MatrixXd constraint_matrix
+ */
+MatrixXd ConstraintsManager::_raw_add_matrix_constraint(const MatrixXd& A0, const MatrixXd& A)
+{
+        int m = A.rows();
+        int n = A.cols();
+        int m0 = A0.rows();
+        int n0 = A0.cols();
+        MatrixXd constraint_matrix = MatrixXd::Zero(m0+m, dim_configuration_);
+        if(n != n0)
+        {
+            throw std::runtime_error(std::string("Incompatible sizes. The equality matrix must be ")
+                                   + std::string("m x ")+ std::to_string(n0)
+                                   + std::string(". But you used m x ")+ std::to_string(n));
+        }
+        constraint_matrix.block(0,0, m0, n0) = A0;
+        constraint_matrix.block(m0,0, m, n) = A;
+        return constraint_matrix;
+}
+
+
+/**
+ * @brief The method _raw_add_vector_constraint(b0, b) returns the vector constraint_vector,
+ *        which is given as
+ *        constraint_vector = [b0
+ *                             b];
+ * @param VectorXd b0
+ * @param VectorXd b
+ * @return VectorXd constraint_vector
+ */
+VectorXd ConstraintsManager::_raw_add_vector_constraint(const VectorXd& b0, const VectorXd& b)
+{
+    int nb = b.size();
+    int nb0 = b0.size();
+    VectorXd constraint_vector =  VectorXd::Zero(nb0+nb);
+    constraint_vector.head(nb0) = b0;
+    constraint_vector.segment(nb0, nb) = b;
+    return constraint_vector;
+}
+
+
+/**
+ * @brief The method add_equality_constraint(A, b) adds the equality constraint A*x = b
+ *        to the set of equality constraints.
+ *
+ * @param MatrixXd A
+ * @param VectorXd b
+ */
+void ConstraintsManager::add_equality_constraint(const MatrixXd& A, const VectorXd& b)
+{
+    _check_matrix_and_vector_sizes(A, b);
+
+    if (equality_constraint_matrix_.size() == 0)
+    {
+        equality_constraint_matrix_ = _create_matrix(A);
+        equality_constraint_vector_ = b;
+    }else
+    {
+        equality_constraint_matrix_ = _raw_add_matrix_constraint(equality_constraint_matrix_, A);
+        equality_constraint_vector_ = _raw_add_vector_constraint(equality_constraint_vector_, b);
+    }
+}
+
+
+
+/**
+ * @brief The method add_inequality_constraint(A, b) adds the inequality constraint A*x <= b
+ *        to the set of ineequality constraints.
+ * @param MatrixXd A
+ * @param VectorXd b
+ */
+void ConstraintsManager::add_inequality_constraint(const MatrixXd& A, const VectorXd& b)
+{
+    _check_matrix_and_vector_sizes(A, b);
+
+    if (inequality_constraint_matrix_.size() == 0)
+    {
+        inequality_constraint_matrix_ = _create_matrix(A);
+        inequality_constraint_vector_ = b;
+    }else
+    {
+        inequality_constraint_matrix_ = _raw_add_matrix_constraint(inequality_constraint_matrix_, A);
+        inequality_constraint_vector_ = _raw_add_vector_constraint(inequality_constraint_vector_, b);
+    }
+}
+
+
+/**
+ * @brief The method get_equality_constraints() returns the set of equality constraints
+ *        composed of the Matrix A and the vector b, where
+ *        A*x = b.
+ *        Warning: This method deletes all the equality constraints stored.
+ * @return std::tuple<MatrixXd A, VectorXd b>
+ */
+std::tuple<MatrixXd, VectorXd> ConstraintsManager::get_equality_constraints()
+{
+    MatrixXd A = equality_constraint_matrix_;
+    equality_constraint_matrix_ = MatrixXd::Zero(0,0);
+    return std::make_tuple(A, equality_constraint_vector_);
+}
+
+
+/**
+ * @brief The method get_inequality_constraints() returns the set of inequality constraints
+ *        composed of the Matrix A and the vector b, where
+ *        A*x <= b
+ *        Warning: This method deletes all the inequality constraints stored.
+ * @return
+ */
+std::tuple<MatrixXd, VectorXd> ConstraintsManager::get_inequality_constraints()
+{
+    MatrixXd A = inequality_constraint_matrix_;
+    inequality_constraint_matrix_ = MatrixXd::Zero(0,0);
+    return std::make_tuple(A, inequality_constraint_vector_);
+}
+
+
+/**
+ * @brief ConstraintsManager::set_joint_position_limits
+ * @param VectorXd q_lower_bound
+ * @param VectorXd q_upper_bound
+ */
+void ConstraintsManager::set_joint_position_limits(const VectorXd& q_lower_bound, const VectorXd& q_upper_bound)
+{
+    _check_vectors_size(q_lower_bound, q_upper_bound,
+                          std::string("The sizes are incompatibles. q_lower_bound has size ") + std::to_string(q_lower_bound.size())
+                          + std::string(" and q_upper_bound has size ") + std::to_string(q_upper_bound.size()));
+    _check_vectors_size(q_lower_bound, VectorXd::Zero(dim_configuration_),
+                          std::string("The sizes are incompatibles. The joint limits have size ") + std::to_string(q_lower_bound.size())
+                          + std::string(" and the robot configuration has size ") + std::to_string(dim_configuration_));
+    q_min_ = q_lower_bound;
+    q_max_ = q_upper_bound;
+}
+
+/**
+ * @brief ConstraintsManager::set_joint_velocity_limits
+ * @param q_dot_lower_bound
+ * @param q_dot_upper_bound
+ */
+void ConstraintsManager::set_joint_velocity_limits(const VectorXd& q_dot_lower_bound, const VectorXd& q_dot_upper_bound)
+{
+    _check_vectors_size(q_dot_lower_bound, q_dot_upper_bound,
+                          std::string("The sizes are incompatibles. q_dot_lower_bound has size ") + std::to_string(q_dot_lower_bound.size())
+                          + std::string(" and q_dot_upper_bound has size ") + std::to_string(q_dot_upper_bound.size()));
+    _check_vectors_size(q_dot_lower_bound, VectorXd::Zero(dim_configuration_),
+                          std::string("The sizes are incompatibles. The joint limits have size ") + std::to_string(q_dot_lower_bound.size())
+                          + std::string(" and the robot configuration has size ") + std::to_string(dim_configuration_));
+    q_dot_min_ = q_dot_lower_bound;
+    q_dot_max_ = q_dot_upper_bound;
+}
--- a/sas_robot_driver_franka/include/sas_robot_driver_franka/coppelia/sas_robot_driver_coppelia.hpp
+++ b/sas_robot_driver_franka/include/sas_robot_driver_franka/coppelia/sas_robot_driver_coppelia.hpp
@@ -0,0 +1,139 @@
+/*
+# Copyright (c) 2023 Juan Jose Quiroz Omana
+#
+#    This file is part of sas_robot_driver_franka.
+#
+#    sas_robot_driver_franka is free software: you can redistribute it and/or modify
+#    it under the terms of the GNU Lesser General Public License as published by
+#    the Free Software Foundation, either version 3 of the License, or
+#    (at your option) any later version.
+#
+#    sas_robot_driver_franka is distributed in the hope that it will be useful,
+#    but WITHOUT ANY WARRANTY; without even the implied warranty of
+#    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+#    GNU Lesser General Public License for more details.
+#
+#    You should have received a copy of the GNU Lesser General Public License
+#    along with sas_robot_driver.  If not, see <https://www.gnu.org/licenses/>.
+#
+# ################################################################
+#
+#   Author: Juan Jose Quiroz Omana, email: juanjqogm@gmail.com
+#
+# ################################################################
+#
+# Contributors:
+#      1. Juan Jose Quiroz Omana (juanjqogm@gmail.com)
+#         - Adapted from sas_robot_driver_denso.cpp
+#           (https://github.com/SmartArmStack/sas_robot_driver_denso/blob/master/src/sas_robot_driver_denso.cpp)
+#
+# ################################################################
+*/
+
+
+#pragma once
+#include <exception>
+#include <tuple>
+#include <atomic>
+#include <vector>
+#include <memory>
+#include <dqrobotics/DQ.h>
+#include <sas_core/sas_clock.hpp>
+// #include <dqrobotics/interfaces/vrep/DQ_VrepRobot.h>
+#include <dqrobotics/interfaces/vrep/DQ_VrepInterface.h>
+#include <dqrobotics/interfaces/json11/DQ_JsonReader.h>
+// #include <dqrobotics/robot_modeling/DQ_SerialManipulatorDH.h>
+#include <thread>
+#include <atomic>
+#include <sas_robot_driver/sas_robot_driver_server.hpp>
+#include <sas_robot_driver/sas_robot_driver_client.hpp>
+
+#define VIRTUAL_ROBOT_SIMULATION_SAMPLING_TIME_SEC 0.002  // 2ms, 500Hz
+#define REAL_ROBOT_INTERFACE_INIT_TIMEOUT_COUNT 500
+using namespace DQ_robotics;
+using namespace Eigen;
+
+namespace qros
+{
+
+
+
+struct RobotDriverCoppeliaConfiguration
+{
+
+    double thread_sampling_time_sec;  // frontend vrep update rate
+    int vrep_port;
+    std::string vrep_ip;
+    std::vector<std::string> vrep_joint_names;
+    bool vrep_dynamically_enabled = false;
+    std::string robot_mode;
+    bool using_real_robot;
+    std::string robot_topic_prefix;
+    std::string robot_parameter_file_path;
+    // VectorXd q_min;
+    // VectorXd q_max;
+};
+
+class RobotDriverCoppelia
+{
+private:
+    enum ControlMode{
+        Position=0,
+        Velocity
+    };
+
+    RobotDriverCoppeliaConfiguration configuration_;
+    std::shared_ptr<Node> node_sptr_;
+
+    sas::Clock clock_;
+    std::atomic_bool* break_loops_;
+    bool _should_shutdown() const {return (*break_loops_);}
+    ControlMode robot_mode_;
+
+    std::shared_ptr<DQ_VrepInterface> vi_;
+    std::shared_ptr<sas::RobotDriverClient> real_robot_interface_ = nullptr;
+    std::shared_ptr<sas::RobotDriverServer> robot_provider_ = nullptr;
+
+    // backend thread for simulaton
+    /**
+     * Current simulation mechanism is not accounting for any robot dynamics, just the joint limits
+     */
+    std::thread velocity_control_simulation_thread_;
+    VectorXd simulated_joint_positions_;
+    VectorXd simulated_joint_velocities_;
+    void start_simulation_thread();  // thread entry point
+    void _velocity_control_simulation_thread_main();
+    std::atomic_bool simulation_thread_started_{false};
+
+    bool initialized_ = false;
+    inline void _assert_initialized() const{
+        if (!initialized_){throw std::runtime_error("[RobotDriverCoppelia] Robot driver not initialized");}
+    }
+    inline void _assert_is_alive() const{
+        if(!configuration_.using_real_robot && !simulation_thread_started_){throw std::runtime_error("[RobotDriverCoppelia] Robot Simulation is not alive");}
+    }
+
+    void _start_control_loop();
+protected:
+    inline void _update_vrep_position(const VectorXd &joint_positions, const bool& force_update=false) const;
+    inline void _update_vrep_velocity(const VectorXd & joint_velocity) const;
+
+public:
+    RobotDriverCoppelia(const RobotDriverCoppelia&)=delete;
+    RobotDriverCoppelia()=delete;
+    ~RobotDriverCoppelia();
+
+    RobotDriverCoppelia(const std::shared_ptr<Node> &node_sptr, const RobotDriverCoppeliaConfiguration& configuration, std::atomic_bool* break_loops);
+
+    int start_control_loop();  // public entry point
+
+    void connect();
+    void disconnect();
+
+    void initialize();
+    void deinitialize();
+
+    std::tuple<VectorXd, VectorXd> joint_limits_;
+
+};
+}
--- a/sas_robot_driver_franka/include/sas_robot_driver_franka/deprecated/robot_coppelia_ros_interface.h
+++ b/sas_robot_driver_franka/include/sas_robot_driver_franka/deprecated/robot_coppelia_ros_interface.h
--- a/sas_robot_driver_franka/include/sas_robot_driver_franka/generator/custom_motion_generation.h
+++ b/sas_robot_driver_franka/include/sas_robot_driver_franka/generator/custom_motion_generation.h
@@ -32,13 +32,13 @@

 #pragma once
 #include <array>
-#include <Eigen/Core>
-#include <Eigen/Dense>
+#include <eigen3/Eigen/Core>
+#include <eigen3/Eigen/Dense>
 #include <vector>
 #include <iostream>
 #include <dqrobotics/solvers/DQ_QPOASESSolver.h>
 #include <memory>
-#include "constraints_manager.h"
+#include <constraints_manager.h>


 using namespace DQ_robotics;
--- a/sas_robot_driver_franka/include/sas_robot_driver_franka/generator/motion_generator.h
+++ b/sas_robot_driver_franka/include/sas_robot_driver_franka/generator/motion_generator.h
@@ -4,8 +4,8 @@

 #include <array>

-#include <Eigen/Core>
-#include <Eigen/Dense>
+#include <eigen3/Eigen/Core>
+#include <eigen3/Eigen/Dense>
 #include <franka/control_types.h>
 #include <franka/duration.h>
 #include <franka/robot.h>
--- a/sas_robot_driver_franka/include/sas_robot_driver_franka/generator/quadratic_program_motion_generator.h
+++ b/sas_robot_driver_franka/include/sas_robot_driver_franka/generator/quadratic_program_motion_generator.h
@@ -1,7 +1,7 @@
 #pragma once
 #include <array>
-#include <Eigen/Core>
-#include <Eigen/Dense>
+#include <eigen3/Eigen/Core>
+#include <eigen3/Eigen/Dense>
 #include <vector>
 #include <iostream>
 #include <dqrobotics/solvers/DQ_QPOASESSolver.h>
--- a/sas_robot_driver_franka/include/sas_robot_driver_franka/interfaces/qros_effector_driver_franka_hand.hpp
+++ b/sas_robot_driver_franka/include/sas_robot_driver_franka/interfaces/qros_effector_driver_franka_hand.hpp
@@ -0,0 +1,145 @@
+#pragma once
+/*
+# Copyright (c) 2023 Juan Jose Quiroz Omana
+#
+#    This file is part of sas_robot_driver_franka.
+#
+#    sas_robot_driver_franka is free software: you can redistribute it and/or modify
+#    it under the terms of the GNU Lesser General Public License as published by
+#    the Free Software Foundation, either version 3 of the License, or
+#    (at your option) any later version.
+#
+#    sas_robot_driver_franka is distributed in the hope that it will be useful,
+#    but WITHOUT ANY WARRANTY; without even the implied warranty of
+#    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+#    GNU Lesser General Public License for more details.
+#
+#    You should have received a copy of the GNU Lesser General Public License
+#    along with sas_robot_driver.  If not, see <https://www.gnu.org/licenses/>.
+#
+# ################################################################
+#
+#   Author: Juan Jose Quiroz Omana, email: juanjqogm@gmail.com
+#
+# ################################################################
+#
+# Contributors:
+#      1. Quenitin Lin
+#
+# ################################################################
+*/
+#include <exception>
+#include <tuple>
+#include <atomic>
+#include <vector>
+#include <memory>
+#include <franka/robot.h>
+#include <franka/gripper.h>
+#include <thread>
+#include <mutex>
+
+// #define BLOCK_READ_IN_USED
+// #define IN_TESTING
+
+// #include <sas_robot_driver/sas_robot_driver.h>
+#include <sas_core/sas_clock.hpp>
+
+#include <rclcpp/service.hpp>
+#include <rclcpp/rclcpp.hpp>
+
+#ifdef IN_TESTING
+#include "local/srv/grasp.hpp"
+#include "local/srv/move.hpp"
+#include "local/msg/gripper_state.hpp"
+#else
+#include <sas_robot_driver_franka_interfaces/srv/grasp.hpp>
+#include <sas_robot_driver_franka_interfaces/srv/move.hpp>
+#include <sas_robot_driver_franka_interfaces/msg/gripper_state.hpp>
+#endif
+#include <std_srvs/srv/trigger.hpp>
+
+// using namespace DQ_robotics;
+// using namespace Eigen;
+using namespace rclcpp;
+using namespace sas_robot_driver_franka_interfaces;
+
+
+namespace qros {
+
+struct EffectorDriverFrankaHandConfiguration
+{
+    std::string robot_ip;
+    bool initialize_with_homing = true;
+    double thread_sampeling_time_s = 1e8;  // 10Hz
+    double default_force = 3.0;
+    double default_speed = 0.1;
+    double default_epsilon_inner = 0.005;
+    double default_epsilon_outer = 0.005;
+};
+
+class EffectorDriverFrankaHand{
+private:
+    std::string driver_node_prefix_;
+    EffectorDriverFrankaHandConfiguration configuration_;
+    std::shared_ptr<Node> node_;
+
+    std::shared_ptr<franka::Gripper> gripper_sptr_;
+
+
+    std::atomic_bool* break_loops_;
+
+    bool _is_connected() const;
+
+    // thread specific functions
+    void _gripper_status_loop();
+    std::thread status_loop_thread_;
+    std::atomic_bool status_loop_running_{false};
+
+    Publisher<msg::GripperState>::SharedPtr gripper_status_publisher_;
+
+    std::mutex gripper_in_use_;
+    Service<srv::Grasp>::SharedPtr grasp_srv_;
+    Service<srv::Move>::SharedPtr move_srv_;
+    Service<std_srvs::srv::Trigger>::SharedPtr homing_srv_;
+
+public:
+
+    bool _grasp_srv_callback(
+        const std::shared_ptr<srv::Grasp::Request> &req,
+        std::shared_ptr<srv::Grasp::Response> res
+        );
+
+    bool _move_srv_callback(
+        const std::shared_ptr<srv::Move::Request> &req,
+        std::shared_ptr<srv::Move::Response> res
+        );
+
+    bool _home_srv_callback(
+        const std::shared_ptr<std_srvs::srv::Trigger::Request> &req,
+        std::shared_ptr<std_srvs::srv::Trigger::Response> res
+        );
+
+    EffectorDriverFrankaHand(const EffectorDriverFrankaHand&)=delete;
+    EffectorDriverFrankaHand()=delete;
+    ~EffectorDriverFrankaHand();
+
+    EffectorDriverFrankaHand(
+        const std::string &driver_node_prefix,
+        const EffectorDriverFrankaHandConfiguration& configuration,
+        std::shared_ptr<Node> node,
+        std::atomic_bool* break_loops);
+
+    void start_control_loop();
+
+    void gripper_homing();
+
+
+    void connect() ;
+    void disconnect() noexcept;
+
+    void initialize() ;
+    void deinitialize() ;
+};
+
+} // qros
+
--- a/sas_robot_driver_franka/include/sas_robot_driver_franka/interfaces/robot_interface_franka.hpp
+++ b/sas_robot_driver_franka/include/sas_robot_driver_franka/interfaces/robot_interface_franka.hpp
@@ -40,12 +40,12 @@
 #include <franka/robot.h>
 #include <franka/gripper.h>
 #include <franka/exception.h>
-#include "generator/motion_generator.h"
+#include <sas_robot_driver_franka/generator/motion_generator.h>
+#include <sas_robot_driver_franka/generator/quadratic_program_motion_generator.h>
+#include <sas_robot_driver_franka/generator/custom_motion_generation.h>
 #include <thread>
-#include "generator/quadratic_program_motion_generator.h"
 #include <dqrobotics/robots/FrankaEmikaPandaRobot.h>
 #include <atomic>
-#include "generator/custom_motion_generation.h"

 using namespace DQ_robotics;
 using namespace Eigen;
--- a/sas_robot_driver_franka/include/sas_robot_driver_franka/robot_dynamic/qros_robot_dynamics_client.hpp
+++ b/sas_robot_driver_franka/include/sas_robot_driver_franka/robot_dynamic/qros_robot_dynamics_client.hpp
@@ -0,0 +1,96 @@
+#pragma once
+/*
+# Copyright (c) 2024 Quenitin Lin
+#
+#    This file is part of sas_robot_driver_franka.
+#
+#    sas_robot_driver_franka is free software: you can redistribute it and/or modify
+#    it under the terms of the GNU Lesser General Public License as published by
+#    the Free Software Foundation, either version 3 of the License, or
+#    (at your option) any later version.
+#
+#    sas_robot_driver_franka is distributed in the hope that it will be useful,
+#    but WITHOUT ANY WARRANTY; without even the implied warranty of
+#    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+#    GNU Lesser General Public License for more details.
+#
+#    You should have received a copy of the GNU Lesser General Public License
+#    along with sas_robot_driver.  If not, see <https://www.gnu.org/licenses/>.
+#
+# ################################################################
+#
+#   Author: Quenitin Lin
+#
+# ################################################################
+#
+# Contributors:
+#      1. Quenitin Lin
+#
+# ################################################################
+*/
+
+#include <rclcpp/rclcpp.hpp>
+#include <rclcpp/node.hpp>
+// #include <sas_common/sas_common.hpp>
+#include <sas_conversions/sas_conversions.hpp>
+#include <geometry_msgs/msg/wrench_stamped.hpp>
+#include <geometry_msgs/msg/transform.hpp>
+#include <geometry_msgs/msg/transform_stamped.hpp>
+#include <tf2_ros/transform_listener.h>
+#include <tf2_ros/buffer.h>
+#include <tf2/time.h>
+// #include <tf2_geometry_msgs/tf2_geometry_msgs.h>
+#include <geometry_msgs/msg/pose.hpp>
+#include <std_msgs/msg/header.hpp>
+#include <dqrobotics/DQ.h>
+
+#define BUFFER_DURATION_DEFAULT_S 2.0   // 2 second
+
+using namespace rclcpp;
+namespace qros {
+
+using namespace DQ_robotics;
+
+class RobotDynamicsClient {
+private:
+    std::shared_ptr<Node> node_;
+
+    std::string topic_prefix_;
+    std::string child_frame_id_;
+    std::string parent_frame_id_;
+
+    Subscription<geometry_msgs::msg::WrenchStamped>::SharedPtr subscriber_cartesian_stiffness_;
+    std::unique_ptr<tf2_ros::Buffer> tf_buffer_;
+    std::shared_ptr<tf2_ros::TransformListener> tf_listener_;
+
+
+    static geometry_msgs::msg::Transform _dq_to_geometry_msgs_transform(const DQ& pose) ;
+
+    Vector3d last_stiffness_force_;
+    Vector3d last_stiffness_torque_;
+    DQ last_stiffness_frame_pose_;
+
+    void _callback_cartesian_stiffness(const geometry_msgs::msg::WrenchStamped &msg);
+
+    static DQ _geometry_msgs_transform_to_dq(const geometry_msgs::msg::Transform& transform);
+
+public:
+    RobotDynamicsClient() = delete;
+    RobotDynamicsClient(const RobotDynamicsClient&) = delete;
+// #ifdef BUILD_PYBIND
+//     explicit RobotDynamicsClient(const std::string& node_prefix):RobotDynamicsClient(sas::common::get_static_node_handle(),node_prefix){}
+// #endif
+    explicit RobotDynamicsClient(const std::shared_ptr<Node> &node,  const std::string& topic_prefix="GET_FROM_NODE");
+
+    VectorXd get_stiffness_force();
+    VectorXd get_stiffness_torque();
+    DQ get_stiffness_frame_pose();
+
+    bool is_enabled() const;
+    std::string get_topic_prefix() const {return topic_prefix_;}
+
+};
+
+
+
+} // namespace sas
--- a/sas_robot_driver_franka/include/sas_robot_driver_franka/robot_dynamic/qros_robot_dynamics_server.hpp
+++ b/sas_robot_driver_franka/include/sas_robot_driver_franka/robot_dynamic/qros_robot_dynamics_server.hpp
@@ -0,0 +1,90 @@
+#pragma once
+/*
+# Copyright (c) 2024 Quenitin Lin
+#
+#    This file is part of sas_robot_driver_franka.
+#
+#    sas_robot_driver_franka is free software: you can redistribute it and/or modify
+#    it under the terms of the GNU Lesser General Public License as published by
+#    the Free Software Foundation, either version 3 of the License, or
+#    (at your option) any later version.
+#
+#    sas_robot_driver_franka is distributed in the hope that it will be useful,
+#    but WITHOUT ANY WARRANTY; without even the implied warranty of
+#    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+#    GNU Lesser General Public License for more details.
+#
+#    You should have received a copy of the GNU Lesser General Public License
+#    along with sas_robot_driver.  If not, see <https://www.gnu.org/licenses/>.
+#
+# ################################################################
+#
+#   Author: Quenitin Lin
+#
+# ################################################################
+#
+# Contributors:
+#      1. Quenitin Lin
+#
+# ################################################################
+*/
+#include <rclcpp/rclcpp.hpp>
+#include <rclcpp/node.hpp>
+// #include <sas_common/sas_common.hpp>
+#include <sas_conversions/sas_conversions.hpp>
+#include <geometry_msgs/msg/wrench_stamped.hpp>
+#include <geometry_msgs/msg/transform.hpp>
+#include <geometry_msgs/msg/transform_stamped.hpp>
+#include <tf2_ros/transform_broadcaster.h>
+#include <tf2_ros//static_transform_broadcaster.h>
+#include <geometry_msgs/msg/pose.hpp>
+#include <std_msgs/msg/header.hpp>
+#include <dqrobotics/DQ.h>
+
+#define REDUCE_TF_PUBLISH_RATE 10
+#define WORLD_FRAME_ID "world"
+using namespace rclcpp;
+
+namespace qros {
+
+using namespace DQ_robotics;
+
+class RobotDynamicsServer {
+private:
+    unsigned int seq_ = 0;
+    std::shared_ptr<Node> node_;
+
+    std::string topic_prefix_;
+    std::string child_frame_id_;
+    std::string parent_frame_id_;
+
+    rclcpp::Publisher<geometry_msgs::msg::WrenchStamped>::SharedPtr publisher_cartesian_stiffness_;
+    std::shared_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster_;
+    std::shared_ptr<tf2_ros::StaticTransformBroadcaster> static_base_tf_broadcaster_;
+
+    DQ world_to_base_tf_ = DQ(0);
+
+    static geometry_msgs::msg::Transform _dq_to_geometry_msgs_transform(const DQ& pose);
+
+    void _publish_base_static_tf();
+
+public:
+    RobotDynamicsServer() = delete;
+    RobotDynamicsServer(const RobotDynamicsServer&) = delete;
+// #ifdef BUILD_PYBIND
+    // explicit RobotDynamicsServer(const std::string& node_prefix):RobotDynamicsServer(sas::common::get_static_node_handle(),node_prefix){}
+// #endif
+    explicit RobotDynamicsServer(const std::shared_ptr<Node> &node,  const std::string& topic_prefix="GET_FROM_NODE");
+
+    void publish_stiffness(const DQ& base_to_stiffness, const Vector3d& force, const Vector3d& torque);
+
+    void set_world_to_base_tf(const DQ& world_to_base_tf);
+
+    bool is_enabled() const;
+    std::string get_topic_prefix() const {return topic_prefix_;}
+
+};
+
+
+
+} // namespace sas
--- a/sas_robot_driver_franka/include/sas_robot_driver_franka/robot_interface_hand.hpp
+++ b/sas_robot_driver_franka/include/sas_robot_driver_franka/robot_interface_hand.hpp
--- a/sas_robot_driver_franka/include/sas_robot_driver_franka/sas_robot_driver_franka.hpp
+++ b/sas_robot_driver_franka/include/sas_robot_driver_franka/sas_robot_driver_franka.hpp
@@ -40,10 +40,10 @@

 #include <dqrobotics/DQ.h>

-#include <sas_robot_driver/sas_robot_driver.h>
-#include "robot_interface_franka.h"
-#include <ros/common.h>
-#include "sas_robot_dynamic_provider.h"
+#include <sas_core/sas_robot_driver.hpp>
+#include <sas_robot_driver_franka/interfaces/robot_interface_franka.hpp>
+#include <rclcpp/rclcpp.hpp>
+#include <sas_robot_driver_franka/robot_dynamic/qros_robot_dynamics_server.hpp>

 using namespace DQ_robotics;
 using namespace Eigen;
@@ -60,17 +60,20 @@ struct RobotDriverFrankaConfiguration
    int port;
    double speed;
    RobotInterfaceFranka::FrankaInterfaceConfiguration interface_configuration;
+    DQ robot_reference_frame = DQ(0);
 };


-class RobotDriverFranka: public RobotDriver
+class RobotDriverFranka: public sas::RobotDriver
 {
 private:
+    std::shared_ptr<Node> node_;
+
    RobotDriverFrankaConfiguration configuration_;

    std::shared_ptr<RobotInterfaceFranka> robot_driver_interface_sptr_ = nullptr;

-    RobotDynamicProvider* robot_dynamic_provider_;
+    std::shared_ptr<qros::RobotDynamicsServer> robot_dynamic_provider_sptr_;

    //Joint positions
    VectorXd joint_positions_;
@@ -86,6 +89,8 @@ private:
    void _update_stiffness_contact_and_pose() const;


+
+
 public:
    //const static int SLAVE_MODE_JOINT_CONTROL;
    //const static int SLAVE_MODE_END_EFFECTOR_CONTROL;
@@ -95,7 +100,8 @@ public:
    ~RobotDriverFranka();

    RobotDriverFranka(
-        RobotDynamicProvider* robot_dynamic_provider,
+        const std::shared_ptr<Node> &node,
+        const std::shared_ptr<qros::RobotDynamicsServer> &robot_dynamic_provider,
        const RobotDriverFrankaConfiguration& configuration,
        std::atomic_bool* break_loops
        );
@@ -107,7 +113,7 @@ public:
    VectorXd get_joint_velocities() override;
    void set_target_joint_velocities(const VectorXd& desired_joint_velocities) override;

-    VectorXd get_joint_forces() override;
+    VectorXd get_joint_torques() override;

    void connect() override;
    void disconnect() override;
--- a/sas_robot_driver_franka/launch/pycache/sas_robot_driver_franka_simulation_example.cpython-312.pyc
+++ b/sas_robot_driver_franka/launch/pycache/sas_robot_driver_franka_simulation_example.cpython-312.pyc
--- a/sas_robot_driver_franka/launch/depreciated/sas_operator_side_receiver.launch
+++ b/sas_robot_driver_franka/launch/depreciated/sas_operator_side_receiver.launch
--- a/sas_robot_driver_franka/launch/depreciated/sas_patient_side_manager.launch
+++ b/sas_robot_driver_franka/launch/depreciated/sas_patient_side_manager.launch
--- a/sas_robot_driver_franka/launch/depreciated/sas_robot_driver_coppelia_franka_1.launch
+++ b/sas_robot_driver_franka/launch/depreciated/sas_robot_driver_coppelia_franka_1.launch
--- a/sas_robot_driver_franka/launch/depreciated/sas_robot_driver_franka_1.launch
+++ b/sas_robot_driver_franka/launch/depreciated/sas_robot_driver_franka_1.launch
--- a/sas_robot_driver_franka/launch/depreciated/sas_robot_driver_franka_hand_1.launch
+++ b/sas_robot_driver_franka/launch/depreciated/sas_robot_driver_franka_hand_1.launch
@@ -6,12 +6,5 @@
 <rosparam file="$(find sas_robot_driver_franka)/cfg/sas_robot_driver_franka_hand_1.yaml" command="load"
 />

-
-
 </node>
-
-
-
-
-
 </launch>
--- a/sas_robot_driver_franka/launch/depreciated/sas_robot_driver_mirror_coppelia_franka_1.launch
+++ b/sas_robot_driver_franka/launch/depreciated/sas_robot_driver_mirror_coppelia_franka_1.launch
--- a/sas_robot_driver_franka/launch/sas_robot_driver_franka_example.py
+++ b/sas_robot_driver_franka/launch/sas_robot_driver_franka_example.py
@@ -0,0 +1,45 @@
+from launch import LaunchDescription
+from launch_ros.actions import Node
+import os
+
+
+def generate_launch_description():
+    return LaunchDescription([
+        Node(
+            package='sas_robot_driver_franka',
+            executable='sas_robot_driver_franka_node',
+            name='arm3',
+            parameters=[{
+                "robot_ip_address": "172.16.0.4",
+                "thread_sampling_time_sec": 0.004,
+                # "thread_sampling_time_nsec": 4000000,
+                "q_min": [-2.3093, -1.5133, -2.4937, -2.7478, -2.4800, 0.8521, -2.6895],
+                "q_max": [2.3093, 1.5133, 2.4937, -0.4461, 2.4800, 4.2094, 2.6895],
+                "force_upper_limits_scaling_factor": 4.0,
+                "upper_torque_threshold": [40.0, 40.0, 40.0, 40.0, 40.0, 40.0, 40.0],
+                "upper_force_threshold": [40.0, 40.0, 40.0, 40.0, 40.0, 40.0],
+                "robot_mode": "VelocityControl",
+                "robot_parameter_file_path": os.environ["ROBOT_3_JSON_PATH"]
+            }],
+            output="screen"
+        ),
+        Node(
+            package='sas_robot_driver_franka',
+            executable='sas_robot_driver_franka_coppelia_node',
+            name='arm3_coppelia',
+            parameters=[{
+                "thread_sampling_time_sec": 0.008,
+                "vrep_ip": os.environ["VREP_IP"],
+                "vrep_port": 20012,
+                "vrep_dynamically_enabled": True,
+                "using_real_robot": True,
+                "vrep_joint_names": ["Franka_joint1#1", "Franka_joint2#1", "Franka_joint3#1", "Franka_joint4#1",
+                                     "Franka_joint5#1", "Franka_joint6#1", "Franka_joint7#1"],
+                "robot_topic_prefix": "/arm3",
+                "robot_mode": "VelocityControl",
+                "robot_parameter_file_path": os.environ["ROBOT_3_JSON_PATH"]
+            }],
+            output="screen"
+        ),
+
+    ])
--- a/sas_robot_driver_franka/launch/sas_robot_driver_franka_hand_example.py
+++ b/sas_robot_driver_franka/launch/sas_robot_driver_franka_hand_example.py
@@ -0,0 +1,24 @@
+from launch import LaunchDescription
+from launch_ros.actions import Node
+import os
+
+
+def generate_launch_description():
+    return LaunchDescription([
+        Node(
+            package='sas_robot_driver_franka',
+            executable='sas_robot_driver_franka_hand_node',
+            name='arm3_hand',
+            parameters=[{
+                "robot_ip_address": "172.16.0.2",
+                # "thread_sampling_time_nsec": 20000000,  # 20ms , 50Hz
+                "thread_sampling_time_sec": 0.02,
+                "default_force": 2.0,
+                "default_speed": 0.07,
+                "default_epsilon_inner": 0.007,
+                "default_epsilon_outer": 0.007,
+            }],
+            output="screen"
+        ),
+
+    ])
--- a/sas_robot_driver_franka/launch/sas_robot_driver_franka_simulation_example.py
+++ b/sas_robot_driver_franka/launch/sas_robot_driver_franka_simulation_example.py
@@ -0,0 +1,27 @@
+from launch import LaunchDescription
+from launch_ros.actions import Node
+import os
+
+
+def generate_launch_description():
+    return LaunchDescription([
+        Node(
+            package='sas_robot_driver_franka',
+            executable='sas_robot_driver_franka_coppelia_node',
+            name='arm3_coppelia',
+            parameters=[{
+                "thread_sampling_time_sec": 0.008,
+                "vrep_ip": os.environ["VREP_IP"],
+                "vrep_port": 20012,
+                "vrep_dynamically_enabled": True,
+                "using_real_robot": False,
+                "vrep_joint_names": ["Franka_joint1#1", "Franka_joint2#1", "Franka_joint3#1", "Franka_joint4#1",
+                                     "Franka_joint5#1", "Franka_joint6#1", "Franka_joint7#1"],
+                "robot_topic_prefix": "/arm3",
+                "robot_mode": "VelocityControl",
+                "robot_parameter_file_path": os.environ["ROBOT_3_JSON_PATH"]
+            }],
+            output="screen"
+        ),
+
+    ])
--- a/sas_robot_driver_franka/package.xml
+++ b/sas_robot_driver_franka/package.xml
@@ -0,0 +1,32 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>sas_robot_driver_franka</name>
+  <version>0.0.15</version>
+  <description>The sas_driver_franka package</description>
+  <maintainer email="qlin1806@g.ecc.u-tokyo.ac.jp">qlin</maintainer>
+
+  <license>LGPLv3</license>
+  <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <depend>rclcpp</depend>
+  <depend>rclpy</depend>
+  <depend>tf2_ros</depend>
+  <depend>tf2</depend>
+  <depend>std_msgs</depend>
+  <depend>sas_core</depend>
+  <depend>sas_robot_driver</depend>
+  <depend>sas_common</depend>
+  <depend>sas_robot_driver_franka_interfaces</depend>
+
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+  <test_depend>ament_copyright</test_depend>
+  <test_depend>ament_flake8</test_depend>
+  <test_depend>ament_pep257</test_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>
--- a/sas_robot_driver_franka/qt/configuration_window/main.cpp
+++ b/sas_robot_driver_franka/qt/configuration_window/main.cpp
--- a/sas_robot_driver_franka/qt/configuration_window/mainwindow.cpp
+++ b/sas_robot_driver_franka/qt/configuration_window/mainwindow.cpp
--- a/sas_robot_driver_franka/qt/configuration_window/mainwindow.h
+++ b/sas_robot_driver_franka/qt/configuration_window/mainwindow.h
@@ -7,7 +7,7 @@

 #include <QMainWindow>
 #include "qspinbox.h"
-#include "robot_interface_franka.h"
+#include <sas_robot_driver_franka/interfaces/robot_interface_franka.hpp>

 QT_BEGIN_NAMESPACE
 namespace Ui { class MainWindow; }
--- a/sas_robot_driver_franka/qt/configuration_window/mainwindow.ui
+++ b/sas_robot_driver_franka/qt/configuration_window/mainwindow.ui
--- a/sas_robot_driver_franka/sas_robot_driver_franka/init.py
+++ b/sas_robot_driver_franka/sas_robot_driver_franka/init.py
@@ -0,0 +1,245 @@
+"""
+"""
+from typing import Union
+
+from sas_robot_driver_franka._qros_franka_robot_dynamics_py import RobotDynamicsClient, RobotDynamicsServer
+
+import rclpy
+from rclpy.node import Node
+from rclpy.client import Client
+from sas_robot_driver_franka_interfaces.srv import Move, Grasp
+from std_srvs.srv import Trigger
+from sas_robot_driver_franka_interfaces.msg import GripperState
+import os
+
+MOVE_TOPIC_SUFFIX = "move"
+GRASP_TOPIC_SUFFIX = "grasp"
+STATUS_TOPIC_SUFFIX = "gripper_status"
+
+
+class FrankaGripperInterface:
+    """
+    Non blocking interface to control the Franka gripper
+    """
+
+    def __init__(self, node: Node, robot_prefix: str):
+        self.last_message = None
+        self.robot_prefix = robot_prefix
+        self.node = node
+        self._current_action = None  # between moving, grasping, and homing
+        self.move_service = node.create_client(Move, os.path.join(robot_prefix, MOVE_TOPIC_SUFFIX))
+        self.grasp_service = node.create_client(Grasp, os.path.join(robot_prefix, GRASP_TOPIC_SUFFIX))
+        self.home_service = node.create_client(Trigger, os.path.join(robot_prefix, "homing"))
+        self.status_subscriber = node.create_subscription(GripperState, os.path.join(robot_prefix, STATUS_TOPIC_SUFFIX),
+                                                          self._status_callback, 10)
+
+        self.service_future: Union[rclpy.Future, None] = None
+
+        # gripper state
+        self.state_width = None
+        self.state_max_width = None
+        self.state_temperature = None
+        self.state_is_grasped = None
+        self.spin_handler = self._default_spin_handler
+
+    def _default_spin_handler(self):
+        rclpy.spin_once(self.node, timeout_sec=0.01)
+
+    def _is_service_ready(self, service: Client) -> bool:
+        try:
+            # self.node.get_logger().info("Waiting for service: " + service.service_name)
+            ret = service.wait_for_service(timeout_sec=0.1)
+            return ret
+        except Exception as e:
+            self.node.get_logger().info("Service error: " + service.service_name + ": " + str(e))
+            return False
+
+    def is_enabled(self) -> bool:
+        if self.state_width is None:
+            return False
+        if not self._is_service_ready(self.move_service):
+            return False
+        if not self._is_service_ready(self.grasp_service):
+            return False
+        return True
+
+    def _status_callback(self, msg: GripperState):
+        self.state_width = msg.width
+        self.state_max_width = msg.max_width
+        self.state_temperature = msg.temperature
+        self.state_is_grasped = msg.is_grasped
+
+    #############################################################################
+    # Public methods to control the gripper (actions)
+    #############################################################################
+    def home(self) -> rclpy.Future:
+        """
+        Reinitialize the gripper
+        :return: None
+        """
+        if self.is_busy():
+            raise Exception("Gripper is already moving or grasping, please wait until the previous action is finished")
+        self._home()
+
+    def move(self, width, speed=0) -> rclpy.Future:
+        """
+        Move the gripper to a specific width
+        :param width: width in meters
+        :param speed: speed in meters per second
+        :return: None
+        """
+        if self.is_busy():
+            raise Exception("Gripper is already moving or grasping, please wait until the previous action is finished")
+        return self._move(width, speed)
+
+    def grasp(self, width, force=0, speed=0, epsilon_inner=0, epsilon_outer=0) -> rclpy.Future:
+        """
+        Grasp an object with the gripper
+        :param width:
+        :param force:
+        :param speed:
+        :param epsilon_inner:
+        :param epsilon_outer:
+        :return:
+        """
+        if self.is_busy():
+            raise Exception("Gripper is already moving or grasping, please wait until the previous action is finished")
+        return self._grasp(width, force, speed, epsilon_inner, epsilon_outer)
+
+    #############################################################################
+    # Public methods to get the gripper state, from ros topics
+    #############################################################################
+    def get_max_width(self) -> float:
+        """ Get the maximum width of the gripper """
+        if not self.is_enabled():
+            raise Exception("Gripper is not enabled")
+        return self.state_max_width
+
+    def get_width(self) -> float:
+        """ Get the current width of the gripper """
+        if not self.is_enabled():
+            raise Exception("Gripper is not enabled")
+        return self.state_width
+
+    def get_temperature(self) -> float:
+        """ Get the temperature of the gripper """
+        if not self.is_enabled():
+            raise Exception("Gripper is not enabled")
+        return self.state_temperature
+
+    def is_grasped(self) -> bool:
+        """ Check if an object is grasped """
+        if not self.is_enabled():
+            raise Exception("Gripper is not enabled")
+        return self.state_is_grasped
+
+    #############################################################################
+    # relating to action state
+    #############################################################################
+    def is_moving(self):
+        """ Check if the gripper is currently moving """
+        return self._current_action == "moving"
+
+    def is_grasping(self):
+        """ Check if the gripper is currently grasping """
+        return self._current_action == "grasping"
+
+    def is_homing(self):
+        """ Check if the gripper is currently homing """
+        return self._current_action == "homing"
+
+    def is_busy(self):
+        """ Check if the gripper is currently moving or grasping """
+        return self._current_action is not None
+
+    def is_done(self):
+        """ Check if the gripper is done moving or grasping """
+        if not self.is_busy():
+            self.node.get_logger().warn("Gripper is not moving or grasping")
+            return True
+        else:
+            if self.service_future is not None:
+                if self.service_future.done():
+                    return True
+                return False
+            else:
+                return True
+
+    ##############################################################################
+    # Public methods to get the result of the last action
+    ##############################################################################
+    def get_result(self):
+        """
+        Get the result of the last action
+        :return:
+        """
+        if self.service_future is not None:
+            if self.service_future.done():
+                response = self.service_future.result()
+                if isinstance(response, Move.Response):
+                    self._current_action = None
+                elif isinstance(response, Grasp.Response):
+                    self._current_action = None
+                elif isinstance(response, Trigger.Response):
+                    self._current_action = None
+                else:
+                    raise Exception("Invalid response type")
+                self.service_future = None
+                self.last_message = hasattr(response, "message") and response.message or ""
+                return response.success
+            else:
+                raise Exception("No result available")
+        else:
+            raise Exception("No result available")
+
+    def get_last_message(self):
+        return self.last_message
+
+    def wait_until_done(self):
+        """
+        Wait until the gripper is done moving or grasping
+        :return: success
+        """
+        rate = self.node.create_rate(100)  # 100 Hz
+        if not self.is_enabled():
+            raise Exception("Gripper is not enabled")
+        if not self.is_busy():
+            return
+        while not self.is_done():
+            self.spin_handler()
+            rate.sleep()
+        return self.get_result()
+
+    def _home(self) -> rclpy.Future:
+        self._current_action = "homing"
+        # self.node.get_logger().info("Homing gripper")
+        request = Trigger.Request()
+        # self.node.get_logger().info("Calling home service")
+        self.service_future = self.home_service.call_async(request)
+        return self.service_future
+
+    def _move(self, width, speed) -> rclpy.Future:
+        self._current_action = "moving"
+        # self.node.get_logger().info("Moving gripper to width: " + str(width) + " speed: " + str(speed))
+        request = Move.Request()
+        request.width = float(width)
+        request.speed = float(speed)
+        # self.node.get_logger().info("Calling move service")
+        self.service_future = self.move_service.call_async(request)
+        return self.service_future
+
+    def _grasp(self, width, force, speed, epsilon_inner, epsilon_outer) -> rclpy.Future:
+        self._current_action = "grasping"
+        # self.node.get_logger().info("Grasping object at width: " + str(width) + " force: " + str(force) + " speed: " + str(speed))
+        request = Grasp.Request()
+        request.width = float(width)
+        request.force = float(force)
+        request.speed = float(speed)
+        request.epsilon_inner = float(epsilon_inner)
+        request.epsilon_outer = float(epsilon_outer)
+        # self.node.get_logger().info("Calling grasp service")
+        self.service_future = self.grasp_service.call_async(request)
+        return self.service_future
+
+    def set_spin_handler(self, spin_handler):
+        self.spin_handler = spin_handler
--- a/sas_robot_driver_franka/scripts/example_gripper_control.py
+++ b/sas_robot_driver_franka/scripts/example_gripper_control.py
@@ -0,0 +1,71 @@
+import threading
+
+import rclpy
+from sas_robot_driver_franka import FrankaGripperInterface
+import time
+
+
+def main_loop(node):
+    iteration_ = 0
+    node_name = node.get_name()
+    hand1 = FrankaGripperInterface(node, "/franka_hand_1")
+    logger = rclpy.node.get_logger(node_name)
+
+    while not hand1.is_enabled():
+        logger.info("Waiting for gripper to be enabled...")
+        time.sleep(0.1)
+    rclpy.node.get_logger(node_name).info("Gripper enabled!")
+
+    def spin_all(node_):
+        while rclpy.ok():
+            rclpy.spin_once(node_, timeout_sec=0.1)
+            time.sleep(0.1)
+            
+    thread = threading.Thread(target=spin_all, args=(node, ), daemon=True)
+    thread.start()
+    
+    rate = node.create_rate(2)
+
+    while rclpy.ok():
+        logger.info("Main loop running...")
+
+        # Get the temperature of the gripper
+        temperature = hand1.get_temperature()
+        logger.info(f"Temperature: {temperature}")
+        max_width = hand1.get_max_width()
+        logger.info(f"Max width: {max_width}")
+        width = hand1.get_width()
+        logger.info(f"Width: {width}")
+        is_grasped = hand1.is_grasped()
+        logger.info(f"Is grasped: {is_grasped}")
+        is_moving = hand1.is_moving()
+        logger.info(f"Is moving: {is_moving}")
+        is_grasping = hand1.is_grasping()
+        logger.info(f"Is grasping: {is_grasping}")
+        logger.warn("calling move(0.01)")
+        if not hand1.is_busy():
+            hand1.grasp(0.01)
+        else:
+            logger.warn("Gripper is busy. Waiting...")
+        result_ready = hand1.is_done()
+        if not result_ready:
+            logger.info("Waiting for gripper to finish moving...")
+        else:
+            result = hand1.get_result()
+            logger.info(f"Result: {result}")
+
+
+        # Check if there is a response in the queue
+
+        iteration_ += 1
+        rate.sleep()
+
+    rclpy.shutdown()
+    thread.join()
+
+
+if __name__ == "__main__":
+    rclpy.init()
+    node_name_ = "example_gripper_control_node"
+    NODE = rclpy.create_node(node_name_)
+    main_loop(NODE)
--- a/sas_robot_driver_franka/scripts/publish_dumy_robot_dynmaics.py
+++ b/sas_robot_driver_franka/scripts/publish_dumy_robot_dynmaics.py
@@ -0,0 +1,46 @@
+import time
+
+import rclpy
+from sas_common import rclcpp_init, rclcpp_Node, rclcpp_spin_some, rclcpp_shutdown
+from sas_robot_driver_franka import RobotDynamicsClient, RobotDynamicsServer
+import dqrobotics as dql
+import numpy as np
+
+
+
+
+if __name__ == "__main__":
+    rclcpp_init()
+    rclpy.init()
+    sas_node = rclcpp_Node("dummy_robot_dynamics_server_sas")
+    node = rclpy.create_node("dummy_robot_dynamics_server")
+    dynam_provider = RobotDynamicsServer(sas_node, "/franka1")
+
+    t = dql.DQ([0, 0, 1])
+    r = dql.DQ([1, 0, 0, 0])
+    base_pose = r + 0.5 * dql.E_ * t * r
+    dynam_provider.set_world_to_base_tf(base_pose)
+    t_ = 0
+    node.get_logger().info("Dummy robot dynamics server started")
+    r = dql.DQ([0, 0, 0, 1])
+    rate = node.create_rate(100)
+    dummy_force = np.random.rand(3) * 100
+    dummy_torque = np.random.rand(3) * 10
+    try:
+        while rclpy.ok():
+            t = dql.DQ([1, 2, 1]) + dql.i_ * np.sin(t_/2/np.pi) + dql.j_ * np.cos(t_/2/np.pi)
+            ee_pose = r + 0.5 * dql.E_ * t * r
+            dummy_force = dummy_force * 0.9 + np.random.rand(3) * 10
+            dummy_torque = dummy_torque * 0.9 + np.random.rand(3) * 1
+            dynam_provider.publish_stiffness(ee_pose, dummy_force, dummy_torque)
+            rclcpp_spin_some(sas_node)
+            rclpy.spin_once(node)
+            t_ += 0.01
+            time.sleep(0.1)
+    except KeyboardInterrupt:
+        pass
+    finally:
+        rclcpp_shutdown()
+        node.destroy_node()
+        rclpy.shutdown()
+
--- a/sas_robot_driver_franka/scripts/subscribe_dummy_dynamic.py
+++ b/sas_robot_driver_franka/scripts/subscribe_dummy_dynamic.py
@@ -0,0 +1,53 @@
+import time
+
+import rclpy
+from sas_common import rclcpp_init, rclcpp_Node, rclcpp_spin_some, rclcpp_shutdown
+from sas_robot_driver_franka import RobotDynamicsClient, RobotDynamicsServer
+import dqrobotics as dql
+import numpy as np
+
+from dqrobotics.interfaces.vrep import DQ_VrepInterface
+
+# vrep = DQ_VrepInterface()
+# vrep.connect("192.168.10.103", 19997, 100, 10)
+vrep = None
+
+if __name__ == "__main__":
+    rclcpp_init()
+    rclpy.init()
+    sas_node = rclcpp_Node("dummy_robot_dynamics_client_sas")
+    node = rclpy.create_node("dummy_robot_dynamics_client")
+    executor = rclpy.executors.MultiThreadedExecutor()
+    executor.add_node(node)
+    try:
+        executor.spin_once(timeout_sec=0.1)
+        dynam_provider = RobotDynamicsClient(sas_node, "/franka1")
+        rclcpp_spin_some(sas_node)
+        while not dynam_provider.is_enabled():
+            node.get_logger().info("Waiting for robot dynamics provider to be enabled")
+            rclcpp_spin_some(sas_node)
+            executor.spin_once(timeout_sec=0.1)
+            node.get_logger().info("retrying...")
+            time.sleep(0.5)
+
+        node.get_logger().info("Dummy robot dynamics client started")
+        while rclpy.ok():
+            force = dynam_provider.get_stiffness_force()
+            torque = dynam_provider.get_stiffness_torque()
+            ee_pose = dynam_provider.get_stiffness_frame_pose()
+            if vrep is not None:
+                vrep.set_object_pose("xd1", ee_pose)
+            node.get_logger().info(f"EE Pose: {ee_pose}")
+            node.get_logger().info(f"Force: {force}")
+            node.get_logger().info(f"Torque: {torque}")
+            rclcpp_spin_some(sas_node)
+            executor.spin_once(timeout_sec=0.1)
+            time.sleep(0.5)
+    except KeyboardInterrupt:
+        pass
+    finally:
+        rclcpp_shutdown()
+        node.destroy_node()
+        if vrep is not None:
+            vrep.disconnect()
+        rclpy.shutdown()
--- a/sas_robot_driver_franka/src/coppelia/sas_robot_driver_coppelia.cpp
+++ b/sas_robot_driver_franka/src/coppelia/sas_robot_driver_coppelia.cpp
@@ -0,0 +1,247 @@
+#include <sas_robot_driver_franka/coppelia/sas_robot_driver_coppelia.hpp>
+
+
+namespace qros
+{
+
+RobotDriverCoppelia::~RobotDriverCoppelia() {
+    deinitialize();
+    disconnect();
+}
+
+RobotDriverCoppelia::RobotDriverCoppelia(const std::shared_ptr<Node> &node_sptr, const RobotDriverCoppeliaConfiguration &configuration, std::atomic_bool *break_loops):
+        configuration_(configuration),
+        node_sptr_(node_sptr),
+        clock_(configuration.thread_sampling_time_sec),
+        break_loops_(break_loops),
+        robot_mode_(ControlMode::Position),
+        vi_(std::make_shared<DQ_VrepInterface>())
+{
+    // should initialize robot driver interface to real robot
+    DQ_SerialManipulatorDH smdh = DQ_JsonReader::get_from_json<DQ_SerialManipulatorDH>(configuration_.robot_parameter_file_path);
+    joint_limits_ = {smdh.get_lower_q_limit(),smdh.get_upper_q_limit()};
+    if(configuration_.using_real_robot)
+    {
+        RCLCPP_INFO_STREAM(node_sptr_->get_logger(), "::Using real robot, Instantiating robot interface to real driver at ["+configuration_.robot_topic_prefix+"].");
+        real_robot_interface_ = std::make_shared<sas::RobotDriverClient>(node_sptr_, configuration_.robot_topic_prefix);
+    }else{
+        RCLCPP_INFO_STREAM(node_sptr_->get_logger(), "::Using simulation robot, simulation mode is set to "+ configuration_.robot_mode);
+        robot_provider_ = std::make_shared<sas::RobotDriverServer>(node_sptr_, configuration_.robot_topic_prefix);
+        std::string _mode_upper = configuration_.robot_mode;
+        std::transform(_mode_upper.begin(), _mode_upper.end(), _mode_upper.begin(), ::toupper);
+        if(_mode_upper == "POSITIONCONTROL"){
+            robot_mode_ = ControlMode::Position;
+        }else if(_mode_upper == "VELOCITYCONTROL"){
+            robot_mode_ = ControlMode::Velocity;
+        }else{
+            throw std::invalid_argument("[" + std::string(node_sptr_->get_name()) + "]::Robot mode must be either 'position' or 'velocity'");
+        }
+    }
+
+}
+
+void RobotDriverCoppelia::_update_vrep_position(const VectorXd &joint_positions, const bool& force_update) const{
+    if(configuration_.vrep_dynamically_enabled){
+        if(force_update){
+            vi_->set_joint_positions(configuration_.vrep_joint_names, joint_positions);
+        }
+        vi_->set_joint_target_positions(configuration_.vrep_joint_names, joint_positions);
+    }else{
+        vi_->set_joint_positions(configuration_.vrep_joint_names, joint_positions);
+    }
+}
+
+void RobotDriverCoppelia::_update_vrep_velocity(const VectorXd & joint_velocity) const{
+    if(!configuration_.vrep_dynamically_enabled){
+        throw std::runtime_error("[RobotDriverCoppelia]::[_update_vrep_velocity]::Vrep is not dynamically enabled");
+    }
+    vi_->set_joint_target_velocities(configuration_.vrep_joint_names, joint_velocity);
+}
+
+void RobotDriverCoppelia::_start_control_loop(){
+    clock_.init();
+    RCLCPP_INFO_STREAM(node_sptr_->get_logger(), "::Starting control loop...");
+    while(!_should_shutdown()){
+        clock_.update_and_sleep();
+        rclcpp::spin_some(node_sptr_);
+        if(!rclcpp::ok()){break_loops_->store(true);}
+        if(configuration_.using_real_robot){
+            // real_robot_interface_
+            auto joint_position = real_robot_interface_->get_joint_positions();
+            _update_vrep_position(joint_position);
+        }else{
+            // robot_provider_
+            VectorXd target_joint_positions;
+            auto current_joint_positions = vi_->get_joint_positions(configuration_.vrep_joint_names);
+            VectorXd current_joint_velocity;
+            if(robot_provider_->is_enabled()) {
+                if(robot_mode_ == ControlMode::Velocity)
+                {
+                    if(robot_provider_->is_enabled(sas::RobotDriver::Functionality::VelocityControl)) {
+                        simulated_joint_velocities_ = robot_provider_->get_target_joint_velocities();
+                        current_joint_velocity = simulated_joint_velocities_;
+                        //                    try{_update_vrep_velocity(simulated_joint_velocities_);}catch (...){}
+                    }
+                    else{
+                        RCLCPP_DEBUG_STREAM(node_sptr_->get_logger(), "::Velocity control is not enabled.");
+                    }
+                    target_joint_positions = simulated_joint_positions_;
+                }else{
+                    target_joint_positions = robot_provider_->get_target_joint_positions();
+                }
+            }else {
+                target_joint_positions = current_joint_positions;
+            }
+            _update_vrep_position(target_joint_positions);
+            robot_provider_->send_joint_states(current_joint_positions, current_joint_velocity, VectorXd());
+            robot_provider_->send_joint_limits(joint_limits_);
+
+        }
+    }
+
+
+}
+
+int RobotDriverCoppelia::start_control_loop(){
+    try{
+        RCLCPP_INFO_STREAM(node_sptr_->get_logger(), "::Waiting to connect with coppelia...");
+        connect();
+        RCLCPP_INFO_STREAM(node_sptr_->get_logger(), "::Connected to coppelia.");
+        RCLCPP_INFO_STREAM(node_sptr_->get_logger(), "::Initializing...");
+        initialize();
+        RCLCPP_INFO_STREAM(node_sptr_->get_logger(), "::initialized.");
+
+        _start_control_loop();
+    }
+    catch(const std::exception& e)
+    {
+        RCLCPP_WARN_STREAM(node_sptr_->get_logger(), "::Error or exception caught::" << e.what());
+    }
+    catch(...)
+    {
+        RCLCPP_WARN_STREAM(node_sptr_->get_logger(), "::Unexpected error or exception caught");
+    }
+    RCLCPP_INFO_STREAM(node_sptr_->get_logger(), "::Deinitializing...");
+    deinitialize();
+    RCLCPP_INFO_STREAM(node_sptr_->get_logger(), "::deinitialized.");
+    RCLCPP_INFO_STREAM(node_sptr_->get_logger(), "::Disconnecting from coppelia...");
+    disconnect();
+    RCLCPP_INFO_STREAM(node_sptr_->get_logger(), "::Disconnected from coppelia.");
+
+    return 0;
+}
+
+
+void RobotDriverCoppelia::connect(){
+    auto ret = vi_->connect(configuration_.vrep_ip, configuration_.vrep_port, 100, 10);
+    if(!ret){
+        throw std::runtime_error("[RobotDriverCoppelia]::connect::Could not connect to Vrep");
+    }
+    RCLCPP_INFO_STREAM(node_sptr_->get_logger(), "::[RobotDriverCoppelia]::connect::Connected to Vrep");
+}
+void RobotDriverCoppelia::disconnect(){
+    vi_->disconnect_all();
+}
+
+void RobotDriverCoppelia::initialize(){
+    if(configuration_.using_real_robot)
+    {
+        RCLCPP_INFO_STREAM(node_sptr_->get_logger(), "::[RobotDriverCoppelia]::initialize::Waiting for real robot interface to initialize...");
+        rclcpp::spin_some(node_sptr_);
+        int count = 0;
+        while (!real_robot_interface_->is_enabled(sas::RobotDriver::Functionality::PositionControl)) {
+            rclcpp::spin_some(node_sptr_);
+            // std::cout<<"Waiting for real robot interface to initialize..."<<std::endl;
+            std::this_thread::sleep_for(std::chrono::milliseconds(100));
+            rclcpp::spin_some(node_sptr_);
+            count++;
+            if(count > REAL_ROBOT_INTERFACE_INIT_TIMEOUT_COUNT){
+                RCLCPP_ERROR_STREAM(node_sptr_->get_logger(), "::[RobotDriverCoppelia]::initialize::Real robot interface not initialized. Exiting on TIMEOUT...");
+                throw std::runtime_error("[" + std::string(node_sptr_->get_name()) +"]::[RobotDriverCoppelia]::initialize::Real robot interface not initialized.");
+            }
+            if(!rclcpp::ok()) {
+                RCLCPP_WARN_STREAM(node_sptr_->get_logger(), "::[RobotDriverCoppelia]::initialize::ROS shutdown received. Exiting...");
+                throw std::runtime_error("[" + std::string(node_sptr_->get_name()) +"]::[RobotDriverCoppelia]::initialize::ROS shutdown received, not OK.");
+            }
+        }
+        RCLCPP_INFO_STREAM(node_sptr_->get_logger(), "::[RobotDriverCoppelia]::initialize::Real robot interface initialized.");
+        joint_limits_ = real_robot_interface_->get_joint_limits();
+        _update_vrep_position(real_robot_interface_->get_joint_positions(), true);
+    }else{
+        RCLCPP_INFO_STREAM(node_sptr_->get_logger(), "::[RobotDriverCoppelia]::initialize::Simulation mode.");
+        // initialization information for robot driver provider
+        /**
+        * TODO: check for making sure real robot is not actually connected
+        */
+        auto current_joint_positions = vi_->get_joint_positions(configuration_.vrep_joint_names);
+        VectorXd current_joint_velocity;
+        if(robot_mode_ == ControlMode::Velocity)
+        {current_joint_velocity = VectorXd::Zero(current_joint_positions.size());}
+        robot_provider_->send_joint_states(current_joint_positions, current_joint_velocity, VectorXd());
+        robot_provider_->send_joint_limits(joint_limits_);
+        // start velocity control simulation thread if needed
+        if(robot_mode_ == ControlMode::Velocity)
+        {
+            simulated_joint_positions_ = current_joint_positions;
+            simulated_joint_velocities_ = current_joint_velocity;
+            start_simulation_thread();
+        }
+    }
+}
+void RobotDriverCoppelia::deinitialize(){
+    // nothing to do
+}
+
+void RobotDriverCoppelia::start_simulation_thread(){ // thread entry point
+    if(simulation_thread_started_){
+        throw std::runtime_error("[RobotDriverCoppelia]::start_simulation_thread::Simulation thread already started");
+    }
+    if(velocity_control_simulation_thread_.joinable()){
+        velocity_control_simulation_thread_.join();
+    }
+
+    velocity_control_simulation_thread_ = std::thread(&RobotDriverCoppelia::_velocity_control_simulation_thread_main, this);
+}
+
+void RobotDriverCoppelia::_velocity_control_simulation_thread_main(){
+    /**
+     * This thread should not access vrep
+     */
+    simulation_thread_started_ = true;
+    try{
+        RCLCPP_INFO_STREAM(node_sptr_->get_logger(), "::RobotDriverCoppelia]::[_velocity_control_simulation_thread_main]::Simulation thread started.");
+        sas::Clock clock = sas::Clock(VIRTUAL_ROBOT_SIMULATION_SAMPLING_TIME_SEC);
+        double tau = VIRTUAL_ROBOT_SIMULATION_SAMPLING_TIME_SEC;
+        auto current_joint_positions = simulated_joint_positions_;
+        clock.init();
+        while (!(*break_loops_) && rclcpp::ok()) {
+
+            current_joint_positions += tau * simulated_joint_velocities_; // no dynamic model
+            // cap joint limit
+            auto q_min = std::get<0>(joint_limits_);
+            auto q_max = std::get<1>(joint_limits_);
+            for (int i = 0; i < current_joint_positions.size(); i++) {
+                if (current_joint_positions(i) < q_min(i)) {
+                    current_joint_positions(i) = q_min(i);
+                }
+                if (current_joint_positions(i) > q_max(i)) {
+                    current_joint_positions(i) = q_max(i);
+                }
+            }
+            simulated_joint_positions_ = current_joint_positions;
+            clock.update_and_sleep();
+        }
+    }catch(std::exception &e){
+        RCLCPP_ERROR_STREAM(node_sptr_->get_logger(), "::[RobotDriverCoppelia]::[_velocity_control_simulation_thread_main]::Exception::" << e.what());
+        simulation_thread_started_ = false;
+    }catch(...){
+        RCLCPP_ERROR_STREAM(node_sptr_->get_logger(), "::[RobotDriverCoppelia]::[_velocity_control_simulation_thread_main]::Exception::Unknown");
+        simulation_thread_started_ = false;
+    }
+    break_loops_->store(true);
+
+}
+
+
+
+} // sas namespace
--- a/sas_robot_driver_franka/src/deprecated/robot_coppelia_ros_interface.cpp
+++ b/sas_robot_driver_franka/src/deprecated/robot_coppelia_ros_interface.cpp
--- a/sas_robot_driver_franka/src/deprecated/robot_coppelia_ros_interface_node.cpp
+++ b/sas_robot_driver_franka/src/deprecated/robot_coppelia_ros_interface_node.cpp
--- a/sas_robot_driver_franka/src/generator/custom_motion_generation.cpp
+++ b/sas_robot_driver_franka/src/generator/custom_motion_generation.cpp
@@ -28,7 +28,7 @@
 #
 # ################################################################
 */
-#include "generator/custom_motion_generation.h"
+#include <sas_robot_driver_franka/generator/custom_motion_generation.h>

 /**
 * @brief CustomMotionGeneration::CustomMotionGeneration
--- a/sas_robot_driver_franka/src/generator/motion_generator.cpp
+++ b/sas_robot_driver_franka/src/generator/motion_generator.cpp
@@ -1,6 +1,6 @@
 // Copyright (c) 2017 Franka Emika GmbH
 // Use of this source code is governed by the Apache-2.0 license, see LICENSE
-#include "generator/motion_generator.h"
+#include <sas_robot_driver_franka/generator/motion_generator.h>
 #include <algorithm>
 #include <array>
 #include <cmath>
--- a/sas_robot_driver_franka/src/generator/quadratic_program_motion_generator.cpp
+++ b/sas_robot_driver_franka/src/generator/quadratic_program_motion_generator.cpp
@@ -1,4 +1,4 @@
-#include "generator/quadratic_program_motion_generator.h"
+#include <sas_robot_driver_franka/generator/quadratic_program_motion_generator.h>


 QuadraticProgramMotionGenerator::QuadraticProgramMotionGenerator(const double &speed_factor,
--- a/sas_robot_driver_franka/src/hand/qros_effector_driver_franka_hand.cpp
+++ b/sas_robot_driver_franka/src/hand/qros_effector_driver_franka_hand.cpp
@@ -0,0 +1,340 @@
+/*
+# Copyright (c) 2023 Juan Jose Quiroz Omana
+#
+#    This file is part of sas_robot_driver_franka.
+#
+#    sas_robot_driver_franka is free software: you can redistribute it and/or modify
+#    it under the terms of the GNU Lesser General Public License as published by
+#    the Free Software Foundation, either version 3 of the License, or
+#    (at your option) any later version.
+#
+#    sas_robot_driver_franka is distributed in the hope that it will be useful,
+#    but WITHOUT ANY WARRANTY; without even the implied warranty of
+#    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+#    GNU Lesser General Public License for more details.
+#
+#    You should have received a copy of the GNU Lesser General Public License
+#    along with sas_robot_driver.  If not, see <https://www.gnu.org/licenses/>.
+#
+# ################################################################
+#
+#   Author: Juan Jose Quiroz Omana, email: juanjqogm@gmail.com
+#
+# ################################################################
+#
+# Contributors:
+#      1. Quenitin Lin
+#
+# ################################################################
+*/
+#include <sas_robot_driver_franka/interfaces/qros_effector_driver_franka_hand.hpp>
+
+#include <franka/exception.h>
+
+using namespace std::placeholders;
+using namespace sas_robot_driver_franka_interfaces;
+
+
+namespace qros
+{
+    //const static int SLAVE_MODE_JOINT_CONTROL;
+    //const static int SLAVE_MODE_END_EFFECTOR_CONTROL;
+
+    EffectorDriverFrankaHand::~EffectorDriverFrankaHand()
+    {
+        if (_is_connected())
+        {
+            disconnect();
+        }
+    }
+
+    EffectorDriverFrankaHand::EffectorDriverFrankaHand(
+        const std::string& driver_node_prefix,
+        const EffectorDriverFrankaHandConfiguration& configuration,
+        std::shared_ptr<Node> node,
+        std::atomic_bool* break_loops):
+        driver_node_prefix_(driver_node_prefix),
+        configuration_(configuration),
+        node_(node),
+        break_loops_(break_loops)
+    {
+        gripper_sptr_ = nullptr;
+        grasp_srv_ = node->create_service<srv::Grasp>(driver_node_prefix_ + "/grasp",
+            std::bind(&EffectorDriverFrankaHand::_grasp_srv_callback, this, _1, _2));
+        move_srv_ = node->create_service<srv::Move>(driver_node_prefix_ + "/move",
+            std::bind(&EffectorDriverFrankaHand::_move_srv_callback, this, _1, _2));
+        homing_srv_ = node->create_service<std_srvs::srv::Trigger>(driver_node_prefix_ + "/homing",
+            std::bind(&EffectorDriverFrankaHand::_home_srv_callback, this, _1, _2));
+        gripper_status_publisher_ = node->create_publisher<msg::GripperState>(
+            driver_node_prefix_ + "/gripper_status", 1);
+    }
+
+    bool EffectorDriverFrankaHand::_is_connected() const
+    {
+#ifdef IN_TESTING
+        return true;
+#endif
+        if (gripper_sptr_ == nullptr) return false;
+        if (!gripper_sptr_) return false;
+        else return true;
+    }
+
+
+    void EffectorDriverFrankaHand::start_control_loop() {
+        sas::Clock clock = sas::Clock(configuration_.thread_sampeling_time_s);
+        clock.init();
+        RCLCPP_INFO_STREAM(node_->get_logger(),"[EffectorDriverFrankaHand]::start_control_loop::Starting control loop.");
+        RCLCPP_WARN_STREAM(node_->get_logger(),"[EffectorDriverFrankaHand]::Gripper READY.");
+        while (!(*break_loops_))
+        {
+            if (!_is_connected()) throw std::runtime_error("[" + std::string(node_->get_name())+"]::[EffectorDriverFrankaHand]::start_control_loop::Robot is not connected.");
+
+            if (!status_loop_running_)
+            {
+                RCLCPP_WARN_STREAM(node_->get_logger(),"::[EffectorDriverFrankaHand]::_is_connected::Status loop is not running.");
+                break_loops_->store(true);
+                break;
+            }
+
+            clock.update_and_sleep();
+            spin_some(node_);
+        }
+        RCLCPP_INFO_STREAM(node_->get_logger(),"::[EffectorDriverFrankaHand]::start_control_loop::Exiting control loop.");
+    }
+
+
+    void EffectorDriverFrankaHand::connect()
+    {
+#ifdef IN_TESTING
+        RCLCPP_WARN_STREAM(node_->get_logger(),"::[EffectorDriverFrankaHand]::connect::In testing mode. to DUMMY");
+        return;
+#endif
+        gripper_sptr_ = std::make_shared<franka::Gripper>(configuration_.robot_ip);
+        if (!_is_connected()) throw std::runtime_error(
+            "[" + std::string(node_->get_name())+
+            "]::[EffectorDriverFrankaHand]::connect::Could not connect to the robot.");
+    }
+
+    void EffectorDriverFrankaHand::disconnect() noexcept
+    {
+#ifdef IN_TESTING
+        RCLCPP_WARN_STREAM(node_->get_logger(),"::[EffectorDriverFrankaHand]::disconnect::In testing mode. from DUMMY");
+        return;
+#endif
+        RCLCPP_WARN_STREAM(node_->get_logger(),"::[EffectorDriverFrankaHand]::disconnecting...");
+        gripper_sptr_.reset();
+        gripper_sptr_ = nullptr;
+    }
+
+    /**
+     * @brief robot_driver_franka::gripper_homing
+     */
+    void EffectorDriverFrankaHand::gripper_homing()
+    {
+#ifdef IN_TESTING
+        RCLCPP_WARN_STREAM(node_->get_logger(),"::[EffectorDriverFrankaHand]::gripper_homing::In testing mode.");
+        return;
+#endif
+        if (!_is_connected()) throw std::runtime_error(
+            "[" + std::string(node_->get_name())+ "]::[EffectorDriverFrankaHand]::gripper_homing::Robot is not connected.");
+        auto ret = gripper_sptr_->homing();
+        if (!ret)
+        {
+            throw std::runtime_error("[" + std::string(node_->get_name())+"]::[EffectorDriverFrankaHand]::gripper_homing::Failed to home the gripper.");
+        }
+        RCLCPP_INFO_STREAM(node_->get_logger(),"[EffectorDriverFrankaHand]::gripper_homing::Gripper homed.");
+    }
+
+    void EffectorDriverFrankaHand::initialize()
+    {
+        if (!_is_connected()) throw std::runtime_error(
+            "[" + std::string(node_->get_name())+ "]::[EffectorDriverFrankaHand]::initialize::Robot is not connected.");
+        if (configuration_.initialize_with_homing) {
+            gripper_homing();
+        }
+        // start gripper status loop
+        status_loop_thread_ = std::thread(&EffectorDriverFrankaHand::_gripper_status_loop, this);
+        // check status loop with timeout
+        auto time_now = std::chrono::system_clock::now();
+        auto time_out = time_now + std::chrono::seconds(5);
+        while (!status_loop_running_)
+        {
+            if (std::chrono::system_clock::now() > time_out){throw std::runtime_error("[" + std::string(node_->get_name()) + "]::[EffectorDriverFrankaHand]::initialize::Could not start status loop.");}
+            std::this_thread::sleep_for(std::chrono::milliseconds(100));
+        }
+    }
+
+    void EffectorDriverFrankaHand::deinitialize()
+    {
+#ifdef IN_TESTING
+        RCLCPP_WARN_STREAM(node_->get_logger(),"::[EffectorDriverFrankaHand]::deinitialize::In testing mode.");
+        return;
+#endif
+        if (_is_connected())
+        {
+            franka::GripperState gripper_state = gripper_sptr_->readOnce();
+            if (gripper_state.is_grasped)
+            {
+                gripper_sptr_->stop();
+            }
+        }
+    }
+
+
+    bool EffectorDriverFrankaHand::_grasp_srv_callback(const std::shared_ptr<srv::Grasp::Request> &req, std::shared_ptr<srv::Grasp::Response> res)
+    {
+        RCLCPP_DEBUG_STREAM(node_->get_logger(),"::[EffectorDriverFrankaHand]::_grasp_srv_callback::Grasping...");
+        auto force = req->force;
+        auto speed = req->speed;
+        auto epsilon_inner = req->epsilon_inner;
+        auto epsilon_outer = req->epsilon_outer;
+        if (force <= 0.0) force = configuration_.default_force;
+        if (speed <= 0.0) speed = configuration_.default_speed;
+        if (epsilon_inner <= 0.0) epsilon_inner = configuration_.default_epsilon_inner;
+        if (epsilon_outer <= 0.0) epsilon_outer = configuration_.default_epsilon_outer;
+        RCLCPP_DEBUG_STREAM(node_->get_logger(),"::[EffectorDriverFrankaHand]::_grasp_srv_callback::Width: " + std::to_string(req->width));
+        RCLCPP_DEBUG_STREAM(node_->get_logger(),"::[EffectorDriverFrankaHand]::_grasp_srv_callback::force: " + std::to_string(force) + " speed: " + std::to_string(speed));
+        bool ret = false;
+        bool function_ret = true;
+#ifdef IN_TESTING
+        ret = true;
+        std::this_thread::sleep_for(std::chrono::milliseconds(2000));
+#else
+        try
+        {
+            std::lock_guard<std::mutex> lock(gripper_in_use_);
+            ret = gripper_sptr_->grasp(req->width, speed, force, epsilon_inner, epsilon_outer);
+        }catch(franka::CommandException& e)
+        {
+            RCLCPP_ERROR_STREAM(node_->get_logger(),"::[EffectorDriverFrankaHand]::_grasp_srv_callback::CommandException::" + std::string(e.what()));
+            function_ret = false;
+        }catch(franka::NetworkException& e)
+        {
+            RCLCPP_ERROR_STREAM(node_->get_logger(),"::[EffectorDriverFrankaHand]::_grasp_srv_callback::NetworkException::" + std::string(e.what()));
+            function_ret = false;
+        }
+#endif
+        res->set__success(ret);
+        return function_ret;
+    }
+
+
+    bool EffectorDriverFrankaHand::_move_srv_callback(const std::shared_ptr<srv::Move::Request> &req, std::shared_ptr<srv::Move::Response> res)
+    {
+        RCLCPP_DEBUG_STREAM(node_->get_logger(),"::[EffectorDriverFrankaHand]::_move_srv_callback::Moving...");
+        auto speed = req->speed;
+        if (speed <= 0.0) speed = configuration_.default_speed;
+        RCLCPP_DEBUG_STREAM(node_->get_logger(),"::[EffectorDriverFrankaHand]::_move_srv_callback::Speed: " + std::to_string(speed) + " Width: " + std::to_string(req->width));
+        bool ret = false;
+        bool function_ret = true;
+#ifdef IN_TESTING
+        ret = true;
+        std::this_thread::sleep_for(std::chrono::milliseconds(2000));
+#else
+        try
+        {
+            std::lock_guard<std::mutex> lock(gripper_in_use_);
+            ret = gripper_sptr_->move(req->width, speed);
+        }catch(franka::CommandException& e)
+        {
+            RCLCPP_ERROR_STREAM(node_->get_logger(),"::[EffectorDriverFrankaHand]::_move_srv_callback::CommandException::" + std::string(e.what()));
+            function_ret = false;
+        }catch(franka::NetworkException& e)
+        {
+            RCLCPP_ERROR_STREAM(node_->get_logger(),"::[EffectorDriverFrankaHand]::_move_srv_callback::NetworkException::" + std::string(e.what()));
+            function_ret = false;
+        }
+#endif
+        res->set__success(ret);
+        return function_ret;
+    }
+
+
+    bool EffectorDriverFrankaHand::_home_srv_callback(const std::shared_ptr<std_srvs::srv::Trigger::Request> &req, std::shared_ptr<std_srvs::srv::Trigger::Response> res)
+    {
+        RCLCPP_DEBUG_STREAM(node_->get_logger(),"::[EffectorDriverFrankaHand]::_home_srv_callback::Homing...");
+        bool ret = false;
+        bool function_ret = true;
+#ifdef IN_TESTING
+        ret = true;
+        std::this_thread::sleep_for(std::chrono::milliseconds(2000));
+#else
+        try
+        {
+            std::lock_guard<std::mutex> lock(gripper_in_use_);
+            ret = gripper_sptr_->homing();
+        }catch(franka::CommandException& e)
+        {
+            RCLCPP_ERROR_STREAM(node_->get_logger(),"::[EffectorDriverFrankaHand]::_home_srv_callback::CommandException::" + std::string(e.what()));
+            res->set__message("_home_srv_callback::CommandException::" + std::string(e.what()));
+            function_ret = false;
+            ret = false;
+        }catch(franka::NetworkException& e)
+        {
+            RCLCPP_ERROR_STREAM(node_->get_logger(),"::[EffectorDriverFrankaHand]::_home_srv_callback::NetworkException::" + std::string(e.what()));
+            res->set__message("_home_srv_callback::NetworkException::" + std::string(e.what()));
+            function_ret = false;
+            ret = false;
+        }
+#endif
+        res->set__success(ret);
+        return function_ret;
+    }
+
+
+    void EffectorDriverFrankaHand::_gripper_status_loop()
+    {
+        sas::Clock clock = sas::Clock(configuration_.thread_sampeling_time_s);
+        RCLCPP_INFO_STREAM(node_->get_logger(),"::[EffectorDriverFrankaHand]::_gripper_status_loop::Starting status loop.");
+        clock.init();
+        try
+        {
+            status_loop_running_ = true;
+            while (status_loop_running_)
+            {
+                if (!_is_connected()) throw std::runtime_error("[" + std::string(node_->get_name())+"]::[EffectorDriverFrankaHand]::_gripper_status_loop::Robot is not connected.");
+                try
+                {
+#ifdef IN_TESTING
+                    RCLCPP_WARN_STREAM(node_->get_logger(),"::[EffectorDriverFrankaHand]::_gripper_status_loop::In testing mode.");
+                    throw std::runtime_error("[" + std::string(node_->get_name())+"]::[EffectorDriverFrankaHand]::_gripper_status_loop::In testing mode.");
+#endif
+                    msg::GripperState msg;
+                    {
+#ifdef BLOCK_READ_IN_USED
+                        std::lock_guard<std::mutex> lock(gripper_in_use_);
+#endif
+                        franka::GripperState gripper_state = gripper_sptr_->readOnce();
+                        msg.set__width(static_cast<float>(gripper_state.width));
+                        msg.set__max_width(static_cast<float>(gripper_state.max_width));
+                        msg.set__is_grasped(gripper_state.is_grasped);
+                        msg.set__temperature(gripper_state.temperature);
+                        msg.set__duration_ms(gripper_state.time.toMSec());
+                    }
+                    gripper_status_publisher_->publish(msg);
+                }
+                catch (...)
+                {
+                    RCLCPP_INFO_STREAM(node_->get_logger(),"::[EffectorDriverFrankaHand]::_gripper_status_loop::Could not read gripper state. Unavailable.");
+                    msg::GripperState msg;
+                    msg.width = 0.0;
+                    gripper_status_publisher_->publish(msg);
+                }
+
+                clock.update_and_sleep();
+            }
+            status_loop_running_ = false;
+        }
+        catch (std::exception& e)
+        {
+            RCLCPP_ERROR_STREAM(node_->get_logger(),"::[EffectorDriverFrankaHand]::_gripper_status_loop::Exception::" + std::string(e.what()));
+            status_loop_running_ = false;
+        }
+        catch (...)
+        {
+            RCLCPP_ERROR_STREAM(node_->get_logger(),"::[EffectorDriverFrankaHand]::_gripper_status_loop::Exception::Unknown exception.");
+            status_loop_running_ = false;
+        }
+        RCLCPP_INFO_STREAM(node_->get_logger(),"::[EffectorDriverFrankaHand]::_gripper_status_loop::Exiting status loop.");
+    }
+} // qros
--- a/sas_robot_driver_franka/src/hand/robot_interface_hand.cpp
+++ b/sas_robot_driver_franka/src/hand/robot_interface_hand.cpp
@@ -1,4 +1,33 @@
-#include "robot_interface_hand.h"
+/*
+# Copyright (c) 2023 Juan Jose Quiroz Omana
+#
+#    This file is part of sas_robot_driver_franka.
+#
+#    sas_robot_driver_franka is free software: you can redistribute it and/or modify
+#    it under the terms of the GNU Lesser General Public License as published by
+#    the Free Software Foundation, either version 3 of the License, or
+#    (at your option) any later version.
+#
+#    sas_robot_driver_franka is distributed in the hope that it will be useful,
+#    but WITHOUT ANY WARRANTY; without even the implied warranty of
+#    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+#    GNU Lesser General Public License for more details.
+#
+#    You should have received a copy of the GNU Lesser General Public License
+#    along with sas_robot_driver.  If not, see <https://www.gnu.org/licenses/>.
+#
+# ################################################################
+#
+#   Author: Juan Jose Quiroz Omana, email: juanjqogm@gmail.com
+#
+# ################################################################
+#
+# Contributors:
+#      1. Quenitin Lin
+#
+# ################################################################
+*/
+#include <sas_robot_driver_franka/robot_interface_hand.hpp>



--- a/sas_robot_driver_franka/src/joint/robot_interface_franka.cpp
+++ b/sas_robot_driver_franka/src/joint/robot_interface_franka.cpp
@@ -30,10 +30,7 @@
 */


-#include "robot_interface_franka.h"
-
-#include <ros/this_node.h>
-#include <rosconsole/macros_generated.h>
+#include <sas_robot_driver_franka/interfaces/robot_interface_franka.hpp>


 /**
@@ -313,6 +310,7 @@ void RobotInterfaceFranka::initialize()
    initialize_flag_ = true;

    // initialize and set the robot to default behavior (collision behavior, impedance, etc)
+    // robot_sptr_->setDefaultBehavior();
    setDefaultBehavior(*robot_sptr_);
    robot_sptr_->setCollisionBehavior(
        franka_configuration_.lower_torque_threshold,
@@ -365,7 +363,7 @@ void RobotInterfaceFranka::_update_robot_state(const franka::RobotState &robot_s
    current_joint_forces_array_ = robot_state.tau_J;
    current_joint_forces_ =  Eigen::Map<VectorXd>(current_joint_forces_array_.data(), 7);

-    current_stiffness_force_torque_array_ = robot_state.O_F_ext_hat_K;
+    current_stiffness_force_torque_array_ = robot_state.K_F_ext_hat_K;
    current_stiffness_force_[0] = current_stiffness_force_torque_array_[0];
    current_stiffness_force_[1] = current_stiffness_force_torque_array_[1];
    current_stiffness_force_[2] = current_stiffness_force_torque_array_[2];
@@ -718,6 +716,7 @@ DQ RobotInterfaceFranka::get_stiffness_pose() {
    const auto base_2_ee = homgenious_tf_to_DQ(current_effeector_pose_);
    const auto ee_2_k = homgenious_tf_to_DQ(current_stiffness_pose_);
    return base_2_ee * ee_2_k;
+    // return base_2_k;
 }

 /**
--- a/sas_robot_driver_franka/src/joint/sas_robot_driver_franka.cpp
+++ b/sas_robot_driver_franka/src/joint/sas_robot_driver_franka.cpp
@@ -31,32 +31,34 @@
 */


-#include "../../include/sas_robot_driver_franka.h"
-#include "sas_clock/sas_clock.h"
+#include <sas_robot_driver_franka/sas_robot_driver_franka.hpp>
+#include <sas_core/sas_clock.hpp>
 #include <dqrobotics/utils/DQ_Math.h>
-#include <ros/this_node.h>
-#include <rosconsole/macros_generated.h>
+

 namespace sas
 {
-    RobotDriverFranka::RobotDriverFranka(RobotDynamicProvider* robot_dynamic_provider, const RobotDriverFrankaConfiguration &configuration, std::atomic_bool *break_loops):
+    RobotDriverFranka::RobotDriverFranka(
+        const std::shared_ptr<Node> &node,
+        const std::shared_ptr<qros::RobotDynamicsServer> &robot_dynamic_provider, const RobotDriverFrankaConfiguration &configuration, std::atomic_bool *break_loops
+    ):
    RobotDriver(break_loops),
+    node_(node),
    configuration_(configuration),
-    robot_dynamic_provider_(robot_dynamic_provider),
+    robot_dynamic_provider_sptr_(robot_dynamic_provider),
    break_loops_(break_loops)
   {
        joint_positions_.resize(7);
        joint_velocities_.resize(7);
-        joint_forces_.resize(7);
+        joint_torques_.resize(7);
        //end_effector_pose_.resize(7);
        //joint_positions_buffer_.resize(8,0);
        //end_effector_pose_euler_buffer_.resize(7,0);
        //end_effector_pose_homogenous_transformation_buffer_.resize(10,0);
-        std::cout<<configuration.ip_address<<std::endl;
+        // std::cout<<configuration.ip_address<<std::endl;

        RobotInterfaceFranka::MODE mode = RobotInterfaceFranka::MODE::None;
-
-        std::cout<<configuration.mode<<std::endl;
+        RCLCPP_INFO_STREAM(node_->get_logger(), "Mode: " << configuration_.mode);

        if (configuration_.mode == std::string("None"))
        {
@@ -90,7 +92,7 @@ namespace sas
        Vector3d force, torque;
        std::tie(force, torque) = robot_driver_interface_sptr_->get_stiffness_force_torque();
        const auto pose = robot_driver_interface_sptr_->get_stiffness_pose();
-        robot_dynamic_provider_->publish_stiffness(pose, force, torque);
+        robot_dynamic_provider_sptr_->publish_stiffness(pose, force, torque);
    }


@@ -143,9 +145,16 @@ namespace sas
    VectorXd  RobotDriverFranka::get_joint_positions()
    {
        if(robot_driver_interface_sptr_->get_err_state()) {
-            ROS_ERROR_STREAM("["+ros::this_node::getName()+"]::driver interface error on:"+robot_driver_interface_sptr_->get_status_message());
+            RCLCPP_ERROR_STREAM(node_->get_logger(),
+                "["+std::string(node_->get_name())+"]::driver interface "
+                "error on:"+robot_driver_interface_sptr_->get_status_message());
            break_loops_->store(true);
        }
+        if(!ok()) {
+            RCLCPP_WARN_STREAM(node_->get_logger(),
+                "["+std::string(node_->get_name())+"]::driver interface exit on shotdown signal recieved."
+                );
+        }
        _update_stiffness_contact_and_pose();
        return robot_driver_interface_sptr_->get_joint_positions();
    }
@@ -160,7 +169,9 @@ namespace sas
    {
        robot_driver_interface_sptr_->set_target_joint_positions(desired_joint_positions_rad);
        if(robot_driver_interface_sptr_->get_err_state()) {
-            ROS_ERROR_STREAM("["+ros::this_node::getName()+"]::driver interface error on:"+robot_driver_interface_sptr_->get_status_message());
+            RCLCPP_ERROR_STREAM(node_->get_logger(),
+                "["+std::string(node_->get_name())+"]::driver interface "
+                "error on:"+robot_driver_interface_sptr_->get_status_message());
            break_loops_->store(true);
        }
    }
@@ -182,10 +193,12 @@ namespace sas
     */
    void RobotDriverFranka::set_target_joint_velocities(const VectorXd &desired_joint_velocities)
    {
-        desired_joint_velocities_ = desired_joint_velocities;
+        // desired_joint_velocities_ = desired_joint_velocities;
        robot_driver_interface_sptr_->set_target_joint_velocities(desired_joint_velocities);
        if(robot_driver_interface_sptr_->get_err_state()) {
-            ROS_ERROR_STREAM("["+ros::this_node::getName()+"]::driver interface error on:"+robot_driver_interface_sptr_->get_status_message());
+            RCLCPP_ERROR_STREAM(node_->get_logger(),
+                                "["+std::string(node_->get_name())+"]::driver interface "
+                                "error on:"+robot_driver_interface_sptr_->get_status_message());
            break_loops_->store(true);
        }
    }
@@ -194,10 +207,10 @@ namespace sas
     * @brief RobotDriverFranka::get_joint_forces
     * @return
     */
-    VectorXd RobotDriverFranka::get_joint_forces()
+    VectorXd RobotDriverFranka::get_joint_torques()
    {
-        joint_forces_ = robot_driver_interface_sptr_->get_joint_forces();
-        return joint_forces_;
+        joint_torques_ = robot_driver_interface_sptr_->get_joint_forces();
+        return joint_torques_;
    }

 }
--- a/sas_robot_driver_franka/src/robot_dynamics/qros_robot_dynamics_client.cpp
+++ b/sas_robot_driver_franka/src/robot_dynamics/qros_robot_dynamics_client.cpp
@@ -0,0 +1,102 @@
+/*
+# Copyright (c) 2024 Quenitin Lin
+#
+#    This file is part of sas_robot_driver_franka.
+#
+#    sas_robot_driver_franka is free software: you can redistribute it and/or modify
+#    it under the terms of the GNU Lesser General Public License as published by
+#    the Free Software Foundation, either version 3 of the License, or
+#    (at your option) any later version.
+#
+#    sas_robot_driver_franka is distributed in the hope that it will be useful,
+#    but WITHOUT ANY WARRANTY; without even the implied warranty of
+#    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+#    GNU Lesser General Public License for more details.
+#
+#    You should have received a copy of the GNU Lesser General Public License
+#    along with sas_robot_driver.  If not, see <https://www.gnu.org/licenses/>.
+#
+# ################################################################
+#
+#   Author: Quenitin Lin
+#
+# ################################################################
+#
+# Contributors:
+#      1. Quenitin Lin
+#
+# ################################################################
+*/
+#include <memory>
+#include <sas_robot_driver_franka/robot_dynamic/qros_robot_dynamics_client.hpp>
+
+using namespace qros;
+
+
+RobotDynamicsClient::RobotDynamicsClient(const std::shared_ptr<Node> &node, const std::string& topic_prefix):
+    node_(node), topic_prefix_(topic_prefix == "GET_FROM_NODE"? node->get_name() : topic_prefix),
+    child_frame_id_(topic_prefix_ + "_stiffness_frame"), parent_frame_id_(topic_prefix_ + "_base"),
+    last_stiffness_force_(Vector3d::Zero()),
+    last_stiffness_torque_(Vector3d::Zero()),
+    last_stiffness_frame_pose_(0)
+{
+    tf_buffer_ = std::make_unique<tf2_ros::Buffer>(node->get_clock());
+    tf_listener_ = std::make_shared<tf2_ros::TransformListener>(*tf_buffer_);
+
+    // Strip potential leading slash
+    if(child_frame_id_.front() == '/'){child_frame_id_ = child_frame_id_.substr(1);}
+    if(parent_frame_id_.front() == '/'){parent_frame_id_ = parent_frame_id_.substr(1);}
+    RCLCPP_INFO_STREAM(node_->get_logger(),
+        "["+ std::string(node_->get_name()) + "]::Initializing RobotDynamicsClient with prefix " + topic_prefix);
+    subscriber_cartesian_stiffness_ = node_->create_subscription<geometry_msgs::msg::WrenchStamped>(topic_prefix + "/get/cartesian_stiffness", 1,
+        std::bind(&RobotDynamicsClient::_callback_cartesian_stiffness, this, std::placeholders::_1)
+        );
+
+}
+
+void RobotDynamicsClient::_callback_cartesian_stiffness(const geometry_msgs::msg::WrenchStamped &msg)
+{
+    last_stiffness_force_(0) = msg.wrench.force.x;
+    last_stiffness_force_(1) = msg.wrench.force.y;
+    last_stiffness_force_(2) = msg.wrench.force.z;
+
+    last_stiffness_torque_(0) = msg.wrench.torque.x;
+    last_stiffness_torque_(1) = msg.wrench.torque.y;
+    last_stiffness_torque_(2) = msg.wrench.torque.z;
+
+    try {
+        const auto transform_stamped = tf_buffer_->lookupTransform( parent_frame_id_, child_frame_id_, tf2::TimePointZero);
+        last_stiffness_frame_pose_ = _geometry_msgs_transform_to_dq(transform_stamped.transform);
+    } catch (tf2::TransformException &ex) {
+        RCLCPP_WARN_STREAM(node_->get_logger(), "["+ std::string(node_->get_name()) + "]::" + ex.what());
+    }
+}
+
+DQ RobotDynamicsClient::_geometry_msgs_transform_to_dq(const geometry_msgs::msg::Transform& transform)
+{
+    const auto t = DQ(0,transform.translation.x,transform.translation.y,transform.translation.z);
+    const auto r = DQ(transform.rotation.w, transform.rotation.x, transform.rotation.y, transform.rotation.z);
+    return r + 0.5 * E_ * t * r;
+}
+
+VectorXd RobotDynamicsClient::get_stiffness_force()
+{
+    if(!is_enabled()) throw std::runtime_error("[RobotDynamicsClient]::calling get_stiffness_force on uninitialized topic");
+    return last_stiffness_force_;
+}
+VectorXd RobotDynamicsClient::get_stiffness_torque()
+{
+    if(!is_enabled()) throw std::runtime_error("[RobotDynamicsClient]::calling get_stiffness_torque on uninitialized topic");
+    return last_stiffness_torque_;
+}
+DQ RobotDynamicsClient::get_stiffness_frame_pose()
+{
+    if(!is_enabled()) throw std::runtime_error("[RobotDynamicsClient]::calling get_stiffness_frame_pose on uninitialized Interface");
+    return last_stiffness_frame_pose_;
+}
+
+bool RobotDynamicsClient::is_enabled() const
+{
+    if(last_stiffness_frame_pose_==0) return false;
+    return true;
+}
--- a/sas_robot_driver_franka/src/robot_dynamics/qros_robot_dynamics_py.cpp
+++ b/sas_robot_driver_franka/src/robot_dynamics/qros_robot_dynamics_py.cpp
@@ -0,0 +1,60 @@
+/*
+# Copyright (c) 2023 Juan Jose Quiroz Omana
+#
+#    This file is part of sas_robot_driver_franka.
+#
+#    sas_robot_driver_franka is free software: you can redistribute it and/or modify
+#    it under the terms of the GNU Lesser General Public License as published by
+#    the Free Software Foundation, either version 3 of the License, or
+#    (at your option) any later version.
+#
+#    sas_robot_driver_franka is distributed in the hope that it will be useful,
+#    but WITHOUT ANY WARRANTY; without even the implied warranty of
+#    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+#    GNU Lesser General Public License for more details.
+#
+#    You should have received a copy of the GNU Lesser General Public License
+#    along with sas_robot_driver.  If not, see <https://www.gnu.org/licenses/>.
+#
+# ################################################################
+#
+#   Author: Quenitin Lin
+#
+# ################################################################
+#
+# Contributors:
+#      1. Quenitin Lin
+#
+# ################################################################
+*/
+
+#include <pybind11/pybind11.h>
+#include <pybind11/stl.h>
+#include <pybind11/eigen.h>
+
+#include <sas_robot_driver_franka/robot_dynamic/qros_robot_dynamics_client.hpp>
+#include <sas_robot_driver_franka/robot_dynamic/qros_robot_dynamics_server.hpp>
+
+namespace py = pybind11;
+using RDC = qros::RobotDynamicsClient;
+using RDS = qros::RobotDynamicsServer;
+
+
+PYBIND11_MODULE(_qros_franka_robot_dynamics_py, m)
+{
+    py::class_<RDC>(m, "RobotDynamicsClient")
+        .def(py::init<const std::shared_ptr<rclcpp::Node>&,const std::string&>())
+        .def("get_stiffness_force",&RDC::get_stiffness_force)
+        .def("get_stiffness_torque",&RDC::get_stiffness_torque)
+        .def("get_stiffness_frame_pose",&RDC::get_stiffness_frame_pose)
+        .def("is_enabled",&RDC::is_enabled,"Returns true if the RobotDynamicInterface is enabled.")
+        .def("get_topic_prefix",&RDC::get_topic_prefix);
+
+    py::class_<RDS>(m, "RobotDynamicsServer")
+        .def(py::init<const std::shared_ptr<rclcpp::Node>&,const std::string&>())
+        .def("publish_stiffness",&RDS::publish_stiffness)
+        .def("set_world_to_base_tf", &RDS::set_world_to_base_tf)
+        .def("is_enabled",&RDS::is_enabled,"Returns true if the RobotDynamicProvider is enabled.")
+        .def("get_topic_prefix",&RDS::get_topic_prefix);
+
+}
--- a/sas_robot_driver_franka/src/robot_dynamics/qros_robot_dynamics_server.cpp
+++ b/sas_robot_driver_franka/src/robot_dynamics/qros_robot_dynamics_server.cpp
@@ -0,0 +1,120 @@
+/*
+# Copyright (c) 2024 Quenitin Lin
+#
+#    This file is part of sas_robot_driver_franka.
+#
+#    sas_robot_driver_franka is free software: you can redistribute it and/or modify
+#    it under the terms of the GNU Lesser General Public License as published by
+#    the Free Software Foundation, either version 3 of the License, or
+#    (at your option) any later version.
+#
+#    sas_robot_driver_franka is distributed in the hope that it will be useful,
+#    but WITHOUT ANY WARRANTY; without even the implied warranty of
+#    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+#    GNU Lesser General Public License for more details.
+#
+#    You should have received a copy of the GNU Lesser General Public License
+#    along with sas_robot_driver.  If not, see <https://www.gnu.org/licenses/>.
+#
+# ################################################################
+#
+#   Author: Quenitin Lin
+#
+# ################################################################
+#
+# Contributors:
+#      1. Quenitin Lin
+#
+# ################################################################
+*/
+#include <sas_robot_driver_franka/robot_dynamic/qros_robot_dynamics_server.hpp>
+
+using namespace qros;
+
+RobotDynamicsServer::RobotDynamicsServer(const std::shared_ptr<Node> &node, const std::string& topic_prefix):
+    node_(node), topic_prefix_(topic_prefix == "GET_FROM_NODE"? node->get_name() : topic_prefix),
+    child_frame_id_(topic_prefix_ + "_stiffness_frame"), parent_frame_id_(topic_prefix_ + "_base"),
+    tf_broadcaster_(std::make_shared<tf2_ros::TransformBroadcaster>(node_)),
+    static_base_tf_broadcaster_(std::make_shared<tf2_ros::StaticTransformBroadcaster>(node_)),
+    world_to_base_tf_(0)
+{
+    // Strip potential leading slash
+    if(child_frame_id_.front() == '/'){child_frame_id_ = child_frame_id_.substr(1);}
+    if(parent_frame_id_.front() == '/'){parent_frame_id_ = parent_frame_id_.substr(1);}
+    RCLCPP_INFO_STREAM(node_->get_logger(),
+        "["+ std::string(node_->get_name()) + "]::Initializing RobotDynamicsServer with prefix " + topic_prefix);
+    publisher_cartesian_stiffness_ = node_->create_publisher<geometry_msgs::msg::WrenchStamped>(topic_prefix + "/get/cartesian_stiffness", 1);
+}
+
+geometry_msgs::msg::Transform RobotDynamicsServer::_dq_to_geometry_msgs_transform(const DQ& pose)
+{
+    geometry_msgs::msg::Transform tf_msg;
+    const auto t = translation(pose);
+    const auto r = rotation(pose);
+    tf_msg.translation.x = t.q(1);
+    tf_msg.translation.y = t.q(2);
+    tf_msg.translation.z = t.q(3);
+    tf_msg.rotation.w = r.q(0);
+    tf_msg.rotation.x = r.q(1);
+    tf_msg.rotation.y = r.q(2);
+    tf_msg.rotation.z = r.q(3);
+    return tf_msg;
+}
+
+void RobotDynamicsServer::set_world_to_base_tf(const DQ& world_to_base_tf)
+{
+    if(world_to_base_tf_==0)
+    {
+        world_to_base_tf_ = world_to_base_tf;
+        _publish_base_static_tf();
+    }else
+    {
+        throw std::runtime_error("["+ std::string(node_->get_name()) + "]::[RobotDynamicsServer]::The world to base transform has already been set");
+    }
+}
+
+void RobotDynamicsServer::_publish_base_static_tf()
+{
+    geometry_msgs::msg::TransformStamped base_tf;
+    base_tf.set__transform(_dq_to_geometry_msgs_transform(world_to_base_tf_));
+    std_msgs::msg::Header header;
+    header.set__stamp(node_->now());
+    header.set__frame_id(WORLD_FRAME_ID);
+    base_tf.set__header(header);
+    base_tf.set__child_frame_id(parent_frame_id_);
+    static_base_tf_broadcaster_->sendTransform(base_tf);
+
+}
+
+
+void RobotDynamicsServer::publish_stiffness(const DQ& base_to_stiffness, const Vector3d& force, const Vector3d& torque)
+{
+    std_msgs::msg::Header header;
+    header.set__stamp(node_->now());
+    geometry_msgs::msg::WrenchStamped msg;
+    msg.set__header(header);
+    msg.header.set__frame_id(child_frame_id_);
+    msg.wrench.force.set__x(force(0));
+    msg.wrench.force.set__y(force(1));
+    msg.wrench.force.set__z(force(2));
+    msg.wrench.torque.set__x(torque(0));
+    msg.wrench.torque.set__y(torque(1));
+    msg.wrench.torque.set__z(torque(2));
+    publisher_cartesian_stiffness_->publish(msg);
+    if(seq_ % REDUCE_TF_PUBLISH_RATE == 0)
+    {
+        header.set__frame_id(parent_frame_id_);
+        geometry_msgs::msg::TransformStamped tf_msg;
+        tf_msg.set__transform(_dq_to_geometry_msgs_transform(base_to_stiffness));
+        tf_msg.set__header(header);
+        tf_msg.set__child_frame_id(child_frame_id_);
+        tf_broadcaster_->sendTransform(tf_msg);
+    }
+}
+
+
+bool RobotDynamicsServer::is_enabled() const
+{
+    return true; //Always enabled
+}
+
--- a/sas_robot_driver_franka/src/sas_robot_driver_coppelia_node.cpp
+++ b/sas_robot_driver_franka/src/sas_robot_driver_coppelia_node.cpp
@@ -0,0 +1,106 @@
+/*
+# Copyright (c) 2023 Juan Jose Quiroz Omana
+#
+#    This file is part of sas_robot_driver_franka.
+#
+#    sas_robot_driver_franka is free software: you can redistribute it and/or modify
+#    it under the terms of the GNU Lesser General Public License as published by
+#    the Free Software Foundation, either version 3 of the License, or
+#    (at your option) any later version.
+#
+#    sas_robot_driver_franka is distributed in the hope that it will be useful,
+#    but WITHOUT ANY WARRANTY; without even the implied warranty of
+#    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+#    GNU Lesser General Public License for more details.
+#
+#    You should have received a copy of the GNU Lesser General Public License
+#    along with sas_robot_driver.  If not, see <https://www.gnu.org/licenses/>.
+#
+# ################################################################
+#
+#   Author: Juan Jose Quiroz Omana, email: juanjqogm@gmail.com
+#
+# ################################################################
+#
+# Contributors:
+#      1. Juan Jose Quiroz Omana (juanjqogm@gmail.com)
+#         - Adapted from sas_robot_driver_denso_node.cpp
+#           (https://github.com/SmartArmStack/sas_robot_driver_denso/blob/master/src/sas_robot_driver_denso_node.cpp)
+#      2. Quentin Lin (qlin1806@g.ecc.u-tokyo.ac.jp)
+#         - Adapted for simplify operation
+#         - porting to ROS2
+#
+# ################################################################
+*/
+
+
+#include <sstream>
+
+#include <exception>
+#include <dqrobotics/utils/DQ_Math.h>
+//#include <dqrobotics/interfaces/json11/DQ_JsonReader.h>
+#include <sas_common/sas_common.hpp>
+#include <sas_conversions/eigen3_std_conversions.hpp>
+#include <sas_robot_driver_franka/coppelia/sas_robot_driver_coppelia.hpp>
+
+/*********************************************
+ * SIGNAL HANDLER
+ * *******************************************/
+#include<signal.h>
+static std::atomic_bool kill_this_process(false);
+void sig_int_handler(int)
+{
+    kill_this_process = true;
+}
+
+
+
+int main(int argc, char **argv)
+{
+    if(signal(SIGINT, sig_int_handler) == SIG_ERR)
+    {
+        throw std::runtime_error("Error setting the signal int handler.");
+    }
+
+    rclcpp::init(argc,argv,rclcpp::InitOptions(),rclcpp::SignalHandlerOptions::None);
+    auto context = rclcpp::contexts::get_global_default_context();
+
+    auto node = std::make_shared<rclcpp::Node>("sas_robot_driver_franka_coppelia_node");
+
+    const auto node_name = std::string(node->get_name());
+    RCLCPP_WARN(node->get_logger(),"=====================================================================");
+    RCLCPP_WARN(node->get_logger(),"-----------------Adapted by Quentin Lin ------------------------");
+    RCLCPP_WARN(node->get_logger(),"=====================================================================");
+    RCLCPP_INFO_STREAM_ONCE(node->get_logger(),":Loading parameters from parameter server.");
+
+
+    qros::RobotDriverCoppeliaConfiguration robot_driver_coppelia_configuration;
+    sas::get_ros_parameter(node,"thread_sampling_time_sec",robot_driver_coppelia_configuration.thread_sampling_time_sec);
+    sas::get_ros_parameter(node,"vrep_ip",robot_driver_coppelia_configuration.vrep_ip);
+    sas::get_ros_parameter(node,"vrep_port",robot_driver_coppelia_configuration.vrep_port);
+    sas::get_ros_parameter(node,"vrep_joint_names", robot_driver_coppelia_configuration.vrep_joint_names);
+    sas::get_ros_parameter(node,"vrep_dynamically_enabled", robot_driver_coppelia_configuration.vrep_dynamically_enabled);
+    sas::get_ros_parameter(node,"robot_mode", robot_driver_coppelia_configuration.robot_mode);
+    sas::get_ros_parameter(node,"using_real_robot", robot_driver_coppelia_configuration.using_real_robot);
+    sas::get_ros_parameter(node,"robot_topic_prefix", robot_driver_coppelia_configuration.robot_topic_prefix);
+    sas::get_ros_parameter(node,"robot_parameter_file_path", robot_driver_coppelia_configuration.robot_parameter_file_path);
+
+    try
+    {
+        RCLCPP_INFO_STREAM_ONCE(node->get_logger(),"::Instantiating Coppelia robot mirror.");
+        qros::RobotDriverCoppelia robot_driver_coppelia(node, robot_driver_coppelia_configuration,
+                                                       &kill_this_process);
+
+        return robot_driver_coppelia.start_control_loop();
+    }
+    catch (const std::exception& e)
+    {
+        kill_this_process = true;
+        RCLCPP_ERROR_STREAM_ONCE(node->get_logger(),"["+node_name+"]::Exception::" + e.what());
+        shutdown(context, "Exception in main function: " + std::string(e.what()));
+        return -1;
+    }
+
+
+    return 0;
+}
--- a/sas_robot_driver_franka/src/sas_robot_driver_franka_hand_node.cpp
+++ b/sas_robot_driver_franka/src/sas_robot_driver_franka_hand_node.cpp
@@ -0,0 +1,122 @@
+/*
+# Copyright (c) 2023 Juan Jose Quiroz Omana
+#
+#    This file is part of sas_robot_driver_franka.
+#
+#    sas_robot_driver_franka is free software: you can redistribute it and/or modify
+#    it under the terms of the GNU Lesser General Public License as published by
+#    the Free Software Foundation, either version 3 of the License, or
+#    (at your option) any later version.
+#
+#    sas_robot_driver_franka is distributed in the hope that it will be useful,
+#    but WITHOUT ANY WARRANTY; without even the implied warranty of
+#    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+#    GNU Lesser General Public License for more details.
+#
+#    You should have received a copy of the GNU Lesser General Public License
+#    along with sas_robot_driver.  If not, see <https://www.gnu.org/licenses/>.
+#
+# ################################################################
+#
+#   Author: Juan Jose Quiroz Omana, email: juanjqogm@gmail.com
+#
+# ################################################################
+#
+# Contributors:
+#      1. Quentin Lin (qlin1806@g.ecc.u-tokyo.ac.jp)
+#         - Adapted from sas_robot_driver_franka
+# ################################################################
+*/
+
+
+#include <sstream>
+
+#include <exception>
+#include <dqrobotics/utils/DQ_Math.h>
+#include <sas_common/sas_common.hpp>
+// #include <sas_conversions/eigen3_std_conversions.hpp>
+#include <sas_robot_driver_franka/interfaces/qros_effector_driver_franka_hand.hpp>
+
+
+/*********************************************
+ * SIGNAL HANDLER
+ * *******************************************/
+#include<signal.h>
+static std::atomic_bool kill_this_process(false);
+void sig_int_handler(int)
+{
+    kill_this_process = true;
+}
+
+
+
+template<typename T>
+void get_optional_parameter(std::shared_ptr<Node> node, const std::string &param_name, T &param)
+{
+    try
+    {
+        sas::get_ros_parameter(node,param_name,param);
+    }catch (const std::exception& e)
+    {
+        RCLCPP_INFO_STREAM(node->get_logger(), "["+std::string(node->get_name())+"]:Parameter " + param_name + " not found. Using default value. " + std::to_string(param));
+    }
+
+}
+
+
+int main(int argc, char **argv)
+{
+    if(signal(SIGINT, sig_int_handler) == SIG_ERR)
+    {
+        throw std::runtime_error("Error setting the signal int handler.");
+    }
+
+    rclcpp::init(argc,argv,rclcpp::InitOptions(),rclcpp::SignalHandlerOptions::None);
+    auto node = std::make_shared<rclcpp::Node>("sas_robot_driver_franka_hand_node");
+
+    const auto node_name = std::string(node->get_name());
+    RCLCPP_WARN(node->get_logger(),"=====================================================================");
+    RCLCPP_WARN(node->get_logger(),"---------------------------Quentin Lin-------------------------------");
+    RCLCPP_WARN(node->get_logger(),"=====================================================================");
+    RCLCPP_INFO_STREAM_ONCE(node->get_logger(),"["+node_name+"]::Loading parameters from parameter server.");
+
+    qros::EffectorDriverFrankaHandConfiguration robot_driver_franka_hand_configuration;
+    get_optional_parameter(node, "initialize_with_homing", robot_driver_franka_hand_configuration.initialize_with_homing);
+    sas::get_ros_parameter(node,"robot_ip_address",robot_driver_franka_hand_configuration.robot_ip);
+    sas::get_ros_parameter(node,"thread_sampling_time_sec",robot_driver_franka_hand_configuration.thread_sampeling_time_s);
+    sas::get_ros_parameter(node,"default_force",robot_driver_franka_hand_configuration.default_force);
+    sas::get_ros_parameter(node,"default_speed",robot_driver_franka_hand_configuration.default_speed);
+    sas::get_ros_parameter(node,"default_epsilon_inner",robot_driver_franka_hand_configuration.default_epsilon_inner);
+    sas::get_ros_parameter(node,"default_epsilon_outer",robot_driver_franka_hand_configuration.default_epsilon_outer);
+
+    qros::EffectorDriverFrankaHand franka_hand_driver(
+        node_name,
+        robot_driver_franka_hand_configuration,
+        node,
+        &kill_this_process
+    );
+    try
+    {
+        RCLCPP_INFO_STREAM(node->get_logger(),"["+node_name+"]::Instantiating Franka hand.");
+        franka_hand_driver.connect();
+        franka_hand_driver.initialize();
+        RCLCPP_INFO_STREAM(node->get_logger(),"["+node_name+"]::Starting control loop.");
+        franka_hand_driver.start_control_loop();
+
+    }
+    catch (const std::exception& e)
+    {
+        RCLCPP_ERROR_STREAM(node->get_logger(), "["+node_name+"]::Exception::" + e.what());
+    }catch (...)
+    {
+        RCLCPP_ERROR_STREAM(node->get_logger(), "["+node_name+"]::Exception::Unknown exception.");
+    }
+    RCLCPP_INFO_STREAM(node->get_logger(),"["+node_name+"]::Exiting...");
+    franka_hand_driver.deinitialize();
+    RCLCPP_INFO_STREAM(node->get_logger(),"["+node_name+"]::Deinitialized.");
+    franka_hand_driver.disconnect();
+    RCLCPP_INFO_STREAM(node->get_logger(),"["+node_name+"]::Disconnected.");
+
+
+  return 0;
+}
--- a/sas_robot_driver_franka/src/sas_robot_driver_franka_node.cpp
+++ b/sas_robot_driver_franka/src/sas_robot_driver_franka_node.cpp
@@ -0,0 +1,191 @@
+/*
+# Copyright (c) 2023 Juan Jose Quiroz Omana
+#
+#    This file is part of sas_robot_driver_franka.
+#
+#    sas_robot_driver_franka is free software: you can redistribute it and/or modify
+#    it under the terms of the GNU Lesser General Public License as published by
+#    the Free Software Foundation, either version 3 of the License, or
+#    (at your option) any later version.
+#
+#    sas_robot_driver_franka is distributed in the hope that it will be useful,
+#    but WITHOUT ANY WARRANTY; without even the implied warranty of
+#    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+#    GNU Lesser General Public License for more details.
+#
+#    You should have received a copy of the GNU Lesser General Public License
+#    along with sas_robot_driver.  If not, see <https://www.gnu.org/licenses/>.
+#
+# ################################################################
+#
+#   Author: Juan Jose Quiroz Omana, email: juanjqogm@gmail.com
+#
+# ################################################################
+#
+# Contributors:
+#      1. Juan Jose Quiroz Omana (juanjqogm@gmail.com)
+#         - Adapted from sas_robot_driver_denso_node.cpp
+#           (https://github.com/SmartArmStack/sas_robot_driver_denso/blob/master/src/sas_robot_driver_denso_node.cpp)
+#      2. Quentin Lin
+#         - Adaption to ROS2
+#
+# ################################################################
+*/
+
+
+#include <sstream>
+
+#include <exception>
+#include <dqrobotics/utils/DQ_Math.h>
+#include <dqrobotics/interfaces/json11/DQ_JsonReader.h>
+#include <sas_common/sas_common.hpp>
+#include <sas_conversions/eigen3_std_conversions.hpp>
+#include <sas_robot_driver/sas_robot_driver_ros.hpp>
+#include <sas_robot_driver_franka/sas_robot_driver_franka.hpp>
+#include <sas_robot_driver_franka/robot_dynamic/qros_robot_dynamics_server.hpp>
+
+
+/*********************************************
+ * SIGNAL HANDLER
+ * *******************************************/
+#include<signal.h>
+static std::atomic_bool kill_this_process(false);
+void sig_int_handler(int)
+{
+    kill_this_process = true;
+}
+
+template<typename T, std::size_t N>
+std::array<T, N> apply_scale_to_std_array(const std::array<T, N>& array, const T& scale)
+{
+    std::array<T, N> scaled_array;
+    for(std::size_t i = 0; i < N; i++)
+    {
+        scaled_array[i] = array[i] * scale;
+    }
+    return scaled_array;
+}
+template<typename T, std::size_t N>
+std::array<T, N> std_vec_to_std_array(const std::vector<T>& vector)
+{
+    if(N != vector.size()){throw std::runtime_error("Size mismatch between vector and array.");}
+    std::array<T, N> array;
+    for(std::size_t i = 0; i < N; i++)
+    {
+        array[i] = vector[i];
+    }
+    return array;
+}
+VectorXd std_vec_to_eigen_vector(const std::vector<double>& vector)
+{
+    VectorXd eigen_vector(vector.size());
+    for(std::size_t i = 0; i < vector.size(); i++)
+    {
+        eigen_vector(i) = vector[i];
+    }
+    return eigen_vector;
+}
+
+
+int main(int argc, char **argv)
+{
+    if(signal(SIGINT, sig_int_handler) == SIG_ERR)
+    {
+        throw std::runtime_error("Error setting the signal int handler.");
+    }
+
+    rclcpp::init(argc,argv,rclcpp::InitOptions(),rclcpp::SignalHandlerOptions::None);
+    auto node = std::make_shared<rclcpp::Node>("sas_robot_driver_franka_node");
+
+    const auto node_name = std::string(node->get_name());
+    RCLCPP_WARN(node->get_logger(),"=====================================================================");
+    RCLCPP_WARN(node->get_logger(),"-----------------Adapted by Quentin Lin ------------------------");
+    RCLCPP_WARN(node->get_logger(),"=====================================================================");
+    RCLCPP_INFO_STREAM_ONCE(node->get_logger(),"["+node_name+"]::Loading parameters from parameter server.");
+    
+    sas::RobotDriverFrankaConfiguration robot_driver_franka_configuration;
+    RobotInterfaceFranka::FrankaInterfaceConfiguration franka_interface_configuration;
+
+    sas::get_ros_parameter(node,"robot_ip_address",robot_driver_franka_configuration.ip_address);
+    sas::get_ros_parameter(node,"robot_mode",      robot_driver_franka_configuration.mode);
+    double upper_scale_factor = 1.0;
+    std::vector<std::string> all_params;
+
+    try {
+        sas::get_ros_parameter(node,"force_upper_limits_scaling_factor",upper_scale_factor);
+        RCLCPP_WARN_STREAM_ONCE(node->get_logger(),"Set force upper limits scaling factor: " << upper_scale_factor);
+    }catch(...) {
+        RCLCPP_INFO_STREAM_ONCE(node->get_logger(),"Force upper limits scaling factor is not set.");
+    }
+
+    try {
+        std::vector<double> upper_torque_threshold_std_vec;
+        sas::get_ros_parameter(node,"upper_torque_threshold",upper_torque_threshold_std_vec);
+        RCLCPP_INFO_STREAM_ONCE(node->get_logger(),"["+node_name+"]::Upper torque threshold override and set.");
+        franka_interface_configuration.upper_torque_threshold = std_vec_to_std_array<double,7>(upper_torque_threshold_std_vec);
+    }catch(...) {
+        RCLCPP_INFO_STREAM_ONCE(node->get_logger(),"["+node_name+"]::Upper torque threshold not set. Using default with value scalling.");
+        franka_interface_configuration.upper_torque_threshold = apply_scale_to_std_array(franka_interface_configuration.upper_torque_threshold, upper_scale_factor);
+    }
+    try {
+        std::vector<double> upper_force_threshold_std_vec;
+        sas::get_ros_parameter(node,"upper_force_threshold",upper_force_threshold_std_vec);
+        RCLCPP_INFO_STREAM_ONCE(node->get_logger(),"["+node_name+"]::Upper force threshold override and set.");
+        franka_interface_configuration.upper_force_threshold = std_vec_to_std_array<double,6>(upper_force_threshold_std_vec);
+    }catch(...) {
+        RCLCPP_INFO_STREAM_ONCE(node->get_logger(),"["+node_name+"]::Upper torque threshold not set. Using default with value scalling.");
+        franka_interface_configuration.upper_force_threshold = apply_scale_to_std_array(franka_interface_configuration.upper_force_threshold, upper_scale_factor);
+    }
+    try {
+        std::string robot_parameter_file_path;
+        sas::get_ros_parameter(node,"robot_parameter_file_path",robot_parameter_file_path);
+        RCLCPP_INFO_STREAM_ONCE(node->get_logger(),"["+node_name+"]::Loading robot parameters from file: " + robot_parameter_file_path);
+        const auto robot = DQ_JsonReader::get_from_json<DQ_SerialManipulatorDH>(robot_parameter_file_path);
+        robot_driver_franka_configuration.robot_reference_frame = robot.get_reference_frame();
+    }catch(...) {
+        RCLCPP_INFO_STREAM_ONCE(node->get_logger(),"["+node_name+"]::Robot parameter file path not set. Robot Model Unknow.");
+    }
+
+    robot_driver_franka_configuration.interface_configuration = franka_interface_configuration;
+
+    sas::RobotDriverROSConfiguration robot_driver_ros_configuration;
+
+    sas::get_ros_parameter(node,"thread_sampling_time_sec",robot_driver_ros_configuration.thread_sampling_time_sec);
+    sas::get_ros_parameter(node,"q_min",robot_driver_ros_configuration.q_min);
+    sas::get_ros_parameter(node,"q_max",robot_driver_ros_configuration.q_max);
+
+    // initialize the robot dynamic provider
+    robot_driver_ros_configuration.robot_driver_provider_prefix = node_name;
+    std::shared_ptr<qros::RobotDynamicsServer> robot_dynamic_provider_ptr = std::make_shared<qros::RobotDynamicsServer>(node, robot_driver_ros_configuration.robot_driver_provider_prefix);
+    if(robot_driver_franka_configuration.robot_reference_frame!=0)
+    {
+        robot_dynamic_provider_ptr->set_world_to_base_tf(robot_driver_franka_configuration.robot_reference_frame);
+    }
+
+    try
+        {
+            RCLCPP_INFO_STREAM_ONCE(node->get_logger(),"["+node_name+"]::Instantiating Franka robot.");
+            std::shared_ptr<sas::RobotDriverFranka> robot_driver_franka = std::make_shared<sas::RobotDriverFranka>(
+                node,
+                robot_dynamic_provider_ptr,
+                robot_driver_franka_configuration,
+                &kill_this_process
+            );
+            std::tuple<VectorXd, VectorXd> joint_limits = {std_vec_to_eigen_vector(robot_driver_ros_configuration.q_min), std_vec_to_eigen_vector(robot_driver_ros_configuration.q_max)};
+            robot_driver_franka -> set_joint_limits(joint_limits);
+            RCLCPP_INFO_STREAM_ONCE(node->get_logger(),"["+node_name+"]::Instantiating RobotDriverROS.");
+            sas::RobotDriverROS robot_driver_ros(node,
+                                                robot_driver_franka,
+                                                robot_driver_ros_configuration,
+                                                &kill_this_process);
+            robot_driver_ros.control_loop();
+        }
+    catch (const std::exception& e)
+    {
+        kill_this_process = true;
+        RCLCPP_ERROR_STREAM_ONCE(node->get_logger(),"["+node_name+"]::Exception::" + e.what());
+    }     
+ 
+
+  return 0;
+}
--- a/sas_robot_driver_franka_interfaces/CMakeLists.txt
+++ b/sas_robot_driver_franka_interfaces/CMakeLists.txt
@@ -0,0 +1,33 @@
+cmake_minimum_required(VERSION 3.8)
+project(sas_robot_driver_franka_interfaces)
+
+if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
+  add_compile_options(-Wall -Wextra -Wpedantic)
+endif()
+
+# find dependencies
+find_package(ament_cmake REQUIRED)
+find_package(std_msgs REQUIRED)
+find_package(geometry_msgs REQUIRED)
+find_package(rosidl_default_generators REQUIRED)
+
+rosidl_generate_interfaces(${PROJECT_NAME}
+        "msg/GripperState.msg"
+        "srv/Move.srv"
+        "srv/Grasp.srv"
+)
+
+install(
+        DIRECTORY msg
+        DESTINATION share/${PROJECT_NAME}/msg
+)
+
+install(
+        DIRECTORY srv
+        DESTINATION share/${PROJECT_NAME}/srv
+)
+
+
+
+
+ament_package()
--- a/sas_robot_driver_franka_interfaces/msg/GripperState.msg
+++ b/sas_robot_driver_franka_interfaces/msg/GripperState.msg
@@ -0,0 +1,5 @@
+float32 width
+float32 max_width
+bool is_grasped
+uint16 temperature
+uint64 duration_ms
--- a/sas_robot_driver_franka_interfaces/package.xml
+++ b/sas_robot_driver_franka_interfaces/package.xml
@@ -0,0 +1,26 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>sas_robot_driver_franka_interfaces</name>
+  <version>0.0.0</version>
+  <description>sas_driver_franka interfaces package</description>
+  <maintainer email="qlin1806@gmail.com">qlin</maintainer>
+  <license>TODO: License declaration</license>
+
+  <depend>std_msgs</depend>
+  <depend>geometry_msgs</depend>
+
+
+  <buildtool_depend>ament_cmake</buildtool_depend>
+  <build_depend>rosidl_default_generators</build_depend>
+  <member_of_group>rosidl_interface_packages</member_of_group>
+
+  <test_depend>ament_lint_auto</test_depend>
+  <test_depend>ament_lint_common</test_depend>
+
+  <exec_depend>rosidl_default_runtime</exec_depend>
+
+  <export>
+    <build_type>ament_cmake</build_type>
+  </export>
+</package>
--- a/sas_robot_driver_franka_interfaces/srv/Grasp.srv
+++ b/sas_robot_driver_franka_interfaces/srv/Grasp.srv
@@ -0,0 +1,7 @@
+float32 width
+float32 speed
+float32 force
+float32 epsilon_inner
+float32 epsilon_outer
+---
+bool success
--- a/sas_robot_driver_franka_interfaces/srv/Move.srv
+++ b/sas_robot_driver_franka_interfaces/srv/Move.srv
@@ -0,0 +1,4 @@
+float32 width
+float32 speed
+---
+bool success
--- a/src/coppelia/sas_robot_driver_coppelia.cpp
+++ b/src/coppelia/sas_robot_driver_coppelia.cpp
@@ -1,253 +0,0 @@
-#include "sas_robot_driver_coppelia.h"
-
-
-namespace sas
-{
-
-
-RobotDriverCoppelia::RobotDriverCoppelia(const RobotDriverCoppeliaConfiguration &configuration, std::atomic_bool *break_loops):
-RobotDriver(break_loops),
-    configuration_(configuration),
-    robot_mode_(configuration.robot_mode),
-    jointnames_(configuration.jointnames),
-    mirror_mode_(configuration.mirror_mode),
-    dim_configuration_space_(configuration.jointnames.size()),
-    real_robot_topic_prefix_(configuration.real_robot_topic_prefix)
-{
-    vi_ = std::make_shared<DQ_VrepInterface>();
-    desired_joint_velocities_ = VectorXd::Zero(dim_configuration_space_);
-    auto nodehandle = ros::NodeHandle();
-    std::cout<<"RobotDriverCoppelia::Rostopic: "<<"/"+real_robot_topic_prefix_<<std::endl;
-    franka1_ros_ = std::make_shared<sas::RobotDriverInterface>(nodehandle, "/"+real_robot_topic_prefix_);
-}
-
-VectorXd RobotDriverCoppelia::get_joint_positions()
-{
-    return current_joint_positions_;
-}
-
-void RobotDriverCoppelia::set_target_joint_positions(const VectorXd &desired_joint_positions_rad)
-{
-    desired_joint_positions_ = desired_joint_positions_rad;
-}
-
-VectorXd RobotDriverCoppelia::get_joint_velocities()
-{
-    return current_joint_velocities_;
-}
-
-void RobotDriverCoppelia::set_target_joint_velocities(const VectorXd &desired_joint_velocities)
-{
-    desired_joint_velocities_ = desired_joint_velocities;
-}
-
-VectorXd RobotDriverCoppelia::get_joint_forces()
-{
-    return current_joint_forces_;
-}
-
-RobotDriverCoppelia::~RobotDriverCoppelia()=default;
-
-void RobotDriverCoppelia::connect()
-{
-
-    vi_->connect(configuration_.ip, configuration_.port, 500, 10);
-    vi_->set_joint_target_velocities(jointnames_, VectorXd::Zero(dim_configuration_space_));
-    std::cout<<"RobotDriverCoppelia::Connecting..."<<std::endl;
-}
-
-void RobotDriverCoppelia::disconnect()
-{
-    vi_->disconnect();
-    if (joint_velocity_control_mode_thread_.joinable())
-    {
-        joint_velocity_control_mode_thread_.join();
-    }
-    if (joint_velocity_control_mirror_mode_thread_.joinable())
-    {
-        joint_velocity_control_mirror_mode_thread_.join();
-    }
-    std::cout<<"RobotDriverCoppelia::Disconnected."<<std::endl;
-}
-
-void RobotDriverCoppelia::initialize()
-{
-    vi_->start_simulation();
-    if (mirror_mode_ == false)
-    {
-        _start_joint_velocity_control_thread();
-    }else{
-        _start_joint_velocity_control_mirror_thread();
-    }
-    std::cout<<"RobotDriverCoppelia::Velocity loop running..."<<std::endl;
-}
-
-void RobotDriverCoppelia::deinitialize()
-{
-    vi_->set_joint_target_velocities(jointnames_, VectorXd::Zero(dim_configuration_space_));
-    vi_->stop_simulation();
-    finish_motion();
-    std::cout<<"RobotDriverCoppelia::Deinitialized."<<std::endl;
-}
-
-
-
-
-void RobotDriverCoppelia::_update_robot_state(const VectorXd &q, const VectorXd &q_dot, const VectorXd &forces)
-{
-    current_joint_positions_  = q;
-    current_joint_velocities_ = q_dot;
-    current_joint_forces_     =  forces;
-}
-
-void RobotDriverCoppelia::finish_motion()
-{
-    for (int i=0;i<1000;i++)
-    {
-        set_target_joint_positions(current_joint_positions_);
-        set_target_joint_velocities(VectorXd::Zero(dim_configuration_space_));
-        std::this_thread::sleep_for(std::chrono::milliseconds(1));
-    }
-    finish_motion_ = true;
-}
-
-void RobotDriverCoppelia::_start_joint_velocity_control_mode()
-{
-    try{
-        finish_motion_ = false;
-        VectorXd q = vi_->get_joint_positions(jointnames_);
-        VectorXd q_dot =  vi_->get_joint_velocities(jointnames_);
-        VectorXd forces = vi_->get_joint_torques(jointnames_);
-        _update_robot_state(q, q_dot, forces);
-
-        desired_joint_positions_ = q;
-        while(true)
-        {
-
-            VectorXd q = vi_->get_joint_positions(jointnames_);
-            VectorXd q_dot =  vi_->get_joint_velocities(jointnames_);
-            VectorXd forces = vi_->get_joint_torques(jointnames_);
-            _update_robot_state(q, q_dot, forces);
-
-
-            if (robot_mode_  == std::string("VelocityControl"))
-            {              
-                    vi_->set_joint_target_velocities(jointnames_, desired_joint_velocities_); 
-            }
-            else if (robot_mode_  == std::string("PositionControl"))
-            {
-                    vi_->set_joint_target_positions(jointnames_, desired_joint_positions_);
-            }
-            if (finish_motion_) {
-                    finish_motion_ = false;
-                    return;
-            }
-        }
-    }
-    catch(const std::exception& e)
-    {
-        std::cout<<"RobotDriverCoppelia::sas_robot_coppelia_driver::_start_joint_velocity_control_mode(). Error or exception caught " << e.what()<<std::endl;
-    }
-    catch(...)
-    {
-        std::cout<<"RobotDriverCoppelia::sas_robot_coppelia_driver::_start_joint_velocity_control_mode(). Error or exception caught " <<std::endl;
-    }
-
-
-}
-
-void RobotDriverCoppelia::_start_joint_velocity_control_thread()
-{
-    finish_motion_ = false;
-    if (joint_velocity_control_mode_thread_.joinable())
-    {
-        joint_velocity_control_mode_thread_.join();
-    }
-    if (joint_velocity_control_mirror_mode_thread_.joinable())
-    {
-        joint_velocity_control_mirror_mode_thread_.join();
-    }
-    joint_velocity_control_mode_thread_ = std::thread(&RobotDriverCoppelia::_start_joint_velocity_control_mode, this);
-}
-
-void RobotDriverCoppelia::_start_joint_velocity_control_mirror_thread()
-{
-    finish_motion_ = false;
-    if (joint_velocity_control_mode_thread_.joinable())
-    {
-        joint_velocity_control_mode_thread_.join();
-    }
-    if (joint_velocity_control_mirror_mode_thread_.joinable())
-    {
-        joint_velocity_control_mirror_mode_thread_.join();
-    }
-    joint_velocity_control_mirror_mode_thread_ = std::thread(&RobotDriverCoppelia::_start_joint_velocity_control_mirror_mode, this);
-}
-
-void RobotDriverCoppelia::_start_joint_velocity_control_mirror_mode()
-{
-
-    try{
-        finish_motion_ = false;
-        std::cout<<"RobotDriverCoppelia::Waiting for real robot topics..."<<std::endl;
-        VectorXd q;
-        while (ros::ok()) {
-            if (franka1_ros_->is_enabled())
-            {
-                    q = franka1_ros_->get_joint_positions();
-                    break;
-            }
-            ros::spinOnce();
-        }
-        std::cout<<"RobotDriverCoppelia::Done!"<<std::endl;
-
-        VectorXd q_vrep = vi_->get_joint_positions(jointnames_);
-        VectorXd q_dot_vrep =  vi_->get_joint_velocities(jointnames_);
-        VectorXd forces_vrep = vi_->get_joint_torques(jointnames_);
-        _update_robot_state(q_vrep, q_dot_vrep, forces_vrep);
-
-        desired_joint_positions_ = q_vrep;
-
-
-        while(ros::ok())
-        {
-            q = franka1_ros_->get_joint_positions();
-            if (q.size() == dim_configuration_space_)
-            {
-                VectorXd q_vrep = vi_->get_joint_positions(jointnames_);
-                VectorXd q_dot_vrep =  vi_->get_joint_velocities(jointnames_);
-                VectorXd forces_vrep = vi_->get_joint_torques(jointnames_);
-                _update_robot_state(q_vrep, q_dot_vrep, forces_vrep);
-
-
-                if (robot_mode_  == std::string("VelocityControl"))
-                {
-                    vi_->set_joint_target_velocities(jointnames_, gain_*(q-q_vrep));
-                }
-                else if (robot_mode_  == std::string("PositionControl"))
-                {
-                        vi_->set_joint_target_positions(jointnames_, q);
-                }
-                if (finish_motion_) {
-                        finish_motion_ = false;
-                        return;
-                }
-            }
-        }
-    }
-    catch(const std::exception& e)
-    {
-        std::cout<<"RobotDriverCoppelia::sas_robot_coppelia_driver::_start_joint_velocity_control_mirror_mode(). Error or exception caught " << e.what()<<std::endl;
-    }
-    catch(...)
-    {
-        std::cout<<"RobotDriverCoppelia::sas_robot_coppelia_driver::_start_joint_velocity_control_mirror_mode(). Error or exception caught " <<std::endl;
-    }
-
-
-}
-
-
-
-
-} // sas namespace
--- a/src/coppelia/sas_robot_driver_coppelia.h
+++ b/src/coppelia/sas_robot_driver_coppelia.h
@@ -1,134 +0,0 @@
-/*
-# Copyright (c) 2023 Juan Jose Quiroz Omana
-#
-#    This file is part of sas_robot_driver_franka.
-#
-#    sas_robot_driver_franka is free software: you can redistribute it and/or modify
-#    it under the terms of the GNU Lesser General Public License as published by
-#    the Free Software Foundation, either version 3 of the License, or
-#    (at your option) any later version.
-#
-#    sas_robot_driver_franka is distributed in the hope that it will be useful,
-#    but WITHOUT ANY WARRANTY; without even the implied warranty of
-#    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
-#    GNU Lesser General Public License for more details.
-#
-#    You should have received a copy of the GNU Lesser General Public License
-#    along with sas_robot_driver.  If not, see <https://www.gnu.org/licenses/>.
-#
-# ################################################################
-#
-#   Author: Juan Jose Quiroz Omana, email: juanjqogm@gmail.com
-#
-# ################################################################
-#
-# Contributors:
-#      1. Juan Jose Quiroz Omana (juanjqogm@gmail.com)
-#         - Adapted from sas_robot_driver_denso.cpp
-#           (https://github.com/SmartArmStack/sas_robot_driver_denso/blob/master/src/sas_robot_driver_denso.cpp)
-#
-# ################################################################
-*/
-
-
-#pragma once
-#include <exception>
-#include <tuple>
-#include <atomic>
-#include <vector>
-#include <memory>
-#include <dqrobotics/DQ.h>
-#include <sas_robot_driver/sas_robot_driver.h>
-#include <dqrobotics/interfaces/vrep/DQ_VrepRobot.h>
-#include <thread>
-#include <sas_robot_driver/sas_robot_driver_interface.h>
-
-using namespace DQ_robotics;
-using namespace Eigen;
-
-namespace sas
-{
-
-
-
-struct RobotDriverCoppeliaConfiguration
-{
-
-    int thread_sampling_time_nsec;
-    int port;
-    std::string ip;
-    std::vector<std::string> jointnames;
-    std::string robot_mode;
-    bool mirror_mode;
-    std::string real_robot_topic_prefix;
-};
-
-class RobotDriverCoppelia: public RobotDriver
-{
-private:
-    RobotDriverCoppeliaConfiguration configuration_;
-
-    std::string robot_mode_ = std::string("VelocityControl");   // PositionControl
-    bool mirror_mode_ = false;
-    double gain_ = 3.0;
-    std::string real_robot_topic_prefix_;
-
-    VectorXd current_joint_positions_;
-    VectorXd current_joint_velocities_;
-    VectorXd current_joint_forces_;
-
-
-    VectorXd desired_joint_velocities_;
-    VectorXd desired_joint_positions_;
-
-    std::atomic<bool> finish_motion_;
-
-    int dim_configuration_space_;
-
-    void _update_robot_state(const VectorXd& q, const VectorXd& q_dot, const VectorXd& forces);
-
-    void finish_motion();
-
-    void _start_joint_velocity_control_mode();
-    std::thread joint_velocity_control_mode_thread_;
-    void _start_joint_velocity_control_thread();
-
-
-    void _start_joint_velocity_control_mirror_mode();
-    std::thread joint_velocity_control_mirror_mode_thread_;
-    void _start_joint_velocity_control_mirror_thread();
-
-    std::shared_ptr<sas::RobotDriverInterface> franka1_ros_;
-
-
-protected:
-    std::shared_ptr<DQ_VrepInterface> vi_;
-    std::vector<std::string> jointnames_;
-
-
-
-public:
-    RobotDriverCoppelia(const RobotDriverCoppelia&)=delete;
-    RobotDriverCoppelia()=delete;
-    ~RobotDriverCoppelia();
-
-    RobotDriverCoppelia(const RobotDriverCoppeliaConfiguration& configuration, std::atomic_bool* break_loops);
-
-
-    VectorXd get_joint_positions() override;
-    void set_target_joint_positions(const VectorXd& desired_joint_positions_rad) override;
-
-    VectorXd get_joint_velocities() override;
-    void set_target_joint_velocities(const VectorXd& desired_joint_velocities) override;
-
-    VectorXd get_joint_forces() override;
-
-    void connect() override;
-    void disconnect() override;
-
-    void initialize() override;
-    void deinitialize() override;
-
-
-};
-}
--- a/src/dynamic_interface/sas_robot_dynamic_provider.cpp
+++ b/src/dynamic_interface/sas_robot_dynamic_provider.cpp
@@ -1,63 +0,0 @@
-/*
-# Copyright (c) 2023 Juan Jose Quiroz Omana
-#
-#    This file is part of sas_robot_driver_franka.
-#
-#    sas_robot_driver_franka is free software: you can redistribute it and/or modify
-#    it under the terms of the GNU Lesser General Public License as published by
-#    the Free Software Foundation, either version 3 of the License, or
-#    (at your option) any later version.
-#
-#    sas_robot_driver_franka is distributed in the hope that it will be useful,
-#    but WITHOUT ANY WARRANTY; without even the implied warranty of
-#    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
-#    GNU Lesser General Public License for more details.
-#
-#    You should have received a copy of the GNU Lesser General Public License
-#    along with sas_robot_driver.  If not, see <https://www.gnu.org/licenses/>.
-#
-# ################################################################
-#
-#   Author: Juan Jose Quiroz Omana, email: juanjqogm@gmail.com
-#
-# ################################################################
-#
-# Contributors:
-#      1. Quenitin Lin
-#
-# ################################################################
-*/
-#include "sas_robot_dynamic_provider.h"
-
-using namespace sas;
-
-RobotDynamicProvider::RobotDynamicProvider(ros::NodeHandle &nodehandle, const std::string &node_prefix):
-    RobotDynamicProvider(nodehandle, nodehandle, node_prefix)
-{
-    //Delegated
-}
-
-RobotDynamicProvider::RobotDynamicProvider(ros::NodeHandle &publisher_nodehandle, ros::NodeHandle &subscriber_nodehandle, const std::string &node_prefix):
-    node_prefix_(node_prefix)
-{
-    ROS_INFO_STREAM(ros::this_node::getName() + "::Initializing RobotDynamicProvider with prefix " + node_prefix);
-    publisher_cartesian_stiffness_ = publisher_nodehandle.advertise<geometry_msgs::WrenchStamped>(node_prefix + "/get/cartesian_stiffness", 1);
-    publisher_stiffness_pose_ = publisher_nodehandle.advertise<geometry_msgs::PoseStamped>(node_prefix + "/get/stiffness_pose", 1);
-
-}
-
-
-void RobotDynamicProvider::publish_stiffness(const DQ& base_to_stiffness, const Vector3d& force, const Vector3d& torque) const
-{
-    publisher_stiffness_pose_.publish(dq_to_geometry_msgs_pose_stamped(base_to_stiffness));
-    geometry_msgs::WrenchStamped msg;
-    msg.header.stamp = ros::Time::now();
-    msg.wrench.force.x = force(0);
-    msg.wrench.force.y = force(1);
-    msg.wrench.force.z = force(2);
-    msg.wrench.torque.x = torque(0);
-    msg.wrench.torque.y = torque(1);
-    msg.wrench.torque.z = torque(2);
-    publisher_cartesian_stiffness_.publish(msg);
-}
-
--- a/src/dynamic_interface/sas_robot_dynamic_provider.h
+++ b/src/dynamic_interface/sas_robot_dynamic_provider.h
@@ -1,60 +0,0 @@
-#pragma once
-/*
-# Copyright (c) 2023 Juan Jose Quiroz Omana
-#
-#    This file is part of sas_robot_driver_franka.
-#
-#    sas_robot_driver_franka is free software: you can redistribute it and/or modify
-#    it under the terms of the GNU Lesser General Public License as published by
-#    the Free Software Foundation, either version 3 of the License, or
-#    (at your option) any later version.
-#
-#    sas_robot_driver_franka is distributed in the hope that it will be useful,
-#    but WITHOUT ANY WARRANTY; without even the implied warranty of
-#    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
-#    GNU Lesser General Public License for more details.
-#
-#    You should have received a copy of the GNU Lesser General Public License
-#    along with sas_robot_driver.  If not, see <https://www.gnu.org/licenses/>.
-#
-# ################################################################
-#
-#   Author: Juan Jose Quiroz Omana, email: juanjqogm@gmail.com
-#
-# ################################################################
-#
-# Contributors:
-#      1. Quenitin Lin
-#
-# ################################################################
-*/
-#include <ros/ros.h>
-#include <ros/node_handle.h>
-#include <sas_common/sas_common.h>
-#include <sas_conversions/sas_conversions.h>
-#include <geometry_msgs/WrenchStamped.h>
-#include <geometry_msgs/PoseStamped.h>
-#include <dqrobotics/DQ.h>
-
-namespace sas {
-
-using namespace DQ_robotics;
-
-class RobotDynamicProvider {
-private:
-    std::string node_prefix_;
-
-    ros::Publisher publisher_cartesian_stiffness_;
-    ros::Publisher publisher_stiffness_pose_;
-
-public:
-    RobotDynamicProvider(ros::NodeHandle& nodehandle, const std::string& node_prefix=ros::this_node::getName());
-    RobotDynamicProvider(ros::NodeHandle& publisher_nodehandle, ros::NodeHandle& subscriber_nodehandle, const std::string& node_prefix=ros::this_node::getName());
-
-    void publish_stiffness(const DQ& base_to_stiffness, const Vector3d& force, const Vector3d& torque) const;
-
-};
-
-
-
-} // namespace sas
--- a/src/hand/sas_robot_driver_franka_hand.cpp
+++ b/src/hand/sas_robot_driver_franka_hand.cpp
@@ -1,152 +0,0 @@
-/*
-# Copyright (c) 2023 Juan Jose Quiroz Omana
-#
-#    This file is part of sas_robot_driver_franka.
-#
-#    sas_robot_driver_franka is free software: you can redistribute it and/or modify
-#    it under the terms of the GNU Lesser General Public License as published by
-#    the Free Software Foundation, either version 3 of the License, or
-#    (at your option) any later version.
-#
-#    sas_robot_driver_franka is distributed in the hope that it will be useful,
-#    but WITHOUT ANY WARRANTY; without even the implied warranty of
-#    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
-#    GNU Lesser General Public License for more details.
-#
-#    You should have received a copy of the GNU Lesser General Public License
-#    along with sas_robot_driver.  If not, see <https://www.gnu.org/licenses/>.
-#
-# ################################################################
-#
-#   Author: Juan Jose Quiroz Omana, email: juanjqogm@gmail.com
-#
-# ################################################################
-#
-# Contributors:
-#      1. Quenitin Lin
-#
-# ################################################################
-*/
-#include "sas_robot_driver_franka_hand.h"
-
-namespace sas {
-
-    //const static int SLAVE_MODE_JOINT_CONTROL;
-    //const static int SLAVE_MODE_END_EFFECTOR_CONTROL;
-
-    RobotDriverFrankaHand::~RobotDriverFrankaHand()
-    {
-        if(_is_connected())
-        {
-            disconnect();
-        }
-    }
-
-    RobotDriverFrankaHand::RobotDriverFrankaHand(
-        const RobotDriverFrankaHandConfiguration& configuration,
-        const RobotDriverROSConfiguration& ros_configuration,
-        std::atomic_bool* break_loops):
-    configuration_(configuration), ros_configuration_(ros_configuration), break_loops_(break_loops)
-    {
-        gripper_sptr_ = nullptr;
-
-    }
-
-    bool RobotDriverFrankaHand::_is_connected() const
-    {
-        if(gripper_sptr_ == nullptr) return false;
-        if(!gripper_sptr_) return false;
-        else return true;
-    }
-
-    VectorXd RobotDriverFrankaHand::get_joint_positions()
-    {
-        return joint_positions_;
-
-    }
-    void RobotDriverFrankaHand::set_target_joint_positions(const VectorXd& desired_joint_positions_rad)
-    {
-
-    }
-
-    VectorXd RobotDriverFrankaHand::get_joint_velocities()
-    {
-        return VectorXd::Zero(1);
-    }
-    void RobotDriverFrankaHand::set_target_joint_velocities(const VectorXd& desired_joint_velocities)
-    {
-
-    }
-
-    VectorXd RobotDriverFrankaHand::get_joint_forces()
-    {
-        return VectorXd::Zero(1);
-    }
-
-    void RobotDriverFrankaHand::start_control_loop()
-    {
-
-        Clock clock = Clock(ros_configuration_.thread_sampling_time_nsec);
-        clock.init();
-        ROS_INFO_STREAM("["+ ros::this_node::getName()+"]::[RobotDriverFrankaHand]::start_control_loop::Starting control loop.");
-        while(!(*break_loops_))
-        {
-            if(!_is_connected()) throw std::runtime_error("["+ ros::this_node::getName()+"]::[RobotDriverFrankaHand]::start_control_loop::Robot is not connected.");
-
-
-            clock.update_and_sleep();
-        }
-        ROS_INFO_STREAM("["+ ros::this_node::getName()+"]::[RobotDriverFrankaHand]::start_control_loop::Exiting control loop.");
-
-    }
-
-
-    void RobotDriverFrankaHand::connect()
-    {
-        gripper_sptr_ = std::make_shared<franka::Gripper>(configuration_.robot_ip);
-        if(!_is_connected()) throw std::runtime_error("["+ ros::this_node::getName()+"]::[RobotDriverFrankaHand]::connect::Could not connect to the robot.");
-
-    }
-    void RobotDriverFrankaHand::disconnect() noexcept
-    {
-        ROS_WARN_STREAM("["+ ros::this_node::getName()+"]::[RobotDriverFrankaHand]::disconnecting...");
-        gripper_sptr_->~Gripper();
-        gripper_sptr_ = nullptr;
-    }
-
-    /**
-     * @brief robot_driver_franka::gripper_homing
-     */
-    void RobotDriverFrankaHand::gripper_homing()
-    {
-        if(!_is_connected()) throw std::runtime_error("["+ ros::this_node::getName()+"]::[RobotDriverFrankaHand]::gripper_homing::Robot is not connected.");
-        auto ret = gripper_sptr_->homing();
-        if(!ret)
-        {
-            throw std::runtime_error("["+ ros::this_node::getName()+"]::[RobotDriverFrankaHand]::gripper_homing::Failed to home the gripper.");
-        }
-        ROS_INFO_STREAM("["+ ros::this_node::getName()+"]::[RobotDriverFrankaHand]::gripper_homing::Gripper homed.");
-    }
-
-    void RobotDriverFrankaHand::initialize()
-    {
-        if(!_is_connected()) throw std::runtime_error("["+ ros::this_node::getName()+"]::[RobotDriverFrankaHand]::initialize::Robot is not connected.");
-        gripper_homing();
-    }
-
-    void RobotDriverFrankaHand::deinitialize()
-    {
-        if(_is_connected())
-        {
-            franka::GripperState gripper_state = gripper_sptr_->readOnce();
-            if(gripper_state.is_grasped)
-            {
-                gripper_sptr_->stop();
-            }
-        }
-    }
-
-
-
-
-} // sas
--- a/src/hand/sas_robot_driver_franka_hand.h
+++ b/src/hand/sas_robot_driver_franka_hand.h
@@ -1,117 +0,0 @@
-#pragma once
-/*
-# Copyright (c) 2023 Juan Jose Quiroz Omana
-#
-#    This file is part of sas_robot_driver_franka.
-#
-#    sas_robot_driver_franka is free software: you can redistribute it and/or modify
-#    it under the terms of the GNU Lesser General Public License as published by
-#    the Free Software Foundation, either version 3 of the License, or
-#    (at your option) any later version.
-#
-#    sas_robot_driver_franka is distributed in the hope that it will be useful,
-#    but WITHOUT ANY WARRANTY; without even the implied warranty of
-#    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
-#    GNU Lesser General Public License for more details.
-#
-#    You should have received a copy of the GNU Lesser General Public License
-#    along with sas_robot_driver.  If not, see <https://www.gnu.org/licenses/>.
-#
-# ################################################################
-#
-#   Author: Juan Jose Quiroz Omana, email: juanjqogm@gmail.com
-#
-# ################################################################
-#
-# Contributors:
-#      1. Quenitin Lin
-#
-# ################################################################
-*/
-#include <exception>
-#include <tuple>
-#include <atomic>
-#include <vector>
-#include <memory>
-#include <franka/robot.h>
-#include <franka/gripper.h>
-// #include <thread>
-
-#include <dqrobotics/DQ.h>
-
-#include <sas_robot_driver/sas_robot_driver.h>
-#include <sas_robot_driver/sas_robot_driver_ros.h>
-#include <sas_clock/sas_clock.h>
-#include <ros/common.h>
-
-using namespace DQ_robotics;
-using namespace Eigen;
-
-
-namespace sas {
-
-struct RobotDriverFrankaHandConfiguration
-{
-    std::string robot_ip;
-
-};
-
-class RobotDriverFrankaHand{
-private:
-    RobotDriverFrankaHandConfiguration configuration_;
-    RobotDriverROSConfiguration ros_configuration_;
-
-    std::shared_ptr<franka::Gripper> gripper_sptr_;
-
-    //Joint positions
-    VectorXd joint_positions_;
-    //VectorXd joint_velocities_;
-    //VectorXd end_effector_pose_;
-
-
-    // std::thread control_loop_thread_;
-    std::atomic_bool* break_loops_;
-
-    bool _is_connected() const;
-
-public:
-    //const static int SLAVE_MODE_JOINT_CONTROL;
-    //const static int SLAVE_MODE_END_EFFECTOR_CONTROL;
-
-    RobotDriverFrankaHand(const RobotDriverFrankaHand&)=delete;
-    RobotDriverFrankaHand()=delete;
-    ~RobotDriverFrankaHand();
-
-    RobotDriverFrankaHand(
-        const RobotDriverFrankaHandConfiguration& configuration,
-        const RobotDriverROSConfiguration& ros_configuration,
-        std::atomic_bool* break_loops);
-
-    void start_control_loop();
-
-
-
-    VectorXd get_joint_positions();
-    void set_target_joint_positions(const VectorXd& desired_joint_positions_rad);
-
-    VectorXd get_joint_velocities();
-    void set_target_joint_velocities(const VectorXd& desired_joint_velocities);
-
-    VectorXd get_joint_forces();
-
-    void gripper_homing();
-
-
-    void connect() ;
-    void disconnect() noexcept;
-
-    void initialize() ;
-    void deinitialize() ;
-
-    //bool set_end_effector_pose_dq(const DQ& pose);
-    //DQ get_end_effector_pose_dq();
-
-};
-
-} // sas
-
--- a/src/sas_robot_driver_coppelia_node.cpp
+++ b/src/sas_robot_driver_coppelia_node.cpp
@@ -1,105 +0,0 @@
-/*
-# Copyright (c) 2023 Juan Jose Quiroz Omana
-#
-#    This file is part of sas_robot_driver_franka.
-#
-#    sas_robot_driver_franka is free software: you can redistribute it and/or modify
-#    it under the terms of the GNU Lesser General Public License as published by
-#    the Free Software Foundation, either version 3 of the License, or
-#    (at your option) any later version.
-#
-#    sas_robot_driver_franka is distributed in the hope that it will be useful,
-#    but WITHOUT ANY WARRANTY; without even the implied warranty of
-#    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
-#    GNU Lesser General Public License for more details.
-#
-#    You should have received a copy of the GNU Lesser General Public License
-#    along with sas_robot_driver.  If not, see <https://www.gnu.org/licenses/>.
-#
-# ################################################################
-#
-#   Author: Juan Jose Quiroz Omana, email: juanjqogm@gmail.com
-#
-# ################################################################
-#
-# Contributors:
-#      1. Juan Jose Quiroz Omana (juanjqogm@gmail.com)
-#         - Adapted from sas_robot_driver_denso_node.cpp
-#           (https://github.com/SmartArmStack/sas_robot_driver_denso/blob/master/src/sas_robot_driver_denso_node.cpp)
-#
-# ################################################################
-*/
-
-
-#include <sstream>
-
-#include <exception>
-#include <dqrobotics/utils/DQ_Math.h>
-//#include <dqrobotics/interfaces/json11/DQ_JsonReader.h>
-#include <sas_common/sas_common.h>
-#include <sas_conversions/eigen3_std_conversions.h>
-#include <sas_robot_driver/sas_robot_driver_ros.h>
-#include "coppelia/sas_robot_driver_coppelia.h"
-
-/*********************************************
- * SIGNAL HANDLER
- * *******************************************/
-#include<signal.h>
-static std::atomic_bool kill_this_process(false);
-void sig_int_handler(int)
-{
-    kill_this_process = true;
-}
-
-
-int main(int argc, char **argv)
-{
-    if(signal(SIGINT, sig_int_handler) == SIG_ERR)
-    {
-        throw std::runtime_error(ros::this_node::getName() + "::Error setting the signal int handler.");
-    }
-
-    ros::init(argc, argv, "sas_robot_driver_coppelia_node", ros::init_options::NoSigintHandler);
-    ROS_WARN("=====================================================================");
-    ROS_WARN("----------------------Juan Jose Quiroz Omana------------------------");
-    ROS_WARN("=====================================================================");
-    ROS_INFO_STREAM(ros::this_node::getName()+"::Loading parameters from parameter server.");
-
-
-    ros::NodeHandle nh;
-    sas::RobotDriverCoppeliaConfiguration robot_driver_coppelia_configuration;
-
-    sas::get_ros_param(nh,"/robot_ip_address",robot_driver_coppelia_configuration.ip);
-    sas::get_ros_param(nh,"/robot_mode",      robot_driver_coppelia_configuration.robot_mode);
-    sas::get_ros_param(nh,"/vrep_port", robot_driver_coppelia_configuration.port);
-    sas::get_ros_param(nh,"/vrep_robot_joint_names", robot_driver_coppelia_configuration.jointnames);
-    sas::get_ros_param(nh,"/mirror_mode", robot_driver_coppelia_configuration.mirror_mode);
-    sas::get_ros_param(nh, "/real_robot_topic_prefix", robot_driver_coppelia_configuration.real_robot_topic_prefix);
-    sas::RobotDriverROSConfiguration robot_driver_ros_configuration;
-
-    sas::get_ros_param(nh,"/thread_sampling_time_nsec",robot_driver_ros_configuration.thread_sampling_time_nsec);
-    sas::get_ros_param(nh,"/q_min",robot_driver_ros_configuration.q_min);
-    sas::get_ros_param(nh,"/q_max",robot_driver_ros_configuration.q_max);
-
-    robot_driver_ros_configuration.robot_driver_provider_prefix = ros::this_node::getName();
-
-    try
-    {
-        ROS_INFO_STREAM(ros::this_node::getName()+"::Instantiating Coppelia robot.");
-        sas::RobotDriverCoppelia robot_driver_coppelia(robot_driver_coppelia_configuration,
-                                                       &kill_this_process);
-        ROS_INFO_STREAM(ros::this_node::getName()+"::Instantiating RobotDriverROS.");
-        sas::RobotDriverROS robot_driver_ros(nh,
-                                             &robot_driver_coppelia,
-                                             robot_driver_ros_configuration,
-                                             &kill_this_process);
-        robot_driver_ros.control_loop();
-    }
-    catch (const std::exception& e)
-    {
-        ROS_ERROR_STREAM(ros::this_node::getName() + "::Exception::" + e.what());
-    }
-
-
-    return 0;
-}
--- a/src/sas_robot_driver_franka_hand_node.cpp
+++ b/src/sas_robot_driver_franka_hand_node.cpp
@@ -1,116 +0,0 @@
-/*
-# Copyright (c) 2023 Juan Jose Quiroz Omana
-#
-#    This file is part of sas_robot_driver_franka.
-#
-#    sas_robot_driver_franka is free software: you can redistribute it and/or modify
-#    it under the terms of the GNU Lesser General Public License as published by
-#    the Free Software Foundation, either version 3 of the License, or
-#    (at your option) any later version.
-#
-#    sas_robot_driver_franka is distributed in the hope that it will be useful,
-#    but WITHOUT ANY WARRANTY; without even the implied warranty of
-#    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
-#    GNU Lesser General Public License for more details.
-#
-#    You should have received a copy of the GNU Lesser General Public License
-#    along with sas_robot_driver.  If not, see <https://www.gnu.org/licenses/>.
-#
-# ################################################################
-#
-#   Author: Juan Jose Quiroz Omana, email: juanjqogm@gmail.com
-#
-# ################################################################
-#
-# Contributors:
-#      1. Quentin Lin (qlin1806@g.ecc.u-tokyo.ac.jp)
-#         - Adapted from sas_robot_driver_franka
-# ################################################################
-*/
-
-
-#include <sstream>
-
-#include <exception>
-#include <dqrobotics/utils/DQ_Math.h>
-#include <sas_common/sas_common.h>
-#include <sas_conversions/eigen3_std_conversions.h>
-#include <sas_robot_driver/sas_robot_driver_ros.h>
-#include "sas_robot_driver_franka_hand.h"
-
-
-/*********************************************
- * SIGNAL HANDLER
- * *******************************************/
-#include<signal.h>
-static std::atomic_bool kill_this_process(false);
-void sig_int_handler(int)
-{
-    kill_this_process = true;
-}
-
-int main(int argc, char **argv)
-{
-    if(signal(SIGINT, sig_int_handler) == SIG_ERR)
-    {
-        throw std::runtime_error(ros::this_node::getName() + "::Error setting the signal int handler.");
-    }
-    ros::init(argc, argv, "sas_robot_driver_franka_hand_node", ros::init_options::NoSigintHandler);
-    ROS_WARN("=====================================================================");
-    ROS_WARN("---------------------------Quentin Lin-------------------------------");
-    ROS_WARN("=====================================================================");
-    ROS_INFO_STREAM(ros::this_node::getName()+"::Loading parameters from parameter server.");
-
-
-    ros::NodeHandle nh;
-    sas::RobotDriverFrankaHandConfiguration robot_driver_franka_hand_configuration;
-
-    sas::get_ros_param(nh,"/robot_ip_address",robot_driver_franka_hand_configuration.robot_ip);
-
-
-    sas::RobotDriverROSConfiguration robot_driver_ros_configuration;
-
-    sas::get_ros_param(nh,"/thread_sampling_time_nsec",robot_driver_ros_configuration.thread_sampling_time_nsec);
-    bool q_lim_override = false;
-    if(nh.hasParam("q_min") || nh.hasParam("q_max"))
-    {
-        sas::get_ros_param(nh,"/q_min",robot_driver_ros_configuration.q_min);
-        sas::get_ros_param(nh,"/q_max",robot_driver_ros_configuration.q_max);
-        q_lim_override = true;
-    }else
-    {
-
-    }
-    robot_driver_ros_configuration.robot_driver_provider_prefix = ros::this_node::getName();
-
-    sas::RobotDriverFrankaHand franka_hand_driver(
-        robot_driver_franka_hand_configuration,
-        robot_driver_ros_configuration,
-        &kill_this_process
-    );
-    try
-    {
-        ROS_INFO_STREAM(ros::this_node::getName()+"::Instantiating Franka hand.");
-        franka_hand_driver.connect();
-        franka_hand_driver.initialize();
-
-        ROS_INFO_STREAM(ros::this_node::getName()+"::Instantiating control loop.");
-        franka_hand_driver.start_control_loop();
-
-    }
-    catch (const std::exception& e)
-    {
-        ROS_ERROR_STREAM(ros::this_node::getName() + "::Exception::" + e.what());
-    }catch (...)
-    {
-        ROS_ERROR_STREAM(ros::this_node::getName() + "::Exception::Unknown exception.");
-    }
-    ROS_INFO_STREAM(ros::this_node::getName()+"::Exiting...");
-    franka_hand_driver.deinitialize();
-    ROS_INFO_STREAM(ros::this_node::getName()+"::deinitialized.");
-    franka_hand_driver.disconnect();
-    ROS_INFO_STREAM(ros::this_node::getName()+"::disconnected.");
-
-
-  return 0;
-}
--- a/src/sas_robot_driver_franka_node.cpp
+++ b/src/sas_robot_driver_franka_node.cpp
@@ -1,163 +0,0 @@
-/*
-# Copyright (c) 2023 Juan Jose Quiroz Omana
-#
-#    This file is part of sas_robot_driver_franka.
-#
-#    sas_robot_driver_franka is free software: you can redistribute it and/or modify
-#    it under the terms of the GNU Lesser General Public License as published by
-#    the Free Software Foundation, either version 3 of the License, or
-#    (at your option) any later version.
-#
-#    sas_robot_driver_franka is distributed in the hope that it will be useful,
-#    but WITHOUT ANY WARRANTY; without even the implied warranty of
-#    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
-#    GNU Lesser General Public License for more details.
-#
-#    You should have received a copy of the GNU Lesser General Public License
-#    along with sas_robot_driver.  If not, see <https://www.gnu.org/licenses/>.
-#
-# ################################################################
-#
-#   Author: Juan Jose Quiroz Omana, email: juanjqogm@gmail.com
-#
-# ################################################################
-#
-# Contributors:
-#      1. Juan Jose Quiroz Omana (juanjqogm@gmail.com)
-#         - Adapted from sas_robot_driver_denso_node.cpp
-#           (https://github.com/SmartArmStack/sas_robot_driver_denso/blob/master/src/sas_robot_driver_denso_node.cpp)
-#
-# ################################################################
-*/
-
-
-#include <sstream>
-
-#include <exception>
-#include <dqrobotics/utils/DQ_Math.h>
-//#include <dqrobotics/interfaces/json11/DQ_JsonReader.h>
-#include <sas_common/sas_common.h>
-#include <sas_conversions/eigen3_std_conversions.h>
-#include <sas_robot_driver/sas_robot_driver_ros.h>
-#include "sas_robot_driver_franka.h"
-#include "sas_robot_dynamic_provider.h"
-
-
-/*********************************************
- * SIGNAL HANDLER
- * *******************************************/
-#include<signal.h>
-static std::atomic_bool kill_this_process(false);
-void sig_int_handler(int)
-{
-    kill_this_process = true;
-}
-
-template<typename T, std::size_t N>
-std::array<T, N> apply_scale_to_std_array(const std::array<T, N>& array, const T& scale)
-{
-    std::array<T, N> scaled_array;
-    for(std::size_t i = 0; i < N; i++)
-    {
-        scaled_array[i] = array[i] * scale;
-    }
-    return scaled_array;
-}
-template<typename T, std::size_t N>
-std::array<T, N> std_vec_to_std_array(const std::vector<T>& vector)
-{
-    if(N != vector.size()){throw std::runtime_error("Size mismatch between vector and array.");}
-    std::array<T, N> array;
-    for(std::size_t i = 0; i < N; i++)
-    {
-        array[i] = vector[i];
-    }
-    return array;
-}
-
-
-int main(int argc, char **argv)
-{
-    if(signal(SIGINT, sig_int_handler) == SIG_ERR)
-    {
-        throw std::runtime_error(ros::this_node::getName() + "::Error setting the signal int handler.");
-    }
-
-    ros::init(argc, argv, "sas_robot_driver_franka_node", ros::init_options::NoSigintHandler);
-    ROS_WARN("=====================================================================");
-    ROS_WARN("----------------------Juan Jose Quiroz Omana------------------------");
-    ROS_WARN("=====================================================================");
-    ROS_INFO_STREAM(ros::this_node::getName()+"::Loading parameters from parameter server.");
-
-
-    ros::NodeHandle nh;
-    sas::RobotDriverFrankaConfiguration robot_driver_franka_configuration;
-    RobotInterfaceFranka::FrankaInterfaceConfiguration franka_interface_configuration;
-
-    sas::get_ros_param(nh,"/robot_ip_address",robot_driver_franka_configuration.ip_address);
-    sas::get_ros_param(nh,"/robot_mode",      robot_driver_franka_configuration.mode);
-    double upper_scale_factor = 1.0;
-    std::vector<std::string> all_params;
-    if(nh.hasParam(ros::this_node::getName()+"/force_upper_limits_scaling_factor"))
-    {
-        sas::get_ros_param(nh,"/force_upper_limits_scaling_factor",upper_scale_factor);
-        ROS_WARN_STREAM(ros::this_node::getName()+"::Set force upper limits scaling factor: " << upper_scale_factor);
-    }
-    if(nh.hasParam(ros::this_node::getName()+"/upper_torque_threshold")) {
-        ROS_INFO_STREAM(ros::this_node::getName()+"::Upper torque threshold override and set.");
-        std::vector<double> upper_torque_threshold_std_vec;
-        sas::get_ros_param(nh,"/upper_torque_threshold",upper_torque_threshold_std_vec);
-        franka_interface_configuration.upper_torque_threshold = std_vec_to_std_array<double,7>(upper_torque_threshold_std_vec);
-    }else {
-        ROS_INFO_STREAM(ros::this_node::getName()+"::Upper torque threshold not set. Using default with value scalling.");
-        franka_interface_configuration.upper_torque_threshold = apply_scale_to_std_array(franka_interface_configuration.upper_torque_threshold, upper_scale_factor);
-    }
-    if(nh.hasParam(ros::this_node::getName()+"/upper_force_threshold")) {
-        ROS_INFO_STREAM(ros::this_node::getName()+"::Upper force threshold override and set.");
-        std::vector<double> upper_torque_threshold_std_vec;
-        sas::get_ros_param(nh,"/upper_force_threshold",upper_torque_threshold_std_vec);
-        franka_interface_configuration.upper_force_threshold = std_vec_to_std_array<double,6>(upper_torque_threshold_std_vec);
-    }else {
-        ROS_INFO_STREAM(ros::this_node::getName()+"::Upper force threshold not set. Using default with value scalling.");
-        franka_interface_configuration.upper_force_threshold = apply_scale_to_std_array(franka_interface_configuration.upper_force_threshold, upper_scale_factor);
-    }
-
-
-    robot_driver_franka_configuration.interface_configuration = franka_interface_configuration;
-
-    sas::RobotDriverROSConfiguration robot_driver_ros_configuration;
-
-    sas::get_ros_param(nh,"/thread_sampling_time_nsec",robot_driver_ros_configuration.thread_sampling_time_nsec);
-    sas::get_ros_param(nh,"/q_min",robot_driver_ros_configuration.q_min);
-    sas::get_ros_param(nh,"/q_max",robot_driver_ros_configuration.q_max);
-
-    // initialize the robot dynamic provider
-    robot_driver_ros_configuration.robot_driver_provider_prefix = ros::this_node::getName();
-    sas::RobotDynamicProvider robot_dynamic_provider(nh, robot_driver_ros_configuration.robot_driver_provider_prefix);
-
-
-    try
-        {
-            ROS_INFO_STREAM(ros::this_node::getName()+"::Instantiating Franka robot.");
-            sas::RobotDriverFranka robot_driver_franka(
-                &robot_dynamic_provider,
-                robot_driver_franka_configuration,
-                &kill_this_process
-            );
-            //robot_driver_franka.set_joint_limits({qmin, qmax});
-            ROS_INFO_STREAM(ros::this_node::getName()+"::Instantiating RobotDriverROS.");
-            sas::RobotDriverROS robot_driver_ros(nh,
-                                                &robot_driver_franka, 
-                                                robot_driver_ros_configuration,
-                                                &kill_this_process);
-            robot_driver_ros.control_loop();
-        }
-    catch (const std::exception& e)
-    {
-        kill_this_process = true;
-        ROS_ERROR_STREAM(ros::this_node::getName() + "::Exception::" + e.what());
-    }     
- 
-
-  return 0;
-}
Author	SHA1	Message	Date
qlin960618	5f19d983cb	[message interface] combining driver and interface package for better future management	2025-11-06 09:21:22 +09:00
qlin960618	afc21b1818	bug fix minor dereference bug	2025-07-07 16:43:41 +09:00
qlin960618	da362ebec1	added python interface additional type hint and misc	2025-07-05 18:09:59 +09:00
qlin960618	6ee5e85a64	updated minor optimization and bug fix for python gripper interface	2025-07-05 18:02:49 +09:00
qlin960618	89e25af2d2	updated minor optimization and bug fix for python gripper interface	2025-07-05 18:02:13 +09:00
qlin960618	a54ddb14ea	fix single typo in package depend	2025-07-04 23:08:23 +09:00
qlin960618	145067cbb4	bumping package version to match libfranka minor update	2025-03-26 10:30:43 +09:00
qlin960618	2fd5688131	minor update to documentation and package.xml	2025-02-10 12:29:26 +09:00
qlin960618	31ac91347a	update readme with basic documentations	2025-01-10 09:43:58 +09:00
qlin960618	f7b1ef6720	Merge remote-tracking branch 'gitea/ros2_jazzy' into ros2_jazzy	2024-12-28 08:58:48 +09:00
qlin960618	b2826cdce3	remove reference to constraint manager submodule	2024-12-28 08:55:43 +09:00
qlin960618	7ad72b3fa1	bug fix hand driver with parameter initialization default issue	2024-12-20 16:54:40 +09:00
qlin960618	fd98ce22ef	[sas_robot_driver_franka_hand_node] added gripper homing option to python module	2024-12-18 15:20:49 +09:00
qlin960618	fc34a7a289	[sas_robot_driver_franka_hand_node] added initialization option for not homing hand on startup	2024-12-18 15:13:33 +09:00
qlin960618	fac7dc597e	fix example script for RobotDynamics and gripper control	2024-12-18 10:23:41 +09:00
quentinlin	ed5dc45719	remove submodule reference to constraints manager	2024-12-16 22:49:21 +09:00
qlin960618	8d29513a6b	[sas_robot_driver_franka] fix namespace change with some sas parents class	2024-12-16 16:23:07 +09:00
qlin960618	787228d507	added homing service to franka hand node	2024-12-10 23:44:07 +09:00
qlin960618	4f0c5fb97d	[EffectorDriverFrankaHand] bug fix and optimization to franka hand code	2024-11-22 23:11:58 +09:00
qlin960618	9968016135	compatability fix for libfranka 0.14	2024-11-21 15:28:20 +09:00
qlin960618	2bcd8e72ff	CmakeList minor bugfix	2024-09-19 14:10:55 +09:00
qlin960618	825d39ae5c	package cleanup	2024-08-22 09:26:28 +09:00
qlin1806	35131ed59e	attempt to merge newer changes	2024-08-21 16:35:22 -07:00
qlin960618	fe5bb31873	bug fix hand initialization	2024-08-20 19:26:34 +09:00
qlin960618	e2f7830e01	bug fix hand initialization	2024-08-20 19:16:23 +09:00
qlin960618	75e00b3111	general bug fix	2024-08-19 15:30:30 +09:00
qlin960618	0358b851df	minor bug fix for franka hand again	2024-08-18 21:40:42 +09:00
qlin960618	27b89070aa	minor general bug fix	2024-08-18 21:16:47 +09:00
qlin960618	9b2061ba01	minor general bug fix	2024-08-18 20:57:24 +09:00
qlin960618	900f6aa2e1	minor renaming of file and working coppelia node	2024-08-16 11:30:58 +09:00
qlin960618	94a32a0f0c	driver and hand tested successful.. next is coppeliamirror node	2024-08-15 20:19:28 +09:00
qlin1806	058c123170	[package] update fix package manifest	2024-07-28 19:49:12 +09:00
qlin960618	db4b1ccd58	attempt to test python	2024-07-27 15:24:26 +09:00
qlin960618	66a45b9ece	added robot ui from JuankaEmika	2024-07-27 14:02:42 +09:00
qlin960618	7dfd4d40b8	migration to ros2 done	2024-07-27 13:07:37 +09:00
qlin960618	f5552be8d6	attempt to port ros2	2024-07-26 12:59:23 +09:00
QuentinLin	e72826aeff	remove gripper debugging output	2024-07-23 19:01:16 +09:00
qlin1806	221ccd1602	update example hand config	2024-07-22 21:50:59 -07:00
qlin1806	c47db0892a	added python interface	2024-07-22 21:33:07 -07:00
qlin1806	101c45c228	added gripper driver for service control	2024-07-22 05:25:28 -07:00
QuentinLin	25e88f56ed	stiffness frame bugfix	2024-07-22 08:54:55 +09:00
qlin1806	fce21f08b9	more optimization..	2024-07-21 03:52:21 -07:00
qlin1806	46668deac2	added proper binding for python for dynamic	2024-07-20 18:24:23 -07:00
qlin1806	1de3c2cbeb	rate optimization	2024-07-20 16:17:51 -07:00
qlin1806	79479ce982	minor uptimizeation	2024-07-20 16:06:49 -07:00
qlin960618	e263d34c0c	Echo contact (#1 ) * added cartesian interface * general fix and optimization	2024-07-20 14:34:54 +09:00