Add support extended position mode

README.md

@@ -36,9 +36,11 @@ Note that `joint_ids` parameters must be splited by `,`.
   <param name="usb_port">/dev/ttyUSB0</param>
   <param name="baud_rate">1000000</param>
   <!-- <param name="use_dummy">true</param> -->
+  <!-- <param name="extended_mode">true</param> -->
 </hardware>
 ```
 
+To use extended position mode on the Dynamixel motor, set the `extended_mode` parameter to `true`.
 - Terminal 1
 
 Launch the `ros2_control` manager for the OpenManipulator-X.

dynamixel_hardware/include/dynamixel_hardware/dynamixel_hardware.hpp

@@ -104,6 +104,7 @@ class DynamixelHardware : public hardware_interface::SystemInterface
   ControlMode control_mode_{ControlMode::Position};
   bool mode_changed_{false};
   bool use_dummy_{false};
+  bool is_extended_mode_{false};
 };
 }  // namespace dynamixel_hardware
 

dynamixel_hardware/src/dynamixel_hardware.cpp

@@ -101,8 +101,12 @@ CallbackReturn DynamixelHardware::on_init(const hardware_interface::HardwareInfo
     }
   }
 
+  if (info_.hardware_parameters.find("extended_mode") != info_.hardware_parameters.end()) {
+    is_extended_mode_ = info_.hardware_parameters.at("extended_mode") == "true" ||
+                        info_.hardware_parameters.at("extended_mode") == "1";
+  }
   enable_torque(false);
-  set_control_mode(ControlMode::Position, true);
+  set_control_mode(is_extended_mode_ ? ControlMode::ExtendedPosition : ControlMode::Position, true);
   set_joint_params();
   enable_torque(true);
 
@@ -326,7 +330,7 @@ return_type DynamixelHardware::write(
         return j.command.position != j.prev_command.position;
       }))
   {
-    set_control_mode(ControlMode::Position);
+    set_control_mode(is_extended_mode_ ? ControlMode::ExtendedPosition : ControlMode::Position);
     if (mode_changed_) {
       set_joint_params();
     }
@@ -350,6 +354,7 @@ return_type DynamixelHardware::write(
       return return_type::OK;
       break;
     case ControlMode::Position:
+    case ControlMode::ExtendedPosition:
       set_joint_positions();
       return return_type::OK;
       break;
@@ -440,7 +445,35 @@ return_type DynamixelHardware::set_control_mode(const ControlMode & mode, const
     return return_type::OK;
   }
 
-  if (control_mode_ != ControlMode::Velocity && control_mode_ != ControlMode::Position) {
+  if (
+    mode == ControlMode::ExtendedPosition &&
+    (force_set || control_mode_ != ControlMode::ExtendedPosition)) {
+    bool torque_enabled = torque_enabled_;
+    if (torque_enabled) {
+      enable_torque(false);
+    }
+
+    for (uint i = 0; i < joint_ids_.size(); ++i) {
+      if (!dynamixel_workbench_.setExtendedPositionControlMode(joint_ids_[i], &log)) {
+        RCLCPP_FATAL(rclcpp::get_logger(kDynamixelHardware), "%s", log);
+        return return_type::ERROR;
+      }
+    }
+    RCLCPP_INFO(rclcpp::get_logger(kDynamixelHardware), "Extended Position control");
+    if (control_mode_ != ControlMode::ExtendedPosition) {
+      mode_changed_ = true;
+      control_mode_ = ControlMode::ExtendedPosition;
+    }
+
+    if (torque_enabled) {
+      enable_torque(true);
+    }
+    return return_type::OK;
+  }
+
+  if (
+    control_mode_ != ControlMode::Velocity && control_mode_ != ControlMode::Position &&
+    control_mode_ != ControlMode::ExtendedPosition) {
     RCLCPP_FATAL(
       rclcpp::get_logger(kDynamixelHardware), "Only position/velocity control are implemented");
     return return_type::ERROR;