added proper binding for python for dynamic

src/dynamic_interface/sas_robot_dynamic_provider.h

@@ -1,74 +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: 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/Transform.h>
-#include <geometry_msgs/TransformStamped.h>
-#include <tf2_ros/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 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_;
-
-
-    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(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);
-
-};
-
-
-
-} // namespace sas

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>
 
 
 /*********************************************