diff --git a/CHANGELOG.rst b/CHANGELOG.rst index d78dec0..2db904f 100644 --- a/CHANGELOG.rst +++ b/CHANGELOG.rst @@ -2,6 +2,14 @@ Changelog for package dynamixel_hardware_interface ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.4.0 (2025-03-20) +------------------ +* Added Torque Constant Parameter to DXL Model Files +* Enhanced Transmission Command Calculation +* Unified Initialization Structure +* Support for Goal Current Control +* Contributors: Woojin Wie + 1.3.0 (2025-02-17) ------------------ * Enhance Error Handling and Timeout Management diff --git a/include/dynamixel_hardware_interface/dynamixel/dynamixel_info.hpp b/include/dynamixel_hardware_interface/dynamixel/dynamixel_info.hpp index e099d3a..a7dc825 100644 --- a/include/dynamixel_hardware_interface/dynamixel/dynamixel_info.hpp +++ b/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) diff --git a/include/dynamixel_hardware_interface/dynamixel_hardware_interface.hpp b/include/dynamixel_hardware_interface/dynamixel_hardware_interface.hpp index 3ac2e04..5fc691a 100644 --- a/include/dynamixel_hardware_interface/dynamixel_hardware_interface.hpp +++ b/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 { diff --git a/package.xml b/package.xml index cc25ff0..fd090d8 100644 --- a/package.xml +++ b/package.xml @@ -1,7 +1,7 @@ dynamixel_hardware_interface - 1.3.0 + 1.4.0 ROS 2 package providing a hardware interface for controlling Dynamixel motors via the ROS 2 control framework. diff --git a/param/dxl_model/xc330_t181.model b/param/dxl_model/xc330_t181.model index cb4a784..21105f7 100644 --- a/param/dxl_model/xc330_t181.model +++ b/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 diff --git a/param/dxl_model/xc330_t288.model b/param/dxl_model/xc330_t288.model index cb4a784..68d3bbb 100644 --- a/param/dxl_model/xc330_t288.model +++ b/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 diff --git a/param/dxl_model/xh540_w150.model b/param/dxl_model/xh540_w150.model index f7d3ae5..11893d2 100644 --- a/param/dxl_model/xh540_w150.model +++ b/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 diff --git a/src/dynamixel/dynamixel.cpp b/src/dynamixel/dynamixel.cpp index 7e1dbbd..0ac8398 100644 --- a/src/dynamixel/dynamixel.cpp +++ b/src/dynamixel/dynamixel.cpp @@ -840,7 +840,7 @@ DxlError Dynamixel::GetDxlValueFromSyncRead() dxl_info_.ConvertValueRPMToVelocityRPS(ID, static_cast(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(dxl_getdata); + dxl_info_.ConvertCurrentToEffort(ID, static_cast(dxl_getdata)); } else { *it_read_data.item_data_ptr_vec.at(item_index) = static_cast(dxl_getdata); @@ -962,7 +962,7 @@ DxlError Dynamixel::GetDxlValueFromBulkRead() dxl_info_.ConvertValueRPMToVelocityRPS(ID, static_cast(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(dxl_getdata); + dxl_info_.ConvertCurrentToEffort(ID, static_cast(dxl_getdata)); } else { *it_read_data.item_data_ptr_vec.at(item_index) = static_cast(dxl_getdata); diff --git a/src/dynamixel/dynamixel_info.cpp b/src/dynamixel/dynamixel_info.cpp index 83565de..c252632 100644 --- a/src/dynamixel/dynamixel_info.cpp +++ b/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; } diff --git a/src/dynamixel_hardware_interface.cpp b/src/dynamixel_hardware_interface.cpp index a435e72..e3822b8 100644 --- a/src/dynamixel_hardware_interface.cpp +++ b/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(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(0.0)); - temp_write.interface_name_vec.push_back(it.name); - temp_write.value_ptr_vec.push_back(std::make_shared(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(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; + } + } + } } }