Dynamixel effort state processing

mep3_controllers/include/mep3_controllers/joint_position_controller.hpp

@@ -18,6 +18,7 @@ namespace mep3_controllers
         Joint(){};
         std::optional<std::reference_wrapper<hardware_interface::LoanedCommandInterface>> position_command_handle;
         std::optional<std::reference_wrapper<hardware_interface::LoanedCommandInterface>> velocity_command_handle;
+        std::optional<std::reference_wrapper<hardware_interface::LoanedCommandInterface>> effort_command_handle;
         std::optional<std::reference_wrapper<hardware_interface::LoanedStateInterface>> position_handle;
         std::optional<std::reference_wrapper<hardware_interface::LoanedStateInterface>> effort_handle;
         std::string name;

mep3_controllers/src/joint_position_controller.cpp

@@ -13,25 +13,26 @@ namespace mep3_controllers
         auto goal = joint->action_server->get_current_goal();
         RCLCPP_INFO(
             get_node()->get_logger(),
-            "Motor %s action called to position %lf (velocity %lf, tolerance %lf)",
+            "Motor %s action called to position %.4lf (velocity %.2lf, tolerance %.3lf, max_effort %.2lf",
             joint->name.c_str(),
             goal->position,
             goal->max_velocity,
-            goal->tolerance);
+            goal->tolerance,
+            goal->max_effort);
 
         double max_velocity = 1.0;
         if (goal->max_velocity != 0)
             max_velocity = goal->max_velocity;
 
-        double tolerance = 99999;
+        double tolerance = std::numeric_limits<double>::max();
         if (goal->tolerance != 0)
             tolerance = goal->tolerance;
 
-        double timeout = 99999;
+        double timeout = std::numeric_limits<double>::max();
         if (goal->timeout != 0)
             timeout = goal->timeout;
 
-        double max_effort = 99999;
+        double max_effort = std::numeric_limits<double>::max();
         if (goal->max_effort != 0)
             max_effort = goal->max_effort;
 
@@ -99,6 +100,7 @@ namespace mep3_controllers
         {
             conf_names.push_back(joint->name + "/position");
             conf_names.push_back(joint->name + "/velocity");
+            conf_names.push_back(joint->name + "/effort");
         }
 
         return {controller_interface::interface_configuration_type::INDIVIDUAL, conf_names};
@@ -125,18 +127,13 @@ namespace mep3_controllers
             {
                 joint->position_command_handle->get().set_value(joint->target_position);
                 joint->velocity_command_handle->get().set_value(joint->max_velocity);
+                joint->effort_command_handle->get().set_value(joint->max_effort);
 
                 // Return the result
                 auto result = std::make_shared<mep3_msgs::action::JointPositionCommand::Result>();
                 result->set__last_effort(joint->effort_handle->get().get_value());
                 result->set__last_position(joint->position_handle->get().get_value());
-                if (fabs(joint->position_handle->get().get_value() - joint->target_position) < joint->tolerance)
-                {
-                    result->set__result(mep3_msgs::action::JointPositionCommand::Goal::RESULT_SUCCESS);
-                    joint->action_server->succeeded_current(result);
-                    joint->active = false;
-                }
-                else if (joint->action_server->is_cancel_requested())
+                if (joint->action_server->is_cancel_requested())
                 {
                     result->set__result(mep3_msgs::action::JointPositionCommand::Goal::RESULT_PREEMPTED);
                     joint->action_server->terminate_current(result);
@@ -157,13 +154,19 @@ namespace mep3_controllers
                     joint->active = false;
                     RCLCPP_ERROR(get_node()->get_logger(), "Joint %s timeout", joint->name.c_str());
                 }
-                else if (joint->effort_handle->get().get_value() > joint->max_effort)
+                else if (std::isinf(joint->effort_handle->get().get_value()) || joint->effort_handle->get().get_value() > joint->max_effort)
                 {
                     result->set__result(mep3_msgs::action::JointPositionCommand::Goal::RESULT_OVERLOAD);
                     joint->action_server->terminate_current(result);
                     joint->active = false;
                     RCLCPP_ERROR(get_node()->get_logger(), "Joint %s overload", joint->name.c_str());
                 }
+                else if (fabs(joint->position_handle->get().get_value() - joint->target_position) < joint->tolerance)
+                {
+                    result->set__result(mep3_msgs::action::JointPositionCommand::Goal::RESULT_SUCCESS);
+                    joint->action_server->succeeded_current(result);
+                    joint->active = false;
+                }
                 else
                 {
                     // TODO: If the action is terminated (with the feedback) then it crashes the whole BT.
@@ -193,7 +196,7 @@ namespace mep3_controllers
                 });
             if (position_command_handle == command_interfaces_.end())
             {
-                RCLCPP_ERROR(get_node()->get_logger(), "Unable to obtain joint command handle for %s", joint->name.c_str());
+                RCLCPP_ERROR(get_node()->get_logger(), "Unable to obtain joint position command handle for %s", joint->name.c_str());
                 return controller_interface::CallbackReturn::FAILURE;
             }
             joint->position_command_handle = std::ref(*position_command_handle);
@@ -208,11 +211,26 @@ namespace mep3_controllers
                 });
             if (velocity_command_handle == command_interfaces_.end())
             {
-                RCLCPP_ERROR(get_node()->get_logger(), "Unable to obtain joint command handle for %s", joint->name.c_str());
+                RCLCPP_ERROR(get_node()->get_logger(), "Unable to obtain joint velocity command handle for %s", joint->name.c_str());
                 return controller_interface::CallbackReturn::FAILURE;
             }
             joint->velocity_command_handle = std::ref(*velocity_command_handle);
 
+            // Effort command
+            const auto effort_command_handle = std::find_if(
+                command_interfaces_.begin(), command_interfaces_.end(),
+                [&joint](const auto &interface)
+                {
+                    return interface.get_prefix_name() == joint->name &&
+                           interface.get_interface_name() == hardware_interface::HW_IF_EFFORT;
+                });
+            if (effort_command_handle == command_interfaces_.end())
+            {
+                RCLCPP_ERROR(get_node()->get_logger(), "Unable to obtain joint effort command handle for %s", joint->name.c_str());
+                return controller_interface::CallbackReturn::FAILURE;
+            }
+            joint->effort_command_handle = std::ref(*effort_command_handle);
+
             // Position state
             const auto position_handle = std::find_if(
                 state_interfaces_.begin(), state_interfaces_.end(),
@@ -223,7 +241,7 @@ namespace mep3_controllers
                 });
             if (position_handle == state_interfaces_.end())
             {
-                RCLCPP_ERROR(get_node()->get_logger(), "Unable to obtain joint state handle for %s", joint->name.c_str());
+                RCLCPP_ERROR(get_node()->get_logger(), "Unable to obtain joint position state handle for %s", joint->name.c_str());
                 return controller_interface::CallbackReturn::FAILURE;
             }
             joint->position_handle = std::ref(*position_handle);
@@ -238,10 +256,11 @@ namespace mep3_controllers
                 });
             if (effort_handle == state_interfaces_.end())
             {
-                RCLCPP_ERROR(get_node()->get_logger(), "Unable to obtain joint state handle for %s", joint->name.c_str());
+                RCLCPP_ERROR(get_node()->get_logger(), "Unable to obtain joint effort state handle for %s", joint->name.c_str());
                 return controller_interface::CallbackReturn::FAILURE;
             }
             joint->effort_handle = std::ref(*effort_handle);
+
         }
         return controller_interface::CallbackReturn::SUCCESS;
     }

mep3_hardware/include/mep3_hardware/dynamixel_hardware_interface/dynamixel_hardware.hpp

@@ -24,6 +24,9 @@
 
 #include <map>
 #include <vector>
+#include <deque>
+#include <optional>
+#include <chrono>
 
 #include "mep3_hardware/dynamixel_hardware_interface/visibility_control.hpp"
 #include "rclcpp/macros.hpp"
@@ -33,7 +36,20 @@ using hardware_interface::return_type;
 
 namespace dynamixel_hardware
 {
-struct JointValue
+struct JointState
+{
+  double position{0.0};
+  double velocity{0.0};
+  double effort{0.0};
+  double voltage{0.0};
+  double temperature{0.0};
+  bool overloaded;
+  double recovery_position_{0.0};
+  std::deque<double> previous_efforts_{};
+  std::optional<std::chrono::time_point<std::chrono::system_clock>> high_torque_start{};
+};
+
+struct JointCommand
 {
   double position{0.0};
   double velocity{0.0};
@@ -42,8 +58,8 @@ struct JointValue
 
 struct Joint
 {
-  JointValue state{};
-  JointValue command{};
+  JointState state{};
+  JointCommand command{};
 };
 
 enum class ControlMode {
@@ -106,6 +122,9 @@ class DynamixelHardware
   bool use_dummy_{false};
   double offset_{0};
   bool keep_read_write_thread_{true};
+  unsigned int effort_average_ {0};
+  double torque_threshold_ {0.9};
+  std::chrono::milliseconds recovery_timeout_{250};
 };
 }  // namespace dynamixel_hardware