Merge pull request #1 from helix-robotics-ag/seb/add_ext_pos_mode

 Swap Position to ExtendedPosition mode

Author: sebtiburzio

dynamixel_hardware/src/dynamixel_hardware.cpp

@@ -102,7 +102,7 @@ CallbackReturn DynamixelHardware::on_init(const hardware_interface::HardwareInfo
   }
 
   enable_torque(false);
-  set_control_mode(ControlMode::Position, true);
+  set_control_mode(ControlMode::ExtendedPosition, true);
   set_joint_params();
   enable_torque(true);
 
@@ -305,7 +305,7 @@ return_type DynamixelHardware::write(const rclcpp::Time & /* time */, const rclc
   // Position control
   if (std::any_of(
         joints_.cbegin(), joints_.cend(), [](auto j) { return j.command.position != j.prev_command.position; })) {
-    set_control_mode(ControlMode::Position);
+    set_control_mode(ControlMode::ExtendedPosition);
     if(mode_changed_)
     {
       set_joint_params();
@@ -327,7 +327,7 @@ return_type DynamixelHardware::write(const rclcpp::Time & /* time */, const rclc
       set_joint_velocities();
       return return_type::OK;
       break;
-    case ControlMode::Position:
+    case ControlMode::ExtendedPosition:
       set_joint_positions();
       return return_type::OK;
       break;
@@ -396,23 +396,23 @@ return_type DynamixelHardware::set_control_mode(const ControlMode & mode, const
     return return_type::OK;
   }
 
-  if (mode == ControlMode::Position && (force_set || 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_.setPositionControlMode(joint_ids_[i], &log)) {
+      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), "Position control");
-    if(control_mode_ != ControlMode::Position)
+    if(control_mode_ != ControlMode::ExtendedPosition)
     {
       mode_changed_ = true;
-      control_mode_ = ControlMode::Position;
+      control_mode_ = ControlMode::ExtendedPosition;
     }
 
     if (torque_enabled) {
@@ -421,7 +421,7 @@ return_type DynamixelHardware::set_control_mode(const ControlMode & mode, const
     return return_type::OK;
   }
 
-  if (control_mode_ != ControlMode::Velocity && control_mode_ != ControlMode::Position) {
+  if (control_mode_ != ControlMode::Velocity && control_mode_ != ControlMode::ExtendedPosition) {
     RCLCPP_FATAL(
       rclcpp::get_logger(kDynamixelHardware), "Only position/velocity control are implemented");
     return return_type::ERROR;