From d62534439240c9b4a5002041e3be2733b35620ea Mon Sep 17 00:00:00 2001
From: Zheng Qu <quzhengrobot@gmail.com>
Date: Wed, 30 Jul 2025 00:02:41 +0200
Subject: [PATCH] Add interface to conditionally enable torque

---
 .../dynamixel_hardware/dynamixel_hardware.hpp   |  3 ++-
 dynamixel_hardware/src/dynamixel_hardware.cpp   | 17 ++++++++++++++---
 2 files changed, 16 insertions(+), 4 deletions(-)

diff --git a/dynamixel_hardware/include/dynamixel_hardware/dynamixel_hardware.hpp b/dynamixel_hardware/include/dynamixel_hardware/dynamixel_hardware.hpp
index d3d5746..238e466 100644
--- a/dynamixel_hardware/include/dynamixel_hardware/dynamixel_hardware.hpp
+++ b/dynamixel_hardware/include/dynamixel_hardware/dynamixel_hardware.hpp
@@ -100,7 +100,8 @@ class DynamixelHardware : public hardware_interface::SystemInterface
   std::map<const char * const, const ControlItem *> control_items_;
   std::vector<Joint> joints_;
   std::vector<uint8_t> joint_ids_;
-  bool torque_enabled_{false};
+  bool should_enable_torque_{false}; // True if torque should be enabled. False otherwise.
+  bool torque_enabled_{false}; // True if torque is enabled. False otherwise.
   ControlMode control_mode_{ControlMode::Position};
   bool mode_changed_{false};
   bool use_dummy_{false};
diff --git a/dynamixel_hardware/src/dynamixel_hardware.cpp b/dynamixel_hardware/src/dynamixel_hardware.cpp
index 720db9e..0a37644 100644
--- a/dynamixel_hardware/src/dynamixel_hardware.cpp
+++ b/dynamixel_hardware/src/dynamixel_hardware.cpp
@@ -81,6 +81,15 @@ CallbackReturn DynamixelHardware::on_init(const hardware_interface::HardwareInfo
     return CallbackReturn::SUCCESS;
   }
 
+  if (
+    info_.hardware_parameters.find("enable_torque") != info_.hardware_parameters.end() &&
+    info_.hardware_parameters.at("enable_torque") == "false")
+  {
+    should_enable_torque_ = false;
+  } else {
+    should_enable_torque_ = true;
+  }
+
   auto usb_port = info_.hardware_parameters.at("usb_port");
   auto baud_rate = std::stoi(info_.hardware_parameters.at("baud_rate"));
   const char * log = nullptr;
@@ -104,7 +113,9 @@ CallbackReturn DynamixelHardware::on_init(const hardware_interface::HardwareInfo
   enable_torque(false);
   set_control_mode(ControlMode::Position, true);
   set_joint_params();
-  enable_torque(true);
+  if (should_enable_torque_) {
+    enable_torque(true);
+  }
 
   const ControlItem * goal_position =
     dynamixel_workbench_.getItemInfo(joint_ids_[0], kGoalPositionItem);
@@ -410,7 +421,7 @@ return_type DynamixelHardware::set_control_mode(const ControlMode & mode, const
       control_mode_ = ControlMode::Velocity;
     }
 
-    if (torque_enabled) {
+    if (torque_enabled && should_enable_torque_) {
       enable_torque(true);
     }
     return return_type::OK;
@@ -434,7 +445,7 @@ return_type DynamixelHardware::set_control_mode(const ControlMode & mode, const
       control_mode_ = ControlMode::Position;
     }
 
-    if (torque_enabled) {
+    if (torque_enabled && should_enable_torque_) {
       enable_torque(true);
     }
     return return_type::OK;