Refactored torque management in Dynamixel interface

- Introduced a new map for torque enable states per Dynamixel ID.
- Removed global torque enable variable and updated logic to enable torque based on individual settings.
- Adjusted parameter handling to include torque enable settings during initialization.
- Updated model files to include additional parameters for current readings.

Author: Woojin-Crive

include/dynamixel_hardware_interface/dynamixel_hardware_interface.hpp

@@ -185,14 +185,13 @@ class DynamixelHardware : public
   std::map<uint8_t /*id*/, uint8_t /*err*/> dxl_hw_err_;
   DxlTorqueStatus dxl_torque_status_;
   std::map<uint8_t /*id*/, bool /*enable*/> dxl_torque_state_;
+  std::map<uint8_t /*id*/, bool /*enable*/> dxl_torque_enable_;
   double err_timeout_ms_;
   rclcpp::Duration read_error_duration_{0, 0};
   rclcpp::Duration write_error_duration_{0, 0};
   bool is_read_in_error_{false};
   bool is_write_in_error_{false};
 
-  bool global_torque_enable_{true};
-
   bool use_revolute_to_prismatic_{false};
   std::string conversion_dxl_name_{""};
   std::string conversion_joint_name_{""};

param/dxl_model/ph42_020_s300.model

@@ -62,7 +62,9 @@ Address	Size	Data Name
 592	2	Present Input Voltage
 594	1	Present Temperature
 878	1	Backup Ready
+168	2	Indirect Address 1
+634	1	Indirect Data 1
 168	2	Indirect Address Write
 634	1	Indirect Data Write
-350	2	Indirect Address Read
+296	2	Indirect Address Read
 698	1	Indirect Data Read

param/dxl_model/xl430_w250.model

@@ -43,6 +43,7 @@ Address	Size	Data Name
 98	1	Bus Watchdog
 100	2	Goal PWM
 104	4	Goal Velocity
+104	2	Goal Current
 108	4	Profile Acceleration
 112	4	Profile Velocity
 116	4	Goal Position
@@ -52,6 +53,7 @@ Address	Size	Data Name
 124	2	Present PWM
 126	2	Present Load
 128	4	Present Velocity
+128	2	Present Current
 132	4	Present Position
 136	4	Velocity Trajectory
 140	4	Position Trajectory

src/dynamixel_hardware_interface.cpp

@@ -22,6 +22,7 @@
 #include <memory>
 #include <vector>
 #include <string>
+#include <map>
 
 #include "hardware_interface/types/hardware_interface_type_values.hpp"
 #include "rclcpp/rclcpp.hpp"
@@ -76,17 +77,6 @@ hardware_interface::CallbackReturn DynamixelHardware::on_init(
       e.what());
   }
 
-  try {
-    global_torque_enable_ =
-      std::stoi(info_.hardware_parameters["torque_enable"]) != 0;
-    RCLCPP_INFO_STREAM(
-      logger_, "Torque enable parameter: " << global_torque_enable_);
-  } catch (const std::exception & e) {
-    RCLCPP_ERROR(
-      logger_, "Failed to parse torque_enable parameter: %s, using default value",
-      e.what());
-  }
-
   RCLCPP_INFO_STREAM(
     logger_,
     "port_name " << port_name_.c_str() << " / baudrate " << baud_rate_.c_str());
@@ -419,11 +409,15 @@ hardware_interface::CallbackReturn DynamixelHardware::start()
   }
   usleep(500 * 1000);
 
-  if (global_torque_enable_) {
-    dxl_comm_->DynamixelEnable(dxl_id_);
-  } else {
-    RCLCPP_INFO_STREAM(logger_, "Global Torque is disabled!");
+  // Enable torque only for Dynamixels that have torque enabled in their parameters
+  std::vector<uint8_t> torque_enabled_ids;
+  for (const auto& [id, enabled] : dxl_torque_enable_) {
+    if (enabled) {
+      torque_enabled_ids.push_back(id);
+    }
   }
+  
+  dxl_comm_->DynamixelEnable(torque_enabled_ids);
 
   RCLCPP_INFO_STREAM(logger_, "Dynamixel Hardware Start!");
 
@@ -666,21 +660,47 @@ bool DynamixelHardware::initItems(const std::string & type_filter)
     }
     uint8_t id = static_cast<uint8_t>(stoi(gpio.parameters.at("ID")));
 
-    // First write items containing "Limit"
-    for (auto it : gpio.parameters) {
-      if (it.first != "ID" && it.first != "type" && it.first.find("Limit") != std::string::npos) {
-        if (!retryWriteItem(id, it.first, static_cast<uint32_t>(stoi(it.second)))) {
+    // Handle torque enable parameter
+    bool torque_enabled = true;  // Default to enabled
+    if (gpio.parameters.find("Torque Enable") != gpio.parameters.end()) {
+      torque_enabled = std::stoi(gpio.parameters.at("Torque Enable")) != 0;
+    }
+    dxl_torque_enable_[id] = torque_enabled;
+
+    // Write parameters in two passes:
+    // 1. First pass: Write all Limit parameters
+    for (const auto& param : gpio.parameters) {
+      const std::string& param_name = param.first;
+      
+      // Skip special parameters
+      if (param_name == "ID" || param_name == "type" || param_name == "Torque Enable") {
+        continue;
+      }
+
+      // Write Limit parameters first
+      if (param_name.find("Limit") != std::string::npos) {
+        if (!retryWriteItem(id, param_name, static_cast<uint32_t>(stoi(param.second)))) {
           return false;
         }
       }
     }
 
-    // Then write the remaining items
-    for (auto it : gpio.parameters) {
-      if (it.first != "ID" && it.first != "type" && it.first.find("Limit") == std::string::npos) {
-        if (!retryWriteItem(id, it.first, static_cast<uint32_t>(stoi(it.second)))) {
-          return false;
-        }
+    // 2. Second pass: Write all non-Limit parameters
+    for (const auto& param : gpio.parameters) {
+      const std::string& param_name = param.first;
+      
+      // Skip special parameters
+      if (param_name == "ID" || param_name == "type" || param_name == "Torque Enable") {
+        continue;
+      }
+
+      // Skip Limit parameters (already written)
+      if (param_name.find("Limit") != std::string::npos) {
+        continue;
+      }
+
+      if (!retryWriteItem(id, param_name, static_cast<uint32_t>(stoi(param.second)))) {
+        return false;
       }
     }
   }