From f317f27c7d8f26cfea54f2c7abdbe9e051dd863c Mon Sep 17 00:00:00 2001 From: mhubii Date: Fri, 8 Dec 2023 11:51:37 +0000 Subject: [PATCH] added ft threshold --- .../include/lbr_fri_ros2/ft_estimator.hpp | 9 ++++++++- lbr_fri_ros2/src/ft_estimator.cpp | 13 ++++++++++++- .../config/lbr_system_interface.xacro | 16 ++++++++++++++-- .../lbr_ros2_control/system_interface.hpp | 6 ++++++ lbr_ros2_control/src/system_interface.cpp | 16 +++++++++++++++- 5 files changed, 55 insertions(+), 5 deletions(-) diff --git a/lbr_fri_ros2/include/lbr_fri_ros2/ft_estimator.hpp b/lbr_fri_ros2/include/lbr_fri_ros2/ft_estimator.hpp index 98d64f76..f71c6eb9 100644 --- a/lbr_fri_ros2/include/lbr_fri_ros2/ft_estimator.hpp +++ b/lbr_fri_ros2/include/lbr_fri_ros2/ft_estimator.hpp @@ -1,7 +1,9 @@ #ifndef LBR_FRI_ROS2__FT_ESTIMATOR_HPP_ #define LBR_FRI_ROS2__FT_ESTIMATOR_HPP_ +#include #include +#include #include #include "eigen3/Eigen/Core" @@ -31,15 +33,20 @@ class FTEstimator { static constexpr uint8_t CARTESIAN_DOF = 6; using cart_array_t = std::array; using cart_array_t_ref = cart_array_t &; + using const_cart_array_t_ref = const cart_array_t &; FTEstimator(const std::string &robot_description, const std::string &chain_root = "link_0", - const std::string &chain_tip = "link_ee"); + const std::string &chain_tip = "link_ee", + const_cart_array_t_ref f_ext_th = {2., 2., 2., 0.5, 0.5, 0.5}); void compute(const_jnt_pos_array_t_ref measured_joint_position, const_ext_tau_array_t_ref external_torque, cart_array_t_ref f_ext, const double &damping = 0.2); void reset(); protected: + // force threshold + cart_array_t f_ext_th_; + KDL::Tree tree_; KDL::Chain chain_; diff --git a/lbr_fri_ros2/src/ft_estimator.cpp b/lbr_fri_ros2/src/ft_estimator.cpp index 3828ee6d..35929be8 100644 --- a/lbr_fri_ros2/src/ft_estimator.cpp +++ b/lbr_fri_ros2/src/ft_estimator.cpp @@ -2,7 +2,8 @@ namespace lbr_fri_ros2 { FTEstimator::FTEstimator(const std::string &robot_description, const std::string &chain_root, - const std::string &chain_tip) { + const std::string &chain_tip, const_cart_array_t_ref f_ext_th) + : f_ext_th_(f_ext_th) { if (!kdl_parser::treeFromString(robot_description, tree_)) { std::string err = "Failed to construct kdl tree from robot description."; RCLCPP_ERROR(rclcpp::get_logger(LOGGER_NAME), err.c_str()); @@ -38,6 +39,16 @@ void FTEstimator::compute(const_jnt_pos_array_t_ref measured_joint_position, f_ext_.bottomRows(3) = Eigen::Matrix3d::Map(chain_tip_frame_.M.data) * f_ext_.bottomRows(3); Eigen::Map>(f_ext.data()) = f_ext_; + + // threshold force-torque + std::transform(f_ext.begin(), f_ext.end(), f_ext_th_.begin(), f_ext.begin(), + [](const double &f_ext_i, const double &f_ext_th_i) { + if (std::abs(f_ext_i) < f_ext_th_i) { + return 0.; + } else { + return std::copysign(1., f_ext_i) * (std::abs(f_ext_i) - f_ext_th_i); + } + }); } void FTEstimator::reset() { diff --git a/lbr_ros2_control/config/lbr_system_interface.xacro b/lbr_ros2_control/config/lbr_system_interface.xacro index 6703d0b0..3f389121 100644 --- a/lbr_ros2_control/config/lbr_system_interface.xacro +++ b/lbr_ros2_control/config/lbr_system_interface.xacro @@ -31,9 +31,15 @@ external_torque_cutoff_frequency:=^|10 measured_torque_cutoff_frequency:=^|10 open_loop:=^|true - damping:=^|0.2 chain_root:=^|link_0 - chain_tip:=^|link_ee"> + chain_tip:=^|link_ee + damping:=^|0.2 + force_x_th:=^|2.0 + force_y_th:=^|2.0 + force_z_th:=^|2.0 + torque_x_th:=^|0.5 + torque_y_th:=^|0.5 + torque_z_th:=^|0.5"> @@ -85,6 +91,12 @@ ${chain_root} ${chain_tip} ${damping} + ${force_x_th} + ${force_y_th} + ${force_z_th} + ${torque_x_th} + ${torque_y_th} + ${torque_z_th} diff --git a/lbr_ros2_control/include/lbr_ros2_control/system_interface.hpp b/lbr_ros2_control/include/lbr_ros2_control/system_interface.hpp index 2d63a003..e6295b7f 100644 --- a/lbr_ros2_control/include/lbr_ros2_control/system_interface.hpp +++ b/lbr_ros2_control/include/lbr_ros2_control/system_interface.hpp @@ -49,6 +49,12 @@ struct EstimatedFTSensorParameters { std::string chain_root{"link_0"}; std::string chain_tip{"link_ee"}; double damping{0.2}; + double force_x_th{2.0}; + double force_y_th{2.0}; + double force_z_th{2.0}; + double torque_x_th{0.5}; + double torque_y_th{0.5}; + double torque_z_th{0.5}; }; class SystemInterface : public hardware_interface::SystemInterface { diff --git a/lbr_ros2_control/src/system_interface.cpp b/lbr_ros2_control/src/system_interface.cpp index fb20bc66..4722e417 100644 --- a/lbr_ros2_control/src/system_interface.cpp +++ b/lbr_ros2_control/src/system_interface.cpp @@ -78,8 +78,22 @@ SystemInterface::on_init(const hardware_interface::HardwareInfo &system_info) { ft_parameters_.chain_root = info_.sensors[1].parameters.at("chain_root"); ft_parameters_.chain_tip = info_.sensors[1].parameters.at("chain_tip"); ft_parameters_.damping = std::stod(info_.sensors[1].parameters.at("damping")); + ft_parameters_.force_x_th = std::stod(info_.sensors[1].parameters.at("force_x_th")); + ft_parameters_.force_y_th = std::stod(info_.sensors[1].parameters.at("force_y_th")); + ft_parameters_.force_z_th = std::stod(info_.sensors[1].parameters.at("force_z_th")); + ft_parameters_.torque_x_th = std::stod(info_.sensors[1].parameters.at("torque_x_th")); + ft_parameters_.torque_y_th = std::stod(info_.sensors[1].parameters.at("torque_y_th")); + ft_parameters_.torque_z_th = std::stod(info_.sensors[1].parameters.at("torque_z_th")); ft_estimator_ptr_ = std::make_unique( - info_.original_xml, ft_parameters_.chain_root, ft_parameters_.chain_tip); + info_.original_xml, ft_parameters_.chain_root, ft_parameters_.chain_tip, + lbr_fri_ros2::FTEstimator::cart_array_t{ + ft_parameters_.force_x_th, + ft_parameters_.force_y_th, + ft_parameters_.force_z_th, + ft_parameters_.torque_x_th, + ft_parameters_.torque_y_th, + ft_parameters_.torque_z_th, + }); if (!verify_number_of_joints_()) { return controller_interface::CallbackReturn::ERROR;