From 01a3e6c347a00e5fefa9abaa3c23f0bb3ff721fd Mon Sep 17 00:00:00 2001
From: Tatsuro Sakaguchi <tatsuro.sakaguchi@g.softbank.co.jp>
Date: Tue, 10 Dec 2024 17:32:50 +0900
Subject: [PATCH 1/3] Support extended position mode

---
 .../dynamixel_hardware/dynamixel_hardware.hpp |  1 +
 dynamixel_hardware/src/dynamixel_hardware.cpp | 39 +++++++++++++++++--
 2 files changed, 37 insertions(+), 3 deletions(-)

diff --git a/dynamixel_hardware/include/dynamixel_hardware/dynamixel_hardware.hpp b/dynamixel_hardware/include/dynamixel_hardware/dynamixel_hardware.hpp
index d3d5746..b276258 100644
--- a/dynamixel_hardware/include/dynamixel_hardware/dynamixel_hardware.hpp
+++ b/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
 
diff --git a/dynamixel_hardware/src/dynamixel_hardware.cpp b/dynamixel_hardware/src/dynamixel_hardware.cpp
index 720db9e..6329d58 100644
--- a/dynamixel_hardware/src/dynamixel_hardware.cpp
+++ b/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;

From 5ae03e1ab773342062f4bc6e00f6eac08570eeeb Mon Sep 17 00:00:00 2001
From: Tatsuro Sakaguchi <tatsuro.sakaguchi@g.softbank.co.jp>
Date: Tue, 10 Dec 2024 17:37:07 +0900
Subject: [PATCH 2/3] Update README

---
 README.md | 4 ++++
 1 file changed, 4 insertions(+)

diff --git a/README.md b/README.md
index a9b8f8d..a6261e4 100644
--- a/README.md
+++ b/README.md
@@ -36,9 +36,13 @@ 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">false</param> <!-- Set true if you use the extended position mode of Dynamixel motors -->
 </hardware>
 ```
 
+Note that the `extended_mode` parameter is optional and set to `false` by default.
+If you use the extended position (multi turn) mode of Dynamixel motors, set it to `true`.
+
 - Terminal 1
 
 Launch the `ros2_control` manager for the OpenManipulator-X.

From 29ffe855477ce4038c8c6f0ac3237b750681f397 Mon Sep 17 00:00:00 2001
From: Tatsuro Sakaguchi <tacchan.mello.ioiq@gmail.com>
Date: Wed, 15 Jan 2025 15:15:23 +0900
Subject: [PATCH 3/3] Apply suggestions from code review

Co-authored-by: Kenji Brameld <kenjibrameld@gmail.com>
---
 README.md | 6 ++----
 1 file changed, 2 insertions(+), 4 deletions(-)

diff --git a/README.md b/README.md
index a6261e4..a115c61 100644
--- a/README.md
+++ b/README.md
@@ -36,13 +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">false</param> <!-- Set true if you use the extended position mode of Dynamixel motors -->
+  <!-- <param name="extended_mode">true</param> -->
 </hardware>
 ```
 
-Note that the `extended_mode` parameter is optional and set to `false` by default.
-If you use the extended position (multi turn) mode of Dynamixel motors, set it to `true`.
-
+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.