initial package cleanup

fix install of some library
working coppelia mirror node (#1 )
2024-08-21 16:30:44 -07:00 · 2024-08-14 20:09:45 -07:00 · 2024-08-14 14:27:29 +09:00 · 2024-08-03 12:25:45 +09:00 · 2024-08-03 12:24:03 +09:00 · 2024-08-02 18:26:24 +09:00
35 changed files with 348 additions and 1638 deletions
--- a/.gitignore
+++ b/.gitignore
@@ -1,3 +1,5 @@
 .idea
 CMakeLists.txt.user
-cmake-build*
+cmake-build*
+build/
+**/__pycache__/
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -117,7 +117,6 @@ target_link_libraries(robot_interface_hand Franka::Franka

 include_directories(
        include
-        include/generator
        src/
        src/robot_dynamics
        src/hand
@@ -137,6 +136,9 @@ target_link_libraries(qros_robot_dynamics_interface
        ${catkin_LIBRARIES}
        dqrobotics)

+add_dependencies(qros_robot_dynamics_interface ${${PROJECT_NAME}_EXPORTED_TARGETS})
+add_dependencies(qros_robot_dynamics_interface ${catkin_EXPORTED_TARGETS})
+

 add_library(sas_robot_driver_franka src/joint/sas_robot_driver_franka.cpp)
 target_link_libraries(sas_robot_driver_franka
@@ -148,13 +150,14 @@ target_link_libraries(sas_robot_driver_franka
 add_library(qros_effector_driver_franka_hand src/hand/qros_effector_driver_franka_hand.cpp)
 target_link_libraries(qros_effector_driver_franka_hand
        dqrobotics
-#        robot_interface_hand
        Franka::Franka)
-
+add_dependencies(qros_effector_driver_franka_hand ${${PROJECT_NAME}_EXPORTED_TARGETS})
+add_dependencies(qros_effector_driver_franka_hand ${catkin_EXPORTED_TARGETS})

 add_library(sas_robot_driver_coppelia src/coppelia/sas_robot_driver_coppelia.cpp)
 target_link_libraries(sas_robot_driver_coppelia
        dqrobotics
+        dqrobotics-interface-json11
        dqrobotics-interface-vrep)

 add_dependencies(sas_robot_driver_franka ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
@@ -195,11 +198,13 @@ target_link_libraries(JuankaEmika PRIVATE Qt${QT_VERSION_MAJOR}::Widgets
 #####################################################################################
 # python binding
 include_directories(
-        include/robot_dynamic
+        include/sas_robot_driver_franka/robot_dynamic
 )
 pybind_add_module(_qros_robot_dynamic SHARED
        src/robot_dynamics/qros_robot_dynamic_py.cpp src/robot_dynamics/qros_robot_dynamics_interface.cpp src/robot_dynamics/qros_robot_dynamics_provider.cpp
 )
+add_dependencies(_qros_robot_dynamic ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
 target_compile_definitions(_qros_robot_dynamic PRIVATE BUILD_PYBIND)
 # https://github.com/pybind/pybind11/issues/387
 target_link_libraries(_qros_robot_dynamic PRIVATE ${catkin_LIBRARIES} -ldqrobotics)
@@ -225,6 +230,14 @@ install(TARGETS sas_robot_driver_franka_hand_node
        DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
 )

+install(TARGETS qros_robot_dynamics_provider
+        DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+)
+
+install(TARGETS qros_robot_dynamics_interface
+        DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+)
+
 ## Mark cpp header files for installation
 install(DIRECTORY include/${PROJECT_NAME}/
        DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
--- a/include/Grasp.h
+++ b/include/Grasp.h
@@ -1,123 +0,0 @@
-// Generated by gencpp from file sas_robot_driver_franka/Grasp.msg
-// DO NOT EDIT!
-
-
-#ifndef SAS_ROBOT_DRIVER_FRANKA_MESSAGE_GRASP_H
-#define SAS_ROBOT_DRIVER_FRANKA_MESSAGE_GRASP_H
-
-#include <ros/service_traits.h>
-
-
-#include <sas_robot_driver_franka/GraspRequest.h>
-#include <sas_robot_driver_franka/GraspResponse.h>
-
-
-namespace sas_robot_driver_franka
-{
-
-struct Grasp
-{
-
-typedef GraspRequest Request;
-typedef GraspResponse Response;
-Request request;
-Response response;
-
-typedef Request RequestType;
-typedef Response ResponseType;
-
-}; // struct Grasp
-} // namespace sas_robot_driver_franka
-
-
-namespace ros
-{
-namespace service_traits
-{
-
-
-template<>
-struct MD5Sum< ::sas_robot_driver_franka::Grasp > {
-  static const char* value()
-  {
-    return "6752ec080e002a60682f31654d420583";
-  }
-
-  static const char* value(const ::sas_robot_driver_franka::Grasp&) { return value(); }
-};
-
-template<>
-struct DataType< ::sas_robot_driver_franka::Grasp > {
-  static const char* value()
-  {
-    return "sas_robot_driver_franka/Grasp";
-  }
-
-  static const char* value(const ::sas_robot_driver_franka::Grasp&) { return value(); }
-};
-
-
-// service_traits::MD5Sum< ::sas_robot_driver_franka::GraspRequest> should match
-// service_traits::MD5Sum< ::sas_robot_driver_franka::Grasp >
-template<>
-struct MD5Sum< ::sas_robot_driver_franka::GraspRequest>
-{
-  static const char* value()
-  {
-    return MD5Sum< ::sas_robot_driver_franka::Grasp >::value();
-  }
-  static const char* value(const ::sas_robot_driver_franka::GraspRequest&)
-  {
-    return value();
-  }
-};
-
-// service_traits::DataType< ::sas_robot_driver_franka::GraspRequest> should match
-// service_traits::DataType< ::sas_robot_driver_franka::Grasp >
-template<>
-struct DataType< ::sas_robot_driver_franka::GraspRequest>
-{
-  static const char* value()
-  {
-    return DataType< ::sas_robot_driver_franka::Grasp >::value();
-  }
-  static const char* value(const ::sas_robot_driver_franka::GraspRequest&)
-  {
-    return value();
-  }
-};
-
-// service_traits::MD5Sum< ::sas_robot_driver_franka::GraspResponse> should match
-// service_traits::MD5Sum< ::sas_robot_driver_franka::Grasp >
-template<>
-struct MD5Sum< ::sas_robot_driver_franka::GraspResponse>
-{
-  static const char* value()
-  {
-    return MD5Sum< ::sas_robot_driver_franka::Grasp >::value();
-  }
-  static const char* value(const ::sas_robot_driver_franka::GraspResponse&)
-  {
-    return value();
-  }
-};
-
-// service_traits::DataType< ::sas_robot_driver_franka::GraspResponse> should match
-// service_traits::DataType< ::sas_robot_driver_franka::Grasp >
-template<>
-struct DataType< ::sas_robot_driver_franka::GraspResponse>
-{
-  static const char* value()
-  {
-    return DataType< ::sas_robot_driver_franka::Grasp >::value();
-  }
-  static const char* value(const ::sas_robot_driver_franka::GraspResponse&)
-  {
-    return value();
-  }
-};
-
-} // namespace service_traits
-} // namespace ros
-
-#endif // SAS_ROBOT_DRIVER_FRANKA_MESSAGE_GRASP_H
--- a/include/GraspRequest.h
+++ b/include/GraspRequest.h
@@ -1,235 +0,0 @@
-// Generated by gencpp from file sas_robot_driver_franka/GraspRequest.msg
-// DO NOT EDIT!
-
-
-#ifndef SAS_ROBOT_DRIVER_FRANKA_MESSAGE_GRASPREQUEST_H
-#define SAS_ROBOT_DRIVER_FRANKA_MESSAGE_GRASPREQUEST_H
-
-
-#include <string>
-#include <vector>
-#include <memory>
-
-#include <ros/types.h>
-#include <ros/serialization.h>
-#include <ros/builtin_message_traits.h>
-#include <ros/message_operations.h>
-
-
-namespace sas_robot_driver_franka
-{
-template <class ContainerAllocator>
-struct GraspRequest_
-{
-  typedef GraspRequest_<ContainerAllocator> Type;
-
-  GraspRequest_()
-    : width(0.0)
-    , speed(0.0)
-    , force(0.0)
-    , epsilon_inner(0.0)
-    , epsilon_outer(0.0)  {
-    }
-  GraspRequest_(const ContainerAllocator& _alloc)
-    : width(0.0)
-    , speed(0.0)
-    , force(0.0)
-    , epsilon_inner(0.0)
-    , epsilon_outer(0.0)  {
-  (void)_alloc;
-    }
-
-
-
-   typedef float _width_type;
-  _width_type width;
-
-   typedef float _speed_type;
-  _speed_type speed;
-
-   typedef float _force_type;
-  _force_type force;
-
-   typedef float _epsilon_inner_type;
-  _epsilon_inner_type epsilon_inner;
-
-   typedef float _epsilon_outer_type;
-  _epsilon_outer_type epsilon_outer;
-
-
-
-
-
-  typedef boost::shared_ptr< ::sas_robot_driver_franka::GraspRequest_<ContainerAllocator> > Ptr;
-  typedef boost::shared_ptr< ::sas_robot_driver_franka::GraspRequest_<ContainerAllocator> const> ConstPtr;
-
-}; // struct GraspRequest_
-
-typedef ::sas_robot_driver_franka::GraspRequest_<std::allocator<void> > GraspRequest;
-
-typedef boost::shared_ptr< ::sas_robot_driver_franka::GraspRequest > GraspRequestPtr;
-typedef boost::shared_ptr< ::sas_robot_driver_franka::GraspRequest const> GraspRequestConstPtr;
-
-// constants requiring out of line definition
-
-
-
-template<typename ContainerAllocator>
-std::ostream& operator<<(std::ostream& s, const ::sas_robot_driver_franka::GraspRequest_<ContainerAllocator> & v)
-{
-ros::message_operations::Printer< ::sas_robot_driver_franka::GraspRequest_<ContainerAllocator> >::stream(s, "", v);
-return s;
-}
-
-
-template<typename ContainerAllocator1, typename ContainerAllocator2>
-bool operator==(const ::sas_robot_driver_franka::GraspRequest_<ContainerAllocator1> & lhs, const ::sas_robot_driver_franka::GraspRequest_<ContainerAllocator2> & rhs)
-{
-  return lhs.width == rhs.width &&
-    lhs.speed == rhs.speed &&
-    lhs.force == rhs.force &&
-    lhs.epsilon_inner == rhs.epsilon_inner &&
-    lhs.epsilon_outer == rhs.epsilon_outer;
-}
-
-template<typename ContainerAllocator1, typename ContainerAllocator2>
-bool operator!=(const ::sas_robot_driver_franka::GraspRequest_<ContainerAllocator1> & lhs, const ::sas_robot_driver_franka::GraspRequest_<ContainerAllocator2> & rhs)
-{
-  return !(lhs == rhs);
-}
-
-
-} // namespace sas_robot_driver_franka
-
-namespace ros
-{
-namespace message_traits
-{
-
-
-
-
-
-template <class ContainerAllocator>
-struct IsMessage< ::sas_robot_driver_franka::GraspRequest_<ContainerAllocator> >
-  : TrueType
-  { };
-
-template <class ContainerAllocator>
-struct IsMessage< ::sas_robot_driver_franka::GraspRequest_<ContainerAllocator> const>
-  : TrueType
-  { };
-
-template <class ContainerAllocator>
-struct IsFixedSize< ::sas_robot_driver_franka::GraspRequest_<ContainerAllocator> >
-  : TrueType
-  { };
-
-template <class ContainerAllocator>
-struct IsFixedSize< ::sas_robot_driver_franka::GraspRequest_<ContainerAllocator> const>
-  : TrueType
-  { };
-
-template <class ContainerAllocator>
-struct HasHeader< ::sas_robot_driver_franka::GraspRequest_<ContainerAllocator> >
-  : FalseType
-  { };
-
-template <class ContainerAllocator>
-struct HasHeader< ::sas_robot_driver_franka::GraspRequest_<ContainerAllocator> const>
-  : FalseType
-  { };
-
-
-template<class ContainerAllocator>
-struct MD5Sum< ::sas_robot_driver_franka::GraspRequest_<ContainerAllocator> >
-{
-  static const char* value()
-  {
-    return "337f46ba15e58c568d47e27881cf893c";
-  }
-
-  static const char* value(const ::sas_robot_driver_franka::GraspRequest_<ContainerAllocator>&) { return value(); }
-  static const uint64_t static_value1 = 0x337f46ba15e58c56ULL;
-  static const uint64_t static_value2 = 0x8d47e27881cf893cULL;
-};
-
-template<class ContainerAllocator>
-struct DataType< ::sas_robot_driver_franka::GraspRequest_<ContainerAllocator> >
-{
-  static const char* value()
-  {
-    return "sas_robot_driver_franka/GraspRequest";
-  }
-
-  static const char* value(const ::sas_robot_driver_franka::GraspRequest_<ContainerAllocator>&) { return value(); }
-};
-
-template<class ContainerAllocator>
-struct Definition< ::sas_robot_driver_franka::GraspRequest_<ContainerAllocator> >
-{
-  static const char* value()
-  {
-    return "float32 width\n"
-"float32 speed\n"
-"float32 force\n"
-"float32 epsilon_inner\n"
-"float32 epsilon_outer\n"
-;
-  }
-
-  static const char* value(const ::sas_robot_driver_franka::GraspRequest_<ContainerAllocator>&) { return value(); }
-};
-
-} // namespace message_traits
-} // namespace ros
-
-namespace ros
-{
-namespace serialization
-{
-
-  template<class ContainerAllocator> struct Serializer< ::sas_robot_driver_franka::GraspRequest_<ContainerAllocator> >
-  {
-    template<typename Stream, typename T> inline static void allInOne(Stream& stream, T m)
-    {
-      stream.next(m.width);
-      stream.next(m.speed);
-      stream.next(m.force);
-      stream.next(m.epsilon_inner);
-      stream.next(m.epsilon_outer);
-    }
-
-    ROS_DECLARE_ALLINONE_SERIALIZER
-  }; // struct GraspRequest_
-
-} // namespace serialization
-} // namespace ros
-
-namespace ros
-{
-namespace message_operations
-{
-
-template<class ContainerAllocator>
-struct Printer< ::sas_robot_driver_franka::GraspRequest_<ContainerAllocator> >
-{
-  template<typename Stream> static void stream(Stream& s, const std::string& indent, const ::sas_robot_driver_franka::GraspRequest_<ContainerAllocator>& v)
-  {
-    s << indent << "width: ";
-    Printer<float>::stream(s, indent + "  ", v.width);
-    s << indent << "speed: ";
-    Printer<float>::stream(s, indent + "  ", v.speed);
-    s << indent << "force: ";
-    Printer<float>::stream(s, indent + "  ", v.force);
-    s << indent << "epsilon_inner: ";
-    Printer<float>::stream(s, indent + "  ", v.epsilon_inner);
-    s << indent << "epsilon_outer: ";
-    Printer<float>::stream(s, indent + "  ", v.epsilon_outer);
-  }
-};
-
-} // namespace message_operations
-} // namespace ros
-
-#endif // SAS_ROBOT_DRIVER_FRANKA_MESSAGE_GRASPREQUEST_H
--- a/include/GraspResponse.h
+++ b/include/GraspResponse.h
@@ -1,195 +0,0 @@
-// Generated by gencpp from file sas_robot_driver_franka/GraspResponse.msg
-// DO NOT EDIT!
-
-
-#ifndef SAS_ROBOT_DRIVER_FRANKA_MESSAGE_GRASPRESPONSE_H
-#define SAS_ROBOT_DRIVER_FRANKA_MESSAGE_GRASPRESPONSE_H
-
-
-#include <string>
-#include <vector>
-#include <memory>
-
-#include <ros/types.h>
-#include <ros/serialization.h>
-#include <ros/builtin_message_traits.h>
-#include <ros/message_operations.h>
-
-
-namespace sas_robot_driver_franka
-{
-template <class ContainerAllocator>
-struct GraspResponse_
-{
-  typedef GraspResponse_<ContainerAllocator> Type;
-
-  GraspResponse_()
-    : success(false)  {
-    }
-  GraspResponse_(const ContainerAllocator& _alloc)
-    : success(false)  {
-  (void)_alloc;
-    }
-
-
-
-   typedef uint8_t _success_type;
-  _success_type success;
-
-
-
-
-
-  typedef boost::shared_ptr< ::sas_robot_driver_franka::GraspResponse_<ContainerAllocator> > Ptr;
-  typedef boost::shared_ptr< ::sas_robot_driver_franka::GraspResponse_<ContainerAllocator> const> ConstPtr;
-
-}; // struct GraspResponse_
-
-typedef ::sas_robot_driver_franka::GraspResponse_<std::allocator<void> > GraspResponse;
-
-typedef boost::shared_ptr< ::sas_robot_driver_franka::GraspResponse > GraspResponsePtr;
-typedef boost::shared_ptr< ::sas_robot_driver_franka::GraspResponse const> GraspResponseConstPtr;
-
-// constants requiring out of line definition
-
-
-
-template<typename ContainerAllocator>
-std::ostream& operator<<(std::ostream& s, const ::sas_robot_driver_franka::GraspResponse_<ContainerAllocator> & v)
-{
-ros::message_operations::Printer< ::sas_robot_driver_franka::GraspResponse_<ContainerAllocator> >::stream(s, "", v);
-return s;
-}
-
-
-template<typename ContainerAllocator1, typename ContainerAllocator2>
-bool operator==(const ::sas_robot_driver_franka::GraspResponse_<ContainerAllocator1> & lhs, const ::sas_robot_driver_franka::GraspResponse_<ContainerAllocator2> & rhs)
-{
-  return lhs.success == rhs.success;
-}
-
-template<typename ContainerAllocator1, typename ContainerAllocator2>
-bool operator!=(const ::sas_robot_driver_franka::GraspResponse_<ContainerAllocator1> & lhs, const ::sas_robot_driver_franka::GraspResponse_<ContainerAllocator2> & rhs)
-{
-  return !(lhs == rhs);
-}
-
-
-} // namespace sas_robot_driver_franka
-
-namespace ros
-{
-namespace message_traits
-{
-
-
-
-
-
-template <class ContainerAllocator>
-struct IsMessage< ::sas_robot_driver_franka::GraspResponse_<ContainerAllocator> >
-  : TrueType
-  { };
-
-template <class ContainerAllocator>
-struct IsMessage< ::sas_robot_driver_franka::GraspResponse_<ContainerAllocator> const>
-  : TrueType
-  { };
-
-template <class ContainerAllocator>
-struct IsFixedSize< ::sas_robot_driver_franka::GraspResponse_<ContainerAllocator> >
-  : TrueType
-  { };
-
-template <class ContainerAllocator>
-struct IsFixedSize< ::sas_robot_driver_franka::GraspResponse_<ContainerAllocator> const>
-  : TrueType
-  { };
-
-template <class ContainerAllocator>
-struct HasHeader< ::sas_robot_driver_franka::GraspResponse_<ContainerAllocator> >
-  : FalseType
-  { };
-
-template <class ContainerAllocator>
-struct HasHeader< ::sas_robot_driver_franka::GraspResponse_<ContainerAllocator> const>
-  : FalseType
-  { };
-
-
-template<class ContainerAllocator>
-struct MD5Sum< ::sas_robot_driver_franka::GraspResponse_<ContainerAllocator> >
-{
-  static const char* value()
-  {
-    return "358e233cde0c8a8bcfea4ce193f8fc15";
-  }
-
-  static const char* value(const ::sas_robot_driver_franka::GraspResponse_<ContainerAllocator>&) { return value(); }
-  static const uint64_t static_value1 = 0x358e233cde0c8a8bULL;
-  static const uint64_t static_value2 = 0xcfea4ce193f8fc15ULL;
-};
-
-template<class ContainerAllocator>
-struct DataType< ::sas_robot_driver_franka::GraspResponse_<ContainerAllocator> >
-{
-  static const char* value()
-  {
-    return "sas_robot_driver_franka/GraspResponse";
-  }
-
-  static const char* value(const ::sas_robot_driver_franka::GraspResponse_<ContainerAllocator>&) { return value(); }
-};
-
-template<class ContainerAllocator>
-struct Definition< ::sas_robot_driver_franka::GraspResponse_<ContainerAllocator> >
-{
-  static const char* value()
-  {
-    return "bool success\n"
-;
-  }
-
-  static const char* value(const ::sas_robot_driver_franka::GraspResponse_<ContainerAllocator>&) { return value(); }
-};
-
-} // namespace message_traits
-} // namespace ros
-
-namespace ros
-{
-namespace serialization
-{
-
-  template<class ContainerAllocator> struct Serializer< ::sas_robot_driver_franka::GraspResponse_<ContainerAllocator> >
-  {
-    template<typename Stream, typename T> inline static void allInOne(Stream& stream, T m)
-    {
-      stream.next(m.success);
-    }
-
-    ROS_DECLARE_ALLINONE_SERIALIZER
-  }; // struct GraspResponse_
-
-} // namespace serialization
-} // namespace ros
-
-namespace ros
-{
-namespace message_operations
-{
-
-template<class ContainerAllocator>
-struct Printer< ::sas_robot_driver_franka::GraspResponse_<ContainerAllocator> >
-{
-  template<typename Stream> static void stream(Stream& s, const std::string& indent, const ::sas_robot_driver_franka::GraspResponse_<ContainerAllocator>& v)
-  {
-    s << indent << "success: ";
-    Printer<uint8_t>::stream(s, indent + "  ", v.success);
-  }
-};
-
-} // namespace message_operations
-} // namespace ros
-
-#endif // SAS_ROBOT_DRIVER_FRANKA_MESSAGE_GRASPRESPONSE_H
--- a/include/GripperState.h
+++ b/include/GripperState.h
@@ -1,235 +0,0 @@
-// Generated by gencpp from file sas_robot_driver_franka/GripperState.msg
-// DO NOT EDIT!
-
-
-#ifndef SAS_ROBOT_DRIVER_FRANKA_MESSAGE_GRIPPERSTATE_H
-#define SAS_ROBOT_DRIVER_FRANKA_MESSAGE_GRIPPERSTATE_H
-
-
-#include <string>
-#include <vector>
-#include <memory>
-
-#include <ros/types.h>
-#include <ros/serialization.h>
-#include <ros/builtin_message_traits.h>
-#include <ros/message_operations.h>
-
-
-namespace sas_robot_driver_franka
-{
-template <class ContainerAllocator>
-struct GripperState_
-{
-  typedef GripperState_<ContainerAllocator> Type;
-
-  GripperState_()
-    : width(0.0)
-    , max_width(0.0)
-    , is_grasped(false)
-    , temperature(0)
-    , duration_ms(0)  {
-    }
-  GripperState_(const ContainerAllocator& _alloc)
-    : width(0.0)
-    , max_width(0.0)
-    , is_grasped(false)
-    , temperature(0)
-    , duration_ms(0)  {
-  (void)_alloc;
-    }
-
-
-
-   typedef float _width_type;
-  _width_type width;
-
-   typedef float _max_width_type;
-  _max_width_type max_width;
-
-   typedef uint8_t _is_grasped_type;
-  _is_grasped_type is_grasped;
-
-   typedef uint16_t _temperature_type;
-  _temperature_type temperature;
-
-   typedef uint64_t _duration_ms_type;
-  _duration_ms_type duration_ms;
-
-
-
-
-
-  typedef boost::shared_ptr< ::sas_robot_driver_franka::GripperState_<ContainerAllocator> > Ptr;
-  typedef boost::shared_ptr< ::sas_robot_driver_franka::GripperState_<ContainerAllocator> const> ConstPtr;
-
-}; // struct GripperState_
-
-typedef ::sas_robot_driver_franka::GripperState_<std::allocator<void> > GripperState;
-
-typedef boost::shared_ptr< ::sas_robot_driver_franka::GripperState > GripperStatePtr;
-typedef boost::shared_ptr< ::sas_robot_driver_franka::GripperState const> GripperStateConstPtr;
-
-// constants requiring out of line definition
-
-
-
-template<typename ContainerAllocator>
-std::ostream& operator<<(std::ostream& s, const ::sas_robot_driver_franka::GripperState_<ContainerAllocator> & v)
-{
-ros::message_operations::Printer< ::sas_robot_driver_franka::GripperState_<ContainerAllocator> >::stream(s, "", v);
-return s;
-}
-
-
-template<typename ContainerAllocator1, typename ContainerAllocator2>
-bool operator==(const ::sas_robot_driver_franka::GripperState_<ContainerAllocator1> & lhs, const ::sas_robot_driver_franka::GripperState_<ContainerAllocator2> & rhs)
-{
-  return lhs.width == rhs.width &&
-    lhs.max_width == rhs.max_width &&
-    lhs.is_grasped == rhs.is_grasped &&
-    lhs.temperature == rhs.temperature &&
-    lhs.duration_ms == rhs.duration_ms;
-}
-
-template<typename ContainerAllocator1, typename ContainerAllocator2>
-bool operator!=(const ::sas_robot_driver_franka::GripperState_<ContainerAllocator1> & lhs, const ::sas_robot_driver_franka::GripperState_<ContainerAllocator2> & rhs)
-{
-  return !(lhs == rhs);
-}
-
-
-} // namespace sas_robot_driver_franka
-
-namespace ros
-{
-namespace message_traits
-{
-
-
-
-
-
-template <class ContainerAllocator>
-struct IsMessage< ::sas_robot_driver_franka::GripperState_<ContainerAllocator> >
-  : TrueType
-  { };
-
-template <class ContainerAllocator>
-struct IsMessage< ::sas_robot_driver_franka::GripperState_<ContainerAllocator> const>
-  : TrueType
-  { };
-
-template <class ContainerAllocator>
-struct IsFixedSize< ::sas_robot_driver_franka::GripperState_<ContainerAllocator> >
-  : TrueType
-  { };
-
-template <class ContainerAllocator>
-struct IsFixedSize< ::sas_robot_driver_franka::GripperState_<ContainerAllocator> const>
-  : TrueType
-  { };
-
-template <class ContainerAllocator>
-struct HasHeader< ::sas_robot_driver_franka::GripperState_<ContainerAllocator> >
-  : FalseType
-  { };
-
-template <class ContainerAllocator>
-struct HasHeader< ::sas_robot_driver_franka::GripperState_<ContainerAllocator> const>
-  : FalseType
-  { };
-
-
-template<class ContainerAllocator>
-struct MD5Sum< ::sas_robot_driver_franka::GripperState_<ContainerAllocator> >
-{
-  static const char* value()
-  {
-    return "53f8669159aaded70b1783087f07679d";
-  }
-
-  static const char* value(const ::sas_robot_driver_franka::GripperState_<ContainerAllocator>&) { return value(); }
-  static const uint64_t static_value1 = 0x53f8669159aaded7ULL;
-  static const uint64_t static_value2 = 0x0b1783087f07679dULL;
-};
-
-template<class ContainerAllocator>
-struct DataType< ::sas_robot_driver_franka::GripperState_<ContainerAllocator> >
-{
-  static const char* value()
-  {
-    return "sas_robot_driver_franka/GripperState";
-  }
-
-  static const char* value(const ::sas_robot_driver_franka::GripperState_<ContainerAllocator>&) { return value(); }
-};
-
-template<class ContainerAllocator>
-struct Definition< ::sas_robot_driver_franka::GripperState_<ContainerAllocator> >
-{
-  static const char* value()
-  {
-    return "float32 width\n"
-"float32 max_width\n"
-"bool is_grasped\n"
-"uint16 temperature\n"
-"uint64 duration_ms\n"
-;
-  }
-
-  static const char* value(const ::sas_robot_driver_franka::GripperState_<ContainerAllocator>&) { return value(); }
-};
-
-} // namespace message_traits
-} // namespace ros
-
-namespace ros
-{
-namespace serialization
-{
-
-  template<class ContainerAllocator> struct Serializer< ::sas_robot_driver_franka::GripperState_<ContainerAllocator> >
-  {
-    template<typename Stream, typename T> inline static void allInOne(Stream& stream, T m)
-    {
-      stream.next(m.width);
-      stream.next(m.max_width);
-      stream.next(m.is_grasped);
-      stream.next(m.temperature);
-      stream.next(m.duration_ms);
-    }
-
-    ROS_DECLARE_ALLINONE_SERIALIZER
-  }; // struct GripperState_
-
-} // namespace serialization
-} // namespace ros
-
-namespace ros
-{
-namespace message_operations
-{
-
-template<class ContainerAllocator>
-struct Printer< ::sas_robot_driver_franka::GripperState_<ContainerAllocator> >
-{
-  template<typename Stream> static void stream(Stream& s, const std::string& indent, const ::sas_robot_driver_franka::GripperState_<ContainerAllocator>& v)
-  {
-    s << indent << "width: ";
-    Printer<float>::stream(s, indent + "  ", v.width);
-    s << indent << "max_width: ";
-    Printer<float>::stream(s, indent + "  ", v.max_width);
-    s << indent << "is_grasped: ";
-    Printer<uint8_t>::stream(s, indent + "  ", v.is_grasped);
-    s << indent << "temperature: ";
-    Printer<uint16_t>::stream(s, indent + "  ", v.temperature);
-    s << indent << "duration_ms: ";
-    Printer<uint64_t>::stream(s, indent + "  ", v.duration_ms);
-  }
-};
-
-} // namespace message_operations
-} // namespace ros
-
-#endif // SAS_ROBOT_DRIVER_FRANKA_MESSAGE_GRIPPERSTATE_H
--- a/include/Move.h
+++ b/include/Move.h
@@ -1,123 +0,0 @@
-// Generated by gencpp from file sas_robot_driver_franka/Move.msg
-// DO NOT EDIT!
-
-
-#ifndef SAS_ROBOT_DRIVER_FRANKA_MESSAGE_MOVE_H
-#define SAS_ROBOT_DRIVER_FRANKA_MESSAGE_MOVE_H
-
-#include <ros/service_traits.h>
-
-
-#include <sas_robot_driver_franka/MoveRequest.h>
-#include <sas_robot_driver_franka/MoveResponse.h>
-
-
-namespace sas_robot_driver_franka
-{
-
-struct Move
-{
-
-typedef MoveRequest Request;
-typedef MoveResponse Response;
-Request request;
-Response response;
-
-typedef Request RequestType;
-typedef Response ResponseType;
-
-}; // struct Move
-} // namespace sas_robot_driver_franka
-
-
-namespace ros
-{
-namespace service_traits
-{
-
-
-template<>
-struct MD5Sum< ::sas_robot_driver_franka::Move > {
-  static const char* value()
-  {
-    return "73e650ba1526b28d9e3f1be7ee33a441";
-  }
-
-  static const char* value(const ::sas_robot_driver_franka::Move&) { return value(); }
-};
-
-template<>
-struct DataType< ::sas_robot_driver_franka::Move > {
-  static const char* value()
-  {
-    return "sas_robot_driver_franka/Move";
-  }
-
-  static const char* value(const ::sas_robot_driver_franka::Move&) { return value(); }
-};
-
-
-// service_traits::MD5Sum< ::sas_robot_driver_franka::MoveRequest> should match
-// service_traits::MD5Sum< ::sas_robot_driver_franka::Move >
-template<>
-struct MD5Sum< ::sas_robot_driver_franka::MoveRequest>
-{
-  static const char* value()
-  {
-    return MD5Sum< ::sas_robot_driver_franka::Move >::value();
-  }
-  static const char* value(const ::sas_robot_driver_franka::MoveRequest&)
-  {
-    return value();
-  }
-};
-
-// service_traits::DataType< ::sas_robot_driver_franka::MoveRequest> should match
-// service_traits::DataType< ::sas_robot_driver_franka::Move >
-template<>
-struct DataType< ::sas_robot_driver_franka::MoveRequest>
-{
-  static const char* value()
-  {
-    return DataType< ::sas_robot_driver_franka::Move >::value();
-  }
-  static const char* value(const ::sas_robot_driver_franka::MoveRequest&)
-  {
-    return value();
-  }
-};
-
-// service_traits::MD5Sum< ::sas_robot_driver_franka::MoveResponse> should match
-// service_traits::MD5Sum< ::sas_robot_driver_franka::Move >
-template<>
-struct MD5Sum< ::sas_robot_driver_franka::MoveResponse>
-{
-  static const char* value()
-  {
-    return MD5Sum< ::sas_robot_driver_franka::Move >::value();
-  }
-  static const char* value(const ::sas_robot_driver_franka::MoveResponse&)
-  {
-    return value();
-  }
-};
-
-// service_traits::DataType< ::sas_robot_driver_franka::MoveResponse> should match
-// service_traits::DataType< ::sas_robot_driver_franka::Move >
-template<>
-struct DataType< ::sas_robot_driver_franka::MoveResponse>
-{
-  static const char* value()
-  {
-    return DataType< ::sas_robot_driver_franka::Move >::value();
-  }
-  static const char* value(const ::sas_robot_driver_franka::MoveResponse&)
-  {
-    return value();
-  }
-};
-
-} // namespace service_traits
-} // namespace ros
-
-#endif // SAS_ROBOT_DRIVER_FRANKA_MESSAGE_MOVE_H
--- a/include/MoveRequest.h
+++ b/include/MoveRequest.h
@@ -1,205 +0,0 @@
-// Generated by gencpp from file sas_robot_driver_franka/MoveRequest.msg
-// DO NOT EDIT!
-
-
-#ifndef SAS_ROBOT_DRIVER_FRANKA_MESSAGE_MOVEREQUEST_H
-#define SAS_ROBOT_DRIVER_FRANKA_MESSAGE_MOVEREQUEST_H
-
-
-#include <string>
-#include <vector>
-#include <memory>
-
-#include <ros/types.h>
-#include <ros/serialization.h>
-#include <ros/builtin_message_traits.h>
-#include <ros/message_operations.h>
-
-
-namespace sas_robot_driver_franka
-{
-template <class ContainerAllocator>
-struct MoveRequest_
-{
-  typedef MoveRequest_<ContainerAllocator> Type;
-
-  MoveRequest_()
-    : width(0.0)
-    , speed(0.0)  {
-    }
-  MoveRequest_(const ContainerAllocator& _alloc)
-    : width(0.0)
-    , speed(0.0)  {
-  (void)_alloc;
-    }
-
-
-
-   typedef float _width_type;
-  _width_type width;
-
-   typedef float _speed_type;
-  _speed_type speed;
-
-
-
-
-
-  typedef boost::shared_ptr< ::sas_robot_driver_franka::MoveRequest_<ContainerAllocator> > Ptr;
-  typedef boost::shared_ptr< ::sas_robot_driver_franka::MoveRequest_<ContainerAllocator> const> ConstPtr;
-
-}; // struct MoveRequest_
-
-typedef ::sas_robot_driver_franka::MoveRequest_<std::allocator<void> > MoveRequest;
-
-typedef boost::shared_ptr< ::sas_robot_driver_franka::MoveRequest > MoveRequestPtr;
-typedef boost::shared_ptr< ::sas_robot_driver_franka::MoveRequest const> MoveRequestConstPtr;
-
-// constants requiring out of line definition
-
-
-
-template<typename ContainerAllocator>
-std::ostream& operator<<(std::ostream& s, const ::sas_robot_driver_franka::MoveRequest_<ContainerAllocator> & v)
-{
-ros::message_operations::Printer< ::sas_robot_driver_franka::MoveRequest_<ContainerAllocator> >::stream(s, "", v);
-return s;
-}
-
-
-template<typename ContainerAllocator1, typename ContainerAllocator2>
-bool operator==(const ::sas_robot_driver_franka::MoveRequest_<ContainerAllocator1> & lhs, const ::sas_robot_driver_franka::MoveRequest_<ContainerAllocator2> & rhs)
-{
-  return lhs.width == rhs.width &&
-    lhs.speed == rhs.speed;
-}
-
-template<typename ContainerAllocator1, typename ContainerAllocator2>
-bool operator!=(const ::sas_robot_driver_franka::MoveRequest_<ContainerAllocator1> & lhs, const ::sas_robot_driver_franka::MoveRequest_<ContainerAllocator2> & rhs)
-{
-  return !(lhs == rhs);
-}
-
-
-} // namespace sas_robot_driver_franka
-
-namespace ros
-{
-namespace message_traits
-{
-
-
-
-
-
-template <class ContainerAllocator>
-struct IsMessage< ::sas_robot_driver_franka::MoveRequest_<ContainerAllocator> >
-  : TrueType
-  { };
-
-template <class ContainerAllocator>
-struct IsMessage< ::sas_robot_driver_franka::MoveRequest_<ContainerAllocator> const>
-  : TrueType
-  { };
-
-template <class ContainerAllocator>
-struct IsFixedSize< ::sas_robot_driver_franka::MoveRequest_<ContainerAllocator> >
-  : TrueType
-  { };
-
-template <class ContainerAllocator>
-struct IsFixedSize< ::sas_robot_driver_franka::MoveRequest_<ContainerAllocator> const>
-  : TrueType
-  { };
-
-template <class ContainerAllocator>
-struct HasHeader< ::sas_robot_driver_franka::MoveRequest_<ContainerAllocator> >
-  : FalseType
-  { };
-
-template <class ContainerAllocator>
-struct HasHeader< ::sas_robot_driver_franka::MoveRequest_<ContainerAllocator> const>
-  : FalseType
-  { };
-
-
-template<class ContainerAllocator>
-struct MD5Sum< ::sas_robot_driver_franka::MoveRequest_<ContainerAllocator> >
-{
-  static const char* value()
-  {
-    return "b2d4f46fe020a06d64128c90310c767d";
-  }
-
-  static const char* value(const ::sas_robot_driver_franka::MoveRequest_<ContainerAllocator>&) { return value(); }
-  static const uint64_t static_value1 = 0xb2d4f46fe020a06dULL;
-  static const uint64_t static_value2 = 0x64128c90310c767dULL;
-};
-
-template<class ContainerAllocator>
-struct DataType< ::sas_robot_driver_franka::MoveRequest_<ContainerAllocator> >
-{
-  static const char* value()
-  {
-    return "sas_robot_driver_franka/MoveRequest";
-  }
-
-  static const char* value(const ::sas_robot_driver_franka::MoveRequest_<ContainerAllocator>&) { return value(); }
-};
-
-template<class ContainerAllocator>
-struct Definition< ::sas_robot_driver_franka::MoveRequest_<ContainerAllocator> >
-{
-  static const char* value()
-  {
-    return "float32 width\n"
-"float32 speed\n"
-;
-  }
-
-  static const char* value(const ::sas_robot_driver_franka::MoveRequest_<ContainerAllocator>&) { return value(); }
-};
-
-} // namespace message_traits
-} // namespace ros
-
-namespace ros
-{
-namespace serialization
-{
-
-  template<class ContainerAllocator> struct Serializer< ::sas_robot_driver_franka::MoveRequest_<ContainerAllocator> >
-  {
-    template<typename Stream, typename T> inline static void allInOne(Stream& stream, T m)
-    {
-      stream.next(m.width);
-      stream.next(m.speed);
-    }
-
-    ROS_DECLARE_ALLINONE_SERIALIZER
-  }; // struct MoveRequest_
-
-} // namespace serialization
-} // namespace ros
-
-namespace ros
-{
-namespace message_operations
-{
-
-template<class ContainerAllocator>
-struct Printer< ::sas_robot_driver_franka::MoveRequest_<ContainerAllocator> >
-{
-  template<typename Stream> static void stream(Stream& s, const std::string& indent, const ::sas_robot_driver_franka::MoveRequest_<ContainerAllocator>& v)
-  {
-    s << indent << "width: ";
-    Printer<float>::stream(s, indent + "  ", v.width);
-    s << indent << "speed: ";
-    Printer<float>::stream(s, indent + "  ", v.speed);
-  }
-};
-
-} // namespace message_operations
-} // namespace ros
-
-#endif // SAS_ROBOT_DRIVER_FRANKA_MESSAGE_MOVEREQUEST_H
--- a/include/MoveResponse.h
+++ b/include/MoveResponse.h
@@ -1,195 +0,0 @@
-// Generated by gencpp from file sas_robot_driver_franka/MoveResponse.msg
-// DO NOT EDIT!
-
-
-#ifndef SAS_ROBOT_DRIVER_FRANKA_MESSAGE_MOVERESPONSE_H
-#define SAS_ROBOT_DRIVER_FRANKA_MESSAGE_MOVERESPONSE_H
-
-
-#include <string>
-#include <vector>
-#include <memory>
-
-#include <ros/types.h>
-#include <ros/serialization.h>
-#include <ros/builtin_message_traits.h>
-#include <ros/message_operations.h>
-
-
-namespace sas_robot_driver_franka
-{
-template <class ContainerAllocator>
-struct MoveResponse_
-{
-  typedef MoveResponse_<ContainerAllocator> Type;
-
-  MoveResponse_()
-    : success(false)  {
-    }
-  MoveResponse_(const ContainerAllocator& _alloc)
-    : success(false)  {
-  (void)_alloc;
-    }
-
-
-
-   typedef uint8_t _success_type;
-  _success_type success;
-
-
-
-
-
-  typedef boost::shared_ptr< ::sas_robot_driver_franka::MoveResponse_<ContainerAllocator> > Ptr;
-  typedef boost::shared_ptr< ::sas_robot_driver_franka::MoveResponse_<ContainerAllocator> const> ConstPtr;
-
-}; // struct MoveResponse_
-
-typedef ::sas_robot_driver_franka::MoveResponse_<std::allocator<void> > MoveResponse;
-
-typedef boost::shared_ptr< ::sas_robot_driver_franka::MoveResponse > MoveResponsePtr;
-typedef boost::shared_ptr< ::sas_robot_driver_franka::MoveResponse const> MoveResponseConstPtr;
-
-// constants requiring out of line definition
-
-
-
-template<typename ContainerAllocator>
-std::ostream& operator<<(std::ostream& s, const ::sas_robot_driver_franka::MoveResponse_<ContainerAllocator> & v)
-{
-ros::message_operations::Printer< ::sas_robot_driver_franka::MoveResponse_<ContainerAllocator> >::stream(s, "", v);
-return s;
-}
-
-
-template<typename ContainerAllocator1, typename ContainerAllocator2>
-bool operator==(const ::sas_robot_driver_franka::MoveResponse_<ContainerAllocator1> & lhs, const ::sas_robot_driver_franka::MoveResponse_<ContainerAllocator2> & rhs)
-{
-  return lhs.success == rhs.success;
-}
-
-template<typename ContainerAllocator1, typename ContainerAllocator2>
-bool operator!=(const ::sas_robot_driver_franka::MoveResponse_<ContainerAllocator1> & lhs, const ::sas_robot_driver_franka::MoveResponse_<ContainerAllocator2> & rhs)
-{
-  return !(lhs == rhs);
-}
-
-
-} // namespace sas_robot_driver_franka
-
-namespace ros
-{
-namespace message_traits
-{
-
-
-
-
-
-template <class ContainerAllocator>
-struct IsMessage< ::sas_robot_driver_franka::MoveResponse_<ContainerAllocator> >
-  : TrueType
-  { };
-
-template <class ContainerAllocator>
-struct IsMessage< ::sas_robot_driver_franka::MoveResponse_<ContainerAllocator> const>
-  : TrueType
-  { };
-
-template <class ContainerAllocator>
-struct IsFixedSize< ::sas_robot_driver_franka::MoveResponse_<ContainerAllocator> >
-  : TrueType
-  { };
-
-template <class ContainerAllocator>
-struct IsFixedSize< ::sas_robot_driver_franka::MoveResponse_<ContainerAllocator> const>
-  : TrueType
-  { };
-
-template <class ContainerAllocator>
-struct HasHeader< ::sas_robot_driver_franka::MoveResponse_<ContainerAllocator> >
-  : FalseType
-  { };
-
-template <class ContainerAllocator>
-struct HasHeader< ::sas_robot_driver_franka::MoveResponse_<ContainerAllocator> const>
-  : FalseType
-  { };
-
-
-template<class ContainerAllocator>
-struct MD5Sum< ::sas_robot_driver_franka::MoveResponse_<ContainerAllocator> >
-{
-  static const char* value()
-  {
-    return "358e233cde0c8a8bcfea4ce193f8fc15";
-  }
-
-  static const char* value(const ::sas_robot_driver_franka::MoveResponse_<ContainerAllocator>&) { return value(); }
-  static const uint64_t static_value1 = 0x358e233cde0c8a8bULL;
-  static const uint64_t static_value2 = 0xcfea4ce193f8fc15ULL;
-};
-
-template<class ContainerAllocator>
-struct DataType< ::sas_robot_driver_franka::MoveResponse_<ContainerAllocator> >
-{
-  static const char* value()
-  {
-    return "sas_robot_driver_franka/MoveResponse";
-  }
-
-  static const char* value(const ::sas_robot_driver_franka::MoveResponse_<ContainerAllocator>&) { return value(); }
-};
-
-template<class ContainerAllocator>
-struct Definition< ::sas_robot_driver_franka::MoveResponse_<ContainerAllocator> >
-{
-  static const char* value()
-  {
-    return "bool success\n"
-;
-  }
-
-  static const char* value(const ::sas_robot_driver_franka::MoveResponse_<ContainerAllocator>&) { return value(); }
-};
-
-} // namespace message_traits
-} // namespace ros
-
-namespace ros
-{
-namespace serialization
-{
-
-  template<class ContainerAllocator> struct Serializer< ::sas_robot_driver_franka::MoveResponse_<ContainerAllocator> >
-  {
-    template<typename Stream, typename T> inline static void allInOne(Stream& stream, T m)
-    {
-      stream.next(m.success);
-    }
-
-    ROS_DECLARE_ALLINONE_SERIALIZER
-  }; // struct MoveResponse_
-
-} // namespace serialization
-} // namespace ros
-
-namespace ros
-{
-namespace message_operations
-{
-
-template<class ContainerAllocator>
-struct Printer< ::sas_robot_driver_franka::MoveResponse_<ContainerAllocator> >
-{
-  template<typename Stream> static void stream(Stream& s, const std::string& indent, const ::sas_robot_driver_franka::MoveResponse_<ContainerAllocator>& v)
-  {
-    s << indent << "success: ";
-    Printer<uint8_t>::stream(s, indent + "  ", v.success);
-  }
-};
-
-} // namespace message_operations
-} // namespace ros
-
-#endif // SAS_ROBOT_DRIVER_FRANKA_MESSAGE_MOVERESPONSE_H
--- a/include/sas_robot_driver_franka/generator/custom_motion_generation.h
+++ b/include/sas_robot_driver_franka/generator/custom_motion_generation.h
--- a/include/sas_robot_driver_franka/generator/motion_generator.h
+++ b/include/sas_robot_driver_franka/generator/motion_generator.h
--- a/include/sas_robot_driver_franka/generator/quadratic_program_motion_generator.h
+++ b/include/sas_robot_driver_franka/generator/quadratic_program_motion_generator.h
--- a/include/sas_robot_driver_franka/robot_dynamic/qros_robot_dynamics_interface.h
+++ b/include/sas_robot_driver_franka/robot_dynamic/qros_robot_dynamics_interface.h
--- a/include/sas_robot_driver_franka/robot_dynamic/qros_robot_dynamics_provider.h
+++ b/include/sas_robot_driver_franka/robot_dynamic/qros_robot_dynamics_provider.h
--- a/include/sas_robot_driver_franka/robot_interface_franka.h
+++ b/include/sas_robot_driver_franka/robot_interface_franka.h
--- a/include/sas_robot_driver_franka/robot_interface_hand.h
+++ b/include/sas_robot_driver_franka/robot_interface_hand.h
--- a/include/sas_robot_driver_franka/sas_robot_driver_franka.h
+++ b/include/sas_robot_driver_franka/sas_robot_driver_franka.h
@@ -43,13 +43,15 @@
 #include <sas_robot_driver/sas_robot_driver.h>
 #include "robot_interface_franka.h"
 #include <ros/common.h>
-#include <robot_dynamic/qros_robot_dynamics_provider.h>
+#include <sas_robot_driver_franka/robot_dynamic/qros_robot_dynamics_provider.h>
+#include <sas_clock/sas_clock.h>
+#include <dqrobotics/utils/DQ_Math.h>
+#include <ros/this_node.h>
+#include <rosconsole/macros_generated.h>

 using namespace DQ_robotics;
 using namespace Eigen;

-struct RobotInterfaceFranka::FrankaInterfaceConfiguration;  // Forward declaration
-
 namespace sas
 {

@@ -61,6 +63,7 @@ struct RobotDriverFrankaConfiguration
    double speed;
    RobotInterfaceFranka::FrankaInterfaceConfiguration interface_configuration;
    DQ robot_reference_frame = DQ(0);
+    bool automatic_error_recovery = false;
 };


--- a/qt/configuration_window/mainwindow.h
+++ b/qt/configuration_window/mainwindow.h
@@ -7,7 +7,7 @@

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

 QT_BEGIN_NAMESPACE
 namespace Ui { class MainWindow; }
--- a/src/coppelia/sas_robot_driver_coppelia.cpp
+++ b/src/coppelia/sas_robot_driver_coppelia.cpp
@@ -4,249 +4,240 @@
 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_);
+RobotDriverCoppelia::~RobotDriverCoppelia() {
+    deinitialize();
+    disconnect();
 }

-VectorXd RobotDriverCoppelia::get_joint_positions()
+RobotDriverCoppelia::RobotDriverCoppelia(ros::NodeHandle nh, const RobotDriverCoppeliaConfiguration &configuration, std::atomic_bool *break_loops):
+        configuration_(configuration),
+        clock_(configuration.thread_sampling_time_nsec),
+        break_loops_(break_loops),
+        robot_mode_(ControlMode::Position),
+        vi_(std::make_shared<DQ_VrepInterface>())
 {
-    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())
+    // 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)
    {
-        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();
+        ROS_INFO_STREAM("[" + ros::this_node::getName() + "]::Using real robot, Instantiating robot interface to real driver.");
+        real_robot_interface_ = std::make_shared<RobotDriverInterface>(nh, configuration_.robot_topic_prefix);
    }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;
-            }
+        ROS_INFO_STREAM("[" + ros::this_node::getName() + "]::Using simulation robot, simulation mode is set to "+ configuration_.robot_mode);
+        robot_provider_ = std::make_shared<RobotDriverProvider>(nh, 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("[" + ros::this_node::getName() + "]::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();
+    ROS_INFO_STREAM("[" + ros::this_node::getName() + "]::Starting control loop...");
+    while(!_should_shutdown()){
+        clock_.update_and_sleep();
+        ros::spinOnce();
+        if(!ros::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{
+                        ROS_DEBUG_STREAM("[" + ros::this_node::getName() + "]::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{
+        ROS_INFO_STREAM(ros::this_node::getName() << "::Waiting to connect with coppelia...");
+        connect();
+        ROS_INFO_STREAM(ros::this_node::getName() << "::Connected to coppelia.");
+        ROS_INFO_STREAM(ros::this_node::getName() << "::Initializing...");
+        initialize();
+        ROS_INFO_STREAM(ros::this_node::getName() << "::initialized.");
+
+        _start_control_loop();
+    }
    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;
+        ROS_WARN_STREAM(ros::this_node::getName() + "::Error or exception caught::" << e.what());
    }
    catch(...)
    {
-        std::cout<<"RobotDriverCoppelia::sas_robot_coppelia_driver::_start_joint_velocity_control_mode(). Error or exception caught " <<std::endl;
+        ROS_WARN_STREAM(ros::this_node::getName() + "::Unexpected error or exception caught");
    }
+    ROS_INFO_STREAM(ros::this_node::getName() << "::Deinitializing...");
+    deinitialize();
+    ROS_INFO_STREAM(ros::this_node::getName() << "::deinitialized.");
+    ROS_INFO_STREAM(ros::this_node::getName() << "::Disconnecting from coppelia...");
+    disconnect();
+    ROS_INFO_STREAM(ros::this_node::getName() << "::Disconnected from coppelia.");

-
+    return 0;
 }

-void RobotDriverCoppelia::_start_joint_velocity_control_thread()
-{
-    finish_motion_ = false;
-    if (joint_velocity_control_mode_thread_.joinable())
-    {
-        joint_velocity_control_mode_thread_.join();
+
+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");
    }
-    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);
+    ROS_INFO_STREAM("[" + ros::this_node::getName() + "]::[RobotDriverCoppelia]::connect::Connected to Vrep");
+}
+void RobotDriverCoppelia::disconnect(){
+    vi_->disconnect_all();
 }

-void RobotDriverCoppelia::_start_joint_velocity_control_mirror_thread()
-{
-    finish_motion_ = false;
-    if (joint_velocity_control_mode_thread_.joinable())
+void RobotDriverCoppelia::initialize(){
+    if(configuration_.using_real_robot)
    {
-        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_INFO_STREAM("[" + ros::this_node::getName() +"]::[RobotDriverCoppelia]::initialize::Waiting for real robot interface to initialize...");
+        ros::spinOnce();
+        int count = 0;
+        while (!real_robot_interface_->is_enabled()) {
            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;
-                }
+            std::this_thread::sleep_for(std::chrono::milliseconds(100));
+            count++;
+            if(count > REAL_ROBOT_INTERFACE_INIT_TIMEOUT_COUNT){
+                ROS_ERROR_STREAM("[" + ros::this_node::getName() +"]::[RobotDriverCoppelia]::initialize::Real robot interface not initialized. Exiting on TIMEOUT...");
+                throw std::runtime_error("[" + ros::this_node::getName() +"]::[RobotDriverCoppelia]::initialize::Real robot interface not initialized.");
+            }
+            if(!ros::ok()) {
+                ROS_WARN_STREAM("[" + ros::this_node::getName() +"]::[RobotDriverCoppelia]::initialize::ROS shutdown recieved. Exiting...");
+                throw std::runtime_error("[" + ros::this_node::getName() +"]::[RobotDriverCoppelia]::initialize::ROS shutdown recieved, not OK.");
            }
        }
+        ROS_INFO_STREAM("[" + ros::this_node::getName() +"]::[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{
+        ROS_INFO_STREAM("[" + ros::this_node::getName() +"]::[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();
+        }
    }
-    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;
-    }
-
-
+}
+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{
+        ROS_INFO_STREAM("[" + ros::this_node::getName() +
+                        "]::[RobotDriverCoppelia]::[_velocity_control_simulation_thread_main]::Simulation thread started.");
+        sas::Clock clock = sas::Clock(VIRTUAL_ROBOT_SIMULATION_SAMPLING_TIME_NSEC);
+        double tau = static_cast<double>(VIRTUAL_ROBOT_SIMULATION_SAMPLING_TIME_NSEC) * 1e-9;
+        auto current_joint_positions = simulated_joint_positions_;
+        clock.init();
+        while (!(*break_loops_) && ros::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){
+        ROS_ERROR_STREAM("[" + ros::this_node::getName() + "]::[RobotDriverCoppelia]::[_velocity_control_simulation_thread_main]::Exception::" + e.what());
+        simulation_thread_started_ = false;
+    }catch(...){
+        ROS_ERROR_STREAM("[" + ros::this_node::getName() + "]::[RobotDriverCoppelia]::[_velocity_control_simulation_thread_main]::Exception::Unknown");
+        simulation_thread_started_ = false;
+    }
+
+}



--- a/src/coppelia/sas_robot_driver_coppelia.h
+++ b/src/coppelia/sas_robot_driver_coppelia.h
@@ -39,10 +39,18 @@
 #include <memory>
 #include <dqrobotics/DQ.h>
 #include <sas_robot_driver/sas_robot_driver.h>
-#include <dqrobotics/interfaces/vrep/DQ_VrepRobot.h>
+#include <sas_clock/sas_clock.h>
+// #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_interface.h>
+#include <sas_robot_driver/sas_robot_driver_provider.h>

+#define VIRTUAL_ROBOT_SIMULATION_SAMPLING_TIME_NSEC 2000000  // 2ms, 500Hz
+#define REAL_ROBOT_INTERFACE_INIT_TIMEOUT_COUNT 500
 using namespace DQ_robotics;
 using namespace Eigen;

@@ -54,81 +62,78 @@ namespace sas
 struct RobotDriverCoppeliaConfiguration
 {

-    int thread_sampling_time_nsec;
-    int port;
-    std::string ip;
-    std::vector<std::string> jointnames;
+    int thread_sampling_time_nsec;  // 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 mirror_mode;
-    std::string real_robot_topic_prefix;
+    bool using_real_robot;
+    std::string robot_topic_prefix;
+    std::string robot_parameter_file_path;
+    // VectorXd q_min;
+    // VectorXd q_max;
 };

-class RobotDriverCoppelia: public RobotDriver
+class RobotDriverCoppelia
 {
 private:
+    enum ControlMode{
+        Position=0,
+        Velocity
+    };
+
    RobotDriverCoppeliaConfiguration configuration_;

-    std::string robot_mode_ = std::string("VelocityControl");   // PositionControl
-    bool mirror_mode_ = false;
-    double gain_ = 3.0;
-    std::string real_robot_topic_prefix_;
+    Clock clock_;
+    std::atomic_bool* break_loops_;
+    bool _should_shutdown() const {return (*break_loops_);}
+    ControlMode robot_mode_;

-    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_;
+    std::shared_ptr<sas::RobotDriverInterface> real_robot_interface_ = nullptr;
+    std::shared_ptr<sas::RobotDriverProvider> 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 RobotDriverCoppeliaConfiguration& configuration, std::atomic_bool* break_loops);
+    RobotDriverCoppelia(ros::NodeHandle nh, const RobotDriverCoppeliaConfiguration& configuration, std::atomic_bool* break_loops);

+    int start_control_loop();  // public entry point

-    VectorXd get_joint_positions() override;
-    void set_target_joint_positions(const VectorXd& desired_joint_positions_rad) override;
+    void connect();
+    void disconnect();

-    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;
+    void initialize();
+    void deinitialize();

+    std::tuple<VectorXd, VectorXd> joint_limits_;

 };
 }
--- a/src/deprecated/robot_coppelia_ros_interface_node.cpp
+++ b/src/deprecated/robot_coppelia_ros_interface_node.cpp
@@ -39,7 +39,7 @@
 #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 "../../include/sas_robot_driver_franka/sas_robot_driver_franka.h"
 #include "robot_coppelia_ros_interface.h"

 /*********************************************
--- a/src/generator/custom_motion_generation.cpp
+++ b/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/src/generator/motion_generator.cpp
+++ b/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/src/generator/quadratic_program_motion_generator.cpp
+++ b/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/src/hand/qros_effector_driver_franka_hand.cpp
+++ b/src/hand/qros_effector_driver_franka_hand.cpp
@@ -27,9 +27,8 @@
 #
 # ################################################################
 */
-#include "hand/qros_effector_driver_franka_hand.h"
+#include "qros_effector_driver_franka_hand.h"

-#include <franka/exception.h>


 namespace qros
--- a/src/hand/qros_effector_driver_franka_hand.h
+++ b/src/hand/qros_effector_driver_franka_hand.h
@@ -38,6 +38,7 @@
 #include <thread>
 #include <mutex>

+#include <franka/exception.h>
 // #define BLOCK_READ_IN_USED
 // #define IN_TESTING

@@ -48,11 +49,7 @@
 #include <sas_robot_driver_franka/Grasp.h>
 #include <sas_robot_driver_franka/Move.h>
 #include <sas_robot_driver_franka/GripperState.h>
-#ifdef IN_TESTING
-#include <Move.h> // dummy include
-#include <Grasp.h>  // dummy include
-#include <GripperState.h> // dummy include
-#endif
+


 // using namespace DQ_robotics;
--- a/src/hand/robot_interface_hand.cpp
+++ b/src/hand/robot_interface_hand.cpp
@@ -1,4 +1,4 @@
-#include "robot_interface_hand.h"
+#include <sas_robot_driver_franka/robot_interface_hand.h>



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


-#include "robot_interface_franka.h"
+#include <sas_robot_driver_franka/robot_interface_franka.h>

 #include <ros/this_node.h>
 #include <rosconsole/macros_generated.h>
--- a/src/joint/sas_robot_driver_franka.cpp
+++ b/src/joint/sas_robot_driver_franka.cpp
@@ -30,12 +30,8 @@
 # ################################################################
 */

+#include <sas_robot_driver_franka/sas_robot_driver_franka.h>

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

 namespace sas
 {
@@ -146,6 +142,10 @@ namespace sas
            ROS_ERROR_STREAM("["+ros::this_node::getName()+"]::driver interface error on:"+robot_driver_interface_sptr_->get_status_message());
            break_loops_->store(true);
        }
+        if(!ros::ok()) {
+            ROS_ERROR_STREAM("["+ros::this_node::getName()+"]::ROS shutdown requested.");
+            break_loops_->store(true);
+        }
        _update_stiffness_contact_and_pose();
        return robot_driver_interface_sptr_->get_joint_positions();
    }
--- a/src/robot_dynamics/qros_robot_dynamic_py.cpp
+++ b/src/robot_dynamics/qros_robot_dynamic_py.cpp
@@ -32,8 +32,8 @@
 #include <pybind11/stl.h>
 #include <pybind11/eigen.h>

-#include <robot_dynamic/qros_robot_dynamics_provider.h>
-#include <robot_dynamic/qros_robot_dynamics_interface.h>
+#include <sas_robot_driver_franka/robot_dynamic/qros_robot_dynamics_provider.h>
+#include <sas_robot_driver_franka/robot_dynamic/qros_robot_dynamics_interface.h>

 namespace py = pybind11;
 using RDI = qros::RobotDynamicInterface;
--- a/src/robot_dynamics/qros_robot_dynamics_interface.cpp
+++ b/src/robot_dynamics/qros_robot_dynamics_interface.cpp
@@ -27,7 +27,7 @@
 #
 # ################################################################
 */
-#include <robot_dynamic/qros_robot_dynamics_interface.h>
+#include <sas_robot_driver_franka/robot_dynamic/qros_robot_dynamics_interface.h>

 using namespace qros;

--- a/src/robot_dynamics/qros_robot_dynamics_provider.cpp
+++ b/src/robot_dynamics/qros_robot_dynamics_provider.cpp
@@ -27,7 +27,7 @@
 #
 # ################################################################
 */
-#include <robot_dynamic/qros_robot_dynamics_provider.h>
+#include <sas_robot_driver_franka/robot_dynamic/qros_robot_dynamics_provider.h>

 using namespace qros;

--- a/src/sas_robot_driver_coppelia_node.cpp
+++ b/src/sas_robot_driver_coppelia_node.cpp
@@ -26,6 +26,8 @@
 #      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
 #
 # ################################################################
 */
@@ -39,7 +41,7 @@
 #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"
+#include "../../src/coppelia/sas_robot_driver_coppelia.h"

 /*********************************************
 * SIGNAL HANDLER
@@ -52,6 +54,7 @@ void sig_int_handler(int)
 }


+
 int main(int argc, char **argv)
 {
    if(signal(SIGINT, sig_int_handler) == SIG_ERR)
@@ -61,39 +64,37 @@ int main(int argc, char **argv)

    ros::init(argc, argv, "sas_robot_driver_coppelia_node", ros::init_options::NoSigintHandler);
    ROS_WARN("=====================================================================");
-    ROS_WARN("----------------------Juan Jose Quiroz Omana------------------------");
+    ROS_WARN("--------------------- Quentin Lin -----------------------------------");
    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,"/thread_sampling_time_nsec",robot_driver_coppelia_configuration.thread_sampling_time_nsec);
+    sas::get_ros_param(nh,"/vrep_ip",robot_driver_coppelia_configuration.vrep_ip);
+    sas::get_ros_param(nh,"/vrep_port",robot_driver_coppelia_configuration.vrep_port);
+    sas::get_ros_param(nh,"/vrep_joint_names", robot_driver_coppelia_configuration.vrep_joint_names);
+    sas::get_ros_param(nh,"/vrep_dynamically_enabled", robot_driver_coppelia_configuration.vrep_dynamically_enabled);
+    sas::get_ros_param(nh,"/robot_mode", robot_driver_coppelia_configuration.robot_mode);
+    sas::get_ros_param(nh,"/using_real_robot", robot_driver_coppelia_configuration.using_real_robot);
+    sas::get_ros_param(nh,"/robot_topic_prefix", robot_driver_coppelia_configuration.robot_topic_prefix);
+    sas::get_ros_param(nh,"/robot_parameter_file_path", robot_driver_coppelia_configuration.robot_parameter_file_path);

-    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;
+    // std::vector<double> q_min_vec, q_max_vec;
+    // sas::get_ros_param(nh,"/q_min", q_min_vec);
+    // robot_driver_coppelia_configuration.q_min = sas::std_vector_double_to_vectorxd(q_min_vec);
+    // sas::get_ros_param(nh,"/q_max", q_max_vec);
+    // robot_driver_coppelia_configuration.q_max = sas::std_vector_double_to_vectorxd(q_max_vec);

-    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,
+        sas::RobotDriverCoppelia robot_driver_coppelia(nh, 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();
+
+        return robot_driver_coppelia.start_control_loop();
    }
    catch (const std::exception& e)
    {
--- a/src/sas_robot_driver_franka_hand_node.cpp
+++ b/src/sas_robot_driver_franka_hand_node.cpp
@@ -36,7 +36,7 @@
 #include <sas_common/sas_common.h>
 #include <sas_conversions/eigen3_std_conversions.h>
 #include <sas_robot_driver/sas_robot_driver_ros.h>
-#include "hand/qros_effector_driver_franka_hand.h"
+#include "../../src/hand/qros_effector_driver_franka_hand.h"


 /*********************************************
--- a/src/sas_robot_driver_franka_node.cpp
+++ b/src/sas_robot_driver_franka_node.cpp
@@ -39,8 +39,8 @@
 #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 <robot_dynamic/qros_robot_dynamics_provider.h>
+#include <sas_robot_driver_franka/sas_robot_driver_franka.h>
+#include <sas_robot_driver_franka/robot_dynamic/qros_robot_dynamics_provider.h>


 /*********************************************
@@ -130,6 +130,16 @@ int main(int argc, char **argv)
        robot_driver_franka_configuration.robot_reference_frame = robot.get_reference_frame();
    }else{ROS_INFO_STREAM(ros::this_node::getName()+"::Robot parameter file path not set. Robot Model Unknow.");}

+    if(nh.hasParam(ros::this_node::getName()+"/automatic_error_recovery")) {
+        sas::get_ros_param(nh,"/automatic_error_recovery",robot_driver_franka_configuration.automatic_error_recovery);
+        if(robot_driver_franka_configuration.automatic_error_recovery)
+        {
+            ROS_WARN_STREAM(ros::this_node::getName()+"::Automatic error recovery enabled. STATUS EXPERIMENTAL");
+        }
+    }
+
+
+
    robot_driver_franka_configuration.interface_configuration = franka_interface_configuration;

    sas::RobotDriverROSConfiguration robot_driver_ros_configuration;
Author	SHA1	Message	Date
qlin1806	dc2dcf51bc	initial package cleanup	2024-08-21 16:30:44 -07:00
qlin1806	a16c4fa5a6	fix install of some library	2024-08-14 20:09:45 -07:00
QuentinLin	d1bc7d5c9f	working coppelia mirror node (#1 ) Reviewed-on: #1 Co-authored-by: QuentinLin <qlin1806@gmail.com> Co-committed-by: QuentinLin <qlin1806@gmail.com>	2024-08-14 14:27:29 +09:00
QuentinLin	1f842c9ba0	remove some unwanted file	2024-08-03 12:25:45 +09:00
QuentinLin	b340cc3d4e	[franka hand node] cmake and header cleanup	2024-08-03 12:24:03 +09:00
QuentinLin	81a6e077d6	[sas robot driver franka] added handeling of shutdown	2024-08-02 18:26:24 +09:00
QuentinLin	c6c67078dd	need to test automatic error recovery	2024-07-30 12:41:51 +09:00