added cartesian interface

CMakeLists.txt

@@ -91,16 +91,21 @@ target_link_libraries(robot_interface_hand Franka::Franka
 include_directories(
     include
     src/
+    src/dynamic_interface
     ${catkin_INCLUDE_DIRS}
     constraints_manager/include
     )
 
 
+add_library(sas_robot_dynamic_provider src/dynamic_interface/sas_robot_dynamic_provider.cpp)
+target_link_libraries(sas_robot_dynamic_provider dqrobotics)
+
 
 add_library(sas_robot_driver_franka src/sas_robot_driver_franka.cpp)
 target_link_libraries(sas_robot_driver_franka
-                      dqrobotics
+                    sas_robot_dynamic_provider
-                      robot_interface_franka)
+                    dqrobotics
+                    robot_interface_franka)
 
 add_library(sas_robot_driver_franka_hand src/sas_robot_driver_franka_hand.cpp)
 target_link_libraries(sas_robot_driver_franka_hand

include/robot_interface_franka.h

@@ -78,6 +78,13 @@ private:
     VectorXd current_joint_forces_;
     std::array<double, 7> current_joint_forces_array_;
 
+    Vector3d current_cartesian_force_;
+    Vector3d current_cartesian_torque_;
+    std::array<double, 6> current_cartesian_force_torque_array_;
+
+    VectorXd current_cartesian_pose_;
+    std::array<double, 16> current_cartesian_pose_array_;
+
     franka::RobotMode robot_mode_;
 
     double time_ = 0;
@@ -187,6 +194,9 @@ public:
     VectorXd get_joint_velocities();
     VectorXd get_joint_forces();
 
+    std::tuple<Vector3d, Vector3d> get_cartesian_force_torque();
+    DQ get_cartesian_pose();
+
     double get_time();
 
     void set_target_joint_positions(const VectorXd& set_target_joint_positions_rad);

include/sas_robot_driver_franka.h

@@ -43,6 +43,7 @@
 #include <sas_robot_driver/sas_robot_driver.h>
 #include "robot_interface_franka.h"
 #include <ros/common.h>
+#include "sas_robot_dynamic_provider.h"
 
 using namespace DQ_robotics;
 using namespace Eigen;
@@ -67,6 +68,8 @@ private:
 
     std::shared_ptr<RobotInterfaceFranka> robot_driver_interface_sptr_ = nullptr;
 
+    RobotDynamicProvider* robot_dynamic_provider_;
+
     //Joint positions
     VectorXd joint_positions_;
     //VectorXd joint_velocities_;
@@ -77,6 +80,9 @@ private:
 
     std::atomic_bool* break_loops_;
 
+    // hotfix function to update cartesian contact and pose information
+    void _update_cartesian_contact_and_pose() const;
+
 
 public:
     //const static int SLAVE_MODE_JOINT_CONTROL;
@@ -86,7 +92,11 @@ public:
     RobotDriverFranka()=delete;
     ~RobotDriverFranka();
 
-    RobotDriverFranka(const RobotDriverFrankaConfiguration& configuration, std::atomic_bool* break_loops);
+    RobotDriverFranka(
+        RobotDynamicProvider* robot_dynamic_provider,
+        const RobotDriverFrankaConfiguration& configuration,
+        std::atomic_bool* break_loops
+        );
 
 
     VectorXd get_joint_positions() override;

src/dynamic_interface/sas_robot_dynamic_provider.cpp

@@ -0,0 +1,65 @@
+/*
+# 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_contact_ = publisher_nodehandle.advertise<geometry_msgs::WrenchStamped>(node_prefix + "/get/cartesian_contact", 1);
+    publisher_cartesian_pose_ = publisher_nodehandle.advertise<geometry_msgs::PoseStamped>(node_prefix + "/get/cartesian_pose", 1);
+
+}
+
+
+void RobotDynamicProvider::publish_cartesian_contact(const Vector3d& force, const Vector3d& torque)
+{
+    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_contact_.publish(msg);
+}
+void RobotDynamicProvider::publish_cartesian_pose(const DQ& pose)
+{
+    publisher_cartesian_pose_.publish(dq_to_geometry_msgs_pose_stamped(pose));
+}

src/dynamic_interface/sas_robot_dynamic_provider.h

@@ -0,0 +1,32 @@
+#pragma once
+#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_contact_;
+    ros::Publisher publisher_cartesian_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_cartesian_contact(const Vector3d& force, const Vector3d& torque);
+    void publish_cartesian_pose(const DQ& pose);
+
+};
+
+
+
+} // namespace sas

src/robot_interface_franka.cpp

@@ -351,6 +351,13 @@ 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_cartesian_force_torque_array_ = robot_state.cartesian_contact;
+    current_cartesian_force_ = Eigen::Map<Vector3d>(current_cartesian_force_torque_array_.data(), 3);
+    current_cartesian_torque_ = Eigen::Map<Vector3d>(current_cartesian_force_torque_array_.data()+3, 3);
+
+    current_cartesian_pose_array_ = robot_state.O_T_EE;
+    current_cartesian_pose_ = Eigen::Map<VectorXd>(current_cartesian_pose_array_.data(), 16);
+
     robot_mode_ = robot_state.robot_mode;
     time_ = time;
 }
@@ -674,6 +681,29 @@ VectorXd RobotInterfaceFranka::get_joint_forces()
 }
 
 
+std::tuple<Vector3d, Vector3d> RobotInterfaceFranka::get_cartesian_force_torque()
+{
+    _check_if_robot_is_connected();
+    return std::make_tuple(current_cartesian_force_, current_cartesian_torque_);
+
+}
+
+
+DQ RobotInterfaceFranka::get_cartesian_pose()
+{
+    _check_if_robot_is_connected();
+    // VectorXd current_cartesian_pose_;
+    const auto t = DQ(0, current_cartesian_pose_[3], current_cartesian_pose_[7], current_cartesian_pose_[11]);
+    Matrix3d rotationMatrix;
+    rotationMatrix << current_cartesian_pose_(0), current_cartesian_pose_(1), current_cartesian_pose_(2),
+                      current_cartesian_pose_(4), current_cartesian_pose_(5), current_cartesian_pose_(6),
+                      current_cartesian_pose_(8), current_cartesian_pose_(9), current_cartesian_pose_(10);
+    Quaterniond quaternion(rotationMatrix);
+    const auto r = DQ(quaternion.w(), quaternion.x(), quaternion.y(), quaternion.z());
+    return r + 0.5 * E_ * t * r;
+}
+
+
 /**
  * @brief robot_driver_franka::get_time
  * @return

src/sas_robot_driver_franka.cpp

@@ -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);
@@ -81,6 +82,17 @@ namespace sas
     RobotDriverFranka::~RobotDriverFranka()=default;
 
 
+    void RobotDriverFranka::_update_cartesian_contact_and_pose() const
+    {
+        Vector3d force, torque;
+        std::tie(force, torque) = robot_driver_interface_sptr_->get_cartesian_force_torque();
+        const auto pose = robot_driver_interface_sptr_->get_cartesian_pose();
+        robot_dynamic_provider_->publish_cartesian_contact(force, torque);
+        robot_dynamic_provider_->publish_cartesian_pose(pose);
+    }
+
+
+
     /**
      * @brief 
      * 
@@ -132,6 +144,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_cartesian_contact_and_pose();
         return robot_driver_interface_sptr_->get_joint_positions();
     }
 

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"
 
 
 /*********************************************
@@ -80,12 +81,16 @@ int main(int argc, char **argv)
     sas::get_ros_param(nh,"/q_max",robot_driver_ros_configuration.q_max);
 
     robot_driver_ros_configuration.robot_driver_provider_prefix = ros::this_node::getName();
+    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, 
+            sas::RobotDriverFranka robot_driver_franka(
-                                                        &kill_this_process);
+                &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 +101,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());
     }