Support Current Control

include/dynamixel_hardware_interface/dynamixel/dynamixel_info.hpp

@@ -78,7 +78,8 @@ class DynamixelInfo
     int32_t & value_of_max_radian_position,
     int32_t & value_of_min_radian_position,
     double & min_radian,
-    double & max_radian);
+    double & max_radian,
+    double & torque_constant);
   int32_t ConvertRadianToValue(uint8_t id, double radian);
   double ConvertValueToRadian(uint8_t id, int32_t value);
   inline int16_t ConvertEffortToCurrent(uint8_t id, double effort)

include/dynamixel_hardware_interface/dynamixel_hardware_interface.hpp

@@ -47,6 +47,10 @@
 #define PRESENT_VELOCITY_INDEX 1
 #define PRESENT_EFFORT_INDEX 2
 
+#define GOAL_POSITION_INDEX 0
+// #define GOAL_VELOCITY_INDEX 1  // TODO: to be implemented
+#define GOAL_CURRENT_INDEX 1
+
 namespace dynamixel_hardware_interface
 {
 

param/dxl_model/xc330_t181.model

@@ -5,6 +5,7 @@ value_of_max_radian_position	4095
 value_of_min_radian_position	0
 min_radian	-3.14159265
 max_radian	3.14159265
+torque_constant	0.0006709470296015791
 
 [control table]
 Address	Size	Data Name

param/dxl_model/xc330_t288.model

@@ -5,6 +5,7 @@ value_of_max_radian_position	4095
 value_of_min_radian_position	0
 min_radian	-3.14159265
 max_radian	3.14159265
+torque_constant	0.0009674796739867748
 
 [control table]
 Address	Size	Data Name

param/dxl_model/xh540_w150.model

@@ -5,6 +5,7 @@ value_of_max_radian_position	4095
 value_of_min_radian_position	0
 min_radian	-3.14159265
 max_radian	3.14159265
+torque_constant	0.0045
 
 [control table]
 Address	Size	Data Name

src/dynamixel/dynamixel.cpp

@@ -840,7 +840,7 @@ DxlError Dynamixel::GetDxlValueFromSyncRead()
           dxl_info_.ConvertValueRPMToVelocityRPS(ID, static_cast<int32_t>(dxl_getdata));
       } else if (indirect_info_read_[ID].item_name.at(item_index) == "Present Current") {
         *it_read_data.item_data_ptr_vec.at(item_index) =
-          static_cast<int16_t>(dxl_getdata);
+          dxl_info_.ConvertCurrentToEffort(ID, static_cast<int16_t>(dxl_getdata));
       } else {
         *it_read_data.item_data_ptr_vec.at(item_index) =
           static_cast<double>(dxl_getdata);
@@ -962,7 +962,7 @@ DxlError Dynamixel::GetDxlValueFromBulkRead()
           dxl_info_.ConvertValueRPMToVelocityRPS(ID, static_cast<int32_t>(dxl_getdata));
       } else if (indirect_info_read_[ID].item_name.at(item_index) == "Present Current") {
         *it_read_data.item_data_ptr_vec.at(item_index) =
-          static_cast<int16_t>(dxl_getdata);
+          dxl_info_.ConvertCurrentToEffort(ID, static_cast<int16_t>(dxl_getdata));
       } else {
         *it_read_data.item_data_ptr_vec.at(item_index) =
           static_cast<double>(dxl_getdata);

src/dynamixel/dynamixel_info.cpp

@@ -149,13 +149,15 @@ bool DynamixelInfo::GetDxlTypeInfo(
   int32_t & value_of_max_radian_position,
   int32_t & value_of_min_radian_position,
   double & min_radian,
-  double & max_radian)
+  double & max_radian,
+  double & torque_constant)
 {
   value_of_zero_radian_position = dxl_info_[id].value_of_zero_radian_position;
   value_of_max_radian_position = dxl_info_[id].value_of_max_radian_position;
   value_of_min_radian_position = dxl_info_[id].value_of_min_radian_position;
   min_radian = dxl_info_[id].min_radian;
   max_radian = dxl_info_[id].max_radian;
+  torque_constant = dxl_info_[id].torque_constant;
   return true;
 }
 

src/dynamixel_hardware_interface.cpp

@@ -711,15 +711,21 @@ bool DynamixelHardware::InitDxlWriteItems()
     for (const hardware_interface::ComponentInfo & gpio : info_.gpios) {
       if (gpio.command_interfaces.size()) {
         uint8_t id = static_cast<uint8_t>(stoi(gpio.parameters.at("ID")));
-        for (auto it : gpio.command_interfaces) {
-          HandlerVarType temp_write;
-          temp_write.id = id;
-          temp_write.name = gpio.name;
+        HandlerVarType temp_write;
+        temp_write.id = id;
+        temp_write.name = gpio.name;
+
+        temp_write.interface_name_vec.push_back("Goal Position");
+        temp_write.value_ptr_vec.push_back(std::make_shared<double>(0.0));
 
-          temp_write.interface_name_vec.push_back(it.name);
-          temp_write.value_ptr_vec.push_back(std::make_shared<double>(0.0));
-          hdl_trans_commands_.push_back(temp_write);
+        for (auto it : gpio.command_interfaces) {
+          if (it.name == "Goal Current") {
+            temp_write.interface_name_vec.push_back("Goal Current");
+            temp_write.value_ptr_vec.push_back(std::make_shared<double>(0.0));
+          }
         }
+
+        hdl_trans_commands_.push_back(temp_write);
       }
     }
     is_set_hdl = true;
@@ -853,14 +859,33 @@ void DynamixelHardware::CalcJointToTransmission()
     double value = 0.0;
     for (size_t j = 0; j < num_of_joints_; j++) {
       value += joint_to_transmission_matrix_[i][j] *
-        (*hdl_joint_commands_.at(j).value_ptr_vec.at(0));
+        (*hdl_joint_commands_.at(j).value_ptr_vec.at(GOAL_POSITION_INDEX));
     }
 
     if (hdl_trans_commands_.at(i).name == conversion_dxl_name_) {
       value = prismaticToRevolute(value);
     }
+    *hdl_trans_commands_.at(i).value_ptr_vec.at(GOAL_POSITION_INDEX) = value;
+  }
 
-    *hdl_trans_commands_.at(i).value_ptr_vec.at(0) = value;
+  for (size_t i = 0; i < num_of_transmissions_; i++) {
+    if (hdl_trans_commands_.at(i).interface_name_vec.size() > GOAL_CURRENT_INDEX &&
+      hdl_trans_commands_.at(i).interface_name_vec.at(GOAL_CURRENT_INDEX) == "Goal Current")
+    {
+      for (size_t j = 0; j < hdl_joint_commands_.size(); j++) {
+        if (hdl_joint_commands_.at(j).interface_name_vec.size() > GOAL_CURRENT_INDEX &&
+          hdl_joint_commands_.at(j).interface_name_vec.at(GOAL_CURRENT_INDEX) ==
+          hardware_interface::HW_IF_EFFORT)
+        {
+          double value = 0.0;
+          for (size_t k = 0; k < num_of_joints_; k++) {
+            value += joint_to_transmission_matrix_[i][k] *
+              (*hdl_joint_commands_.at(k).value_ptr_vec.at(GOAL_CURRENT_INDEX));
+          }
+          *hdl_trans_commands_.at(i).value_ptr_vec.at(GOAL_CURRENT_INDEX) = value;
+        }
+      }
+    }
   }
 }