added ft threshold

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 <algorithm>
 #include <array>
+#include <cmath>
 #include <string>
 
 #include "eigen3/Eigen/Core"
@@ -31,15 +33,20 @@ class FTEstimator {
   static constexpr uint8_t CARTESIAN_DOF = 6;
   using cart_array_t = std::array<double, CARTESIAN_DOF>;
   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_;
 

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<Eigen::Matrix<double, CARTESIAN_DOF, 1>>(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() {

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">
         <ros2_control name="lbr_system_interface" type="system">
             <!-- define hardware including parameters, also gazebo -->
             <xacro:if value="${sim}">
@@ -85,6 +91,12 @@
                     <param name="chain_root">${chain_root}</param>
                     <param name="chain_tip">${chain_tip}</param>
                     <param name="damping">${damping}</param>
+                    <param name="force_x_th">${force_x_th}</param>
+                    <param name="force_y_th">${force_y_th}</param>
+                    <param name="force_z_th">${force_z_th}</param>
+                    <param name="torque_x_th">${torque_x_th}</param>
+                    <param name="torque_y_th">${torque_y_th}</param>
+                    <param name="torque_z_th">${torque_z_th}</param>
                     <state_interface name="force.x" />
                     <state_interface name="force.y" />
                     <state_interface name="force.z" />

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 {

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<lbr_fri_ros2::FTEstimator>(
-      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;