qlin/sas_robot_driver_franka
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

test implimentation for automatic error recovery not done

Browse Source
This commit is contained in:
QuentinLin
2024-08-03 13:53:06 +09:00
parent 1f842c9ba0
commit a0f33a9768
5 changed files with 61 additions and 14 deletions
Download Patch File Download Diff File Expand all files Collapse all files

1
include/sas_robot_driver_franka.h
View File

@@ -61,7 +61,6 @@ struct RobotDriverFrankaConfiguration
double speed;
RobotInterfaceFranka::FrankaInterfaceConfiguration interface_configuration;
DQ robot_reference_frame = DQ(0);
bool automatic_error_recovery = false;
};

31
src/joint/robot_interface_franka.cpp
View File

@@ -346,6 +346,17 @@ void RobotInterfaceFranka::initialize()
break;
}
_update_status_message("Initialized.",verbose_);
restart_on_err_.store(false);
if(franka_configuration_.automatic_error_recovery) {
_start_automatic_error_recovery_monitor();
}
}
void RobotInterfaceFranka::_start_automatic_error_recovery_monitor() {
std::cerr<<"automatic error recovery is not handeled"<<std::endl;
}
void RobotInterfaceFranka::_automatic_error_recovery_monitor() {
std::cerr<<"automatic error recovery is not handeled"<<std::endl;
}
@@ -405,8 +416,12 @@ void RobotInterfaceFranka::_start_echo_robot_state_mode(){
_update_status_message("Finished echo_robot_state.",verbose_);
}
catch (franka::Exception const& e) {
std::cout << e.what() << std::endl;
std::cerr << e.what() << std::endl;
if(!franka_configuration_.automatic_error_recovery) {
exit_on_err_.store(true);
}else {
_trigger_automatic_error_recovery();
}
status_message_=e.what();
// ROS_ERROR_STREAM("["+ros::this_node::getName()+"]::echo_robot_state_mode::Error on:"+e.what());
}
@@ -485,8 +500,12 @@ void RobotInterfaceFranka::_start_joint_position_control_mode()
}
catch (franka::Exception const& e) {
std::cout << e.what() << std::endl;
std::cerr << e.what() << std::endl;
if(!franka_configuration_.automatic_error_recovery) {
exit_on_err_.store(true);
}else {
_trigger_automatic_error_recovery();
}
status_message_=e.what();
// ROS_ERROR_STREAM("["+ros::this_node::getName()+"]::joint_position_control_mode::Error on:"+e.what());
}
@@ -559,11 +578,17 @@ void RobotInterfaceFranka::_start_joint_velocity_control_mode()
});
}
catch (franka::Exception const& e) {
std::cout << e.what() << std::endl;
std::cerr << e.what() << std::endl;
if(!franka_configuration_.automatic_error_recovery) {
exit_on_err_.store(true);
}else {
_trigger_automatic_error_recovery();
}
status_message_=e.what();
// ROS_ERROR_STREAM("["+ros::this_node::getName()+"]::joint_velocity_control_mode::Error on:"+e.what());
}
}

14
src/joint/robot_interface_franka.h
View File

@@ -77,6 +77,7 @@ public:
std::array<double, 6> upper_force_threshold={10.0, 10.0, 10.0, 10.0, 10.0, 10.0};
std::array<double, 7> joint_impedance={3000, 3000, 3000, 2500, 2500, 2000, 2000}; // K_theta (Nm/rad)
std::array<double, 6> cartesian_impedance={3000, 3000, 3000, 300, 300, 300}; // K_x (N/m)
bool automatic_error_recovery = false;
};
enum class MODE{
@@ -155,6 +156,8 @@ private:
//-------To handle the threads-----------------
std::atomic<bool> exit_on_err_={false};
// monitor restart progress
std::atomic<bool> restart_on_err_={false};
void _start_joint_position_control_mode();
@@ -171,6 +174,16 @@ private:
void _start_joint_velocity_control_mode();
std::thread joint_velocity_control_mode_thread_;
void _start_joint_velocity_control_thread();
void _start_automatic_error_recovery_monitor();
std::thread automatic_error_recovery_thread;
void _automatic_error_recovery_monitor();
void _trigger_automatic_error_recovery() {
restart_on_err_.store(true);
// before implimentation
exit_on_err_.store(true);
}
//----------------------------------------------
@@ -214,6 +227,7 @@ public:
std::string get_status_message();
bool get_err_state() const {return exit_on_err_.load();}
bool get_restart_state() const {return restart_on_err_.load();}
std::string get_robot_mode();

19
src/joint/sas_robot_driver_franka.cpp
View File

@@ -52,7 +52,8 @@ namespace sas
//joint_positions_buffer_.resize(8,0);
//end_effector_pose_euler_buffer_.resize(7,0);
//end_effector_pose_homogenous_transformation_buffer_.resize(10,0);
std::cout<<configuration.ip_address<<std::endl;
// std::cout<<configuration.ip_address<<std::endl;
ROS_INFO_STREAM(ros::this_node::getName() + " RobotDriverFranka::RobotDriverFranka: connecting to robot at " + configuration.ip_address);
RobotInterfaceFranka::MODE mode = RobotInterfaceFranka::MODE::None;
@@ -72,7 +73,9 @@ namespace sas
throw std::runtime_error(std::string("Wrong mode. ") + std::string("You used ") + configuration_.mode
+ std::string(". However, you must use None, PositionControl or VelocityControl"));
}
if(configuration.interface_configuration.automatic_error_recovery) {
ROS_WARN_STREAM("["+ros::this_node::getName()+"]::Automatic error recovery is enabled. THIS FEATURE IS VERY EXPERIMENTAL");
}
robot_driver_interface_sptr_ = std::make_shared<RobotInterfaceFranka>(
configuration.interface_configuration,
@@ -146,9 +149,9 @@ namespace sas
ROS_ERROR_STREAM("["+ros::this_node::getName()+"]::driver interface error on:"+robot_driver_interface_sptr_->get_status_message());
break_loops_->store(true);
}
if(!ros::ok()) {
ROS_ERROR_STREAM("["+ros::this_node::getName()+"]::ROS shutdown requested.");
break_loops_->store(true);
if(robot_driver_interface_sptr_->get_restart_state()) {
ROS_WARN_STREAM("["+ros::this_node::getName()+"]::driver interface error on:"+robot_driver_interface_sptr_->get_status_message()+". attempting restart");
return robot_driver_interface_sptr_->get_joint_positions();
}
_update_stiffness_contact_and_pose();
return robot_driver_interface_sptr_->get_joint_positions();
@@ -162,6 +165,9 @@ namespace sas
*/
void RobotDriverFranka::set_target_joint_positions(const VectorXd& desired_joint_positions_rad)
{
if(robot_driver_interface_sptr_->get_restart_state()) {
return;
}
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());
@@ -186,6 +192,9 @@ namespace sas
*/
void RobotDriverFranka::set_target_joint_velocities(const VectorXd &desired_joint_velocities)
{
if(robot_driver_interface_sptr_->get_restart_state()) {
return;
}
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()) {

4
src/sas_robot_driver_franka_node.cpp
View File

@@ -131,8 +131,8 @@ int main(int argc, char **argv)
}else{ROS_INFO_STREAM(ros::this_node::getName()+"::Robot parameter file path not set. Robot Model Unknow.");}
if(nh.hasParam(ros::this_node::getName()+"/automatic_error_recovery")) {
sas::get_ros_param(nh,"/automatic_error_recovery",robot_driver_franka_configuration.automatic_error_recovery);
if(robot_driver_franka_configuration.automatic_error_recovery)
sas::get_ros_param(nh,"/automatic_error_recovery",franka_interface_configuration.automatic_error_recovery);
if(franka_interface_configuration.automatic_error_recovery)
{
ROS_WARN_STREAM(ros::this_node::getName()+"::Automatic error recovery enabled. STATUS EXPERIMENTAL");
}