From e263d34c0c9dcd0e448d3fa9feb564417868fdf2 Mon Sep 17 00:00:00 2001 From: qlin960618 <31228437+qlin960618@users.noreply.github.com> Date: Fri, 19 Jul 2024 22:34:54 -0700 Subject: [PATCH] Echo contact (#1) * added cartesian interface * general fix and optimization --- CMakeLists.txt | 35 ++++++---- cfg/sas_robot_driver_franka_1.yaml | 4 +- cfg/sas_robot_driver_franka_2.yaml | 8 --- cfg/sas_robot_driver_franka_3.yaml | 8 --- cfg/sas_robot_driver_franka_4.yaml | 8 --- .../custom_motion_generation.h | 0 include/{ => generator}/motion_generator.h | 0 .../quadratic_program_motion_generator.h | 0 include/sas_robot_driver_franka.h | 16 ++++- qt/configuration_window/mainwindow.cpp | 10 ++- .../sas_robot_driver_coppelia.cpp | 0 .../coppelia}/sas_robot_driver_coppelia.h | 0 .../sas_robot_dynamic_provider.cpp | 63 ++++++++++++++++++ .../sas_robot_dynamic_provider.h | 60 +++++++++++++++++ .../custom_motion_generation.cpp | 4 +- src/{ => generator}/motion_generator.cpp | 2 +- .../quadratic_program_motion_generator.cpp | 2 +- src/{ => hand}/robot_interface_hand.cpp | 0 .../sas_robot_driver_franka_hand.cpp | 33 ++++++++-- src/{ => hand}/sas_robot_driver_franka_hand.h | 29 +++++++++ src/{ => joint}/robot_interface_franka.cpp | 52 ++++++++++++++- .../joint}/robot_interface_franka.h | 54 +++++++++++++-- src/{ => joint}/sas_robot_driver_franka.cpp | 25 +++++-- src/sas_robot_driver_coppelia_node.cpp | 2 +- src/sas_robot_driver_franka_node.cpp | 65 ++++++++++++++++++- 25 files changed, 412 insertions(+), 68 deletions(-) delete mode 100644 cfg/sas_robot_driver_franka_2.yaml delete mode 100644 cfg/sas_robot_driver_franka_3.yaml delete mode 100644 cfg/sas_robot_driver_franka_4.yaml rename include/{ => generator}/custom_motion_generation.h (100%) rename include/{ => generator}/motion_generator.h (100%) rename include/{ => generator}/quadratic_program_motion_generator.h (100%) rename src/{ => coppelia}/sas_robot_driver_coppelia.cpp (100%) rename {include => src/coppelia}/sas_robot_driver_coppelia.h (100%) create mode 100644 src/dynamic_interface/sas_robot_dynamic_provider.cpp create mode 100644 src/dynamic_interface/sas_robot_dynamic_provider.h rename src/{ => generator}/custom_motion_generation.cpp (99%) rename src/{ => generator}/motion_generator.cpp (99%) rename src/{ => generator}/quadratic_program_motion_generator.cpp (98%) rename src/{ => hand}/robot_interface_hand.cpp (100%) rename src/{ => hand}/sas_robot_driver_franka_hand.cpp (77%) rename src/{ => hand}/sas_robot_driver_franka_hand.h (63%) rename src/{ => joint}/robot_interface_franka.cpp (92%) rename {include => src/joint}/robot_interface_franka.h (74%) rename src/{ => joint}/sas_robot_driver_franka.cpp (86%) diff --git a/CMakeLists.txt b/CMakeLists.txt index a14ff2d..277d16a 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -49,25 +49,25 @@ find_package(QT NAMES Qt6 Qt5 REQUIRED COMPONENTS Widgets) find_package(Qt${QT_VERSION_MAJOR} REQUIRED COMPONENTS Widgets) -add_library(MotionGenerator src/motion_generator.cpp) +add_library(MotionGenerator src/generator/motion_generator.cpp) target_link_libraries(MotionGenerator Franka::Franka) add_library(ConstraintsManager constraints_manager/src/constraints_manager.cpp) -add_library(QuadraticProgramMotionGenerator src/quadratic_program_motion_generator.cpp) +add_library(QuadraticProgramMotionGenerator src/generator/quadratic_program_motion_generator.cpp) target_link_libraries(QuadraticProgramMotionGenerator qpOASES dqrobotics ConstraintsManager) -add_library(CustomMotionGeneration src/custom_motion_generation.cpp) +add_library(CustomMotionGeneration src/generator/custom_motion_generation.cpp) target_link_libraries(CustomMotionGeneration qpOASES dqrobotics ConstraintsManager) -add_library(robot_interface_franka src/robot_interface_franka.cpp) +add_library(robot_interface_franka src/joint/robot_interface_franka.cpp) target_link_libraries(robot_interface_franka Franka::Franka dqrobotics MotionGenerator @@ -75,7 +75,7 @@ target_link_libraries(robot_interface_franka Franka::Franka QuadraticProgramMotionGenerator CustomMotionGeneration) -add_library(robot_interface_hand src/robot_interface_hand.cpp) +add_library(robot_interface_hand src/hand/robot_interface_hand.cpp) target_link_libraries(robot_interface_hand Franka::Franka dqrobotics) @@ -90,25 +90,33 @@ target_link_libraries(robot_interface_hand Franka::Franka include_directories( include + include/generator src/ + src/dynamic_interface + src/hand + src/joint ${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) + +add_library(sas_robot_driver_franka src/joint/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) +add_library(sas_robot_driver_franka_hand src/hand/sas_robot_driver_franka_hand.cpp) target_link_libraries(sas_robot_driver_franka_hand dqrobotics Franka::Franka) -add_library(sas_robot_driver_coppelia src/sas_robot_driver_coppelia.cpp) +add_library(sas_robot_driver_coppelia src/coppelia/sas_robot_driver_coppelia.cpp) target_link_libraries(sas_robot_driver_coppelia dqrobotics dqrobotics-interface-vrep) @@ -123,7 +131,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}) @@ -142,7 +150,6 @@ add_executable(JuankaEmika target_link_libraries(JuankaEmika PRIVATE Qt${QT_VERSION_MAJOR}::Widgets dqrobotics - ${catkin_LIBRARIES} robot_interface_franka ) @@ -166,6 +173,10 @@ install(TARGETS sas_robot_driver_coppelia_node DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} ) +install(TARGETS sas_robot_driver_franka_hand_node +DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) + ## Mark cpp header files for installation install(DIRECTORY include/${PROJECT_NAME}/ DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} diff --git a/cfg/sas_robot_driver_franka_1.yaml b/cfg/sas_robot_driver_franka_1.yaml index 95e85b3..1626053 100644 --- a/cfg/sas_robot_driver_franka_1.yaml +++ b/cfg/sas_robot_driver_franka_1.yaml @@ -5,4 +5,6 @@ "thread_sampling_time_nsec": 4000000 "q_min": [-2.3093,-1.5133,-2.4937, -2.7478,-2.4800, 0.8521, -2.6895] "q_max": [ 2.3093, 1.5133, 2.4937, -0.4461, 2.4800, 4.2094, 2.6895] - +"force_upper_limits_scaling_factor": 4.0 +"upper_torque_threshold": [40.0, 40.0, 40.0, 40.0, 40.0, 40.0, 40.0] +"upper_force_threshold": [40.0, 40.0, 40.0, 40.0, 40.0, 40.0] diff --git a/cfg/sas_robot_driver_franka_2.yaml b/cfg/sas_robot_driver_franka_2.yaml deleted file mode 100644 index 228729e..0000000 --- a/cfg/sas_robot_driver_franka_2.yaml +++ /dev/null @@ -1,8 +0,0 @@ -"robot_ip_address": "172.16.0.3" -"robot_mode": "PositionControl" -"robot_port": 5007 -"robot_speed": 20.0 -"thread_sampling_time_nsec": 4000000 -"q_min": [-2.3093,-1.5133,-2.4937, -2.7478,-2.4800, 0.8521, -2.6895] -"q_max": [ 2.3093, 1.5133, 2.4937, -0.4461, 2.4800, 4.2094, 2.6895] - diff --git a/cfg/sas_robot_driver_franka_3.yaml b/cfg/sas_robot_driver_franka_3.yaml deleted file mode 100644 index 4a95f7c..0000000 --- a/cfg/sas_robot_driver_franka_3.yaml +++ /dev/null @@ -1,8 +0,0 @@ -"robot_ip_address": "172.16.0.4" -"robot_mode": "PositionControl" -"robot_port": 5007 -"robot_speed": 20.0 -"thread_sampling_time_nsec": 4000000 -"q_min": [-2.3093,-1.5133,-2.4937, -2.7478,-2.4800, 0.8521, -2.6895] -"q_max": [ 2.3093, 1.5133, 2.4937, -0.4461, 2.4800, 4.2094, 2.6895] - diff --git a/cfg/sas_robot_driver_franka_4.yaml b/cfg/sas_robot_driver_franka_4.yaml deleted file mode 100644 index 8c81933..0000000 --- a/cfg/sas_robot_driver_franka_4.yaml +++ /dev/null @@ -1,8 +0,0 @@ -"robot_ip_address": "172.16.0.5" -"robot_mode": "PositionControl" -"robot_port": 5007 -"robot_speed": 20.0 -"thread_sampling_time_nsec": 4000000 -"q_min": [-2.3093,-1.5133,-2.4937, -2.7478,-2.4800, 0.8521, -2.6895] -"q_max": [ 2.3093, 1.5133, 2.4937, -0.4461, 2.4800, 4.2094, 2.6895] - diff --git a/include/custom_motion_generation.h b/include/generator/custom_motion_generation.h similarity index 100% rename from include/custom_motion_generation.h rename to include/generator/custom_motion_generation.h diff --git a/include/motion_generator.h b/include/generator/motion_generator.h similarity index 100% rename from include/motion_generator.h rename to include/generator/motion_generator.h diff --git a/include/quadratic_program_motion_generator.h b/include/generator/quadratic_program_motion_generator.h similarity index 100% rename from include/quadratic_program_motion_generator.h rename to include/generator/quadratic_program_motion_generator.h diff --git a/include/sas_robot_driver_franka.h b/include/sas_robot_driver_franka.h index 6739331..ef43158 100644 --- a/include/sas_robot_driver_franka.h +++ b/include/sas_robot_driver_franka.h @@ -43,20 +43,23 @@ #include #include "robot_interface_franka.h" #include +#include "sas_robot_dynamic_provider.h" using namespace DQ_robotics; using namespace Eigen; +struct RobotInterfaceFranka::FrankaInterfaceConfiguration; // Forward declaration + namespace sas { - struct RobotDriverFrankaConfiguration { std::string ip_address; std::string mode; int port; double speed; + RobotInterfaceFranka::FrankaInterfaceConfiguration interface_configuration; }; @@ -67,6 +70,8 @@ private: std::shared_ptr robot_driver_interface_sptr_ = nullptr; + RobotDynamicProvider* robot_dynamic_provider_; + //Joint positions VectorXd joint_positions_; //VectorXd joint_velocities_; @@ -77,6 +82,9 @@ private: std::atomic_bool* break_loops_; + // hotfix function to update cartesian contact and pose information + void _update_stiffness_contact_and_pose() const; + public: //const static int SLAVE_MODE_JOINT_CONTROL; @@ -86,7 +94,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/qt/configuration_window/mainwindow.cpp b/qt/configuration_window/mainwindow.cpp index 6e1bd2d..dd5d238 100644 --- a/qt/configuration_window/mainwindow.cpp +++ b/qt/configuration_window/mainwindow.cpp @@ -216,9 +216,13 @@ void MainWindow::on_pushButton_connect_clicked() if (!robot_driver_franka_sptr_) { //qDebug()<<"Create Pointer "; - robot_driver_franka_sptr_ = std::make_shared(robotIp_std, - MODE, - RobotInterfaceFranka::HAND::ON); + RobotInterfaceFranka::FrankaInterfaceConfiguration default_config; + robot_driver_franka_sptr_ = std::make_shared( + default_config, + robotIp_std, + MODE, + RobotInterfaceFranka::HAND::ON + ); } diff --git a/src/sas_robot_driver_coppelia.cpp b/src/coppelia/sas_robot_driver_coppelia.cpp similarity index 100% rename from src/sas_robot_driver_coppelia.cpp rename to src/coppelia/sas_robot_driver_coppelia.cpp diff --git a/include/sas_robot_driver_coppelia.h b/src/coppelia/sas_robot_driver_coppelia.h similarity index 100% rename from include/sas_robot_driver_coppelia.h rename to src/coppelia/sas_robot_driver_coppelia.h 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..30c0bd9 --- /dev/null +++ b/src/dynamic_interface/sas_robot_dynamic_provider.cpp @@ -0,0 +1,63 @@ +/* +# 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_stiffness_ = publisher_nodehandle.advertise(node_prefix + "/get/cartesian_stiffness", 1); + publisher_stiffness_pose_ = publisher_nodehandle.advertise(node_prefix + "/get/stiffness_pose", 1); + +} + + +void RobotDynamicProvider::publish_stiffness(const DQ& base_to_stiffness, const Vector3d& force, const Vector3d& torque) const +{ + publisher_stiffness_pose_.publish(dq_to_geometry_msgs_pose_stamped(base_to_stiffness)); + 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_stiffness_.publish(msg); +} + 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..81c030c --- /dev/null +++ b/src/dynamic_interface/sas_robot_dynamic_provider.h @@ -0,0 +1,60 @@ +#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: Juan Jose Quiroz Omana, email: juanjqogm@gmail.com +# +# ################################################################ +# +# Contributors: +# 1. Quenitin Lin +# +# ################################################################ +*/ +#include +#include +#include +#include +#include +#include +#include + +namespace sas { + +using namespace DQ_robotics; + +class RobotDynamicProvider { +private: + std::string node_prefix_; + + ros::Publisher publisher_cartesian_stiffness_; + ros::Publisher publisher_stiffness_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) const; + +}; + + + +} // namespace sas \ No newline at end of file diff --git a/src/custom_motion_generation.cpp b/src/generator/custom_motion_generation.cpp similarity index 99% rename from src/custom_motion_generation.cpp rename to src/generator/custom_motion_generation.cpp index b6c84b8..a4ea61a 100644 --- a/src/custom_motion_generation.cpp +++ b/src/generator/custom_motion_generation.cpp @@ -28,9 +28,7 @@ # # ################################################################ */ - - -#include "custom_motion_generation.h" +#include "generator/custom_motion_generation.h" /** * @brief CustomMotionGeneration::CustomMotionGeneration diff --git a/src/motion_generator.cpp b/src/generator/motion_generator.cpp similarity index 99% rename from src/motion_generator.cpp rename to src/generator/motion_generator.cpp index f323020..8318f92 100644 --- a/src/motion_generator.cpp +++ b/src/generator/motion_generator.cpp @@ -1,6 +1,6 @@ // Copyright (c) 2017 Franka Emika GmbH // Use of this source code is governed by the Apache-2.0 license, see LICENSE -#include "motion_generator.h" +#include "generator/motion_generator.h" #include #include #include diff --git a/src/quadratic_program_motion_generator.cpp b/src/generator/quadratic_program_motion_generator.cpp similarity index 98% rename from src/quadratic_program_motion_generator.cpp rename to src/generator/quadratic_program_motion_generator.cpp index 9700246..448a8d8 100644 --- a/src/quadratic_program_motion_generator.cpp +++ b/src/generator/quadratic_program_motion_generator.cpp @@ -1,4 +1,4 @@ -#include "quadratic_program_motion_generator.h" +#include "generator/quadratic_program_motion_generator.h" QuadraticProgramMotionGenerator::QuadraticProgramMotionGenerator(const double &speed_factor, diff --git a/src/robot_interface_hand.cpp b/src/hand/robot_interface_hand.cpp similarity index 100% rename from src/robot_interface_hand.cpp rename to src/hand/robot_interface_hand.cpp diff --git a/src/sas_robot_driver_franka_hand.cpp b/src/hand/sas_robot_driver_franka_hand.cpp similarity index 77% rename from src/sas_robot_driver_franka_hand.cpp rename to src/hand/sas_robot_driver_franka_hand.cpp index 3cbefaf..5492425 100644 --- a/src/sas_robot_driver_franka_hand.cpp +++ b/src/hand/sas_robot_driver_franka_hand.cpp @@ -1,7 +1,32 @@ -// -// Created by qlin on 7/17/24. -// - +/* +# 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_driver_franka_hand.h" namespace sas { diff --git a/src/sas_robot_driver_franka_hand.h b/src/hand/sas_robot_driver_franka_hand.h similarity index 63% rename from src/sas_robot_driver_franka_hand.h rename to src/hand/sas_robot_driver_franka_hand.h index ed17cc9..6c8c9cd 100644 --- a/src/sas_robot_driver_franka_hand.h +++ b/src/hand/sas_robot_driver_franka_hand.h @@ -1,4 +1,33 @@ #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: Juan Jose Quiroz Omana, email: juanjqogm@gmail.com +# +# ################################################################ +# +# Contributors: +# 1. Quenitin Lin +# +# ################################################################ +*/ #include #include #include diff --git a/src/robot_interface_franka.cpp b/src/joint/robot_interface_franka.cpp similarity index 92% rename from src/robot_interface_franka.cpp rename to src/joint/robot_interface_franka.cpp index 3de52e8..264b54c 100644 --- a/src/robot_interface_franka.cpp +++ b/src/joint/robot_interface_franka.cpp @@ -38,13 +38,17 @@ /** * @brief robot_driver_franka::robot_driver_franka + * @param configuration The configuration of the robot for setting control beheavior * @param ROBOT_IP The IP address of the FCI * @param mode The operation mode {None, PositionControl}. * @param hand The hand option {ONFinished, OFF}. */ -RobotInterfaceFranka::RobotInterfaceFranka(const std::string &ROBOT_IP, - const MODE& mode, - const HAND& hand):ip_(ROBOT_IP),mode_(mode) +RobotInterfaceFranka::RobotInterfaceFranka( + const FrankaInterfaceConfiguration &configuration, + const std::string &ROBOT_IP, + const MODE& mode, + const HAND& hand + ):ip_(ROBOT_IP),franka_configuration_(configuration),mode_(mode) { _set_driver_mode(mode); if (hand == RobotInterfaceFranka::HAND::ON) @@ -308,6 +312,16 @@ void RobotInterfaceFranka::initialize() initialize_flag_ = true; + // initialize and set the robot to default behavior (collision behavior, impedance, etc) + setDefaultBehavior(*robot_sptr_); + robot_sptr_->setCollisionBehavior( + franka_configuration_.lower_torque_threshold, + franka_configuration_.upper_torque_threshold, + franka_configuration_.lower_force_threshold, + franka_configuration_.upper_force_threshold + ); + robot_sptr_->setJointImpedance(franka_configuration_.joint_impedance); + robot_sptr_->setCartesianImpedance(franka_configuration_.cartesian_impedance); switch (mode_) { @@ -351,6 +365,19 @@ 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_stiffness_force_torque_array_ = robot_state.O_F_ext_hat_K; + current_stiffness_force_[0] = current_stiffness_force_torque_array_[0]; + current_stiffness_force_[1] = current_stiffness_force_torque_array_[1]; + current_stiffness_force_[2] = current_stiffness_force_torque_array_[2]; + current_stiffness_torque_[0] = current_stiffness_force_torque_array_[3]; + current_stiffness_torque_[1] = current_stiffness_force_torque_array_[4]; + current_stiffness_torque_[2] = current_stiffness_force_torque_array_[5]; + + current_effector_pose_array_ = robot_state.O_T_EE; + current_stiffness_pose_array_ = robot_state.EE_T_K; + current_effeector_pose_ = Eigen::Map(current_effector_pose_array_.data(), 16); + current_stiffness_pose_ = Eigen::Map(current_stiffness_pose_array_.data(), 16); + robot_mode_ = robot_state.robot_mode; time_ = time; } @@ -674,6 +701,25 @@ VectorXd RobotInterfaceFranka::get_joint_forces() } +std::tuple RobotInterfaceFranka::get_stiffness_force_torque() +{ + _check_if_robot_is_connected(); + return std::make_tuple(current_stiffness_force_, current_stiffness_torque_); + +} + + +/** + * @brief robot_driver_franka::get_stiffness_pose + * @return + */ +DQ RobotInterfaceFranka::get_stiffness_pose() { + _check_if_robot_is_connected(); + const auto base_2_ee = homgenious_tf_to_DQ(current_effeector_pose_); + const auto ee_2_k = homgenious_tf_to_DQ(current_stiffness_pose_); + return base_2_ee * ee_2_k; +} + /** * @brief robot_driver_franka::get_time * @return diff --git a/include/robot_interface_franka.h b/src/joint/robot_interface_franka.h similarity index 74% rename from include/robot_interface_franka.h rename to src/joint/robot_interface_franka.h index 8bbc794..f34ae77 100644 --- a/include/robot_interface_franka.h +++ b/src/joint/robot_interface_franka.h @@ -40,19 +40,46 @@ #include #include #include -#include "motion_generator.h" +#include "generator/motion_generator.h" #include -#include "quadratic_program_motion_generator.h" +#include "generator/quadratic_program_motion_generator.h" #include #include -#include "custom_motion_generation.h" +#include "generator/custom_motion_generation.h" using namespace DQ_robotics; using namespace Eigen; +/** + * @brief get homgenious_tf_to_DQ + * @param pose_vector (column major) + * @return pose + */ +inline DQ homgenious_tf_to_DQ(const VectorXd& pose_vector) { + // VectorXd column major + const auto t = DQ(0, pose_vector(12), pose_vector(13), pose_vector(14)); + Eigen::Matrix rot_mat; + rot_mat << pose_vector(0), pose_vector(4), pose_vector(8), + pose_vector(1), pose_vector(5), pose_vector(9), + pose_vector(2), pose_vector(6), pose_vector(10); + Quaternion quaternion(rot_mat); + const auto r = normalize(DQ(quaternion.w(), quaternion.x(), quaternion.y(), quaternion.z())); + return r + 0.5 * E_ * t * r; +} + class RobotInterfaceFranka { -public: enum class MODE{ +public: + struct FrankaInterfaceConfiguration { + std::array lower_torque_threshold={10.0, 10.0, 10.0, 10.0, 10.0, 10.0, 10.0}; + std::array upper_torque_threshold={10.0, 10.0, 10.0, 10.0, 10.0, 10.0, 10.0}; + std::array lower_force_threshold={10.0, 10.0, 10.0, 10.0, 10.0, 10.0}; + std::array upper_force_threshold={10.0, 10.0, 10.0, 10.0, 10.0, 10.0}; + std::array joint_impedance={3000, 3000, 3000, 2500, 2500, 2000, 2000}; // K_theta (Nm/rad) + std::array cartesian_impedance={3000, 3000, 3000, 300, 300, 300}; // K_x (N/m) + }; + + enum class MODE{ None=0, PositionControl, VelocityControl, @@ -64,6 +91,7 @@ public: enum class MODE{ private: using Vector7d = Eigen::Matrix; std::string ip_ = "172.16.0.2"; + FrankaInterfaceConfiguration franka_configuration_; double speed_factor_joint_position_controller_ = 0.2; double speed_gripper_ = 0.02; VectorXd desired_joint_positions_; @@ -78,6 +106,15 @@ private: VectorXd current_joint_forces_; std::array current_joint_forces_array_; + Vector3d current_stiffness_force_; + Vector3d current_stiffness_torque_; + std::array current_stiffness_force_torque_array_; + + VectorXd current_stiffness_pose_; + VectorXd current_effeector_pose_; + std::array current_stiffness_pose_array_; + std::array current_effector_pose_array_; + franka::RobotMode robot_mode_; double time_ = 0; @@ -151,7 +188,11 @@ public: RobotInterfaceFranka() = delete; RobotInterfaceFranka(const RobotInterfaceFranka&) = delete; RobotInterfaceFranka& operator= (const RobotInterfaceFranka&) = delete; - RobotInterfaceFranka(const std::string &ROBOT_IP, const MODE& mode, const HAND& hand = ON); + RobotInterfaceFranka( + const FrankaInterfaceConfiguration &configuration, + const std::string &ROBOT_IP, + const MODE& mode, + const HAND& hand = ON); @@ -187,6 +228,9 @@ public: VectorXd get_joint_velocities(); VectorXd get_joint_forces(); + std::tuple get_stiffness_force_torque(); + DQ get_stiffness_pose(); + double get_time(); void set_target_joint_positions(const VectorXd& set_target_joint_positions_rad); diff --git a/src/sas_robot_driver_franka.cpp b/src/joint/sas_robot_driver_franka.cpp similarity index 86% rename from src/sas_robot_driver_franka.cpp rename to src/joint/sas_robot_driver_franka.cpp index 95ad9c2..6a4a0e7 100644 --- a/src/sas_robot_driver_franka.cpp +++ b/src/joint/sas_robot_driver_franka.cpp @@ -31,7 +31,7 @@ */ -#include "sas_robot_driver_franka.h" +#include "../../include/sas_robot_driver_franka.h" #include "sas_clock/sas_clock.h" #include #include @@ -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); @@ -73,14 +74,27 @@ namespace sas } - robot_driver_interface_sptr_ = std::make_shared(configuration.ip_address, - mode, //None, PositionControl, VelocityControl - RobotInterfaceFranka::HAND::OFF); + robot_driver_interface_sptr_ = std::make_shared( + configuration.interface_configuration, + configuration.ip_address, + mode, //None, PositionControl, VelocityControl + RobotInterfaceFranka::HAND::OFF + ); } RobotDriverFranka::~RobotDriverFranka()=default; + void RobotDriverFranka::_update_stiffness_contact_and_pose() const + { + Vector3d force, torque; + std::tie(force, torque) = robot_driver_interface_sptr_->get_stiffness_force_torque(); + const auto pose = robot_driver_interface_sptr_->get_stiffness_pose(); + robot_dynamic_provider_->publish_stiffness(pose, force, torque); + } + + + /** * @brief * @@ -132,6 +146,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_stiffness_contact_and_pose(); return robot_driver_interface_sptr_->get_joint_positions(); } diff --git a/src/sas_robot_driver_coppelia_node.cpp b/src/sas_robot_driver_coppelia_node.cpp index de0fe50..04c4654 100644 --- a/src/sas_robot_driver_coppelia_node.cpp +++ b/src/sas_robot_driver_coppelia_node.cpp @@ -39,7 +39,7 @@ #include #include #include -#include "sas_robot_driver_coppelia.h" +#include "coppelia/sas_robot_driver_coppelia.h" /********************************************* * SIGNAL HANDLER diff --git a/src/sas_robot_driver_franka_node.cpp b/src/sas_robot_driver_franka_node.cpp index 6359b37..0207796 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" /********************************************* @@ -52,6 +53,28 @@ void sig_int_handler(int) kill_this_process = true; } +template +std::array apply_scale_to_std_array(const std::array& array, const T& scale) +{ + std::array scaled_array; + for(std::size_t i = 0; i < N; i++) + { + scaled_array[i] = array[i] * scale; + } + return scaled_array; +} +template +std::array std_vec_to_std_array(const std::vector& vector) +{ + if(N != vector.size()){throw std::runtime_error("Size mismatch between vector and array.");} + std::array array; + for(std::size_t i = 0; i < N; i++) + { + array[i] = vector[i]; + } + return array; +} + int main(int argc, char **argv) { @@ -69,9 +92,38 @@ int main(int argc, char **argv) ros::NodeHandle nh; sas::RobotDriverFrankaConfiguration robot_driver_franka_configuration; + RobotInterfaceFranka::FrankaInterfaceConfiguration franka_interface_configuration; sas::get_ros_param(nh,"/robot_ip_address",robot_driver_franka_configuration.ip_address); sas::get_ros_param(nh,"/robot_mode", robot_driver_franka_configuration.mode); + double upper_scale_factor = 1.0; + std::vector all_params; + if(nh.hasParam(ros::this_node::getName()+"/force_upper_limits_scaling_factor")) + { + sas::get_ros_param(nh,"/force_upper_limits_scaling_factor",upper_scale_factor); + ROS_WARN_STREAM(ros::this_node::getName()+"::Set force upper limits scaling factor: " << upper_scale_factor); + } + if(nh.hasParam(ros::this_node::getName()+"/upper_torque_threshold")) { + ROS_INFO_STREAM(ros::this_node::getName()+"::Upper torque threshold override and set."); + std::vector upper_torque_threshold_std_vec; + sas::get_ros_param(nh,"/upper_torque_threshold",upper_torque_threshold_std_vec); + franka_interface_configuration.upper_torque_threshold = std_vec_to_std_array(upper_torque_threshold_std_vec); + }else { + ROS_INFO_STREAM(ros::this_node::getName()+"::Upper torque threshold not set. Using default with value scalling."); + franka_interface_configuration.upper_torque_threshold = apply_scale_to_std_array(franka_interface_configuration.upper_torque_threshold, upper_scale_factor); + } + if(nh.hasParam(ros::this_node::getName()+"/upper_force_threshold")) { + ROS_INFO_STREAM(ros::this_node::getName()+"::Upper force threshold override and set."); + std::vector upper_torque_threshold_std_vec; + sas::get_ros_param(nh,"/upper_force_threshold",upper_torque_threshold_std_vec); + franka_interface_configuration.upper_force_threshold = std_vec_to_std_array(upper_torque_threshold_std_vec); + }else { + ROS_INFO_STREAM(ros::this_node::getName()+"::Upper force threshold not set. Using default with value scalling."); + franka_interface_configuration.upper_force_threshold = apply_scale_to_std_array(franka_interface_configuration.upper_force_threshold, upper_scale_factor); + } + + + robot_driver_franka_configuration.interface_configuration = franka_interface_configuration; sas::RobotDriverROSConfiguration robot_driver_ros_configuration; @@ -79,13 +131,19 @@ int main(int argc, char **argv) sas::get_ros_param(nh,"/q_min",robot_driver_ros_configuration.q_min); sas::get_ros_param(nh,"/q_max",robot_driver_ros_configuration.q_max); + // initialize the robot dynamic provider 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 +154,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()); }