Dynamixel effort state processing

mep3_behavior/include/mep3_behavior/joint_position_command_action.hpp

@@ -26,6 +26,18 @@
 
 namespace mep3_behavior
 {
+ std::string recovery_mode(const int8_t mode) {
+ switch (mode)
+ {
+ case mep3_msgs::action::JointPositionCommand::Goal::RECOVERY_STAY:
+ return "STAY";
+ case mep3_msgs::action::JointPositionCommand::Goal::RECOVERY_RETURN:
+ return "RETURN";
+ default:
+ return "";
+ }
+ }
+
  class JointPositionCommandAction
  : public BT::RosActionNode<mep3_msgs::action::JointPositionCommand>
  {
@@ -37,18 +49,63 @@ namespace mep3_behavior
  : BT::RosActionNode<mep3_msgs::action::JointPositionCommand>(name, conf, params, action_client)
  {
  if (!getInput("position", position_))
- throw BT::RuntimeError(
- "Dynamixel action requires 'position' argument");
+ throw BT::RuntimeError("Dynamixel action requires 'position' argument");
  if (!getInput("max_velocity", max_velocity_))
- max_velocity_ = 99999;
+ max_velocity_ = std::numeric_limits<double>::max(); // 99999
  if (!getInput("max_acceleration", max_acceleration_))
- max_acceleration_ = 99999;
+ max_acceleration_ = std::numeric_limits<double>::max(); // 99999
  if (!getInput("tolerance", tolerance_))
- tolerance_ = 9;
+ tolerance_ = std::numeric_limits<double>::max(); // 9
  if (!getInput("timeout", timeout_))
- timeout_ = 5;
+ timeout_ = std::numeric_limits<double>::max(); // 5
  if (!getInput("max_effort", max_effort_))
- max_effort_ = 99999;
+ max_effort_ = std::numeric_limits<double>::max(); // 99999
+
+ std::string recovery_combo;
+ if (getInput("recovery", recovery_combo)) {
+ if (recovery_combo == "stay:fail") {
+ // Stay but fail on recovery
+ fail_on_recovery_ = true;
+ recovery_mode_ = mep3_msgs::action::JointPositionCommand::Goal::RECOVERY_STAY;
+ recovery_position_ = std::numeric_limits<double>::quiet_NaN();
+ } else if (recovery_combo == "stay:ok") {
+ // Stay and succeed on recovery
+ fail_on_recovery_ = false;
+ recovery_mode_ = mep3_msgs::action::JointPositionCommand::Goal::RECOVERY_STAY;
+ recovery_position_ = std::numeric_limits<double>::quiet_NaN();
+ } else if (recovery_combo == "return:fail") {
+ // Return but fail on recovery
+ fail_on_recovery_ = true;
+ recovery_mode_ = mep3_msgs::action::JointPositionCommand::Goal::RECOVERY_RETURN;
+ recovery_position_ = std::numeric_limits<double>::quiet_NaN();
+ } else if (recovery_combo == "return:ok") {
+ // Return and succeed on recovery
+ fail_on_recovery_ = false;
+ recovery_mode_ = mep3_msgs::action::JointPositionCommand::Goal::RECOVERY_RETURN;
+ recovery_position_ = std::numeric_limits<double>::quiet_NaN();
+ } else if (recovery_combo.rfind("goto:", 0) == 0) {
+ // Recover to position...
+ recovery_mode_ = mep3_msgs::action::JointPositionCommand::Goal::RECOVERY_RETURN;
+ if (std::sscanf(recovery_combo.c_str(),"goto:%lf:fail", &position_) != EOF) {
+ // ...and fail
+ fail_on_recovery_ = true;
+ } else if (std::sscanf(recovery_combo.c_str(),"goto:%lf:ok", &position_) != EOF) {
+ // ...and succeed
+ fail_on_recovery_ = false;
+ } else {
+ // Parsing error
+ throw BT::RuntimeError("Dynamixel action parsing error for 'recovery' argument");
+ }
+ } else {
+ // Unknown argument value
+ throw BT::RuntimeError("Dynamixel action unknown value for 'recovery' argument");
+ }
+ } else {
+ // Default recovery behavior
+ fail_on_recovery_ = true;
+ recovery_mode_ = mep3_msgs::action::JointPositionCommand::Goal::RECOVERY_STAY;
+ recovery_position_ = std::numeric_limits<double>::quiet_NaN();
+ }
 
  std::string table = this->config().blackboard->get<std::string>("table");
  double position_offset;
@@ -63,14 +120,19 @@ namespace mep3_behavior
  std::cout << "Dynamixel desired position to θ=" << position_
  << " max_velocity=" << max_velocity_
  << " max effort=" << max_effort_
- << " max_acceleration=" << max_acceleration_ << std::endl;
+ << " max_acceleration=" << max_acceleration_ << std::endl
+ << " recovery_mode=" << recovery_mode(recovery_mode_) << std::endl
+ << " recovery_position=" << recovery_position_ << std::endl
+ << " fail_on_recovery_=" << fail_on_recovery_ << std::endl;
 
  goal.position = position_ * M_PI / 180;
  goal.max_velocity = max_velocity_ * M_PI / 180;
  goal.max_acceleration = max_acceleration_ * M_PI / 180;
  goal.tolerance = tolerance_ * M_PI / 180;
  goal.timeout = timeout_;
  goal.max_effort = max_effort_;
+ goal.recovery_mode = recovery_mode_;
+ goal.recovery_position = recovery_position_;
  return true;
  }
 
@@ -85,6 +147,7 @@ namespace mep3_behavior
  BT::InputPort<double>("max_effort"),
  BT::InputPort<double>("tolerance"),
  BT::InputPort<double>("timeout"),
+ BT::InputPort<double>("recovery"),
  BT::OutputPort<double>("feedback_effort"),
  BT::OutputPort<double>("feedback_position"),
  BT::OutputPort<double>("result")};
@@ -105,6 +168,20 @@ namespace mep3_behavior
  setOutput("feedback_position", wr.result->last_position);
  std::cout << "Last result: " << (double)wr.result->result << "; last effort: " << (double)wr.result->last_effort << "; last position: " << (double)wr.result->last_position << std::endl;
 
+ switch (wr.result->result)
+ {
+ case mep3_msgs::action::JointPositionCommand::Goal::RESULT_SUCCESS:
+ return BT::NodeStatus::SUCCESS;
+ case mep3_msgs::action::JointPositionCommand::Goal::RESULT_TIMEOUT:
+ case mep3_msgs::action::JointPositionCommand::Goal::RESULT_OVERLOAD:
+ case mep3_msgs::action::JointPositionCommand::Goal::RESULT_PREEMPTED:
+ return BT::NodeStatus::FAILURE;
+ case mep3_msgs::action::JointPositionCommand::Goal::RESULT_RECOVERY:
+ return fail_on_recovery_ ? BT::NodeStatus::FAILURE : BT::NodeStatus::SUCCESS;
+ default:
+ throw BT::RuntimeError("Dynamixel action got invalid result number");
+ }
+
  return BT::NodeStatus::SUCCESS;
  }
 
@@ -125,6 +202,9 @@ namespace mep3_behavior
  double tolerance_;
  double timeout_;
  double max_effort_;
+ bool fail_on_recovery_;
+ double recovery_position_;
+ int8_t recovery_mode_;
  };
 
 } // namespace mep3_behavior

mep3_controllers/include/mep3_controllers/joint_position_controller.hpp

@@ -13,23 +13,31 @@
 
 namespace mep3_controllers
 {
+
  struct Joint
  {
  Joint(){};
  std::optional<std::reference_wrapper<hardware_interface::LoanedCommandInterface>> position_command_handle;
+ std::optional<std::reference_wrapper<hardware_interface::LoanedCommandInterface>> recovery_position_command_handle;
+ std::optional<std::reference_wrapper<hardware_interface::LoanedCommandInterface>> recovery_mode_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::LoanedCommandInterface>> timeout_command_handle;
  std::optional<std::reference_wrapper<hardware_interface::LoanedStateInterface>> position_handle;
  std::optional<std::reference_wrapper<hardware_interface::LoanedStateInterface>> effort_handle;
+ std::optional<std::reference_wrapper<hardware_interface::LoanedStateInterface>> recovery_state_handle;
  std::string name;
 
  uint64_t start_time_ns;
  std::shared_ptr<nav2_util::SimpleActionServer<mep3_msgs::action::JointPositionCommand>> action_server;
 
  double target_position;
  double max_velocity;
+ double max_effort;
  double tolerance;
  double timeout;
- double max_effort;
+ int8_t recovery_mode;
+ double recovery_position;
  bool active;
  };