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