refactor(franka): use thread safe value class

Author: juelg

extensions/rcs_fr3/src/hw/Franka.cpp

@@ -86,8 +86,7 @@ FrankaState* Franka::get_state() {
   if (this->running_controller.load() == Controller::none) {
     current_robot_state = this->robot.readOnce();
   } else {
-    std::lock_guard<std::mutex> lock(this->interpolator_mutex);
-    current_robot_state = this->curr_state;
+    current_robot_state = this->curr_state.load();
   }
   auto* state = new FrankaState();
   state->robot_state = current_robot_state;
@@ -111,12 +110,10 @@ common::Pose Franka::get_cartesian_position() {
   this->check_for_background_errors();
   franka::RobotState robot_state;
   if (this->running_controller.load() == Controller::none) {
-    this->curr_state = this->robot.readOnce();
-    robot_state = this->curr_state;
+    robot_state = this->robot.readOnce();
+    this->curr_state.store(robot_state);
   } else {
-    this->interpolator_mutex.lock();
-    robot_state = this->curr_state;
-    this->interpolator_mutex.unlock();
+    robot_state = this->curr_state.load();
   }
   return GetTCPInBaseFrame(robot_state, this->m_cfg.tcp_offset);
 }
@@ -125,11 +122,10 @@ common::Pose Franka::get_cartesian_flange_position() {
   this->check_for_background_errors();
   franka::RobotState robot_state;
   if (this->running_controller.load() == Controller::none) {
-    this->curr_state = this->robot.readOnce();
-    robot_state = this->curr_state;
+    robot_state = this->robot.readOnce();
+    this->curr_state.store(robot_state);
   } else {
-    std::lock_guard<std::mutex> lock(this->interpolator_mutex);
-    robot_state = this->curr_state;
+    robot_state = this->curr_state.load();
   }
   return GetFlangeInBaseFrame(robot_state);
 }
@@ -146,16 +142,14 @@ void Franka::set_joint_position(const common::VectorXd& q) {
 
 common::VectorXd Franka::get_joint_position() {
   this->check_for_background_errors();
-  common::Vector7d joints;
+  franka::RobotState robot_state;
   if (this->running_controller.load() == Controller::none) {
-    this->curr_state = this->robot.readOnce();
-    joints = common::Vector7d(this->curr_state.q.data());
+    robot_state = this->robot.readOnce();
+    this->curr_state.store(robot_state);
   } else {
-    this->interpolator_mutex.lock();
-    joints = common::Vector7d(this->curr_state.q.data());
-    this->interpolator_mutex.unlock();
+    robot_state = this->curr_state.load();
   }
-  return joints;
+  return common::Vector7d(robot_state.q.data());
 }
 
 void Franka::set_guiding_mode(bool x, bool y, bool z, bool roll, bool pitch,
@@ -219,25 +213,25 @@ void Franka::controller_set_joint_position(const common::Vector7d& desired_q) {
     this->interpolator_mutex.lock();
   }
 
+  franka::RobotState state_now = this->curr_state.load();
+  const common::Vector7d q_now =
+      Eigen::Map<common::Vector7d>(state_now.q.data());
+
   const bool approach_on_start =
       starting_fresh && this->m_cfg.blocking_move_on_start;
   double approach_time = -1.0;
   if (approach_on_start) {
     const double kMinApproachTime = 0.3;  // s
     const double kMaxApproachTime = 5.0;  // s
-    const common::Vector7d q_now =
-        Eigen::Map<common::Vector7d>(this->curr_state.q.data());
     const double max_gap = (desired_q - q_now).cwiseAbs().maxCoeff();
     const double speed = std::max(this->m_cfg.approach_joint_speed, 1e-6);
     approach_time =
         std::clamp(max_gap / speed, kMinApproachTime, kMaxApproachTime);
   }
 
   this->joint_interpolator.reset(
-      this->controller_time,
-      Eigen::Map<common::Vector7d>(this->curr_state.q.data()), desired_q,
-      this->m_active_policy_rate, traj_rate, traj_interpolation_time_fraction,
-      approach_time);
+      this->controller_time, q_now, desired_q, this->m_active_policy_rate,
+      traj_rate, traj_interpolation_time_fraction, approach_time);
 
   // if not thread is running, then start
   if (starting_fresh) {
@@ -256,16 +250,12 @@ void Franka::controller_set_joint_position(const common::Vector7d& desired_q) {
     const auto start = std::chrono::steady_clock::now();
     while (true) {
       this->check_for_background_errors();
-      common::Vector7d q;
-      common::Vector7d dq;
-      {
-        std::lock_guard<std::mutex> lock(this->interpolator_mutex);
-        q = Eigen::Map<common::Vector7d>(this->curr_state.q.data());
-        dq = Eigen::Map<common::Vector7d>(this->curr_state.dq.data());
-      }
+      franka::RobotState state = this->curr_state.load();
+      const common::Vector7d q = Eigen::Map<common::Vector7d>(state.q.data());
+      const common::Vector7d dq = Eigen::Map<common::Vector7d>(state.dq.data());
       const double elapsed = std::chrono::duration<double>(
                                  std::chrono::steady_clock::now() - start)
-              .count();
+                                 .count();
       const double pos_err = (q - desired_q).cwiseAbs().maxCoeff();
       const double vel = dq.cwiseAbs().maxCoeff();
       if (elapsed >= approach_time && pos_err < pos_tol && vel < vel_tol) {
@@ -280,18 +270,15 @@ void Franka::controller_set_joint_position(const common::Vector7d& desired_q) {
 }
 
 void Franka::check_for_background_errors() {
-  std::lock_guard<std::mutex> lock(this->exception_mutex);
-  if (this->background_exception) {
+  std::exception_ptr ex = this->background_exception.load_and_clear();
+  if (ex) {
     this->stop_control_thread();
-    std::exception_ptr ex = this->background_exception;
-    this->background_exception = nullptr;
     std::rethrow_exception(ex);
   }
 }
 
 void Franka::clear_background_error() {
-  std::lock_guard<std::mutex> lock(this->exception_mutex);
-  this->background_exception = nullptr;
+  this->background_exception.store(nullptr);
 }
 
 void Franka::osc_set_cartesian_position(
@@ -308,10 +295,7 @@ void Franka::osc_set_cartesian_position(
   if (starting_fresh) {
     this->controller_time = 0.0;
     this->m_active_policy_rate = this->m_cfg.policy_rate;
-    {
-      std::lock_guard<std::mutex> lock(this->interpolator_mutex);
-      this->curr_state = this->robot.readOnce();
-    }
+    this->curr_state.store(this->robot.readOnce());
     this->traj_interpolator = common::LinearPoseTrajInterpolator();
   } else if (this->running_controller.load() != Controller::osc) {
     throw std::runtime_error(
@@ -323,7 +307,7 @@ void Franka::osc_set_cartesian_position(
   }
 
   common::Pose curr_pose =
-      GetTCPInBaseFrame(this->curr_state, this->m_cfg.tcp_offset);
+      GetTCPInBaseFrame(this->curr_state.load(), this->m_cfg.tcp_offset);
 
   const bool approach_on_start =
       starting_fresh && this->m_cfg.blocking_move_on_start;
@@ -375,13 +359,8 @@ void Franka::osc_set_cartesian_position(
         desired_pose_EE_in_base_frame.quaternion().normalized();
     while (true) {
       this->check_for_background_errors();
-      franka::RobotState state;
-      common::Vector7d dq;
-      {
-        std::lock_guard<std::mutex> lock(this->interpolator_mutex);
-        state = this->curr_state;
-        dq = Eigen::Map<common::Vector7d>(this->curr_state.dq.data());
-      }
+      franka::RobotState state = this->curr_state.load();
+      const common::Vector7d dq = Eigen::Map<common::Vector7d>(state.dq.data());
       const common::Pose meas_pose =
           GetTCPInBaseFrame(state, this->m_cfg.tcp_offset);
       const double pos_err = (target_p - meas_pose.translation()).norm();
@@ -391,7 +370,7 @@ void Franka::osc_set_cartesian_position(
       const double vel = dq.cwiseAbs().maxCoeff();
       const double elapsed = std::chrono::duration<double>(
                                  std::chrono::steady_clock::now() - start)
-              .count();
+                                 .count();
       if (elapsed >= approach_time && pos_err < pos_tol && ori_err < ori_tol &&
           vel < vel_tol) {
         break;
@@ -494,8 +473,9 @@ void Franka::osc() {
       int policy_rate = 20;
       int traj_rate = 500;
 
+      this->curr_state.store(robot_state);
+
       this->interpolator_mutex.lock();
-      this->curr_state = robot_state;
       this->controller_time += period.toSec();
       this->traj_interpolator.next_step(this->controller_time,
                                         desired_pos_EE_in_base_frame,
@@ -638,8 +618,7 @@ void Franka::osc() {
       return tau_d_rate_limited;
     });
   } catch (...) {
-    std::lock_guard<std::mutex> lock(this->exception_mutex);
-    this->background_exception = std::current_exception();
+    this->background_exception.store(std::current_exception());
   }
 
   // Ensure we mark the controller as stopped so we can restart later
@@ -685,8 +664,9 @@ void Franka::joint_controller() {
 
       common::Vector7d desired_q;
 
+      this->curr_state.store(robot_state);
+
       this->interpolator_mutex.lock();
-      this->curr_state = robot_state;
       this->controller_time += period.toSec();
       this->joint_interpolator.next_step(this->controller_time, desired_q);
       this->interpolator_mutex.unlock();
@@ -734,8 +714,7 @@ void Franka::joint_controller() {
       return tau_d_rate_limited;
     });
   } catch (...) {
-    std::lock_guard<std::mutex> lock(this->exception_mutex);
-    this->background_exception = std::current_exception();
+    this->background_exception.store(std::current_exception());
   }
 
   this->running_controller.store(Controller::none);
@@ -767,8 +746,9 @@ void Franka::zero_torque_controller() {
   try {
     this->robot.control([&](const franka::RobotState& robot_state,
                             franka::Duration period) -> franka::Torques {
+      this->curr_state.store(robot_state);
+
       this->interpolator_mutex.lock();
-      this->curr_state = robot_state;
       this->controller_time += period.toSec();
       this->interpolator_mutex.unlock();
       if (this->running_controller.load() == Controller::none) {
@@ -778,8 +758,7 @@ void Franka::zero_torque_controller() {
       return franka::Torques({0, 0, 0, 0, 0, 0, 0});
     });
   } catch (...) {
-    std::lock_guard<std::mutex> lock(this->exception_mutex);
-    this->background_exception = std::current_exception();
+    this->background_exception.store(std::current_exception());
   }
 
   this->running_controller.store(Controller::none);

extensions/rcs_fr3/src/hw/Franka.h

@@ -119,11 +119,10 @@ class Franka : public common::Robot {
   // the interpolation window stays consistent for the controller's lifetime.
   int m_active_policy_rate = 20;
   common::LinearJointPositionTrajInterpolator joint_interpolator;
-  franka::RobotState curr_state;
+  common::ThreadSafeValue<franka::RobotState> curr_state;
   std::mutex interpolator_mutex;
   std::atomic<Controller> running_controller{Controller::none};
-  std::exception_ptr background_exception = nullptr;
-  std::mutex exception_mutex;
+  common::ThreadSafeValue<std::exception_ptr> background_exception;
   void osc();
   void joint_controller();
   void zero_torque_controller();

include/rcs/utils.h

@@ -2,7 +2,12 @@
 #define RCS_UTIL_H
 
 #include <Eigen/Eigen>
+#include <array>
+#include <deque>
 #include <memory>
+#include <mutex>
+#include <utility>
+#include <vector>
 
 namespace rcs {
 namespace common {
@@ -39,6 +44,83 @@ void bootstrap_egl(std::uintptr_t fn_addr, std::uintptr_t display,
                    std::uintptr_t context);
 void ensure_current();
 
+/***
+ * @brief thread safe holder for a single value, e.g. to hand data from a
+ * non-realtime thread to a control loop
+ */
+template <typename T>
+class ThreadSafeValue {
+ private:
+  T value_;
+  mutable std::mutex mutex_;
+
+ public:
+  ThreadSafeValue() = default;
+  explicit ThreadSafeValue(const T& value) : value_(value) {}
+
+  void store(const T& value) {
+    std::lock_guard<std::mutex> lock(mutex_);
+    value_ = value;
+  }
+
+  T load() const {
+    std::lock_guard<std::mutex> lock(mutex_);
+    return value_;
+  }
+
+  /***
+   * @brief atomically read the value and reset it to a default constructed one
+   */
+  T load_and_clear() {
+    std::lock_guard<std::mutex> lock(mutex_);
+    return std::exchange(value_, T{});
+  }
+};
+
+/***
+ * @brief thread safe ring buffer of fixed maximum size, drops the oldest
+ * element once the buffer is full
+ */
+template <typename T>
+class ThreadSafeFixedBuffer {
+ private:
+  std::deque<T> deque_;
+  size_t max_size_;
+  mutable std::mutex mutex_;
+
+ public:
+  explicit ThreadSafeFixedBuffer(size_t max_size) : max_size_(max_size) {}
+
+  void push_back(const T& value) {
+    std::lock_guard<std::mutex> lock(mutex_);
+
+    deque_.push_back(value);
+    if (deque_.size() > max_size_) {
+      deque_.pop_front();
+    }
+  }
+
+  T get(size_t index) const {
+    std::lock_guard<std::mutex> lock(mutex_);
+    return deque_[index];
+  }
+
+  size_t size() const {
+    std::lock_guard<std::mutex> lock(mutex_);
+    return deque_.size();
+  }
+
+  void clear() {
+    std::lock_guard<std::mutex> lock(mutex_);
+    deque_.clear();
+  }
+
+  std::vector<T> to_vector() const {
+    std::lock_guard<std::mutex> lock(mutex_);
+    return std::vector<T>(deque_.begin(), deque_.end());
+  }
+};
+
 }  // namespace common
 }  // namespace rcs