diff --git a/CMakeLists.txt b/CMakeLists.txt index a14ff2d..a204721 100644 --- a/CMakeLists.txt +++ b/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 - robot_interface_franka) + sas_robot_dynamic_provider + 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 @@ -123,7 +128,7 @@ target_link_libraries(sas_robot_driver_coppelia_node add_executable(sas_robot_driver_franka_node src/sas_robot_driver_franka_node.cpp) target_link_libraries(sas_robot_driver_franka_node - sas_robot_driver_franka + sas_robot_driver_franka ${catkin_LIBRARIES}) diff --git a/include/robot_interface_franka.h b/include/robot_interface_franka.h index 8bbc794..2f5cbb5 100644 --- a/include/robot_interface_franka.h +++ b/include/robot_interface_franka.h @@ -78,6 +78,13 @@ private: VectorXd current_joint_forces_; std::array current_joint_forces_array_; + Vector3d current_cartesian_force_; + Vector3d current_cartesian_torque_; + std::array current_cartesian_force_torque_array_; + + VectorXd current_cartesian_pose_; + std::array 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 get_cartesian_force_torque(); + DQ get_cartesian_pose(); + double get_time(); void set_target_joint_positions(const VectorXd& set_target_joint_positions_rad); diff --git a/include/sas_robot_driver_franka.h b/include/sas_robot_driver_franka.h index 6739331..45f1420 100644 --- a/include/sas_robot_driver_franka.h +++ b/include/sas_robot_driver_franka.h @@ -43,6 +43,7 @@ #include #include "robot_interface_franka.h" #include +#include "sas_robot_dynamic_provider.h" using namespace DQ_robotics; using namespace Eigen; @@ -67,6 +68,8 @@ private: std::shared_ptr 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; diff --git a/src/dynamic_interface/sas_robot_dynamic_provider.cpp b/src/dynamic_interface/sas_robot_dynamic_provider.cpp new file mode 100644 index 0000000..702c9b4 --- /dev/null +++ b/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 . +# +# ################################################################ +# +# 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(node_prefix + "/get/cartesian_contact", 1); + publisher_cartesian_pose_ = publisher_nodehandle.advertise(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)); +} diff --git a/src/dynamic_interface/sas_robot_dynamic_provider.h b/src/dynamic_interface/sas_robot_dynamic_provider.h new file mode 100644 index 0000000..79ce033 --- /dev/null +++ b/src/dynamic_interface/sas_robot_dynamic_provider.h @@ -0,0 +1,32 @@ +#pragma once +#include +#include +#include +#include +#include +#include +#include + +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 \ No newline at end of file diff --git a/src/robot_interface_franka.cpp b/src/robot_interface_franka.cpp index 3de52e8..10f630a 100644 --- a/src/robot_interface_franka.cpp +++ b/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(current_joint_forces_array_.data(), 7); + current_cartesian_force_torque_array_ = robot_state.cartesian_contact; + current_cartesian_force_ = Eigen::Map(current_cartesian_force_torque_array_.data(), 3); + current_cartesian_torque_ = Eigen::Map(current_cartesian_force_torque_array_.data()+3, 3); + + current_cartesian_pose_array_ = robot_state.O_T_EE; + current_cartesian_pose_ = Eigen::Map(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 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 diff --git a/src/sas_robot_driver_franka.cpp b/src/sas_robot_driver_franka.cpp index 95ad9c2..8b8b7d8 100644 --- a/src/sas_robot_driver_franka.cpp +++ b/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(); } diff --git a/src/sas_robot_driver_franka_node.cpp b/src/sas_robot_driver_franka_node.cpp index 6359b37..bce0b2f 100644 --- a/src/sas_robot_driver_franka_node.cpp +++ b/src/sas_robot_driver_franka_node.cpp @@ -40,6 +40,7 @@ #include #include #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, - &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 +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()); }