moved the FT estimation into an asynchronous thread (#213)

Author: mhubii

lbr_description/ros2_control/lbr_system_config.yaml

@@ -19,6 +19,9 @@ hardware:
   open_loop: true # KUKA works the best in open_loop control mode
 
 estimated_ft_sensor: # estimates the external force-torque from the external joint torque values
+  enabled: true # whether to enable the force-torque estimation
+  update_rate: 100 # update rate for the force-torque estimation [Hz] (less or equal to controller manager update rate)
+  rt_prio: 30 # real-time priority for the force-torque estimation
   chain_root: lbr_link_0
   chain_tip: lbr_link_ee
   damping: 0.2 # damping factor for the pseudo-inverse of the Jacobian

lbr_description/ros2_control/lbr_system_interface.xacro

@@ -61,6 +61,9 @@
 
                 <sensor
                     name="estimated_ft_sensor">
+                    <param name="enabled">${system_config['estimated_ft_sensor']['enabled']}</param>
+                    <param name="update_rate">${system_config['estimated_ft_sensor']['update_rate']}</param>
+                    <param name="rt_prio">${system_config['estimated_ft_sensor']['rt_prio']}</param>
                     <param name="chain_root">${system_config['estimated_ft_sensor']['chain_root']}</param>
                     <param name="chain_tip">${system_config['estimated_ft_sensor']['chain_tip']}</param>
                     <param name="damping">${system_config['estimated_ft_sensor']['damping']}</param>
@@ -70,12 +73,14 @@
                     <param name="torque_x_th">${system_config['estimated_ft_sensor']['torque_x_th']}</param>
                     <param name="torque_y_th">${system_config['estimated_ft_sensor']['torque_y_th']}</param>
                     <param name="torque_z_th">${system_config['estimated_ft_sensor']['torque_z_th']}</param>
-                    <state_interface name="force.x" />
-                    <state_interface name="force.y" />
-                    <state_interface name="force.z" />
-                    <state_interface name="torque.x" />
-                    <state_interface name="torque.y" />
-                    <state_interface name="torque.z" />
+                    <xacro:if value="${system_config['estimated_ft_sensor']['enabled']}">
+                        <state_interface name="force.x" />
+                        <state_interface name="force.y" />
+                        <state_interface name="force.z" />
+                        <state_interface name="torque.x" />
+                        <state_interface name="torque.y" />
+                        <state_interface name="torque.z" />
+                    </xacro:if>
                 </sensor>
 
                 <!-- FRI Cartesian impedance control mode -->
@@ -114,7 +119,8 @@
                         <param name="max_position">${max_position}</param>
                         <param name="max_velocity">${max_velocity}</param>
                         <param name="max_torque">${max_torque}</param>
-                        <xacro:if value="${system_config['hardware']['fri_client_sdk']['major_version'] == 1}">
+                        <xacro:if
+                            value="${system_config['hardware']['fri_client_sdk']['major_version'] == 1}">
                             <state_interface name="commanded_joint_position" />
                         </xacro:if>
                         <state_interface name="commanded_torque" />

lbr_fri_ros2/include/lbr_fri_ros2/app.hpp

@@ -2,6 +2,7 @@
 #define LBR_FRI_ROS2__APP_HPP_
 
 #include <memory>
+#include <string>
 
 #include "rclcpp/logger.hpp"
 #include "rclcpp/logging.hpp"
@@ -22,9 +23,6 @@ class App : public Worker {
    * KUKA::FRI::ClientApplication::step() (this is by KUKA's design).
    *
    */
-protected:
-  static constexpr char LOGGER_NAME[] = "lbr_fri_ros2::App";
-
 public:
   App(const std::shared_ptr<AsyncClient> async_client_ptr);
   ~App();
@@ -33,6 +31,8 @@ class App : public Worker {
   bool close_udp_socket();
   void run_async(int rt_prio = 80) override;
 
+  inline std::string LOGGER_NAME() const override { return "lbr_fri_ros2::App"; };
+
 protected:
   void perform_work_() override;
   bool valid_port_(const int &port_id);

lbr_fri_ros2/include/lbr_fri_ros2/ft_estimator.hpp

@@ -6,6 +6,7 @@
 #include <cmath>
 #include <memory>
 #include <string>
+#include <thread>
 
 #include "eigen3/Eigen/Core"
 #include "rclcpp/logger.hpp"
@@ -17,31 +18,74 @@
 #include "lbr_fri_ros2/kinematics.hpp"
 #include "lbr_fri_ros2/pinv.hpp"
 #include "lbr_fri_ros2/types.hpp"
+#include "lbr_fri_ros2/worker.hpp"
 
 namespace lbr_fri_ros2 {
-class FTEstimator {
+class FTEstimatorImpl {
+  /**
+   * @brief A class to estimate force-torques from external joint torque readings. Note that only
+   * forces beyond a specified threshold are returned. The specified threshold is removed from the
+   * estimated force-torque.
+   *
+   */
 protected:
-  static constexpr char LOGGER_NAME[] = "lbr_fri_ros2::FTEstimator";
+  static constexpr char LOGGER_NAME[] = "lbr_fri_ros2::FTEstimatorImpl";
 
 public:
-  FTEstimator(const std::string &robot_description, const std::string &chain_root = "lbr_link_0",
-              const std::string &chain_tip = "lbr_link_ee",
-              const_cart_array_t_ref f_ext_th = {2., 2., 2., 0.5, 0.5, 0.5});
-  void compute(const_jnt_array_t_ref measured_joint_position, const_jnt_array_t_ref external_torque,
-               cart_array_t_ref f_ext, const double &damping = 0.2);
+  FTEstimatorImpl(const std::string &robot_description,
+                  const std::string &chain_root = "lbr_link_0",
+                  const std::string &chain_tip = "lbr_link_ee",
+                  const_cart_array_t_ref f_ext_th = {2., 2., 2., 0.5, 0.5, 0.5},
+                  const double &damping = 0.2);
+  void compute();
   void reset();
 
+  inline void get_f_ext(cart_array_t_ref f_ext) const {
+    Eigen::Map<Eigen::Matrix<double, CARTESIAN_DOF, 1>>(f_ext.data()) = f_ext_;
+  }
+  inline void get_f_ext_tf(cart_array_t_ref f_ext) const {
+    Eigen::Map<Eigen::Matrix<double, CARTESIAN_DOF, 1>>(f_ext.data()) = f_ext_tf_;
+  }
+  inline void set_tau_ext(const_jnt_array_t_ref tau_ext) {
+    tau_ext_ = Eigen::Map<const Eigen::Matrix<double, N_JNTS, 1>>(tau_ext.data());
+  }
+  inline void set_q(const_jnt_array_t_ref q) { q_ = q; }
+
 protected:
   // force threshold
   cart_array_t f_ext_th_;
 
+  // damping for pseudo-inverse of Jacobian
+  double damping_;
+
+  // joint positions and external joint torques
+  jnt_array_t q_;
+
   // kinematics
   std::unique_ptr<Kinematics> kinematics_ptr_;
 
   // force estimation
   Eigen::Matrix<double, N_JNTS, CARTESIAN_DOF> jacobian_inv_;
   Eigen::Matrix<double, N_JNTS, 1> tau_ext_;
-  Eigen::Matrix<double, CARTESIAN_DOF, 1> f_ext_;
+  Eigen::Matrix<double, CARTESIAN_DOF, 1> f_ext_raw_, f_ext_, f_ext_tf_;
+};
+
+class FTEstimator : public Worker {
+  /**
+   * @brief A simple class to run the FTEstimatorImpl asynchronously at a specified update rate.
+   *
+   */
+public:
+  FTEstimator(const std::shared_ptr<FTEstimatorImpl> ft_estimator,
+              const std::uint16_t &update_rate = 100);
+
+  inline std::string LOGGER_NAME() const override { return "lbr_fri_ros2::FTEstimator"; };
+
+protected:
+  void perform_work_() override;
+
+  std::shared_ptr<FTEstimatorImpl> ft_estimator_impl_ptr_;
+  std::uint16_t update_rate_;
 };
 } // namespace lbr_fri_ros2
 #endif // LBR_FRI_ROS2__FT_ESTIMATOR_HPP_

lbr_fri_ros2/include/lbr_fri_ros2/worker.hpp

@@ -2,6 +2,7 @@
 #define LBR_FRI_ROS2__WORKER_HPP_
 
 #include <atomic>
+#include <string>
 #include <thread>
 
 #include "rclcpp/logger.hpp"
@@ -12,15 +13,13 @@
 
 namespace lbr_fri_ros2 {
 class Worker {
-protected:
-  static constexpr char LOGGER_NAME[] = "lbr_fri_ros2::Worker";
-
 public:
   Worker();
   ~Worker();
 
   virtual void run_async(int rt_prio = 80);
   void request_stop();
+  inline virtual std::string LOGGER_NAME() const = 0;
 
 protected:
   virtual void perform_work_() = 0;

lbr_fri_ros2/src/app.cpp

@@ -15,70 +15,70 @@ App::~App() {
 
 bool App::open_udp_socket(const int &port_id, const char *const remote_host) {
   if (!connection_ptr_) {
-    RCLCPP_ERROR_STREAM(rclcpp::get_logger(LOGGER_NAME),
+    RCLCPP_ERROR_STREAM(rclcpp::get_logger(LOGGER_NAME()),
                         ColorScheme::ERROR << "Connection not configured" << ColorScheme::ENDC);
     return false;
   }
-  RCLCPP_INFO_STREAM(rclcpp::get_logger(LOGGER_NAME),
+  RCLCPP_INFO_STREAM(rclcpp::get_logger(LOGGER_NAME()),
                      ColorScheme::OKBLUE << "Opening UDP socket with port_id '" << ColorScheme::BOLD
                                          << port_id << "'" << ColorScheme::ENDC);
   if (!valid_port_(port_id)) {
     return false;
   }
   if (connection_ptr_->isOpen()) {
-    RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "Socket already open");
+    RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME()), "Socket already open");
     return true;
   }
   if (!connection_ptr_->open(port_id, remote_host)) {
-    RCLCPP_ERROR_STREAM(rclcpp::get_logger(LOGGER_NAME),
+    RCLCPP_ERROR_STREAM(rclcpp::get_logger(LOGGER_NAME()),
                         ColorScheme::ERROR << "Failed to open socket" << ColorScheme::ENDC);
     return false;
   }
-  RCLCPP_INFO_STREAM(rclcpp::get_logger(LOGGER_NAME),
+  RCLCPP_INFO_STREAM(rclcpp::get_logger(LOGGER_NAME()),
                      ColorScheme::OKGREEN << "Socket opened successfully" << ColorScheme::ENDC);
   return true;
 }
 
 bool App::close_udp_socket() {
   if (!connection_ptr_) {
-    RCLCPP_ERROR_STREAM(rclcpp::get_logger(LOGGER_NAME),
+    RCLCPP_ERROR_STREAM(rclcpp::get_logger(LOGGER_NAME()),
                         ColorScheme::ERROR << "Connection not configured" << ColorScheme::ENDC);
     return false;
   }
   while (running_) {
-    RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "Waiting for run thread termination");
+    RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME()), "Waiting for run thread termination");
     std::this_thread::sleep_for(std::chrono::milliseconds(100));
   }
-  RCLCPP_INFO_STREAM(rclcpp::get_logger(LOGGER_NAME),
+  RCLCPP_INFO_STREAM(rclcpp::get_logger(LOGGER_NAME()),
                      ColorScheme::OKBLUE << "Closing UDP socket" << ColorScheme::ENDC);
   if (!connection_ptr_->isOpen()) {
-    RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "Socket already closed");
+    RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME()), "Socket already closed");
     return true;
   }
   connection_ptr_->close();
-  RCLCPP_INFO_STREAM(rclcpp::get_logger(LOGGER_NAME),
+  RCLCPP_INFO_STREAM(rclcpp::get_logger(LOGGER_NAME()),
                      ColorScheme::OKGREEN << "Socket closed successfully" << ColorScheme::ENDC);
   return true;
 }
 
 void App::run_async(int rt_prio) {
   if (!async_client_ptr_) {
-    RCLCPP_ERROR_STREAM(rclcpp::get_logger(LOGGER_NAME),
+    RCLCPP_ERROR_STREAM(rclcpp::get_logger(LOGGER_NAME()),
                         ColorScheme::ERROR << "AsyncClient not configured" << ColorScheme::ENDC);
     return;
   }
   if (!connection_ptr_) {
-    RCLCPP_ERROR_STREAM(rclcpp::get_logger(LOGGER_NAME),
+    RCLCPP_ERROR_STREAM(rclcpp::get_logger(LOGGER_NAME()),
                         ColorScheme::ERROR << "Connection not configured" << ColorScheme::ENDC);
     return;
   }
   if (!connection_ptr_->isOpen()) {
-    RCLCPP_ERROR_STREAM(rclcpp::get_logger(LOGGER_NAME),
+    RCLCPP_ERROR_STREAM(rclcpp::get_logger(LOGGER_NAME()),
                         ColorScheme::ERROR << "Connection not open" << ColorScheme::ENDC);
     return;
   }
   if (!app_ptr_) {
-    RCLCPP_ERROR_STREAM(rclcpp::get_logger(LOGGER_NAME),
+    RCLCPP_ERROR_STREAM(rclcpp::get_logger(LOGGER_NAME()),
                         ColorScheme::ERROR << "App not configured" << ColorScheme::ENDC);
     return;
   }
@@ -95,7 +95,7 @@ void App::perform_work_() {
                                 // may never return
     running_ = true;
     if (async_client_ptr_->robotState().getSessionState() == KUKA::FRI::ESessionState::IDLE) {
-      RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME), "LBR in session state idle, exiting");
+      RCLCPP_INFO(rclcpp::get_logger(LOGGER_NAME()), "LBR in session state idle, exiting");
       break;
     }
   }
@@ -104,7 +104,7 @@ void App::perform_work_() {
 
 bool App::valid_port_(const int &port_id) {
   if (port_id < 30200 || port_id > 30209) {
-    RCLCPP_ERROR_STREAM(rclcpp::get_logger(LOGGER_NAME),
+    RCLCPP_ERROR_STREAM(rclcpp::get_logger(LOGGER_NAME()),
                         ColorScheme::ERROR << "Expected port_id in [30200, 30209], got '"
                                            << ColorScheme::BOLD << port_id << "'"
                                            << ColorScheme::ENDC);

lbr_fri_ros2/src/ft_estimator.cpp

@@ -1,41 +1,54 @@
 #include "lbr_fri_ros2/ft_estimator.hpp"
 
 namespace lbr_fri_ros2 {
-FTEstimator::FTEstimator(const std::string &robot_description, const std::string &chain_root,
-                         const std::string &chain_tip, const_cart_array_t_ref f_ext_th)
-    : f_ext_th_(f_ext_th) {
+FTEstimatorImpl::FTEstimatorImpl(const std::string &robot_description,
+                                 const std::string &chain_root, const std::string &chain_tip,
+                                 const_cart_array_t_ref f_ext_th, const double &damping)
+    : f_ext_th_(f_ext_th), damping_(damping) {
   kinematics_ptr_ = std::make_unique<Kinematics>(robot_description, chain_root, chain_tip);
   reset();
 }
 
-void FTEstimator::compute(const_jnt_array_t_ref measured_joint_position,
-                          const_jnt_array_t_ref external_torque, cart_array_t_ref f_ext,
-                          const double &damping) {
-  tau_ext_ = Eigen::Map<const Eigen::Matrix<double, N_JNTS, 1>>(external_torque.data());
-  auto jacobian = kinematics_ptr_->compute_jacobian(measured_joint_position);
-  jacobian_inv_ = pinv(jacobian.data, damping);
-  f_ext_ = jacobian_inv_.transpose() * tau_ext_;
+void FTEstimatorImpl::compute() {
+  auto jacobian = kinematics_ptr_->compute_jacobian(q_);
+  jacobian_inv_ = pinv(jacobian.data, damping_);
+  f_ext_raw_ = jacobian_inv_.transpose() * tau_ext_;
+  int i = -1;
+  f_ext_ = f_ext_raw_.unaryExpr([&](double v) {
+    ++i;
+    if (std::abs(v) < f_ext_th_[i]) {
+      return 0.;
+    } else {
+      return std::copysign(1., v) * (std::abs(v) - f_ext_th_[i]);
+    }
+  });
 
   // rotate into chain tip frame
-  auto chain_tip_frame = kinematics_ptr_->compute_fk(measured_joint_position);
-  f_ext_.topRows(3) = Eigen::Matrix3d::Map(chain_tip_frame.M.data) * f_ext_.topRows(3);
-  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);
-                   }
-                 });
+  auto chain_tip_frame = kinematics_ptr_->compute_fk(q_);
+  f_ext_tf_.topRows(3) = Eigen::Matrix3d::Map(chain_tip_frame.M.data) * f_ext_.topRows(3);
+  f_ext_tf_.bottomRows(3) = Eigen::Matrix3d::Map(chain_tip_frame.M.data) * f_ext_.bottomRows(3);
 }
 
-void FTEstimator::reset() {
+void FTEstimatorImpl::reset() {
+  std::for_each(q_.begin(), q_.end(), [](double &q_i) { q_i = 0.; });
   tau_ext_.setZero();
+  f_ext_raw_.setZero();
   f_ext_.setZero();
+  f_ext_tf_.setZero();
+  jacobian_inv_.setZero();
 }
+
+FTEstimator::FTEstimator(const std::shared_ptr<FTEstimatorImpl> ft_estimator_impl_ptr,
+                         const std::uint16_t &update_rate)
+    : ft_estimator_impl_ptr_(ft_estimator_impl_ptr), update_rate_(update_rate) {}
+
+void FTEstimator::perform_work_() {
+  running_ = true;
+  while (rclcpp::ok() && !should_stop_) {
+    auto start = std::chrono::high_resolution_clock::now();
+    ft_estimator_impl_ptr_->compute();
+    std::this_thread::sleep_until(start + std::chrono::nanoseconds(static_cast<int>(
+                                              1.e9 / static_cast<double>(update_rate_))));
+  }
+};
 } // namespace lbr_fri_ros2