From 3271e2a8a54f5302f3ffe153cfdcbe5edabf14cd Mon Sep 17 00:00:00 2001 From: QuentinLin Date: Sun, 14 Jul 2024 19:24:08 +0900 Subject: [PATCH] impliment improved error handeling on driver failed --- cfg/sas_robot_driver_franka_2.yaml | 8 ++++++++ cfg/sas_robot_driver_franka_3.yaml | 8 ++++++++ cfg/sas_robot_driver_franka_4.yaml | 8 ++++++++ include/robot_interface_franka.h | 5 +++++ include/robot_interface_hand.h | 2 +- include/sas_robot_driver_franka.h | 2 ++ src/robot_interface_franka.cpp | 13 +++++++++++++ src/sas_robot_driver_franka.cpp | 17 ++++++++++++++++- 8 files changed, 61 insertions(+), 2 deletions(-) create mode 100644 cfg/sas_robot_driver_franka_2.yaml create mode 100644 cfg/sas_robot_driver_franka_3.yaml create mode 100644 cfg/sas_robot_driver_franka_4.yaml diff --git a/cfg/sas_robot_driver_franka_2.yaml b/cfg/sas_robot_driver_franka_2.yaml new file mode 100644 index 0000000..228729e --- /dev/null +++ b/cfg/sas_robot_driver_franka_2.yaml @@ -0,0 +1,8 @@ +"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 new file mode 100644 index 0000000..4a95f7c --- /dev/null +++ b/cfg/sas_robot_driver_franka_3.yaml @@ -0,0 +1,8 @@ +"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 new file mode 100644 index 0000000..8c81933 --- /dev/null +++ b/cfg/sas_robot_driver_franka_4.yaml @@ -0,0 +1,8 @@ +"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/robot_interface_franka.h b/include/robot_interface_franka.h index 2bfda17..8bbc794 100644 --- a/include/robot_interface_franka.h +++ b/include/robot_interface_franka.h @@ -117,6 +117,9 @@ private: //-------To handle the threads----------------- + std::atomic exit_on_err_={false}; + + void _start_joint_position_control_mode(); std::thread joint_position_control_mode_thread_; void _start_joint_position_control_thread(); @@ -169,6 +172,8 @@ public: std::string get_status_message(); + bool get_err_state() const {return exit_on_err_.load();} + std::string get_robot_mode(); diff --git a/include/robot_interface_hand.h b/include/robot_interface_hand.h index cdb904f..9b08d35 100644 --- a/include/robot_interface_hand.h +++ b/include/robot_interface_hand.h @@ -52,7 +52,7 @@ using namespace Eigen; class RobotInterfaceHand { protected: - double speed_gripper_ = 0.02; + double speed_gripper_ = 0.05;//0.02 std::string ip_ = "172.16.0.2"; std::shared_ptr gripper_sptr_; void _check_if_hand_is_connected(); diff --git a/include/sas_robot_driver_franka.h b/include/sas_robot_driver_franka.h index 7e9a302..6739331 100644 --- a/include/sas_robot_driver_franka.h +++ b/include/sas_robot_driver_franka.h @@ -42,6 +42,7 @@ #include #include "robot_interface_franka.h" +#include using namespace DQ_robotics; using namespace Eigen; @@ -74,6 +75,7 @@ private: //std::vector end_effector_pose_euler_buffer_; //std::vector end_effector_pose_homogenous_transformation_buffer_; + std::atomic_bool* break_loops_; public: diff --git a/src/robot_interface_franka.cpp b/src/robot_interface_franka.cpp index f0239fe..3de52e8 100644 --- a/src/robot_interface_franka.cpp +++ b/src/robot_interface_franka.cpp @@ -32,6 +32,9 @@ #include "robot_interface_franka.h" +#include +#include + /** * @brief robot_driver_franka::robot_driver_franka @@ -376,6 +379,9 @@ void RobotInterfaceFranka::_start_echo_robot_state_mode(){ } catch (franka::Exception const& e) { std::cout << e.what() << std::endl; + exit_on_err_.store(true); + status_message_=e.what(); + // ROS_ERROR_STREAM("["+ros::this_node::getName()+"]::echo_robot_state_mode::Error on:"+e.what()); } } @@ -453,6 +459,9 @@ void RobotInterfaceFranka::_start_joint_position_control_mode() } catch (franka::Exception const& e) { std::cout << e.what() << std::endl; + exit_on_err_.store(true); + status_message_=e.what(); + // ROS_ERROR_STREAM("["+ros::this_node::getName()+"]::joint_position_control_mode::Error on:"+e.what()); } @@ -524,6 +533,9 @@ void RobotInterfaceFranka::_start_joint_velocity_control_mode() } catch (franka::Exception const& e) { std::cout << e.what() << std::endl; + exit_on_err_.store(true); + status_message_=e.what(); + // ROS_ERROR_STREAM("["+ros::this_node::getName()+"]::joint_velocity_control_mode::Error on:"+e.what()); } } @@ -534,6 +546,7 @@ void RobotInterfaceFranka::_start_joint_velocity_control_mode() */ void RobotInterfaceFranka::_setDefaultRobotBehavior() { + robot_sptr_->setCollisionBehavior( {{20.0, 20.0, 20.0, 20.0, 20.0, 20.0, 20.0}}, {{20.0, 20.0, 20.0, 20.0, 20.0, 20.0, 20.0}}, {{10.0, 10.0, 10.0, 10.0, 10.0, 10.0, 10.0}}, {{10.0, 10.0, 10.0, 10.0, 10.0, 10.0, 10.0}}, diff --git a/src/sas_robot_driver_franka.cpp b/src/sas_robot_driver_franka.cpp index c697911..95ad9c2 100644 --- a/src/sas_robot_driver_franka.cpp +++ b/src/sas_robot_driver_franka.cpp @@ -34,12 +34,15 @@ #include "sas_robot_driver_franka.h" #include "sas_clock/sas_clock.h" #include +#include +#include namespace sas { RobotDriverFranka::RobotDriverFranka(const RobotDriverFrankaConfiguration &configuration, std::atomic_bool *break_loops): RobotDriver(break_loops), - configuration_(configuration) + configuration_(configuration), + break_loops_(break_loops) { joint_positions_.resize(7); joint_velocities_.resize(7); @@ -125,6 +128,10 @@ namespace sas */ VectorXd RobotDriverFranka::get_joint_positions() { + if(robot_driver_interface_sptr_->get_err_state()) { + ROS_ERROR_STREAM("["+ros::this_node::getName()+"]::driver interface error on:"+robot_driver_interface_sptr_->get_status_message()); + break_loops_->store(true); + } return robot_driver_interface_sptr_->get_joint_positions(); } @@ -137,6 +144,10 @@ namespace sas void RobotDriverFranka::set_target_joint_positions(const VectorXd& desired_joint_positions_rad) { robot_driver_interface_sptr_->set_target_joint_positions(desired_joint_positions_rad); + if(robot_driver_interface_sptr_->get_err_state()) { + ROS_ERROR_STREAM("["+ros::this_node::getName()+"]::driver interface error on:"+robot_driver_interface_sptr_->get_status_message()); + break_loops_->store(true); + } } /** @@ -158,6 +169,10 @@ namespace sas { desired_joint_velocities_ = desired_joint_velocities; robot_driver_interface_sptr_->set_target_joint_velocities(desired_joint_velocities); + if(robot_driver_interface_sptr_->get_err_state()) { + ROS_ERROR_STREAM("["+ros::this_node::getName()+"]::driver interface error on:"+robot_driver_interface_sptr_->get_status_message()); + break_loops_->store(true); + } } /**