Support 'Goal Current' control for OpenMANIPULATOR-Y:

- Added new models: xh540_w150 and xc330_t288.
- Introduced torque constant parameter in relevant functions.

Author: Woojin-Crive

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
 {
 

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

@@ -712,13 +712,13 @@ bool DynamixelHardware::InitDxlWriteItems()
       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(it.name);
-          temp_write.value_ptr_vec.push_back(std::make_shared<double>(0.0));
-          hdl_trans_commands_.push_back(temp_write);
+        temp_write.value_ptr_vec.push_back(std::make_shared<double>(0.0));
+        hdl_trans_commands_.push_back(temp_write);
         }
       }
     }
@@ -853,14 +853,22 @@ 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++) {
+    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(GOAL_CURRENT_INDEX));
+    }
+    *hdl_trans_commands_.at(i).value_ptr_vec.at(GOAL_CURRENT_INDEX) = value;
   }
 }