Echo contact (#1)

* added cartesian interface

* general fix and optimization

src/coppelia/sas_robot_driver_coppelia.h

@@ -0,0 +1,134 @@
+/*
+# Copyright (c) 2023 Juan Jose Quiroz Omana
+#
+#    This file is part of sas_robot_driver_franka.
+#
+#    sas_robot_driver_franka is free software: you can redistribute it and/or modify
+#    it under the terms of the GNU Lesser General Public License as published by
+#    the Free Software Foundation, either version 3 of the License, or
+#    (at your option) any later version.
+#
+#    sas_robot_driver_franka is distributed in the hope that it will be useful,
+#    but WITHOUT ANY WARRANTY; without even the implied warranty of
+#    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+#    GNU Lesser General Public License for more details.
+#
+#    You should have received a copy of the GNU Lesser General Public License
+#    along with sas_robot_driver.  If not, see <https://www.gnu.org/licenses/>.
+#
+# ################################################################
+#
+#   Author: Juan Jose Quiroz Omana, email: juanjqogm@gmail.com
+#
+# ################################################################
+#
+# Contributors:
+#      1. Juan Jose Quiroz Omana (juanjqogm@gmail.com)
+#         - Adapted from sas_robot_driver_denso.cpp
+#           (https://github.com/SmartArmStack/sas_robot_driver_denso/blob/master/src/sas_robot_driver_denso.cpp)
+#
+# ################################################################
+*/
+
+
+#pragma once
+#include <exception>
+#include <tuple>
+#include <atomic>
+#include <vector>
+#include <memory>
+#include <dqrobotics/DQ.h>
+#include <sas_robot_driver/sas_robot_driver.h>
+#include <dqrobotics/interfaces/vrep/DQ_VrepRobot.h>
+#include <thread>
+#include <sas_robot_driver/sas_robot_driver_interface.h>
+
+using namespace DQ_robotics;
+using namespace Eigen;
+
+namespace sas
+{
+
+
+
+struct RobotDriverCoppeliaConfiguration
+{
+
+    int thread_sampling_time_nsec;
+    int port;
+    std::string ip;
+    std::vector<std::string> jointnames;
+    std::string robot_mode;
+    bool mirror_mode;
+    std::string real_robot_topic_prefix;
+};
+
+class RobotDriverCoppelia: public RobotDriver
+{
+private:
+    RobotDriverCoppeliaConfiguration configuration_;
+
+    std::string robot_mode_ = std::string("VelocityControl");   // PositionControl
+    bool mirror_mode_ = false;
+    double gain_ = 3.0;
+    std::string real_robot_topic_prefix_;
+
+    VectorXd current_joint_positions_;
+    VectorXd current_joint_velocities_;
+    VectorXd current_joint_forces_;
+
+
+    VectorXd desired_joint_velocities_;
+    VectorXd desired_joint_positions_;
+
+    std::atomic<bool> finish_motion_;
+
+    int dim_configuration_space_;
+
+    void _update_robot_state(const VectorXd& q, const VectorXd& q_dot, const VectorXd& forces);
+
+    void finish_motion();
+
+    void _start_joint_velocity_control_mode();
+    std::thread joint_velocity_control_mode_thread_;
+    void _start_joint_velocity_control_thread();
+
+
+    void _start_joint_velocity_control_mirror_mode();
+    std::thread joint_velocity_control_mirror_mode_thread_;
+    void _start_joint_velocity_control_mirror_thread();
+
+    std::shared_ptr<sas::RobotDriverInterface> franka1_ros_;
+
+
+protected:
+    std::shared_ptr<DQ_VrepInterface> vi_;
+    std::vector<std::string> jointnames_;
+
+
+
+public:
+    RobotDriverCoppelia(const RobotDriverCoppelia&)=delete;
+    RobotDriverCoppelia()=delete;
+    ~RobotDriverCoppelia();
+
+    RobotDriverCoppelia(const RobotDriverCoppeliaConfiguration& configuration, std::atomic_bool* break_loops);
+
+
+    VectorXd get_joint_positions() override;
+    void set_target_joint_positions(const VectorXd& desired_joint_positions_rad) override;
+
+    VectorXd get_joint_velocities() override;
+    void set_target_joint_velocities(const VectorXd& desired_joint_velocities) override;
+
+    VectorXd get_joint_forces() override;
+
+    void connect() override;
+    void disconnect() override;
+
+    void initialize() override;
+    void deinitialize() override;
+
+
+};
+}

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

@@ -0,0 +1,60 @@
+#pragma once
+/*
+# Copyright (c) 2023 Juan Jose Quiroz Omana
+#
+#    This file is part of sas_robot_driver_franka.
+#
+#    sas_robot_driver_franka is free software: you can redistribute it and/or modify
+#    it under the terms of the GNU Lesser General Public License as published by
+#    the Free Software Foundation, either version 3 of the License, or
+#    (at your option) any later version.
+#
+#    sas_robot_driver_franka is distributed in the hope that it will be useful,
+#    but WITHOUT ANY WARRANTY; without even the implied warranty of
+#    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+#    GNU Lesser General Public License for more details.
+#
+#    You should have received a copy of the GNU Lesser General Public License
+#    along with sas_robot_driver.  If not, see <https://www.gnu.org/licenses/>.
+#
+# ################################################################
+#
+#   Author: Juan Jose Quiroz Omana, email: juanjqogm@gmail.com
+#
+# ################################################################
+#
+# Contributors:
+#      1. Quenitin Lin
+#
+# ################################################################
+*/
+#include <ros/ros.h>
+#include <ros/node_handle.h>
+#include <sas_common/sas_common.h>
+#include <sas_conversions/sas_conversions.h>
+#include <geometry_msgs/WrenchStamped.h>
+#include <geometry_msgs/PoseStamped.h>
+#include <dqrobotics/DQ.h>
+
+namespace sas {
+
+using namespace DQ_robotics;
+
+class RobotDynamicProvider {
+private:
+    std::string node_prefix_;
+
+    ros::Publisher publisher_cartesian_stiffness_;
+    ros::Publisher publisher_stiffness_pose_;
+
+public:
+    RobotDynamicProvider(ros::NodeHandle& nodehandle, const std::string& node_prefix=ros::this_node::getName());
+    RobotDynamicProvider(ros::NodeHandle& publisher_nodehandle, ros::NodeHandle& subscriber_nodehandle, const std::string& node_prefix=ros::this_node::getName());
+
+    void publish_stiffness(const DQ& base_to_stiffness, const Vector3d& force, const Vector3d& torque) const;
+
+};
+
+
+
+} // namespace sas

src/generator/custom_motion_generation.cpp

@@ -28,9 +28,7 @@
 #
 # ################################################################
 */
-
-
-#include "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 "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 "quadratic_program_motion_generator.h"
+#include "generator/quadratic_program_motion_generator.h"
 
 
 QuadraticProgramMotionGenerator::QuadraticProgramMotionGenerator(const double &speed_factor,

src/hand/sas_robot_driver_franka_hand.cpp

@@ -1,7 +1,32 @@
-//
-// Created by qlin on 7/17/24.
-//
-
+/*
+# 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 {

src/hand/sas_robot_driver_franka_hand.h

@@ -1,4 +1,33 @@
 #pragma once
+/*
+# Copyright (c) 2023 Juan Jose Quiroz Omana
+#
+#    This file is part of sas_robot_driver_franka.
+#
+#    sas_robot_driver_franka is free software: you can redistribute it and/or modify
+#    it under the terms of the GNU Lesser General Public License as published by
+#    the Free Software Foundation, either version 3 of the License, or
+#    (at your option) any later version.
+#
+#    sas_robot_driver_franka is distributed in the hope that it will be useful,
+#    but WITHOUT ANY WARRANTY; without even the implied warranty of
+#    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+#    GNU Lesser General Public License for more details.
+#
+#    You should have received a copy of the GNU Lesser General Public License
+#    along with sas_robot_driver.  If not, see <https://www.gnu.org/licenses/>.
+#
+# ################################################################
+#
+#   Author: Juan Jose Quiroz Omana, email: juanjqogm@gmail.com
+#
+# ################################################################
+#
+# Contributors:
+#      1. Quenitin Lin
+#
+# ################################################################
+*/
 #include <exception>
 #include <tuple>
 #include <atomic>

src/joint/robot_interface_franka.cpp

@@ -38,13 +38,17 @@
 
 /**
  * @brief robot_driver_franka::robot_driver_franka
+ * @param configuration The configuration of the robot for setting control beheavior
  * @param ROBOT_IP The IP address of the FCI
  * @param mode The operation mode {None, PositionControl}.
  * @param hand The hand option {ONFinished, OFF}.
  */
-RobotInterfaceFranka::RobotInterfaceFranka(const std::string &ROBOT_IP,
-                                         const MODE& mode,
-                                         const HAND& hand):ip_(ROBOT_IP),mode_(mode)
+RobotInterfaceFranka::RobotInterfaceFranka(
+    const FrankaInterfaceConfiguration &configuration,
+    const std::string &ROBOT_IP,
+    const MODE& mode,
+    const HAND& hand
+    ):ip_(ROBOT_IP),franka_configuration_(configuration),mode_(mode)
 {
     _set_driver_mode(mode);
     if (hand == RobotInterfaceFranka::HAND::ON)
@@ -308,6 +312,16 @@ void RobotInterfaceFranka::initialize()
 
     initialize_flag_ = true;
 
+    // initialize and set the robot to default behavior (collision behavior, impedance, etc)
+    setDefaultBehavior(*robot_sptr_);
+    robot_sptr_->setCollisionBehavior(
+        franka_configuration_.lower_torque_threshold,
+        franka_configuration_.upper_torque_threshold,
+        franka_configuration_.lower_force_threshold,
+        franka_configuration_.upper_force_threshold
+        );
+    robot_sptr_->setJointImpedance(franka_configuration_.joint_impedance);
+    robot_sptr_->setCartesianImpedance(franka_configuration_.cartesian_impedance);
 
     switch (mode_)
     {
@@ -351,6 +365,19 @@ void RobotInterfaceFranka::_update_robot_state(const franka::RobotState &robot_s
     current_joint_forces_array_ = robot_state.tau_J;
     current_joint_forces_ =  Eigen::Map<VectorXd>(current_joint_forces_array_.data(), 7);
 
+    current_stiffness_force_torque_array_ = robot_state.O_F_ext_hat_K;
+    current_stiffness_force_[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];
+    current_stiffness_torque_[0] = current_stiffness_force_torque_array_[3];
+    current_stiffness_torque_[1] = current_stiffness_force_torque_array_[4];
+    current_stiffness_torque_[2] = current_stiffness_force_torque_array_[5];
+
+    current_effector_pose_array_ = robot_state.O_T_EE;
+    current_stiffness_pose_array_ = robot_state.EE_T_K;
+    current_effeector_pose_ = Eigen::Map<VectorXd>(current_effector_pose_array_.data(), 16);
+    current_stiffness_pose_ = Eigen::Map<VectorXd>(current_stiffness_pose_array_.data(), 16);
+
     robot_mode_ = robot_state.robot_mode;
     time_ = time;
 }
@@ -674,6 +701,25 @@ VectorXd RobotInterfaceFranka::get_joint_forces()
 }
 
 
+std::tuple<Vector3d, Vector3d> RobotInterfaceFranka::get_stiffness_force_torque()
+{
+    _check_if_robot_is_connected();
+    return std::make_tuple(current_stiffness_force_, current_stiffness_torque_);
+
+}
+
+
+/**
+ * @brief robot_driver_franka::get_stiffness_pose
+ * @return
+ */
+DQ RobotInterfaceFranka::get_stiffness_pose() {
+    _check_if_robot_is_connected();
+    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;
+}
+
 /**
  * @brief robot_driver_franka::get_time
  * @return

src/joint/robot_interface_franka.h

@@ -0,0 +1,257 @@
+/*
+# Copyright (c) 2023 Juan Jose Quiroz Omana
+#
+#    This file is part of sas_robot_driver_franka.
+#
+#    sas_robot_driver_franka is free software: you can redistribute it and/or modify
+#    it under the terms of the GNU Lesser General Public License as published by
+#    the Free Software Foundation, either version 3 of the License, or
+#    (at your option) any later version.
+#
+#    sas_robot_driver_franka is distributed in the hope that it will be useful,
+#    but WITHOUT ANY WARRANTY; without even the implied warranty of
+#    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+#    GNU Lesser General Public License for more details.
+#
+#    You should have received a copy of the GNU Lesser General Public License
+#    along with sas_robot_driver.  If not, see <https://www.gnu.org/licenses/>.
+#
+# ################################################################
+#
+#   Author: Juan Jose Quiroz Omana, email: juanjqogm@gmail.com
+#
+# ################################################################
+#
+# Contributors:
+#      1. Juan Jose Quiroz Omana (juanjqogm@gmail.com)
+#         - Original implementation
+#
+# ################################################################
+*/
+
+
+#pragma once
+
+#include <dqrobotics/DQ.h>
+#include <memory.h>
+#include <tuple>
+#include <exception>
+#include <vector>
+#include <franka/robot.h>
+#include <franka/gripper.h>
+#include <franka/exception.h>
+#include "generator/motion_generator.h"
+#include <thread>
+#include "generator/quadratic_program_motion_generator.h"
+#include <dqrobotics/robots/FrankaEmikaPandaRobot.h>
+#include <atomic>
+#include "generator/custom_motion_generation.h"
+
+using namespace DQ_robotics;
+using namespace Eigen;
+
+/**
+ * @brief get homgenious_tf_to_DQ
+ * @param pose_vector (column major)
+ * @return pose
+ */
+inline DQ homgenious_tf_to_DQ(const VectorXd& pose_vector) {
+    // VectorXd column major
+    const auto t = DQ(0,  pose_vector(12), pose_vector(13), pose_vector(14));
+    Eigen::Matrix<double, 3, 3, Eigen::ColMajor> rot_mat;
+    rot_mat << pose_vector(0), pose_vector(4), pose_vector(8),
+               pose_vector(1), pose_vector(5), pose_vector(9),
+               pose_vector(2), pose_vector(6), pose_vector(10);
+    Quaternion<double> quaternion(rot_mat);
+    const auto r = normalize(DQ(quaternion.w(), quaternion.x(), quaternion.y(), quaternion.z()));
+    return r + 0.5 * E_ * t * r;
+}
+
+class RobotInterfaceFranka
+{
+public:
+    struct FrankaInterfaceConfiguration {
+        std::array<double, 7> lower_torque_threshold={10.0, 10.0, 10.0, 10.0, 10.0, 10.0, 10.0};
+        std::array<double, 7> upper_torque_threshold={10.0, 10.0, 10.0, 10.0, 10.0, 10.0, 10.0};
+        std::array<double, 6> lower_force_threshold={10.0, 10.0, 10.0, 10.0, 10.0, 10.0};
+        std::array<double, 6> upper_force_threshold={10.0, 10.0, 10.0, 10.0, 10.0, 10.0};
+        std::array<double, 7> joint_impedance={3000, 3000, 3000, 2500, 2500, 2000, 2000};  // K_theta (Nm/rad)
+        std::array<double, 6> cartesian_impedance={3000, 3000, 3000, 300, 300, 300}; // K_x (N/m)
+    };
+
+    enum class MODE{
+        None=0,
+        PositionControl,
+        VelocityControl,
+        ForceControl,
+        Homing,
+        ClearPositions,
+    };
+
+private:
+    using Vector7d = Eigen::Matrix<double, 7, 1, Eigen::ColMajor>;
+    std::string ip_ = "172.16.0.2";
+    FrankaInterfaceConfiguration franka_configuration_;
+    double speed_factor_joint_position_controller_ = 0.2;
+    double speed_gripper_ = 0.02;
+    VectorXd desired_joint_positions_;
+    VectorXd desired_joint_velocities_ = VectorXd::Zero(7);
+
+    VectorXd current_joint_positions_;
+    std::array<double, 7> current_joint_positions_array_;
+
+    VectorXd current_joint_velocities_;
+    std::array<double, 7> current_joint_velocities_array_;
+
+    VectorXd current_joint_forces_;
+    std::array<double, 7> current_joint_forces_array_;
+
+    Vector3d current_stiffness_force_;
+    Vector3d current_stiffness_torque_;
+    std::array<double, 6> current_stiffness_force_torque_array_;
+
+    VectorXd current_stiffness_pose_;
+    VectorXd current_effeector_pose_;
+    std::array<double, 16> current_stiffness_pose_array_;
+    std::array<double, 16> current_effector_pose_array_;
+
+    franka::RobotMode robot_mode_;
+
+    double time_ = 0;
+    bool initialize_flag_ = false;
+    void _check_if_robot_is_connected();
+    void _check_if_hand_is_connected();
+
+    RobotInterfaceFranka::MODE mode_;
+    void _set_driver_mode(const RobotInterfaceFranka::MODE& mode);
+
+    void _restart_default_parameters();
+
+
+    void _update_robot_state(const franka::RobotState& robot_state, const double& time);
+
+    std::shared_ptr<franka::Robot> robot_sptr_;
+    std::shared_ptr<franka::Gripper> gripper_sptr_;
+    std::unique_ptr<QuadraticProgramMotionGenerator> trajectory_generator_sptr_;
+    std::unique_ptr<CustomMotionGeneration> custom_generator_sptr_;
+
+    void _setDefaultRobotBehavior();
+    //bool hand_enabled_ = false;
+    const VectorXd q_home_configuration_ =(VectorXd(7)<<0, -M_PI_4, 0, -3 * M_PI_4, 0, M_PI_2, M_PI_4).finished();
+
+    const VectorXd q_max_ = ((VectorXd(7) <<  2.3093, 1.5133, 2.4937, -0.4461, 2.4800, 4.2094,  2.6895).finished());
+    const VectorXd q_min_ = ((VectorXd(7) << -2.3093,-1.5133,-2.4937, -2.7478,-2.4800, 0.8521, -2.6895).finished());
+    const VectorXd q_min_dot_ = ((VectorXd(7) << -2, -1, -1.5, -1.25, -3, -1.5, -3).finished());
+    const VectorXd q_max_dot_ = ((VectorXd(7) <<  2,  1,  1.5,  1.25,  3,  1.5,  3).finished());
+
+    const double samples_ = 20;
+
+    VectorXd desired_mean_joint_positions_;
+
+    bool verbose_ = true;
+    std::string status_message_ = " ";
+    void _update_status_message(const std::string& message, const bool& verbose = true);
+
+
+    //-------To handle the threads-----------------
+    std::atomic<bool> exit_on_err_={false};
+
+
+    void _start_joint_position_control_mode();
+    std::thread joint_position_control_mode_thread_;
+    void _start_joint_position_control_thread();
+    std::atomic<bool> finish_motion_;
+
+    void _start_echo_robot_state_mode();
+    std::thread echo_robot_state_mode_thread_;
+    void _start_echo_robot_state_mode_thread();
+    std::atomic<bool> finish_echo_robot_state_;
+    void _finish_echo_robot_state();
+
+    void _start_joint_velocity_control_mode();
+    std::thread joint_velocity_control_mode_thread_;
+    void _start_joint_velocity_control_thread();
+    //----------------------------------------------
+
+
+
+    VectorXd _compute_recursive_mean(const double& samples, const VectorXd& q);
+    VectorXd _read_once_smooth_initial_positions(const double& samples);
+
+
+public:
+
+
+    enum HAND{ON=0, OFF};
+    enum CONNECTION{AUTOMATIC};
+    enum GRIPPER_STATES{WIDTH=0, MAX_WIDTH=1};
+    RobotInterfaceFranka() = delete;
+    RobotInterfaceFranka(const RobotInterfaceFranka&) = delete;
+    RobotInterfaceFranka& operator= (const RobotInterfaceFranka&) = delete;
+    RobotInterfaceFranka(
+        const FrankaInterfaceConfiguration &configuration,
+        const std::string &ROBOT_IP,
+        const MODE& mode,
+        const HAND& hand = ON);
+
+
+
+    VectorXd read_once_initial_positions();
+
+    VectorXd get_joint_target_positions();
+
+    void move_robot_to_target_joint_positions(const VectorXd& q_target);
+
+
+
+
+    double read_gripper(const GRIPPER_STATES& gripper_state = WIDTH);
+    bool read_grasped_status();
+    void set_gripper(const double& width);
+    void gripper_homing();
+    VectorXd get_home_robot_configuration();
+
+    std::string get_status_message();
+
+    bool get_err_state() const {return exit_on_err_.load();}
+
+    std::string get_robot_mode();
+
+
+    void finish_motion();
+
+    std::shared_ptr<franka::Robot> get_robot_pointer();
+
+
+    //--------sas compatible methods----------//
+    VectorXd get_joint_positions();
+    VectorXd get_joint_velocities();
+    VectorXd get_joint_forces();
+
+    std::tuple<Vector3d, Vector3d> get_stiffness_force_torque();
+    DQ get_stiffness_pose();
+
+    double get_time();
+
+    void set_target_joint_positions(const VectorXd& set_target_joint_positions_rad);
+    void connect();
+    void initialize();
+    void deinitialize();
+    void disconnect();
+
+    void set_target_joint_velocities(const VectorXd& target_joint_velocities);
+
+
+
+
+
+
+    /* To be implemented
+    std::tuple<VectorXd, VectorXd> get_joint_limits() const;
+    void set_joint_limits(const std::tuple<VectorXd, VectorXd>& joint_limits);
+    */
+
+
+};
+
+

src/joint/sas_robot_driver_franka.cpp

@@ -31,7 +31,7 @@
 */
 
 
-#include "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>
@@ -39,9 +39,10 @@
 
 namespace sas
 {
-    RobotDriverFranka::RobotDriverFranka(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),
     break_loops_(break_loops)
    {
         joint_positions_.resize(7);
@@ -73,14 +74,27 @@ namespace sas
         }
 
 
-        robot_driver_interface_sptr_ = std::make_shared<RobotInterfaceFranka>(configuration.ip_address,
-                                                                        mode, //None, PositionControl, VelocityControl
-                                                                        RobotInterfaceFranka::HAND::OFF);
+        robot_driver_interface_sptr_ = std::make_shared<RobotInterfaceFranka>(
+            configuration.interface_configuration,
+            configuration.ip_address,
+            mode, //None, PositionControl, VelocityControl
+            RobotInterfaceFranka::HAND::OFF
+        );
     }
 
     RobotDriverFranka::~RobotDriverFranka()=default;
 
 
+    void RobotDriverFranka::_update_stiffness_contact_and_pose() const
+    {
+        Vector3d force, torque;
+        std::tie(force, torque) = robot_driver_interface_sptr_->get_stiffness_force_torque();
+        const auto pose = robot_driver_interface_sptr_->get_stiffness_pose();
+        robot_dynamic_provider_->publish_stiffness(pose, force, torque);
+    }
+
+
+
     /**
      * @brief 
      * 
@@ -132,6 +146,7 @@ namespace sas
             ROS_ERROR_STREAM("["+ros::this_node::getName()+"]::driver interface error on:"+robot_driver_interface_sptr_->get_status_message());
             break_loops_->store(true);
         }
+        _update_stiffness_contact_and_pose();
         return robot_driver_interface_sptr_->get_joint_positions();
     }
 

src/sas_robot_driver_coppelia_node.cpp

@@ -39,7 +39,7 @@
 #include <sas_common/sas_common.h>
 #include <sas_conversions/eigen3_std_conversions.h>
 #include <sas_robot_driver/sas_robot_driver_ros.h>
-#include "sas_robot_driver_coppelia.h"
+#include "coppelia/sas_robot_driver_coppelia.h"
 
 /*********************************************
  * SIGNAL HANDLER

src/sas_robot_driver_franka_node.cpp

@@ -40,6 +40,7 @@
 #include <sas_conversions/eigen3_std_conversions.h>
 #include <sas_robot_driver/sas_robot_driver_ros.h>
 #include "sas_robot_driver_franka.h"
+#include "sas_robot_dynamic_provider.h"
 
 
 /*********************************************
@@ -52,6 +53,28 @@ void sig_int_handler(int)
     kill_this_process = true;
 }
 
+template<typename T, std::size_t N>
+std::array<T, N> apply_scale_to_std_array(const std::array<T, N>& array, const T& scale)
+{
+    std::array<T, N> scaled_array;
+    for(std::size_t i = 0; i < N; i++)
+    {
+        scaled_array[i] = array[i] * scale;
+    }
+    return scaled_array;
+}
+template<typename T, std::size_t N>
+std::array<T, N> std_vec_to_std_array(const std::vector<T>& vector)
+{
+    if(N != vector.size()){throw std::runtime_error("Size mismatch between vector and array.");}
+    std::array<T, N> array;
+    for(std::size_t i = 0; i < N; i++)
+    {
+        array[i] = vector[i];
+    }
+    return array;
+}
+
 
 int main(int argc, char **argv)
 {
@@ -69,9 +92,38 @@ int main(int argc, char **argv)
 
     ros::NodeHandle nh;
     sas::RobotDriverFrankaConfiguration robot_driver_franka_configuration;
+    RobotInterfaceFranka::FrankaInterfaceConfiguration franka_interface_configuration;
 
     sas::get_ros_param(nh,"/robot_ip_address",robot_driver_franka_configuration.ip_address);
     sas::get_ros_param(nh,"/robot_mode",      robot_driver_franka_configuration.mode);
+    double upper_scale_factor = 1.0;
+    std::vector<std::string> all_params;
+    if(nh.hasParam(ros::this_node::getName()+"/force_upper_limits_scaling_factor"))
+    {
+        sas::get_ros_param(nh,"/force_upper_limits_scaling_factor",upper_scale_factor);
+        ROS_WARN_STREAM(ros::this_node::getName()+"::Set force upper limits scaling factor: " << upper_scale_factor);
+    }
+    if(nh.hasParam(ros::this_node::getName()+"/upper_torque_threshold")) {
+        ROS_INFO_STREAM(ros::this_node::getName()+"::Upper torque threshold override and set.");
+        std::vector<double> upper_torque_threshold_std_vec;
+        sas::get_ros_param(nh,"/upper_torque_threshold",upper_torque_threshold_std_vec);
+        franka_interface_configuration.upper_torque_threshold = std_vec_to_std_array<double,7>(upper_torque_threshold_std_vec);
+    }else {
+        ROS_INFO_STREAM(ros::this_node::getName()+"::Upper torque threshold not set. Using default with value scalling.");
+        franka_interface_configuration.upper_torque_threshold = apply_scale_to_std_array(franka_interface_configuration.upper_torque_threshold, upper_scale_factor);
+    }
+    if(nh.hasParam(ros::this_node::getName()+"/upper_force_threshold")) {
+        ROS_INFO_STREAM(ros::this_node::getName()+"::Upper force threshold override and set.");
+        std::vector<double> upper_torque_threshold_std_vec;
+        sas::get_ros_param(nh,"/upper_force_threshold",upper_torque_threshold_std_vec);
+        franka_interface_configuration.upper_force_threshold = std_vec_to_std_array<double,6>(upper_torque_threshold_std_vec);
+    }else {
+        ROS_INFO_STREAM(ros::this_node::getName()+"::Upper force threshold not set. Using default with value scalling.");
+        franka_interface_configuration.upper_force_threshold = apply_scale_to_std_array(franka_interface_configuration.upper_force_threshold, upper_scale_factor);
+    }
+
+
+    robot_driver_franka_configuration.interface_configuration = franka_interface_configuration;
 
     sas::RobotDriverROSConfiguration robot_driver_ros_configuration;
 
@@ -79,13 +131,19 @@ int main(int argc, char **argv)
     sas::get_ros_param(nh,"/q_min",robot_driver_ros_configuration.q_min);
     sas::get_ros_param(nh,"/q_max",robot_driver_ros_configuration.q_max);
 
+    // initialize the robot dynamic provider
     robot_driver_ros_configuration.robot_driver_provider_prefix = ros::this_node::getName();
-    
+    sas::RobotDynamicProvider robot_dynamic_provider(nh, robot_driver_ros_configuration.robot_driver_provider_prefix);
+
+
     try
         {
             ROS_INFO_STREAM(ros::this_node::getName()+"::Instantiating Franka robot.");
-            sas::RobotDriverFranka robot_driver_franka(robot_driver_franka_configuration, 
-                                                        &kill_this_process);
+            sas::RobotDriverFranka robot_driver_franka(
+                &robot_dynamic_provider,
+                robot_driver_franka_configuration,
+                &kill_this_process
+            );
             //robot_driver_franka.set_joint_limits({qmin, qmax});
             ROS_INFO_STREAM(ros::this_node::getName()+"::Instantiating RobotDriverROS.");
             sas::RobotDriverROS robot_driver_ros(nh,
@@ -96,6 +154,7 @@ int main(int argc, char **argv)
         }
     catch (const std::exception& e)
     {
+        kill_this_process = true;
         ROS_ERROR_STREAM(ros::this_node::getName() + "::Exception::" + e.what());
     }