From 46668deac21b319638e98709019bbdcb7caf6c47 Mon Sep 17 00:00:00 2001 From: qlin1806 Date: Sat, 20 Jul 2024 18:24:23 -0700 Subject: [PATCH] added proper binding for python for dynamic --- CMakeLists.txt | 38 ++++++- .../qros_robot_dynamics_interface.h | 93 ++++++++++++++++ .../qros_robot_dynamics_provider.h | 13 ++- include/sas_robot_driver_franka.h | 3 +- package.xml | 2 + setup.py | 12 +++ src/robot_dynamics/qros_robot_dynamic_py.cpp | 59 ++++++++++ .../qros_robot_dynamics_interface.cpp | 101 ++++++++++++++++++ .../qros_robot_dynamics_provider.cpp} | 8 +- src/sas_robot_driver_franka/__init__.py | 3 + src/sas_robot_driver_franka_node.cpp | 2 +- 11 files changed, 324 insertions(+), 10 deletions(-) create mode 100644 include/robot_dynamic/qros_robot_dynamics_interface.h rename src/dynamic_interface/sas_robot_dynamic_provider.h => include/robot_dynamic/qros_robot_dynamics_provider.h (81%) create mode 100644 setup.py create mode 100644 src/robot_dynamics/qros_robot_dynamic_py.cpp create mode 100644 src/robot_dynamics/qros_robot_dynamics_interface.cpp rename src/{dynamic_interface/sas_robot_dynamic_provider.cpp => robot_dynamics/qros_robot_dynamics_provider.cpp} (95%) create mode 100644 src/sas_robot_driver_franka/__init__.py diff --git a/CMakeLists.txt b/CMakeLists.txt index 1f319f4..f96e5b6 100644 --- a/CMakeLists.txt +++ b/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) -target_link_libraries(sas_robot_dynamic_provider +add_library(qros_robot_dynamics_provider src/robot_dynamics/qros_robot_dynamics_provider.cpp) +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() diff --git a/include/robot_dynamic/qros_robot_dynamics_interface.h b/include/robot_dynamic/qros_robot_dynamics_interface.h new file mode 100644 index 0000000..d883db4 --- /dev/null +++ b/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 . +# +# ################################################################ +# +# Author: Quenitin Lin +# +# ################################################################ +# +# Contributors: +# 1. Quenitin Lin +# +# ################################################################ +*/ +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#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 \ No newline at end of file diff --git a/src/dynamic_interface/sas_robot_dynamic_provider.h b/include/robot_dynamic/qros_robot_dynamics_provider.h similarity index 81% rename from src/dynamic_interface/sas_robot_dynamic_provider.h rename to include/robot_dynamic/qros_robot_dynamics_provider.h index cb63117..6bc21a7 100644 --- a/src/dynamic_interface/sas_robot_dynamic_provider.h +++ b/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_;} + }; diff --git a/include/sas_robot_driver_franka.h b/include/sas_robot_driver_franka.h index ad27e2d..61a4090 100644 --- a/include/sas_robot_driver_franka.h +++ b/include/sas_robot_driver_franka.h @@ -43,8 +43,7 @@ #include #include "robot_interface_franka.h" #include -#include "sas_robot_dynamic_provider.h" -#include +#include using namespace DQ_robotics; using namespace Eigen; diff --git a/package.xml b/package.xml index e77d4bc..0b00c45 100644 --- a/package.xml +++ b/package.xml @@ -59,6 +59,7 @@ sas_robot_driver sas_common sas_patient_side_manager + pybind11_catkin roscpp rospy @@ -69,6 +70,7 @@ sas_robot_driver sas_common sas_patient_side_manager + pybind11_catkin roscpp diff --git a/setup.py b/setup.py new file mode 100644 index 0000000..1d7a146 --- /dev/null +++ b/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) diff --git a/src/robot_dynamics/qros_robot_dynamic_py.cpp b/src/robot_dynamics/qros_robot_dynamic_py.cpp new file mode 100644 index 0000000..3c50652 --- /dev/null +++ b/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 . +# +# ################################################################ +# +# Author: Quenitin Lin +# +# ################################################################ +# +# Contributors: +# 1. Quenitin Lin +# +# ################################################################ +*/ + +#include +#include +#include + +#include +#include + +namespace py = pybind11; +using RDI = qros::RobotDynamicInterface; +using RDP = qros::RobotDynamicProvider; + + +PYBIND11_MODULE(_qros_robot_dynamic, m) +{ + py::class_(m, "RobotDynamicsInterface") + .def(py::init()) + .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_(m, "RobotDynamicsProvider") + .def(py::init()) + .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); + +} diff --git a/src/robot_dynamics/qros_robot_dynamics_interface.cpp b/src/robot_dynamics/qros_robot_dynamics_interface.cpp new file mode 100644 index 0000000..0e4d21a --- /dev/null +++ b/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 . +# +# ################################################################ +# +# Author: Juan Jose Quiroz Omana, email: juanjqogm@gmail.com +# +# ################################################################ +# +# Contributors: +# 1. Quenitin Lin +# +# ################################################################ +*/ +#include + +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; +} \ No newline at end of file diff --git a/src/dynamic_interface/sas_robot_dynamic_provider.cpp b/src/robot_dynamics/qros_robot_dynamics_provider.cpp similarity index 95% rename from src/dynamic_interface/sas_robot_dynamic_provider.cpp rename to src/robot_dynamics/qros_robot_dynamics_provider.cpp index 0f29ac3..8dc555e 100644 --- a/src/dynamic_interface/sas_robot_dynamic_provider.cpp +++ b/src/robot_dynamics/qros_robot_dynamics_provider.cpp @@ -27,7 +27,7 @@ # # ################################################################ */ -#include "sas_robot_dynamic_provider.h" +#include 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 +} + diff --git a/src/sas_robot_driver_franka/__init__.py b/src/sas_robot_driver_franka/__init__.py new file mode 100644 index 0000000..4da9f7e --- /dev/null +++ b/src/sas_robot_driver_franka/__init__.py @@ -0,0 +1,3 @@ +""" +""" +from _qros_robot_dynamic import RobotDynamicsInterface, RobotDynamicsProvider diff --git a/src/sas_robot_driver_franka_node.cpp b/src/sas_robot_driver_franka_node.cpp index 94ef0d3..a14ff77 100644 --- a/src/sas_robot_driver_franka_node.cpp +++ b/src/sas_robot_driver_franka_node.cpp @@ -40,7 +40,7 @@ #include #include #include "sas_robot_driver_franka.h" -#include "sas_robot_dynamic_provider.h" +#include /*********************************************