added proper binding for python for dynamic

CMakeLists.txt

@@ -25,11 +25,15 @@ find_package(catkin REQUIRED COMPONENTS
   sas_clock
   sas_robot_driver
   sas_patient_side_manager
+        pybind11_catkin
+
 )
 
 catkin_package(
   INCLUDE_DIRS include
   CATKIN_DEPENDS roscpp rospy sas_common sas_clock sas_robot_driver tf2_ros tf2
+        pybind11_catkin
+
 )
 
 find_package(Franka REQUIRED)
@@ -94,7 +98,7 @@ include_directories(
     include
     include/generator
     src/
-    src/dynamic_interface
+        src/robot_dynamics
     src/hand
     src/joint
     ${catkin_INCLUDE_DIRS}
@@ -102,15 +106,20 @@ include_directories(
     )
 
 
-add_library(sas_robot_dynamic_provider src/dynamic_interface/sas_robot_dynamic_provider.cpp)
+add_library(qros_robot_dynamics_provider src/robot_dynamics/qros_robot_dynamics_provider.cpp)
-target_link_libraries(sas_robot_dynamic_provider
+target_link_libraries(qros_robot_dynamics_provider
+        ${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_library(sas_robot_driver_franka src/joint/sas_robot_driver_franka.cpp)
 target_link_libraries(sas_robot_driver_franka
-                    sas_robot_dynamic_provider
+                    qros_robot_dynamics_provider
                     dqrobotics
                     robot_interface_franka)
 
@@ -160,6 +169,20 @@ target_link_libraries(JuankaEmika  PRIVATE Qt${QT_VERSION_MAJOR}::Widgets
 
 
 
+#####################################################################################
+# python binding
+include_directories(
+        include/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
+)
+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)
     qt_finalize_executable(JuankaEmika)
@@ -187,3 +210,10 @@ install(DIRECTORY include/${PROJECT_NAME}/
   FILES_MATCHING PATTERN "*.h"
 #   PATTERN ".svn" EXCLUDE
 )                        
+
+
+install(TARGETS _qros_robot_dynamic
+        LIBRARY DESTINATION ${PYTHON_INSTALL_DIR}
+)
+
+catkin_python_setup()

include/robot_dynamic/qros_robot_dynamics_interface.h

@@ -0,0 +1,93 @@
+#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

include/robot_dynamic/qros_robot_dynamics_provider.h

@@ -19,7 +19,7 @@
 #
 # ################################################################
 #
-#   Author: Juan Jose Quiroz Omana, email: juanjqogm@gmail.com
+#   Author: Quenitin Lin
 #
 # ################################################################
 #
@@ -62,11 +62,20 @@ private:
     static geometry_msgs::Transform _dq_to_geometry_msgs_transform(const DQ& pose) ;
 
 public:
-    RobotDynamicProvider(ros::NodeHandle& nodehandle, const std::string& node_prefix=ros::this_node::getName());
+    RobotDynamicProvider() = delete;
+    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);
 
+
+    bool is_enabled() const;
+    std::string get_topic_prefix() const {return node_prefix_;}
+
 };
 
 

include/sas_robot_driver_franka.h

@@ -43,8 +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"
+#include <robot_dynamic/qros_robot_dynamics_provider.h>
-#include <thread>
 
 using namespace DQ_robotics;
 using namespace Eigen;

package.xml

@@ -59,6 +59,7 @@
   <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_export_depend>roscpp</build_export_depend>
   <build_export_depend>rospy</build_export_depend>
@@ -69,6 +70,7 @@
   <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>

setup.py

@@ -0,0 +1,12 @@
+# ! 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/robot_dynamics/qros_robot_dynamic_py.cpp

@@ -0,0 +1,59 @@
+/*
+# 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 <robot_dynamic/qros_robot_dynamics_provider.h>
+#include <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("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

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

@@ -27,7 +27,7 @@
 #
 # ################################################################
 */
-#include "sas_robot_dynamic_provider.h"
+#include <robot_dynamic/qros_robot_dynamics_provider.h>
 
 using namespace qros;
 
@@ -91,3 +91,9 @@ void RobotDynamicProvider::publish_stiffness(const DQ& base_to_stiffness, const
     }
 }
 
+
+bool RobotDynamicProvider::is_enabled() const
+{
+    return true; //Always enabled
+}
+

src/sas_robot_driver_franka/__init__.py

@@ -0,0 +1,3 @@
+"""
+"""
+from _qros_robot_dynamic import RobotDynamicsInterface, RobotDynamicsProvider

src/sas_robot_driver_franka_node.cpp

@@ -40,7 +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"
+#include <robot_dynamic/qros_robot_dynamics_provider.h>
 
 
 /*********************************************