stuck joint recovery

Author: filiparag

mep3_hardware/include/mep3_hardware/dynamixel_hardware_interface/dynamixel_hardware.hpp

@@ -25,6 +25,8 @@
 #include <map>
 #include <vector>
 #include <deque>
+#include <optional>
+#include <chrono>
 
 #include "mep3_hardware/dynamixel_hardware_interface/visibility_control.hpp"
 #include "rclcpp/macros.hpp"
@@ -34,7 +36,7 @@ using hardware_interface::return_type;
 
 namespace dynamixel_hardware
 {
-struct JointValue
+struct JointState
 {
   double position{0.0};
   double velocity{0.0};
@@ -43,12 +45,21 @@ struct JointValue
   double temperature{0.0};
   bool overloaded;
   std::deque<double> previous_efforts_{};
+  std::optional<std::chrono::time_point<std::chrono::system_clock>> last_max_effort_{};
+};
+
+struct JointCommand
+{
+  double position{0.0};
+  double velocity{0.0};
+  double effort{0.0};
+  double initial_position_{0.0};
 };
 
 struct Joint
 {
-  JointValue state{};
-  JointValue command{};
+  JointState state{};
+  JointCommand command{};
 };
 
 enum class ControlMode {
@@ -111,7 +122,8 @@ class DynamixelHardware
   bool use_dummy_{false};
   double offset_{0};
   bool keep_read_write_thread_{true};
-  unsigned int effort_filter_ {0};
+  unsigned int effort_average_ {0};
+  std::chrono::milliseconds recovery_timeout_{250};
 };
 }  // namespace dynamixel_hardware
 

mep3_hardware/src/dynamixel_hardware_interface/dynamixel_hardware.cpp

@@ -60,26 +60,31 @@ namespace dynamixel_hardware
     joint_ids_.resize(info_.joints.size(), 0);
 
     try {
-      effort_filter_ = std::stoi(info_.hardware_parameters.at("effort_filter"));
-      RCLCPP_INFO(rclcpp::get_logger(kDynamixelHardware), "effort_filter: %d", effort_filter_);
+      effort_average_ = std::stoi(info_.hardware_parameters.at("effort_average"));
+      RCLCPP_INFO(rclcpp::get_logger(kDynamixelHardware), "effort_average: %d samples", effort_average_);
     } catch (const std::out_of_range& e) {
-      RCLCPP_INFO(rclcpp::get_logger(kDynamixelHardware), "effort_filter: %d [default]", effort_filter_);
+      RCLCPP_INFO(rclcpp::get_logger(kDynamixelHardware), "effort_average: %d samples [default]", effort_average_);
     }
 
     for (uint i = 0; i < info_.joints.size(); i++)
     {
       joint_ids_[i] = std::stoi(info_.joints[i].parameters.at("id"));
+      // Command
+      joints_[i].command.position = std::numeric_limits<double>::quiet_NaN();
+      joints_[i].command.velocity = std::numeric_limits<double>::quiet_NaN();
+      joints_[i].command.effort = std::numeric_limits<double>::quiet_NaN();
+      // State
       joints_[i].state.position = std::numeric_limits<double>::quiet_NaN();
       joints_[i].state.velocity = std::numeric_limits<double>::quiet_NaN();
       joints_[i].state.effort = std::numeric_limits<double>::quiet_NaN();
       joints_[i].state.voltage = std::numeric_limits<double>::quiet_NaN();
       joints_[i].state.temperature = std::numeric_limits<double>::quiet_NaN();
+      // Bookkeeping
+      joints_[i].command.initial_position_ = std::numeric_limits<double>::quiet_NaN();
       joints_[i].state.overloaded = false;
       joints_[i].state.previous_efforts_ = std::deque<double>();
-      joints_[i].state.previous_efforts_.resize(effort_filter_);
-      joints_[i].command.position = std::numeric_limits<double>::quiet_NaN();
-      joints_[i].command.velocity = std::numeric_limits<double>::quiet_NaN();
-      joints_[i].command.effort = std::numeric_limits<double>::quiet_NaN();
+      joints_[i].state.previous_efforts_.resize(effort_average_);
+      joints_[i].state.last_max_effort_.reset();
       RCLCPP_INFO(rclcpp::get_logger(kDynamixelHardware), "joint_id %d: %d", i, joint_ids_[i]);
     }
 
@@ -101,6 +106,13 @@ namespace dynamixel_hardware
     RCLCPP_INFO(rclcpp::get_logger(kDynamixelHardware), "usb_port: %s", usb_port.c_str());
     RCLCPP_INFO(rclcpp::get_logger(kDynamixelHardware), "baud_rate: %d", baud_rate);
 
+    try {
+      recovery_timeout_ = std::chrono::milliseconds(stoi(info_.hardware_parameters.at("recovery_timeout")));
+      RCLCPP_INFO(rclcpp::get_logger(kDynamixelHardware), "recovery_timeout: %ld ms", recovery_timeout_.count());
+    } catch (const std::out_of_range& e) {
+      RCLCPP_INFO(rclcpp::get_logger(kDynamixelHardware), "recovery_timeout: %ld ms [default]", recovery_timeout_.count());
+    }
+
     if (!dynamixel_workbench_.init(usb_port.c_str(), baud_rate, &log))
     {
       RCLCPP_FATAL(rclcpp::get_logger(kDynamixelHardware), "%s", log);
@@ -271,6 +283,11 @@ namespace dynamixel_hardware
     }
 
     read_from_hardware();
+    for (uint i = 0; i < joints_.size(); i++)
+    {
+      // Save initial position for recovery
+      joints_[i].command.initial_position_ = joints_[i].state.position;
+    }
     reset_command();
     write_to_hardware();
 
@@ -450,6 +467,7 @@ namespace dynamixel_hardware
 
       int16_t load =     (present_data[4] | ((0x3 & present_data[5]) << 8));
       bool overload = (unsigned) load > TORQUE_LOAD_MAX;
+      bool max_torque = (unsigned) load == TORQUE_LOAD_MAX;
       // data[5] third bit determines effort sign
       if (present_data[5] & 0x4)
         load = -load;
@@ -465,7 +483,7 @@ namespace dynamixel_hardware
 
       if (!overload) {
         double effort = dynamixel_workbench_.convertValue2Current(load);
-        if (joints_[i].state.previous_efforts_.size() >= effort_filter_) {
+        if (joints_[i].state.previous_efforts_.size() >= effort_average_) {
           joints_[i].state.previous_efforts_.pop_front();
         }
         joints_[i].state.previous_efforts_.push_back(effort);
@@ -478,6 +496,27 @@ namespace dynamixel_hardware
         joints_[i].state.effort = std::numeric_limits<double>::infinity();
       }
 
+      if (max_torque) {
+        if (joints_[i].state.last_max_effort_.has_value()) {
+          const auto time_delta = std::chrono::system_clock::now() - joints_[i].state.last_max_effort_.value();
+          const auto duration = std::chrono::duration_cast<std::chrono::milliseconds>(time_delta);
+          if (duration > recovery_timeout_) {
+            RCLCPP_WARN(
+              rclcpp::get_logger(kDynamixelHardware),
+              "Recovering stuck joint %s to initial position %.3lf",
+              info_.joints[i].name.c_str(),
+              joints_[i].command.initial_position_
+            );
+            joints_[i].command.position = joints_[i].command.initial_position_;
+            joints_[i].state.effort = std::numeric_limits<double>::infinity();
+          }
+        } else {
+          joints_[i].state.last_max_effort_ = std::chrono::system_clock::now();
+        }
+      } else {
+        joints_[i].state.last_max_effort_.reset();
+      }
+
       // RCLCPP_WARN(
       //   rclcpp::get_logger(kDynamixelHardware),
       //   "DYNAMIXEL [ position: %6.2f rad | speed: %6.2f rad/s | load: %6.2f mA | voltage: %6.2f V | temperature: %6.2f C ]",

mep3_hardware/test/dynamixel/description.urdf

@@ -8,7 +8,8 @@
             <param name="baud_rate">115200</param>
             <param name="use_dummy">false</param>
             <param name="offset">0.0</param>
-            <param name="effort_filter">2</param>
+            <param name="effort_average">2</param>
+            <param name="recovery_timeout">100</param>
         </hardware>
         <joint name="joint1">
             <param name="id">5</param>