general fix and optimization

.gitignore

@@ -1,5 +1,3 @@
 .idea
 CMakeLists.txt.user
 cmake-build*
-build/
-**/__pycache__/

CMakeLists.txt

@@ -19,42 +19,15 @@ find_package(catkin REQUIRED COMPONENTS
   roscpp
   rospy
   std_msgs
-        tf2_ros
-        tf2
   sas_common
   sas_clock
   sas_robot_driver
   sas_patient_side_manager
-        message_generation
-        pybind11_catkin
 )
 
-
-add_service_files(
-        DIRECTORY srv
-        FILES
-        Move.srv
-        Grasp.srv
-)
-
-add_message_files(
-        DIRECTORY msg
-        FILES
-        GripperState.msg
-)
-
-catkin_python_setup()
-
-generate_messages(
-        DEPENDENCIES
-        std_msgs
-
-)
-
-
 catkin_package(
   INCLUDE_DIRS include
-        CATKIN_DEPENDS roscpp rospy sas_common sas_clock sas_robot_driver tf2_ros tf2 pybind11_catkin message_runtime std_msgs
+  CATKIN_DEPENDS roscpp rospy sas_common sas_clock sas_robot_driver
 )
 
 find_package(Franka REQUIRED)
@@ -117,8 +90,9 @@ target_link_libraries(robot_interface_hand Franka::Franka
 
 include_directories(
     include
+    include/generator
     src/
-        src/robot_dynamics
+    src/dynamic_interface
     src/hand
     src/joint
     ${catkin_INCLUDE_DIRS}
@@ -126,44 +100,29 @@ include_directories(
     )
 
 
-add_library(qros_robot_dynamics_provider src/robot_dynamics/qros_robot_dynamics_provider.cpp)
+add_library(sas_robot_dynamic_provider src/dynamic_interface/sas_robot_dynamic_provider.cpp)
-target_link_libraries(qros_robot_dynamics_provider
+target_link_libraries(sas_robot_dynamic_provider dqrobotics)
-        ${catkin_LIBRARIES}
-        dqrobotics)
-
-add_library(qros_robot_dynamics_interface src/robot_dynamics/qros_robot_dynamics_interface.cpp)
-target_link_libraries(qros_robot_dynamics_interface
-        ${catkin_LIBRARIES}
-        dqrobotics)
-
-add_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
-        qros_robot_dynamics_provider
+                    sas_robot_dynamic_provider
                     dqrobotics
-        dqrobotics-interface-json11
                     robot_interface_franka)
 
-add_library(qros_effector_driver_franka_hand src/hand/qros_effector_driver_franka_hand.cpp)
+add_library(sas_robot_driver_franka_hand src/hand/sas_robot_driver_franka_hand.cpp)
-target_link_libraries(qros_effector_driver_franka_hand
+target_link_libraries(sas_robot_driver_franka_hand
         dqrobotics
         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})
 add_dependencies(sas_robot_driver_coppelia ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
-add_dependencies(qros_effector_driver_franka_hand ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
-
 
 add_executable(sas_robot_driver_coppelia_node src/sas_robot_driver_coppelia_node.cpp)
 target_link_libraries(sas_robot_driver_coppelia_node
@@ -178,10 +137,11 @@ target_link_libraries(sas_robot_driver_franka_node
 
 add_executable(sas_robot_driver_franka_hand_node src/sas_robot_driver_franka_hand_node.cpp)
 target_link_libraries(sas_robot_driver_franka_hand_node
-        qros_effector_driver_franka_hand
+        sas_robot_driver_franka_hand
         ${catkin_LIBRARIES})
 
 
+
 add_executable(JuankaEmika
   qt/configuration_window/main.cpp
   qt/configuration_window/mainwindow.cpp
@@ -195,19 +155,6 @@ target_link_libraries(JuankaEmika PRIVATE Qt${QT_VERSION_MAJOR}::Widgets
     )
 
 
-#####################################################################################
-# python binding
-include_directories(
-        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)
 
 
 if(QT_VERSION_MAJOR EQUAL 6)
@@ -230,23 +177,9 @@ 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}
   FILES_MATCHING PATTERN "*.h"
 #   PATTERN ".svn" EXCLUDE
 )                        
-
-
-install(TARGETS _qros_robot_dynamic
-        LIBRARY DESTINATION ${PYTHON_INSTALL_DIR}
-)
-

cfg/sas_robot_driver_franka_hand_1.yaml

@@ -1,3 +1,7 @@
 "robot_ip_address": "172.16.0.2"
-"thread_sampling_time_nsec": 20000000  # 20ms , 50Hz
+"robot_mode": "PositionControl"
+"robot_speed": 20.0
+"thread_sampling_time_nsec": 40000000
+"q_min": [0.00]
+"q_max": [0.04]
 

include/sas_robot_driver_franka.h

@@ -43,15 +43,13 @@
 #include <sas_robot_driver/sas_robot_driver.h>
 #include "robot_interface_franka.h"
 #include <ros/common.h>
-#include <sas_robot_driver_franka/robot_dynamic/qros_robot_dynamics_provider.h>
+#include "sas_robot_dynamic_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
 {
 
@@ -62,8 +60,6 @@ struct RobotDriverFrankaConfiguration
     int port;
     double speed;
     RobotInterfaceFranka::FrankaInterfaceConfiguration interface_configuration;
-    DQ robot_reference_frame = DQ(0);
-    bool automatic_error_recovery = false;
 };
 
 
@@ -74,7 +70,7 @@ private:
 
     std::shared_ptr<RobotInterfaceFranka> robot_driver_interface_sptr_ = nullptr;
 
-    qros::RobotDynamicProvider* robot_dynamic_provider_;
+    RobotDynamicProvider* robot_dynamic_provider_;
 
     //Joint positions
     VectorXd joint_positions_;
@@ -90,8 +86,6 @@ private:
     void _update_stiffness_contact_and_pose() const;
 
 
-
-
 public:
     //const static int SLAVE_MODE_JOINT_CONTROL;
     //const static int SLAVE_MODE_END_EFFECTOR_CONTROL;
@@ -101,7 +95,7 @@ public:
     ~RobotDriverFranka();
 
     RobotDriverFranka(
-        qros::RobotDynamicProvider* robot_dynamic_provider,
+        RobotDynamicProvider* robot_dynamic_provider,
         const RobotDriverFrankaConfiguration& configuration,
         std::atomic_bool* break_loops
         );

include/sas_robot_driver_franka/robot_dynamic/qros_robot_dynamics_interface.h

@@ -1,93 +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: Quenitin Lin
-#
-# ################################################################
-#
-# 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/Transform.h>
-#include <geometry_msgs/TransformStamped.h>
-#include <tf2_ros/transform_listener.h>
-#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
-#include <geometry_msgs/Pose.h>
-#include <std_msgs/Header.h>
-#include <dqrobotics/DQ.h>
-
-#define BUFFER_DURATION_DEFAULT_S 2.0   // 2 second
-
-namespace qros {
-
-using namespace DQ_robotics;
-
-class RobotDynamicInterface {
-private:
-    unsigned int seq_ = 0;
-
-    std::string node_prefix_;
-    std::string child_frame_id_;
-    std::string parent_frame_id_;
-
-    ros::Subscriber subscriber_cartesian_stiffness_;
-    tf2_ros::Buffer tf_buffer_;
-    tf2_ros::TransformListener tf_listener_;
-
-
-    static geometry_msgs::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::WrenchStampedConstPtr &msg);
-
-    static DQ _geometry_msgs_transform_to_dq(const geometry_msgs::Transform& transform);
-
-public:
-    RobotDynamicInterface() = delete;
-    RobotDynamicInterface(const RobotDynamicInterface&) = delete;
-#ifdef BUILD_PYBIND
-    explicit RobotDynamicInterface(const std::string& node_prefix):RobotDynamicInterface(sas::common::get_static_node_handle(),node_prefix){}
-#endif
-    explicit RobotDynamicInterface(ros::NodeHandle& nodehandle, const std::string& node_prefix=ros::this_node::getName());
-    RobotDynamicInterface(ros::NodeHandle& publisher_nodehandle, ros::NodeHandle& subscriber_nodehandle, const std::string& node_prefix=ros::this_node::getName());
-
-    VectorXd get_stiffness_force();
-    VectorXd get_stiffness_torque();
-    DQ get_stiffness_frame_pose();
-
-    bool is_enabled() const;
-    std::string get_topic_prefix() const {return node_prefix_;}
-
-};
-
-
-
-} // namespace sas

launch/sas_robot_driver_franka_hand_1.launch

@@ -6,5 +6,12 @@
 <rosparam file="$(find sas_robot_driver_franka)/cfg/sas_robot_driver_franka_hand_1.yaml" command="load"
 />
 
+
+
 </node>
+
+
+
+
+
 </launch>

msg/GripperState.msg

@@ -1,5 +0,0 @@
-float32 width
-float32 max_width
-bool is_grasped
-uint16 temperature
-uint64 duration_ms

package.xml

@@ -52,38 +52,28 @@
 
   <build_depend>roscpp</build_depend>
   <build_depend>rospy</build_depend>
-  <build_depend>tf2_ros</build_depend>
-  <build_depend>tf2</build_depend>
   <build_depend>std_msgs</build_depend>
   <build_depend>sas_clock</build_depend>
   <build_depend>sas_robot_driver</build_depend>
   <build_depend>sas_common</build_depend>
   <build_depend>sas_patient_side_manager</build_depend>
-  <build_depend>pybind11_catkin</build_depend>
-  <build_depend>message_generation</build_depend>
 
   <build_export_depend>roscpp</build_export_depend>
   <build_export_depend>rospy</build_export_depend>
-  <build_export_depend>tf2_ros</build_export_depend>
-  <build_export_depend>tf2</build_export_depend>
   <build_export_depend>std_msgs</build_export_depend>
   <build_export_depend>sas_clock</build_export_depend>
   <build_export_depend>sas_robot_driver</build_export_depend>
   <build_export_depend>sas_common</build_export_depend>
   <build_export_depend>sas_patient_side_manager</build_export_depend>
-  <build_export_depend>pybind11_catkin</build_export_depend>
 
 
   <exec_depend>roscpp</exec_depend>
   <exec_depend>rospy</exec_depend>
-  <exec_depend>tf2_ros</exec_depend>
-  <exec_depend>tf2</exec_depend>
   <exec_depend>std_msgs</exec_depend>
   <exec_depend>sas_clock</exec_depend>
   <exec_depend>sas_robot_driver</exec_depend>
   <exec_depend>sas_common</exec_depend>
   <exec_depend>sas_patient_side_manager</exec_depend>
-  <exec_depend>message_runtime</exec_depend>
 
 
   <!-- The export tag contains other, unspecified, tags -->

qt/configuration_window/mainwindow.h

@@ -7,7 +7,7 @@
 
 #include <QMainWindow>
 #include "qspinbox.h"
-#include <sas_robot_driver_franka/robot_interface_franka.h>
+#include "robot_interface_franka.h"
 
 QT_BEGIN_NAMESPACE
 namespace Ui { class MainWindow; }

scripts/example_gripper_control.py

@@ -1,56 +0,0 @@
-import rospy
-
-from sas_robot_driver_franka import FrankaGripperInterface
-
-
-def main_loop():
-    rate = rospy.Rate(10)  # 10 Hz
-    iteration_ = 0
-
-    hand1 = FrankaGripperInterface("/franka_hand_1")
-
-    while not hand1.is_enabled():
-        rospy.loginfo("Waiting for gripper to be enabled...")
-        rate.sleep()
-    rospy.loginfo("Gripper enabled!")
-
-    rate = rospy.Rate(2)  # 0.5 Hz
-
-    while not rospy.is_shutdown():
-        rospy.loginfo("Main loop running...")
-
-        # Get the temperature of the gripper
-        temperature = hand1.get_temperature()
-        rospy.loginfo(f"Temperature: {temperature}")
-        max_width = hand1.get_max_width()
-        rospy.loginfo(f"Max width: {max_width}")
-        width = hand1.get_width()
-        rospy.loginfo(f"Width: {width}")
-        is_grasped = hand1.is_grasped()
-        rospy.loginfo(f"Is grasped: {is_grasped}")
-        is_moving = hand1.is_moving()
-        rospy.loginfo(f"Is moving: {is_moving}")
-        is_grasping = hand1.is_grasping()
-        rospy.loginfo(f"Is grasping: {is_grasping}")
-        rospy.logwarn("calling move(0.01)")
-        if not hand1.is_busy():
-            hand1.grasp(0.01)
-        else:
-            rospy.logwarn("Gripper is busy. Waiting...")
-        result_ready = hand1.is_done()
-        if not result_ready:
-            rospy.loginfo("Waiting for gripper to finish moving...")
-        else:
-            result = hand1.get_result()
-            rospy.loginfo(f"Result: {result}")
-
-
-        # Check if there is a response in the queue
-
-        iteration_ += 1
-        rate.sleep()
-
-
-if __name__ == "__main__":
-    rospy.init_node("example_gripper_control_node")
-    main_loop()

scripts/publish_dumy_robot_dynmaics.py

@@ -1,29 +0,0 @@
-import rospy
-from sas_robot_driver_franka import RobotDynamicsProvider
-import dqrobotics as dql
-import numpy as np
-
-
-if __name__ == "__main__":
-    rospy.init_node("dummy_robot_dynamics_provider")
-
-    dynam_provider = RobotDynamicsProvider("/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
-    rospy.loginfo("Publishing dummy robot dynamics")
-    r = dql.DQ([0, 0, 0, 1])
-    rate = rospy.Rate(100)
-    dummy_force = np.random.rand(3) * 100
-    dummy_torque = np.random.rand(3) * 10
-
-    while not rospy.is_shutdown():
-        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)
-        rate.sleep()
-        t_ += 0.01

scripts/subscribe_dummy_dynamic.py

@@ -1,30 +0,0 @@
-import rospy
-from sas_robot_driver_franka import RobotDynamicsInterface
-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)
-
-if __name__ == "__main__":
-    rospy.init_node("dummy_robot_dynamics_subscriber")
-
-    dynam_provider = RobotDynamicsInterface("/franka1")
-    while not dynam_provider.is_enabled():
-        rospy.loginfo("Waiting for robot dynamics provider to be enabled")
-        rospy.sleep(1)
-
-    rospy.loginfo("Subscribing to dummy robot dynamics")
-    rate = rospy.Rate(50)
-
-    while not rospy.is_shutdown():
-        force = dynam_provider.get_stiffness_force()
-        torque = dynam_provider.get_stiffness_torque()
-        ee_pose = dynam_provider.get_stiffness_frame_pose()
-        vrep.set_object_pose("xd1", ee_pose)
-        rospy.loginfo("EE Pose: %s", ee_pose)
-        rospy.loginfo("Force: %s", force)
-        rospy.loginfo("Torque: %s", torque)
-        rate.sleep()

setup.py

@@ -1,12 +0,0 @@
-# ! DO NOT MANUALLY INVOKE THIS setup.py, USE CATKIN INSTEAD
-
-from distutils.core import setup
-from catkin_pkg.python_setup import generate_distutils_setup
-
-# fetch values from package.xml
-setup_args = generate_distutils_setup(
-    packages=['sas_robot_driver_franka'],
-    package_dir={'': 'src'},
-)
-
-setup(**setup_args)

src/coppelia/sas_robot_driver_coppelia.cpp

@@ -4,241 +4,250 @@
 namespace sas
 {
 
-RobotDriverCoppelia::~RobotDriverCoppelia() {
-    deinitialize();
-    disconnect();
-}
 
-RobotDriverCoppelia::RobotDriverCoppelia(ros::NodeHandle nh, const RobotDriverCoppeliaConfiguration &configuration, std::atomic_bool *break_loops):
+RobotDriverCoppelia::RobotDriverCoppelia(const RobotDriverCoppeliaConfiguration &configuration, std::atomic_bool *break_loops):
+RobotDriver(break_loops),
     configuration_(configuration),
-        clock_(configuration.thread_sampling_time_nsec),
+    robot_mode_(configuration.robot_mode),
-        break_loops_(break_loops),
+    jointnames_(configuration.jointnames),
-        robot_mode_(ControlMode::Position),
+    mirror_mode_(configuration.mirror_mode),
-        vi_(std::make_shared<DQ_VrepInterface>())
+    dim_configuration_space_(configuration.jointnames.size()),
+    real_robot_topic_prefix_(configuration.real_robot_topic_prefix)
 {
-    // should initialize robot driver interface to real robot
+    vi_ = std::make_shared<DQ_VrepInterface>();
-    DQ_SerialManipulatorDH smdh = DQ_JsonReader::get_from_json<DQ_SerialManipulatorDH>(configuration_.robot_parameter_file_path);
+    desired_joint_velocities_ = VectorXd::Zero(dim_configuration_space_);
-    joint_limits_ = {smdh.get_lower_q_limit(),smdh.get_upper_q_limit()};
+    auto nodehandle = ros::NodeHandle();
-    if(configuration_.using_real_robot)
+    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()
 {
-        ROS_INFO_STREAM("[" + ros::this_node::getName() + "]::Using real robot, Instantiating robot interface to real driver.");
+    return current_joint_positions_;
-        real_robot_interface_ = std::make_shared<RobotDriverInterface>(nh, configuration_.robot_topic_prefix);
-    }else{
-        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::set_target_joint_positions(const VectorXd &desired_joint_positions_rad)
-
-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)) {
+    desired_joint_positions_ = desired_joint_positions_rad;
-                        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.");
+VectorXd RobotDriverCoppelia::get_joint_velocities()
+{
+    return current_joint_velocities_;
 }
-                    target_joint_positions = simulated_joint_positions_;
+
+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{
-                    target_joint_positions = robot_provider_->get_target_joint_positions();
+        _start_joint_velocity_control_mirror_thread();
     }
-            }else {
+    std::cout<<"RobotDriverCoppelia::Velocity loop running..."<<std::endl;
-                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_);
 
-        }
+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;
 }
 
-int RobotDriverCoppelia::start_control_loop(){
+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{
-        ROS_INFO_STREAM(ros::this_node::getName() << "::Waiting to connect with coppelia...");
+        finish_motion_ = false;
-        connect();
+        VectorXd q = vi_->get_joint_positions(jointnames_);
-        ROS_INFO_STREAM(ros::this_node::getName() << "::Connected to coppelia.");
+        VectorXd q_dot =  vi_->get_joint_velocities(jointnames_);
-        ROS_INFO_STREAM(ros::this_node::getName() << "::Initializing...");
+        VectorXd forces = vi_->get_joint_torques(jointnames_);
-        initialize();
+        _update_robot_state(q, q_dot, forces);
-        ROS_INFO_STREAM(ros::this_node::getName() << "::initialized.");
 
-        _start_control_loop();
+        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)
     {
-        ROS_WARN_STREAM(ros::this_node::getName() + "::Error or exception caught::" << e.what());
+        std::cout<<"RobotDriverCoppelia::sas_robot_coppelia_driver::_start_joint_velocity_control_mode(). Error or exception caught " << e.what()<<std::endl;
     }
     catch(...)
     {
-        ROS_WARN_STREAM(ros::this_node::getName() + "::Unexpected error or exception caught");
+        std::cout<<"RobotDriverCoppelia::sas_robot_coppelia_driver::_start_joint_velocity_control_mode(). Error or exception caught " <<std::endl;
-    }
-    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::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");
-    }
-    ROS_INFO_STREAM("[" + ros::this_node::getName() + "]::[RobotDriverCoppelia]::connect::Connected to Vrep");
-}
-void RobotDriverCoppelia::disconnect(){
-    vi_->disconnect_all();
 }
 
-void RobotDriverCoppelia::initialize(){
+void RobotDriverCoppelia::_start_joint_velocity_control_thread()
-    if(configuration_.using_real_robot)
 {
-        ROS_INFO_STREAM("[" + ros::this_node::getName() +"]::[RobotDriverCoppelia]::initialize::Waiting for real robot interface to initialize...");
+    finish_motion_ = false;
-        ros::spinOnce();
+    if (joint_velocity_control_mode_thread_.joinable())
-        int count = 0;
-        while (!real_robot_interface_->is_enabled()) {
-            ros::spinOnce();
-            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;
+        joint_velocity_control_mode_thread_.join();
-            simulated_joint_velocities_ = current_joint_velocity;
-            start_simulation_thread();
     }
+    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::deinitialize(){
-    // nothing to do
 }
 
-void RobotDriverCoppelia::start_simulation_thread(){ // thread entry point
+void RobotDriverCoppelia::_start_joint_velocity_control_mirror_thread()
-    if(simulation_thread_started_){
+{
-        throw std::runtime_error("[RobotDriverCoppelia]::start_simulation_thread::Simulation thread already started");
+    finish_motion_ = false;
+    if (joint_velocity_control_mode_thread_.joinable())
+    {
+        joint_velocity_control_mode_thread_.join();
     }
-    if(velocity_control_simulation_thread_.joinable()){
+    if (joint_velocity_control_mirror_mode_thread_.joinable())
-        velocity_control_simulation_thread_.join();
+    {
+        joint_velocity_control_mirror_mode_thread_.join();
+    }
+    joint_velocity_control_mirror_mode_thread_ = std::thread(&RobotDriverCoppelia::_start_joint_velocity_control_mirror_mode, this);
 }
 
-    velocity_control_simulation_thread_ = std::thread(&RobotDriverCoppelia::_velocity_control_simulation_thread_main, this);
+void RobotDriverCoppelia::_start_joint_velocity_control_mirror_mode()
-}
+{
 
-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() +
+        finish_motion_ = false;
-                        "]::[RobotDriverCoppelia]::[_velocity_control_simulation_thread_main]::Simulation thread started.");
+        std::cout<<"RobotDriverCoppelia::Waiting for real robot topics..."<<std::endl;
-        sas::Clock clock = sas::Clock(VIRTUAL_ROBOT_SIMULATION_SAMPLING_TIME_NSEC);
+        VectorXd q;
-        double tau = static_cast<double>(VIRTUAL_ROBOT_SIMULATION_SAMPLING_TIME_NSEC) * 1e-9;
+        while (ros::ok()) {
-        auto current_joint_positions = simulated_joint_positions_;
+            if (franka1_ros_->is_enabled())
-        clock.init();
+            {
-        while (!(*break_loops_) && ros::ok()) {
+                    q = franka1_ros_->get_joint_positions();
+                    break;
+            }
+            ros::spinOnce();
+        }
+        std::cout<<"RobotDriverCoppelia::Done!"<<std::endl;
 
-            current_joint_positions += tau * simulated_joint_velocities_; // no dynamic model
+        VectorXd q_vrep = vi_->get_joint_positions(jointnames_);
-            // cap joint limit
+        VectorXd q_dot_vrep =  vi_->get_joint_velocities(jointnames_);
-            auto q_min = std::get<0>(joint_limits_);
+        VectorXd forces_vrep = vi_->get_joint_torques(jointnames_);
-            auto q_max = std::get<1>(joint_limits_);
+        _update_robot_state(q_vrep, q_dot_vrep, forces_vrep);
-            for (int i = 0; i < current_joint_positions.size(); i++) {
+
-                if (current_joint_positions(i) < q_min(i)) {
+        desired_joint_positions_ = q_vrep;
-                    current_joint_positions(i) = q_min(i);
+
+
+        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));
                 }
-                if (current_joint_positions(i) > q_max(i)) {
+                else if (robot_mode_  == std::string("PositionControl"))
-                    current_joint_positions(i) = q_max(i);
+                {
+                        vi_->set_joint_target_positions(jointnames_, q);
+                }
+                if (finish_motion_) {
+                        finish_motion_ = false;
+                        return;
                 }
             }
-            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());
+    catch(const std::exception& e)
-        simulation_thread_started_ = false;
+    {
-    }catch(...){
+        std::cout<<"RobotDriverCoppelia::sas_robot_coppelia_driver::_start_joint_velocity_control_mirror_mode(). Error or exception caught " << e.what()<<std::endl;
-        ROS_ERROR_STREAM("[" + ros::this_node::getName() + "]::[RobotDriverCoppelia]::[_velocity_control_simulation_thread_main]::Exception::Unknown");
+    }
-        simulation_thread_started_ = false;
+    catch(...)
+    {
+        std::cout<<"RobotDriverCoppelia::sas_robot_coppelia_driver::_start_joint_velocity_control_mirror_mode(). Error or exception caught " <<std::endl;
     }
 
+
 }
 
 
 
+
 } // sas namespace

src/coppelia/sas_robot_driver_coppelia.h

@@ -39,18 +39,10 @@
 #include <memory>
 #include <dqrobotics/DQ.h>
 #include <sas_robot_driver/sas_robot_driver.h>
-#include <sas_clock/sas_clock.h>
+#include <dqrobotics/interfaces/vrep/DQ_VrepRobot.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;
 
@@ -62,78 +54,81 @@ namespace sas
 struct RobotDriverCoppeliaConfiguration
 {
 
-    int thread_sampling_time_nsec;  // frontend vrep update rate
+    int thread_sampling_time_nsec;
-    int vrep_port;
+    int port;
-    std::string vrep_ip;
+    std::string ip;
-    std::vector<std::string> vrep_joint_names;
+    std::vector<std::string> jointnames;
-    bool vrep_dynamically_enabled = false;
     std::string robot_mode;
-    bool using_real_robot;
+    bool mirror_mode;
-    std::string robot_topic_prefix;
+    std::string real_robot_topic_prefix;
-    std::string robot_parameter_file_path;
-    // VectorXd q_min;
-    // VectorXd q_max;
 };
 
-class RobotDriverCoppelia
+class RobotDriverCoppelia: public RobotDriver
 {
 private:
-    enum ControlMode{
-        Position=0,
-        Velocity
-    };
-
     RobotDriverCoppeliaConfiguration configuration_;
 
-    Clock clock_;
+    std::string robot_mode_ = std::string("VelocityControl");   // PositionControl
-    std::atomic_bool* break_loops_;
+    bool mirror_mode_ = false;
-    bool _should_shutdown() const {return (*break_loops_);}
+    double gain_ = 3.0;
-    ControlMode robot_mode_;
+    std::string real_robot_topic_prefix_;
 
-    std::shared_ptr<DQ_VrepInterface> vi_;
+    VectorXd current_joint_positions_;
-    std::shared_ptr<sas::RobotDriverInterface> real_robot_interface_ = nullptr;
+    VectorXd current_joint_velocities_;
-    std::shared_ptr<sas::RobotDriverProvider> robot_provider_ = nullptr;
+    VectorXd current_joint_forces_;
 
-    // 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;
+    VectorXd desired_joint_velocities_;
-    inline void _assert_initialized() const{
+    VectorXd desired_joint_positions_;
-        if (!initialized_){throw std::runtime_error("[RobotDriverCoppelia] Robot driver not initialized");}
+
-    }
+    std::atomic<bool> finish_motion_;
-    inline void _assert_is_alive() const{
+
-        if(!configuration_.using_real_robot && !simulation_thread_started_){throw std::runtime_error("[RobotDriverCoppelia] Robot Simulation is not alive");}
+    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_;
+
 
-    void _start_control_loop();
 protected:
-    inline void _update_vrep_position(const VectorXd &joint_positions, const bool& force_update=false) const;
+    std::shared_ptr<DQ_VrepInterface> vi_;
-    inline void _update_vrep_velocity(const VectorXd & joint_velocity) const;
+    std::vector<std::string> jointnames_;
+
+
 
 public:
     RobotDriverCoppelia(const RobotDriverCoppelia&)=delete;
     RobotDriverCoppelia()=delete;
     ~RobotDriverCoppelia();
 
-    RobotDriverCoppelia(ros::NodeHandle nh, const RobotDriverCoppeliaConfiguration& configuration, std::atomic_bool* break_loops);
+    RobotDriverCoppelia(const RobotDriverCoppeliaConfiguration& configuration, std::atomic_bool* break_loops);
 
-    int start_control_loop();  // public entry point
 
-    void connect();
+    VectorXd get_joint_positions() override;
-    void disconnect();
+    void set_target_joint_positions(const VectorXd& desired_joint_positions_rad) override;
 
-    void initialize();
+    VectorXd get_joint_velocities() override;
-    void deinitialize();
+    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;
 
-    std::tuple<VectorXd, VectorXd> joint_limits_;
 
 };
 }

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 "../../include/sas_robot_driver_franka/sas_robot_driver_franka.h"
+#include "sas_robot_driver_franka.h"
 #include "robot_coppelia_ros_interface.h"
 
 /*********************************************

src/dynamic_interface/sas_robot_dynamic_provider.cpp

@@ -0,0 +1,63 @@
+/*
+# 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);
+}
+

src/dynamic_interface/sas_robot_dynamic_provider.h

@@ -19,7 +19,7 @@
 #
 # ################################################################
 #
-#   Author: Quenitin Lin
+#   Author: Juan Jose Quiroz Omana, email: juanjqogm@gmail.com
 #
 # ################################################################
 #
@@ -33,55 +33,25 @@
 #include <sas_common/sas_common.h>
 #include <sas_conversions/sas_conversions.h>
 #include <geometry_msgs/WrenchStamped.h>
-#include <geometry_msgs/Transform.h>
+#include <geometry_msgs/PoseStamped.h>
-#include <geometry_msgs/TransformStamped.h>
-#include <tf2_ros/transform_broadcaster.h>
-#include <tf2_ros//static_transform_broadcaster.h>
-#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
-#include <geometry_msgs/Pose.h>
-#include <std_msgs/Header.h>
 #include <dqrobotics/DQ.h>
 
-#define REDUCE_TF_PUBLISH_RATE 10
+namespace sas {
-#define WORLD_FRAME_ID "world"
-
-namespace qros {
 
 using namespace DQ_robotics;
 
 class RobotDynamicProvider {
 private:
-    unsigned int seq_ = 0;
-
     std::string node_prefix_;
-    std::string child_frame_id_;
-    std::string parent_frame_id_;
 
     ros::Publisher publisher_cartesian_stiffness_;
-    tf2_ros::TransformBroadcaster tf_broadcaster_;
+    ros::Publisher publisher_stiffness_pose_;
-    tf2_ros::StaticTransformBroadcaster static_base_tf_broadcaster_;
-
-    DQ world_to_base_tf_;
-
-    static geometry_msgs::Transform _dq_to_geometry_msgs_transform(const DQ& pose);
-
-    void _publish_base_static_tf();
 
 public:
-    RobotDynamicProvider() = delete;
+    RobotDynamicProvider(ros::NodeHandle& nodehandle, const std::string& node_prefix=ros::this_node::getName());
-    RobotDynamicProvider(const RobotDynamicProvider&) = delete;
-#ifdef BUILD_PYBIND
-    explicit RobotDynamicProvider(const std::string& node_prefix):RobotDynamicProvider(sas::common::get_static_node_handle(),node_prefix){}
-#endif
-    explicit 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);
+    void publish_stiffness(const DQ& base_to_stiffness, const Vector3d& force, const Vector3d& torque) const;
-
-    void set_world_to_base_tf(const DQ& world_to_base_tf);
-
-    bool is_enabled() const;
-    std::string get_topic_prefix() const {return node_prefix_;}
 
 };
 

src/generator/custom_motion_generation.cpp

@@ -28,7 +28,7 @@
 #
 # ################################################################
 */
-#include <sas_robot_driver_franka/generator/custom_motion_generation.h>
+#include "generator/custom_motion_generation.h"
 
 /**
  * @brief CustomMotionGeneration::CustomMotionGeneration

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 <sas_robot_driver_franka/generator/motion_generator.h>
+#include "generator/motion_generator.h"
 #include <algorithm>
 #include <array>
 #include <cmath>

src/generator/quadratic_program_motion_generator.cpp

@@ -1,4 +1,4 @@
-#include <sas_robot_driver_franka/generator/quadratic_program_motion_generator.h>
+#include "generator/quadratic_program_motion_generator.h"
 
 
 QuadraticProgramMotionGenerator::QuadraticProgramMotionGenerator(const double &speed_factor,

src/hand/qros_effector_driver_franka_hand.cpp

@@ -1,326 +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 "qros_effector_driver_franka_hand.h"
-
-
-
-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,
-        ros::NodeHandle& node_handel,
-        std::atomic_bool* break_loops):
-        driver_node_prefix_(driver_node_prefix),
-        configuration_(configuration),
-        node_handel_(node_handel),
-        break_loops_(break_loops)
-    {
-        gripper_sptr_ = nullptr;
-        grasp_server_ = node_handel_.advertiseService(driver_node_prefix_ + "/grasp",
-                                                      &EffectorDriverFrankaHand::_grasp_srv_callback, this);
-        move_server_ = node_handel_.advertiseService(driver_node_prefix_ + "/move",
-                                                     &EffectorDriverFrankaHand::_move_srv_callback, this);
-        gripper_status_publisher_ = node_handel_.advertise<sas_robot_driver_franka::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_ns);
-        clock.init();
-        ROS_INFO_STREAM(
-            "["+ ros::this_node::getName()+"]::[EffectorDriverFrankaHand]::start_control_loop::Starting control loop.");
-        while (!(*break_loops_))
-        {
-            if (!_is_connected()) throw std::runtime_error(
-                "[" + ros::this_node::getName() +
-                "]::[EffectorDriverFrankaHand]::start_control_loop::Robot is not connected.");
-
-            if (!status_loop_running_)
-            {
-                ROS_WARN_STREAM(
-                    "["+ ros::this_node::getName()+
-                    "]::[EffectorDriverFrankaHand]::_is_connected::Status loop is not running.");
-                break_loops_->store(true);
-                break;
-            }
-
-            clock.update_and_sleep();
-            ros::spinOnce();
-        }
-        ROS_INFO_STREAM(
-            "["+ ros::this_node::getName()+"]::[EffectorDriverFrankaHand]::start_control_loop::Exiting control loop.");
-    }
-
-
-    void EffectorDriverFrankaHand::connect()
-    {
-#ifdef IN_TESTING
-        ROS_WARN_STREAM(
-            "["+ ros::this_node::getName()+"]::[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(
-            "[" + ros::this_node::getName() +
-            "]::[EffectorDriverFrankaHand]::connect::Could not connect to the robot.");
-    }
-
-    void EffectorDriverFrankaHand::disconnect() noexcept
-    {
-#ifdef IN_TESTING
-        ROS_WARN_STREAM(
-            "["+ ros::this_node::getName()+"]::[EffectorDriverFrankaHand]::disconnect::In testing mode. from DUMMY");
-        return;
-#endif
-        ROS_WARN_STREAM("["+ ros::this_node::getName()+"]::[EffectorDriverFrankaHand]::disconnecting...");
-        gripper_sptr_->~Gripper();
-        gripper_sptr_ = nullptr;
-    }
-
-    /**
-     * @brief robot_driver_franka::gripper_homing
-     */
-    void EffectorDriverFrankaHand::gripper_homing()
-    {
-#ifdef IN_TESTING
-        ROS_WARN_STREAM(
-            "["+ ros::this_node::getName()+"]::[EffectorDriverFrankaHand]::gripper_homing::In testing mode.");
-        return;
-#endif
-        if (!_is_connected()) throw std::runtime_error(
-            "[" + ros::this_node::getName() + "]::[EffectorDriverFrankaHand]::gripper_homing::Robot is not connected.");
-        auto ret = gripper_sptr_->homing();
-        if (!ret)
-        {
-            throw std::runtime_error(
-                "[" + ros::this_node::getName() +
-                "]::[EffectorDriverFrankaHand]::gripper_homing::Failed to home the gripper.");
-        }
-        ROS_INFO_STREAM("["+ ros::this_node::getName()+"]::[EffectorDriverFrankaHand]::gripper_homing::Gripper homed.");
-    }
-
-    void EffectorDriverFrankaHand::initialize()
-    {
-        if (!_is_connected()) throw std::runtime_error(
-            "[" + ros::this_node::getName() + "]::[EffectorDriverFrankaHand]::initialize::Robot is not connected.");
-        gripper_homing();
-        // start gripper status loop
-        status_loop_thread_ = std::thread(&EffectorDriverFrankaHand::_gripper_status_loop, this);
-    }
-
-    void EffectorDriverFrankaHand::deinitialize()
-    {
-#ifdef IN_TESTING
-        ROS_WARN_STREAM("["+ ros::this_node::getName()+"]::[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(sas_robot_driver_franka::Grasp::Request& req,
-                                                       sas_robot_driver_franka::Grasp::Response& res)
-    {
-        ROS_DEBUG_STREAM("["+ ros::this_node::getName()+"]::[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;
-        ROS_DEBUG_STREAM("["+ ros::this_node::getName()+"]::[EffectorDriverFrankaHand]::_grasp_srv_callback::Width: " + std::to_string(req.width));
-        ROS_DEBUG_STREAM("["+ ros::this_node::getName()+"]::[EffectorDriverFrankaHand]::_grasp_srv_callback::force: " + std::to_string(force) + " speed: " + std::to_string(speed));
-        bool ret = false;
-        bool function_ret = true;
-        gripper_in_use_.lock();
-#ifdef IN_TESTING
-        ret = true;
-        std::this_thread::sleep_for(std::chrono::milliseconds(2000));
-#else
-        try
-        {
-            ret = gripper_sptr_->grasp(req.width, speed, force, epsilon_inner, epsilon_outer);
-        }catch(franka::CommandException& e)
-        {
-            ROS_ERROR_STREAM("["+ ros::this_node::getName()+"]::[EffectorDriverFrankaHand]::_grasp_srv_callback::CommandException::" + e.what());
-            function_ret = false;
-        }catch(franka::NetworkException& e)
-        {
-            ROS_ERROR_STREAM("["+ ros::this_node::getName()+"]::[EffectorDriverFrankaHand]::_grasp_srv_callback::NetworkException::" + e.what());
-            function_ret = false;
-        }
-#endif
-        gripper_in_use_.unlock();
-        res.success = ret;
-        return function_ret;
-    }
-
-
-    bool EffectorDriverFrankaHand::_move_srv_callback(sas_robot_driver_franka::Move::Request& req,
-                                                      sas_robot_driver_franka::Move::Response& res)
-    {
-        ROS_DEBUG_STREAM("["+ ros::this_node::getName()+"]::[EffectorDriverFrankaHand]::_move_srv_callback::Moving...");
-        auto speed = req.speed;
-        if (speed <= 0.0) speed = configuration_.default_speed;
-        ROS_DEBUG_STREAM("["+ ros::this_node::getName()+"]::[EffectorDriverFrankaHand]::_move_srv_callback::Speed: " + std::to_string(speed) + " Width: " + std::to_string(req.width));
-        bool ret = false;
-        bool function_ret = true;
-        gripper_in_use_.lock();
-#ifdef IN_TESTING
-        ret = true;
-        std::this_thread::sleep_for(std::chrono::milliseconds(2000));
-#else
-        try
-        {
-            ret = gripper_sptr_->move(req.width, speed);
-        }catch(franka::CommandException& e)
-        {
-            ROS_ERROR_STREAM("["+ ros::this_node::getName()+"]::[EffectorDriverFrankaHand]::_move_srv_callback::CommandException::" + e.what());
-            function_ret = false;
-        }catch(franka::NetworkException& e)
-        {
-            ROS_ERROR_STREAM("["+ ros::this_node::getName()+"]::[EffectorDriverFrankaHand]::_move_srv_callback::NetworkException::" + e.what());
-            function_ret = false;
-        }
-#endif
-        gripper_in_use_.unlock();
-        res.success = ret;
-        return function_ret;
-    }
-
-
-    void EffectorDriverFrankaHand::_gripper_status_loop()
-    {
-        status_loop_running_ = true;
-        sas::Clock clock = sas::Clock(configuration_.thread_sampeling_time_ns);
-        ROS_INFO_STREAM(
-            "["+ ros::this_node::getName()+"]::[EffectorDriverFrankaHand]::_gripper_status_loop::Starting status loop.")
-        ;
-        clock.init();
-        try
-        {
-            while (status_loop_running_)
-            {
-                bool should_unlock = false;
-                if (!_is_connected()) throw std::runtime_error(
-                    "[" + ros::this_node::getName() +
-                    "]::[EffectorDriverFrankaHand]::_gripper_status_loop::Robot is not connected.");
-                try
-                {
-#ifdef IN_TESTING
-                    ROS_WARN_STREAM(
-                        "["+ ros::this_node::getName()+
-                        "]::[EffectorDriverFrankaHand]::_gripper_status_loop::In testing mode.");
-                    throw std::runtime_error(
-                        "[" + ros::this_node::getName() +
-                        "]::[EffectorDriverFrankaHand]::_gripper_status_loop::In testing mode.");
-#endif
-#ifdef BLOCK_READ_IN_USED
-                    gripper_in_use_.lock();
-                    should_unlock = true;
-#endif
-                    franka::GripperState gripper_state = gripper_sptr_->readOnce();
-#ifdef BLOCK_READ_IN_USED
-                    gripper_in_use_.unlock();
-#endif
-                    sas_robot_driver_franka::GripperState msg;
-                    msg.width = static_cast<float>(gripper_state.width);
-                    msg.max_width = static_cast<float>(gripper_state.max_width);
-                    msg.is_grasped = gripper_state.is_grasped;
-                    msg.temperature = gripper_state.temperature;
-                    msg.duration_ms = gripper_state.time.toMSec();
-                    gripper_status_publisher_.publish(msg);
-                }
-                catch (...)
-                {
-#ifdef BLOCK_READ_IN_USED
-                    if (should_unlock) gripper_in_use_.unlock();
-#endif
-                    ROS_INFO_STREAM("["+ ros::this_node::getName()+"]::[EffectorDriverFrankaHand]::_gripper_status_loop::Could not read gripper state. Unavailable.");
-                    sas_robot_driver_franka::GripperState msg;
-                    msg.width = 0.0;
-                    gripper_status_publisher_.publish(msg);
-                }
-
-                clock.update_and_sleep();
-            }
-            status_loop_running_ = false;
-        }
-        catch (std::exception& e)
-        {
-            ROS_ERROR_STREAM(
-                "["+ ros::this_node::getName()+"]::[EffectorDriverFrankaHand]::_gripper_status_loop::Exception::" + e.
-                what());
-            status_loop_running_ = false;
-        }
-        catch (...)
-        {
-            ROS_ERROR_STREAM(
-                "["+ ros::this_node::getName()+
-                "]::[EffectorDriverFrankaHand]::_gripper_status_loop::Exception::Unknown exception.");
-            status_loop_running_ = false;
-        }
-        ROS_INFO_STREAM(
-            "["+ ros::this_node::getName()+"]::[EffectorDriverFrankaHand]::_gripper_status_loop::Exiting status loop.");
-    }
-} // qros

src/hand/robot_interface_hand.cpp

@@ -1,4 +1,4 @@
-#include <sas_robot_driver_franka/robot_interface_hand.h>
+#include "robot_interface_hand.h"
 
 
 

src/hand/sas_robot_driver_franka_hand.cpp

@@ -0,0 +1,152 @@
+/*
+# 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

src/hand/sas_robot_driver_franka_hand.h

@@ -35,80 +35,70 @@
 #include <memory>
 #include <franka/robot.h>
 #include <franka/gripper.h>
-#include <thread>
+// #include <thread>
-#include <mutex>
 
-#include <franka/exception.h>
+#include <dqrobotics/DQ.h>
-// #define BLOCK_READ_IN_USED
-// #define IN_TESTING
 
-// #include <sas_robot_driver/sas_robot_driver.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/ros.h>
+#include <ros/common.h>
-#include <ros/service.h>
+
-#include <sas_robot_driver_franka/Grasp.h>
+using namespace DQ_robotics;
-#include <sas_robot_driver_franka/Move.h>
+using namespace Eigen;
-#include <sas_robot_driver_franka/GripperState.h>
 
 
+namespace sas {
 
-// using namespace DQ_robotics;
+struct RobotDriverFrankaHandConfiguration
-// using namespace Eigen;
-
-
-namespace qros {
-
-struct EffectorDriverFrankaHandConfiguration
 {
     std::string robot_ip;
-    int thread_sampeling_time_ns = 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{
+class RobotDriverFrankaHand{
 private:
-    std::string driver_node_prefix_;
+    RobotDriverFrankaHandConfiguration configuration_;
-    EffectorDriverFrankaHandConfiguration configuration_;
+    RobotDriverROSConfiguration ros_configuration_;
-    ros::NodeHandle& node_handel_;
 
     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;
 
-    // thread specific functions
-    void _gripper_status_loop();
-    std::thread status_loop_thread_;
-    std::atomic_bool status_loop_running_{false};
-    ros::Publisher gripper_status_publisher_;
-
-    std::mutex gripper_in_use_;
-    ros::ServiceServer grasp_server_;
-    ros::ServiceServer move_server_;
-
 public:
+    //const static int SLAVE_MODE_JOINT_CONTROL;
+    //const static int SLAVE_MODE_END_EFFECTOR_CONTROL;
 
-    bool _grasp_srv_callback(sas_robot_driver_franka::Grasp::Request& req, sas_robot_driver_franka::Grasp::Response& res);
+    RobotDriverFrankaHand(const RobotDriverFrankaHand&)=delete;
+    RobotDriverFrankaHand()=delete;
+    ~RobotDriverFrankaHand();
 
-    bool _move_srv_callback(sas_robot_driver_franka::Move::Request& req, sas_robot_driver_franka::Move::Response& res);
+    RobotDriverFrankaHand(
-
+        const RobotDriverFrankaHandConfiguration& configuration,
-    EffectorDriverFrankaHand(const EffectorDriverFrankaHand&)=delete;
+        const RobotDriverROSConfiguration& ros_configuration,
-    EffectorDriverFrankaHand()=delete;
-    ~EffectorDriverFrankaHand();
-
-    EffectorDriverFrankaHand(
-        const std::string &driver_node_prefix,
-        const EffectorDriverFrankaHandConfiguration& configuration,
-        ros::NodeHandle& node_handel,
         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();
 
 
@@ -117,7 +107,11 @@ public:
 
     void initialize() ;
     void deinitialize() ;
+
+    //bool set_end_effector_pose_dq(const DQ& pose);
+    //DQ get_end_effector_pose_dq();
+
 };
 
-} // qros
+} // sas
 

src/joint/robot_interface_franka.cpp

@@ -30,7 +30,7 @@
 */
 
 
-#include <sas_robot_driver_franka/robot_interface_franka.h>
+#include "robot_interface_franka.h"
 
 #include <ros/this_node.h>
 #include <rosconsole/macros_generated.h>
@@ -365,7 +365,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.K_F_ext_hat_K;
+    current_stiffness_force_torque_array_ = robot_state.O_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,7 +718,6 @@ 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;
 }
 
 /**

src/joint/sas_robot_driver_franka.cpp

@@ -30,12 +30,16 @@
 # ################################################################
 */
 
-#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
 {
-    RobotDriverFranka::RobotDriverFranka(qros::RobotDynamicProvider* robot_dynamic_provider, const RobotDriverFrankaConfiguration &configuration, std::atomic_bool *break_loops):
+    RobotDriverFranka::RobotDriverFranka(RobotDynamicProvider* robot_dynamic_provider, const RobotDriverFrankaConfiguration &configuration, std::atomic_bool *break_loops):
     RobotDriver(break_loops),
     configuration_(configuration),
     robot_dynamic_provider_(robot_dynamic_provider),
@@ -142,10 +146,6 @@ 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();
     }

src/robot_dynamics/qros_robot_dynamic_py.cpp

@@ -1,60 +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: 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_provider.h>
-#include <sas_robot_driver_franka/robot_dynamic/qros_robot_dynamics_interface.h>
-
-namespace py = pybind11;
-using RDI = qros::RobotDynamicInterface;
-using RDP = qros::RobotDynamicProvider;
-
-
-PYBIND11_MODULE(_qros_robot_dynamic, m)
-{
-    py::class_<RDI>(m, "RobotDynamicsInterface")
-        .def(py::init<const std::string&>())
-        .def("get_stiffness_force",&RDI::get_stiffness_force)
-        .def("get_stiffness_torque",&RDI::get_stiffness_torque)
-        .def("get_stiffness_frame_pose",&RDI::get_stiffness_frame_pose)
-        .def("is_enabled",&RDI::is_enabled,"Returns true if the RobotDynamicInterface is enabled.")
-        .def("get_topic_prefix",&RDI::get_topic_prefix);
-
-    py::class_<RDP>(m, "RobotDynamicsProvider")
-        .def(py::init<const std::string&>())
-        .def("publish_stiffness",&RDP::publish_stiffness)
-        .def("set_world_to_base_tf", &RDP::set_world_to_base_tf)
-        .def("is_enabled",&RDP::is_enabled,"Returns true if the RobotDynamicProvider is enabled.")
-        .def("get_topic_prefix",&RDP::get_topic_prefix);
-
-}

src/robot_dynamics/qros_robot_dynamics_interface.cpp

@@ -1,103 +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/robot_dynamic/qros_robot_dynamics_interface.h>
-
-using namespace qros;
-
-RobotDynamicInterface::RobotDynamicInterface(ros::NodeHandle &nodehandle, const std::string &node_prefix):
-    RobotDynamicInterface(nodehandle, nodehandle, node_prefix)
-{
-    //Delegated
-}
-
-RobotDynamicInterface::RobotDynamicInterface(ros::NodeHandle &publisher_nodehandle, ros::NodeHandle &subscriber_nodehandle, const std::string &node_prefix):
-    node_prefix_(node_prefix),
-    child_frame_id_(node_prefix + "_stiffness_frame"),
-    parent_frame_id_(node_prefix + "_base"),
-    tf_buffer_(ros::Duration(BUFFER_DURATION_DEFAULT_S)),
-    tf_listener_(tf_buffer_, subscriber_nodehandle),
-    last_stiffness_force_(Vector3d::Zero()),
-    last_stiffness_torque_(Vector3d::Zero()),
-    last_stiffness_frame_pose_(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);}
-    ROS_INFO_STREAM(ros::this_node::getName() + "::Initializing RobotDynamicInterface with prefix " + node_prefix);
-    subscriber_cartesian_stiffness_ = subscriber_nodehandle.subscribe(node_prefix_ + "/get/cartesian_stiffness", 1, &RobotDynamicInterface::_callback_cartesian_stiffness, this);
-
-}
-
-void RobotDynamicInterface::_callback_cartesian_stiffness(const geometry_msgs::WrenchStampedConstPtr &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_, ros::Time(0));
-        last_stiffness_frame_pose_ = _geometry_msgs_transform_to_dq(transform_stamped.transform);
-    } catch (tf2::TransformException &ex) {
-        ROS_WARN_STREAM(ros::this_node::getName() + "::" + ex.what());
-    }
-}
-
-DQ RobotDynamicInterface::_geometry_msgs_transform_to_dq(const geometry_msgs::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 RobotDynamicInterface::get_stiffness_force()
-{
-    if(!is_enabled()) throw std::runtime_error("[RobotDynamicInterface]::calling get_stiffness_force on uninitialized topic");
-    return last_stiffness_force_;
-}
-VectorXd RobotDynamicInterface::get_stiffness_torque()
-{
-    if(!is_enabled()) throw std::runtime_error("[RobotDynamicInterface]::calling get_stiffness_torque on uninitialized topic");
-    return last_stiffness_torque_;
-}
-DQ RobotDynamicInterface::get_stiffness_frame_pose()
-{
-    if(!is_enabled()) throw std::runtime_error("[RobotDynamicInterface]::calling get_stiffness_frame_pose on uninitialized Interface");
-    return last_stiffness_frame_pose_;
-}
-
-bool RobotDynamicInterface::is_enabled() const
-{
-    if(last_stiffness_frame_pose_==0) return false;
-    return true;
-}

src/robot_dynamics/qros_robot_dynamics_provider.cpp

@@ -1,126 +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/robot_dynamic/qros_robot_dynamics_provider.h>
-
-using namespace qros;
-
-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),
-    child_frame_id_(node_prefix + "_stiffness_frame"),
-    parent_frame_id_(node_prefix + "_base"),
-    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);}
-
-    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);
-
-}
-
-
-geometry_msgs::Transform RobotDynamicProvider::_dq_to_geometry_msgs_transform(const DQ& pose)
-{
-    geometry_msgs::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 RobotDynamicProvider::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("The world to base transform has already been set");
-    }
-}
-
-void RobotDynamicProvider::_publish_base_static_tf()
-{
-    geometry_msgs::TransformStamped base_tf;
-    base_tf.transform = _dq_to_geometry_msgs_transform(world_to_base_tf_);
-    base_tf.header.stamp = ros::Time::now();
-    base_tf.header.frame_id = WORLD_FRAME_ID;
-    base_tf.child_frame_id = parent_frame_id_;
-    static_base_tf_broadcaster_.sendTransform(base_tf);
-
-}
-
-
-void RobotDynamicProvider::publish_stiffness(const DQ& base_to_stiffness, const Vector3d& force, const Vector3d& torque)
-{
-    std_msgs::Header header;
-    header.stamp = ros::Time::now();
-    header.seq = seq_++;
-    geometry_msgs::WrenchStamped msg;
-    msg.header = header;
-    msg.header.frame_id = child_frame_id_;
-    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);
-    if(seq_ % REDUCE_TF_PUBLISH_RATE == 0)
-    {
-        geometry_msgs::TransformStamped tf_msg;
-        tf_msg.transform = _dq_to_geometry_msgs_transform(base_to_stiffness);
-        tf_msg.header = header;
-        tf_msg.header.frame_id = parent_frame_id_;
-        tf_msg.child_frame_id = child_frame_id_;
-        tf_broadcaster_.sendTransform(tf_msg);
-    }
-}
-
-
-bool RobotDynamicProvider::is_enabled() const
-{
-    return true; //Always enabled
-}
-

src/sas_robot_driver_coppelia_node.cpp

@@ -26,8 +26,6 @@
 #      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
 #
 # ################################################################
 */
@@ -41,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 "../../src/coppelia/sas_robot_driver_coppelia.h"
+#include "coppelia/sas_robot_driver_coppelia.h"
 
 /*********************************************
  * SIGNAL HANDLER
@@ -54,7 +52,6 @@ void sig_int_handler(int)
 }
 
 
-
 int main(int argc, char **argv)
 {
     if(signal(SIGINT, sig_int_handler) == SIG_ERR)
@@ -64,37 +61,39 @@ int main(int argc, char **argv)
 
     ros::init(argc, argv, "sas_robot_driver_coppelia_node", ros::init_options::NoSigintHandler);
     ROS_WARN("=====================================================================");
-    ROS_WARN("--------------------- Quentin Lin -----------------------------------");
+    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,"/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,"/robot_ip_address",robot_driver_coppelia_configuration.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,"/vrep_port", robot_driver_coppelia_configuration.port);
-    sas::get_ros_param(nh,"/robot_topic_prefix", robot_driver_coppelia_configuration.robot_topic_prefix);
+    sas::get_ros_param(nh,"/vrep_robot_joint_names", robot_driver_coppelia_configuration.jointnames);
-    sas::get_ros_param(nh,"/robot_parameter_file_path", robot_driver_coppelia_configuration.robot_parameter_file_path);
+    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,"/thread_sampling_time_nsec",robot_driver_ros_configuration.thread_sampling_time_nsec);
-    // sas::get_ros_param(nh,"/q_min", q_min_vec);
+    sas::get_ros_param(nh,"/q_min",robot_driver_ros_configuration.q_min);
-    // robot_driver_coppelia_configuration.q_min = sas::std_vector_double_to_vectorxd(q_min_vec);
+    sas::get_ros_param(nh,"/q_max",robot_driver_ros_configuration.q_max);
-    // 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);
 
+    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(nh, robot_driver_coppelia_configuration,
+        sas::RobotDriverCoppelia robot_driver_coppelia(robot_driver_coppelia_configuration,
                                                        &kill_this_process);
-
+        ROS_INFO_STREAM(ros::this_node::getName()+"::Instantiating RobotDriverROS.");
-        return robot_driver_coppelia.start_control_loop();
+        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)
     {

src/sas_robot_driver_franka/__init__.py

@@ -1,4 +0,0 @@
-"""
-"""
-from _qros_robot_dynamic import RobotDynamicsInterface, RobotDynamicsProvider
-from .franka_gripper_interface import FrankaGripperInterface

src/sas_robot_driver_franka/franka_gripper_interface.py

@@ -1,182 +0,0 @@
-import rospy
-from sas_robot_driver_franka.srv import Move, MoveRequest, MoveResponse, Grasp, GraspRequest, GraspResponse
-from sas_robot_driver_franka.msg import GripperState
-import os
-import threading
-from queue import Queue
-import struct
-
-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, robot_prefix):
-        self.robot_prefix = robot_prefix
-        self.move_service = rospy.ServiceProxy(os.path.join(robot_prefix, MOVE_TOPIC_SUFFIX), Move)
-        self._moving = False
-        self.grasp_service = rospy.ServiceProxy(os.path.join(robot_prefix, GRASP_TOPIC_SUFFIX), Grasp)
-        self._grasping = False
-        self.status_subscriber = rospy.Subscriber(os.path.join(robot_prefix, STATUS_TOPIC_SUFFIX), GripperState,
-                                                  self._status_callback)
-
-        self.result_queue = Queue()
-
-        # gripper state
-        self.state_width = None
-        self.state_max_width = None
-        self.state_temperature = None
-        self.state_is_grasped = None
-
-    def _is_service_ready(self, service):
-        try:
-            rospy.wait_for_service(service, timeout=0.1)
-            return True
-        except rospy.ROSException:
-            return False
-
-    def is_enabled(self):
-        if self.state_width is None:
-            return False
-        if not self._is_service_ready(self.move_service.resolved_name):
-            return False
-        if not self._is_service_ready(self.grasp_service.resolved_name):
-            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
-
-    def move(self, width, speed=0):
-        """
-        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")
-        threading.Thread(target=self._move, args=(width, speed)).start()
-
-    def grasp(self, width, force=0, speed=0, epsilon_inner=0, epsilon_outer=0):
-        """
-        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")
-        threading.Thread(target=self._grasp, args=(width, force, speed, epsilon_inner, epsilon_outer)).start()
-
-    def get_max_width(self):
-        """ 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):
-        """ 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):
-        """ 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):
-        """ Check if an object is grasped """
-        if not self.is_enabled():
-            raise Exception("Gripper is not enabled")
-        return self.state_is_grasped
-
-    def is_moving(self):
-        """ Check if the gripper is currently moving """
-        return self._moving
-
-    def is_grasping(self):
-        """ Check if the gripper is currently grasping """
-        return self._grasping
-
-    def is_busy(self):
-        """ Check if the gripper is currently moving or grasping """
-        return self._moving or self._grasping
-
-    def is_done(self):
-        """ Check if the gripper is done moving or grasping """
-        if not self.is_busy():
-            rospy.logwarn("Gripper is not moving or grasping")
-            return False
-        else:
-            if self.result_queue.empty():
-                return False
-            else:
-                return True
-
-    def get_result(self):
-        """
-        Get the result of the last action
-        :return:
-        """
-        if not self.result_queue.empty():
-            response = self.result_queue.get()
-            self._moving = False
-            self._grasping = False
-            return response.success
-        else:
-            raise Exception("No result available")
-
-    def wait_until_done(self):
-        """
-        Wait until the gripper is done moving or grasping
-        :return: success
-        """
-        if not self.is_enabled():
-            raise Exception("Gripper is not enabled")
-        if not self._moving and not self._grasping:
-            raise Exception("Gripper is not moving or grasping")
-        while self._moving or self._grasping:
-            rospy.sleep(0.1)
-            if not self.result_queue.empty():
-                response = self.result_queue.get()
-                if isinstance(response, MoveResponse):
-                    self._moving = False
-                elif isinstance(response, GraspResponse):
-                    self._grasping = False
-                else:
-                    raise Exception("Invalid response type")
-                break
-        return response.success
-
-    def _move(self, width, speed):
-        self._moving = True
-        request = MoveRequest()
-        request.width = width
-        request.speed = speed
-        response = self.move_service(request)
-        self.result_queue.put(response)
-
-    def _grasp(self, width, force, speed, epsilon_inner, epsilon_outer):
-        self._grasping = True
-        request = GraspRequest()
-        request.width = width
-        request.force = force
-        request.speed = speed
-        request.epsilon_inner = epsilon_inner
-        request.epsilon_outer = epsilon_outer
-        response = self.grasp_service(request)
-        self.result_queue.put(response)

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 "../../src/hand/qros_effector_driver_franka_hand.h"
+#include "sas_robot_driver_franka_hand.h"
 
 
 /*********************************************
@@ -49,22 +49,6 @@ void sig_int_handler(int)
     kill_this_process = true;
 }
 
-
-
-template<typename T>
-void get_optional_parameter(const std::string &node_prefix, ros::NodeHandle &nh, const std::string &param_name, T &param)
-{
-    if(nh.hasParam(node_prefix + param_name))
-    {
-        sas::get_ros_param(nh,param_name,param);
-    }else
-    {
-        ROS_INFO_STREAM(ros::this_node::getName() + "::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)
@@ -76,24 +60,32 @@ int main(int argc, char **argv)
     ROS_WARN("---------------------------Quentin Lin-------------------------------");
     ROS_WARN("=====================================================================");
     ROS_INFO_STREAM(ros::this_node::getName()+"::Loading parameters from parameter server.");
-    const std::string& effector_driver_provider_prefix = ros::this_node::getName();
 
 
     ros::NodeHandle nh;
-    qros::EffectorDriverFrankaHandConfiguration robot_driver_franka_hand_configuration;
+    sas::RobotDriverFrankaHandConfiguration robot_driver_franka_hand_configuration;
 
     sas::get_ros_param(nh,"/robot_ip_address",robot_driver_franka_hand_configuration.robot_ip);
 
-    get_optional_parameter(effector_driver_provider_prefix,nh,"/thread_sampling_time_nsec",robot_driver_franka_hand_configuration.thread_sampeling_time_ns);
-    get_optional_parameter(effector_driver_provider_prefix,nh,"/default_force",robot_driver_franka_hand_configuration.default_force);
-    get_optional_parameter(effector_driver_provider_prefix,nh,"/default_speed",robot_driver_franka_hand_configuration.default_speed);
-    get_optional_parameter(effector_driver_provider_prefix,nh,"/default_epsilon_inner",robot_driver_franka_hand_configuration.default_epsilon_inner);
-    get_optional_parameter(effector_driver_provider_prefix,nh,"/default_epsilon_outer",robot_driver_franka_hand_configuration.default_epsilon_outer);
 
-    qros::EffectorDriverFrankaHand franka_hand_driver(
+    sas::RobotDriverROSConfiguration robot_driver_ros_configuration;
-        effector_driver_provider_prefix,
+
+    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,
-        nh,
+        robot_driver_ros_configuration,
         &kill_this_process
     );
     try

src/sas_robot_driver_franka_node.cpp

@@ -35,12 +35,12 @@
 
 #include <exception>
 #include <dqrobotics/utils/DQ_Math.h>
-#include <dqrobotics/interfaces/json11/DQ_JsonReader.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/sas_robot_driver_franka.h>
+#include "sas_robot_driver_franka.h"
-#include <sas_robot_driver_franka/robot_dynamic/qros_robot_dynamics_provider.h>
+#include "sas_robot_dynamic_provider.h"
 
 
 /*********************************************
@@ -121,23 +121,6 @@ int main(int argc, char **argv)
         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);
     }
-    if(nh.hasParam(ros::this_node::getName()+"/robot_parameter_file_path"))
-    {
-        std::string robot_parameter_file_path;
-        sas::get_ros_param(nh,"/robot_parameter_file_path",robot_parameter_file_path);
-        ROS_INFO_STREAM(ros::this_node::getName()+"::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();
-    }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;
@@ -150,11 +133,8 @@ int main(int argc, char **argv)
 
     // initialize the robot dynamic provider
     robot_driver_ros_configuration.robot_driver_provider_prefix = ros::this_node::getName();
-    qros::RobotDynamicProvider robot_dynamic_provider(nh, robot_driver_ros_configuration.robot_driver_provider_prefix);
+    sas::RobotDynamicProvider robot_dynamic_provider(nh, robot_driver_ros_configuration.robot_driver_provider_prefix);
-    if(robot_driver_franka_configuration.robot_reference_frame!=0)
+
-    {
-        robot_dynamic_provider.set_world_to_base_tf(robot_driver_franka_configuration.robot_reference_frame);
-    }
 
     try
         {

srv/Grasp.srv

@@ -1,7 +0,0 @@
-float32 width
-float32 speed
-float32 force
-float32 epsilon_inner
-float32 epsilon_outer
----
-bool success

srv/Move.srv

@@ -1,4 +0,0 @@
-float32 width
-float32 speed
----
-bool success