Skip to content

Bump 1.5.0 #95

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
robotpilot merged 27 commits into from
Nov 26, 2025
Merged

Bump 1.5.0 #95

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
27 commits
Select commit Hold shift + click to select a range
7835626
Refactor: Update Dynamixel interface to support communication ID hand…
Woojin-Crive Oct 14, 2025
ab4879c
Update Dynamixel model file to reflect new model number for hand join…
Woojin-Crive Oct 14, 2025
3229677
Refactor Dynamixel model files
Woojin-Crive Oct 16, 2025
1c8563b
Update Dynamixel model file names to include 'd20_r' prefix for synch…
Woojin-Crive Oct 16, 2025
d09ef99
Update Dynamixel model file names for synchronization tables to corre…
Woojin-Crive Oct 16, 2025
9574a83
Update Dynamixel model files to include unit info for position data
Woojin-Crive Nov 3, 2025
f292f69
feat: Add OverrideUnitInfo method and support for unit info overrides…
Woojin-Crive Nov 3, 2025
668afd0
feat: Trim whitespace in Dynamixel hardware interface item initializa…
Woojin-Crive Nov 4, 2025
fe2eb62
refactor: Update Dynamixel interface for improved item initialization…
Woojin-Crive Nov 12, 2025
ad32bde
refactor: Update synchronization table model names in Dynamixel confi…
Woojin-Crive Nov 12, 2025
d562b83
feat: Implement reboot functionality for Dynamixel devices during ini…
Woojin-Crive Nov 12, 2025
324fd61
fix: Correct reboot logic in Dynamixel hardware interface to ensure p…
Woojin-Crive Nov 12, 2025
fcb8b29
fix: Reduce sleep duration during reboot in Dynamixel hardware interf…
Woojin-Crive Nov 13, 2025
4343a04
Update Dynamixel model files to change unit notation from 'N/m' to 'N…
Woojin-Crive Nov 14, 2025
88ae1fb
refactor: Replace dynamic memory allocation with std::vector for tran…
Woojin-Crive Nov 17, 2025
d4c7ad6
Update Dynamixel model files to set Present Input Voltage offset to 0…
Woojin-Crive Nov 17, 2025
ee76bca
Update unit info for rh_p12_rna
Woojin-Crive Nov 25, 2025
73e1f97
refactor: Standardize formatting in Dynamixel interface header and im…
Woojin-Crive Nov 25, 2025
0477c66
refactor: Add <utility> header to Dynamixel interface files for enhan…
Woojin-Crive Nov 25, 2025
8c3990f
refactor: Improve code formatting and readability in SyncTable model …
Woojin-Crive Nov 25, 2025
02d1c65
refactor: Update parameter documentation in SyncTable model generator…
Woojin-Crive Nov 25, 2025
81087c2
chore: Add copyright notice and licensing information to SyncTable mo…
Woojin-Crive Nov 25, 2025
156b962
refactor: Enhance code readability by standardizing formatting and in…
Woojin-Crive Nov 25, 2025
9ba58cf
refactor: Standardize error message formatting in Dynamixel implement…
Woojin-Crive Nov 25, 2025
bd00040
refactor: Update Offset column to unit info in Dynamixel model files …
Woojin-Crive Nov 26, 2025
931e073
chore: Release version 1.5.0
Woojin-Crive Nov 26, 2025
68d0d28
Merge pull request #93 from ROBOTIS-GIT/feature-major-update
Woojin-Crive Nov 26, 2025
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
9 changes: 9 additions & 0 deletions CHANGELOG.rst
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -2,6 +2,15 @@
Changelog for package dynamixel_hardware_interface
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^

1.5.0 (2025-11-26)
------------------
* Added comm_id/id concept for virtual_* devices.
* Added unit info system for Unified unit conversion logic.
* Added sequential initialization logic.
* Fixed memory leak of matrix malloc.
* Refactored every type info based unit conversion to unit info based system.
* Contributors: Woojin Wie

1.4.16 (2025-10-14)
-------------------
* Added support for default unit information for Present Input Voltage to model files
Expand Down
49 changes: 31 additions & 18 deletions include/dynamixel_hardware_interface/dynamixel/dynamixel.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -28,6 +28,7 @@
#include <cstdarg>
#include <memory>
#include <functional>
#include <utility>

namespace dynamixel_hardware_interface
{
Expand Down Expand Up @@ -164,6 +165,7 @@ typedef struct
*/
typedef struct
{
uint8_t comm_id; ///< Communication ID used to reach the device.
uint8_t id; ///< ID of the Dynamixel motor.
ControlItem control_item; ///< Control item details.
uint32_t data; ///< Data associated with the control item.
Expand Down Expand Up @@ -197,7 +199,7 @@ class Dynamixel
// item write variable
std::vector<RWItemBufInfo> write_item_buf_;
std::vector<RWItemBufInfo> read_item_buf_;
std::map<uint8_t /*id*/, bool> torque_state_;
std::map<std::pair<uint8_t /*comm_id*/, uint8_t /*id*/>, bool> torque_state_;

// read item (sync or bulk) variable
bool read_type_;
Expand Down Expand Up @@ -234,26 +236,25 @@ class Dynamixel
// direct inform for bulk write
std::map<uint8_t /*id*/, IndirectInfo> direct_info_write_;

std::map<uint8_t /*id*/, uint8_t> comm_id_;

public:
explicit Dynamixel(const char * path);
~Dynamixel();

// DXL Communication Setting
DxlError InitDxlComm(std::vector<uint8_t> id_arr, std::string port_name, std::string baudrate);
DxlError SetupPort(const std::string & port_name, const std::string & baudrate);
DxlError InitDxlComm(uint8_t comm_id, uint8_t id);
DxlError Reboot(uint8_t id);
void RWDataReset();

// DXL Read Setting
DxlError SetDxlReadItems(
uint8_t id, uint8_t comm_id, std::vector<std::string> item_names,
uint8_t comm_id, uint8_t id, std::vector<std::string> item_names,
std::vector<std::shared_ptr<double>> data_vec_ptr);
DxlError SetMultiDxlRead();

// DXL Write Setting
DxlError SetDxlWriteItems(
uint8_t id, uint8_t comm_id, std::vector<std::string> item_names,
uint8_t comm_id, uint8_t id, std::vector<std::string> item_names,
std::vector<std::shared_ptr<double>> data_vec_ptr);
DxlError SetMultiDxlWrite();

Expand All @@ -264,34 +265,44 @@ class Dynamixel

// Set Dxl Option
// DxlError SetOperatingMode(uint8_t id, uint8_t dynamixel_mode);
DxlError DynamixelEnable(std::vector<uint8_t> id_arr);
DxlError DynamixelDisable(std::vector<uint8_t> id_arr);
DxlError DynamixelEnable(const std::vector<std::pair<uint8_t, uint8_t>> & comm_id_id_arr);
DxlError DynamixelDisable(const std::vector<std::pair<uint8_t, uint8_t>> & comm_id_id_arr);

// DXL Item Write
DxlError WriteItem(uint8_t id, std::string item_name, uint32_t data);
DxlError WriteItem(uint8_t id, uint16_t addr, uint8_t size, uint32_t data);
DxlError WriteItem(uint8_t comm_id, uint8_t id, std::string item_name, uint32_t data);
DxlError WriteItem(uint8_t comm_id, uint8_t id, uint16_t addr, uint8_t size, uint32_t data);
DxlError InsertWriteItemBuf(uint8_t id, std::string item_name, uint32_t data);

@gemini-code-assist gemini-code-assist Bot Nov 26, 2025

Copy link
Copy Markdown
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

critical

The signature for InsertWriteItemBuf should include comm_id to properly support virtual devices, where comm_id can be different from id. The current implementation assumes comm_id == id, which is incorrect for virtual devices accessed via services.

  DxlError InsertWriteItemBuf(uint8_t comm_id, uint8_t id, std::string item_name, uint32_t data);

DxlError WriteItemBuf();

// DXL Item Read
DxlError ReadItem(uint8_t id, std::string item_name, uint32_t & data);
DxlError ReadItem(uint8_t comm_id, uint8_t id, std::string item_name, uint32_t & data);
DxlError InsertReadItemBuf(uint8_t id, std::string item_name);

@gemini-code-assist gemini-code-assist Bot Nov 26, 2025

Copy link
Copy Markdown
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

critical

The signature for InsertReadItemBuf should include comm_id to properly support virtual devices. The current implementation assumes comm_id == id, which is incorrect for virtual devices accessed via services.

  DxlError InsertReadItemBuf(uint8_t comm_id, uint8_t id, std::string item_name);

DxlError ReadItemBuf();
bool CheckReadItemBuf(uint8_t id, std::string item_name);

@gemini-code-assist gemini-code-assist Bot Nov 26, 2025

Copy link
Copy Markdown
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

critical

The signature for CheckReadItemBuf should include comm_id to correctly identify the device, especially when multiple devices might share the same id but have different comm_ids (e.g., virtual devices). The current implementation could lead to ambiguity.

  bool CheckReadItemBuf(uint8_t comm_id, uint8_t id, std::string item_name);

uint32_t GetReadItemDataBuf(uint8_t id, std::string item_name);

@gemini-code-assist gemini-code-assist Bot Nov 26, 2025

Copy link
Copy Markdown
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

critical

The signature for GetReadItemDataBuf should include comm_id to correctly identify the device and retrieve its data. The current implementation could return data from the wrong device if multiple devices share an id.

  uint32_t GetReadItemDataBuf(uint8_t comm_id, uint8_t id, std::string item_name);


DynamixelInfo GetDxlInfo() {return dxl_info_;}
std::map<uint8_t, bool> GetDxlTorqueState() {return torque_state_;}
std::map<std::pair<uint8_t, uint8_t>, bool> GetDxlTorqueState() {return torque_state_;}

static std::string DxlErrorToString(DxlError error_num);

DxlError ReadDxlModelFile(uint8_t id, uint16_t model_num);
DxlError ReadDxlModelFile(uint8_t id, uint16_t model_num, uint8_t firmware_version);
DxlError ReadFirmwareVersion(uint8_t id, uint8_t & firmware_version);
DxlError ReadDxlModelFile(uint8_t comm_id, uint8_t id, uint16_t model_num);
DxlError ReadDxlModelFile(
uint8_t comm_id, uint8_t id, uint16_t model_num,
uint8_t firmware_version);
DxlError ReadFirmwareVersion(uint8_t comm_id, uint8_t id, uint8_t & firmware_version);

void SetCommId(uint8_t id, uint8_t comm_id) {comm_id_[id] = comm_id;}
DxlError InitTorqueStates(
std::vector<std::pair<uint8_t, uint8_t>> comm_id_id_arr,
bool disable_torque = false);

DxlError InitTorqueStates(std::vector<uint8_t> id_arr, bool disable_torque = false);
void OverrideUnitInfo(
uint8_t comm_id,
uint8_t id,
const std::string & data_name,
double unit_multiplier,
bool is_signed,
double offset_value);

private:
bool checkReadType();
Expand Down Expand Up @@ -334,7 +345,7 @@ class Dynamixel
std::function<uint32_t(uint8_t, uint16_t, uint8_t)> get_data_func);

DxlError ProcessDirectReadData(
uint8_t id,
uint8_t comm_id,
const std::vector<uint16_t> & item_addrs,
const std::vector<std::string> & item_names,
const std::vector<uint8_t> & item_sizes,
Expand Down Expand Up @@ -372,6 +383,7 @@ class Dynamixel

// Helper function for value conversion with unit info
double ConvertValueWithUnitInfo(
uint8_t comm_id,
uint8_t id,
std::string item_name,
uint32_t raw_value,
Expand All @@ -380,6 +392,7 @@ class Dynamixel

// Helper function for converting unit values to raw values
uint32_t ConvertUnitValueToRawValue(
uint8_t comm_id,
uint8_t id,
std::string item_name,
double unit_value,
Expand Down
63 changes: 41 additions & 22 deletions include/dynamixel_hardware_interface/dynamixel/dynamixel_info.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -52,6 +52,7 @@ typedef struct
std::vector<ControlItem> item;
std::map<std::string, double> unit_map;
std::map<std::string, bool> sign_type_map;
std::map<std::string, double> offset_map;
} DxlInfo;

class DynamixelInfo
Expand All @@ -69,56 +70,74 @@ class DynamixelInfo
uint8_t ExtractFirmwareVersionFromFilename(const std::string & filename);

public:
// Id, Control table
std::map<uint8_t, DxlInfo> dxl_info_;
// comm_id -> (id -> Control table)
std::map<uint8_t, std::map<uint8_t, DxlInfo>> dxl_info_by_comm_;

DynamixelInfo() {}
~DynamixelInfo() {}

void SetDxlModelFolderPath(const char * path);
void InitDxlModelInfo();

void ReadDxlModelFile(uint8_t id, uint16_t model_num);
void ReadDxlModelFile(uint8_t id, uint16_t model_num, uint8_t firmware_version);
bool GetDxlControlItem(uint8_t id, std::string item_name, uint16_t & addr, uint8_t & size);
bool CheckDxlControlItem(uint8_t id, std::string item_name);
void ReadDxlModelFile(uint8_t comm_id, uint8_t id, uint16_t model_num);
void ReadDxlModelFile(uint8_t comm_id, uint8_t id, uint16_t model_num, uint8_t firmware_version);
bool GetDxlControlItem(
uint8_t comm_id, uint8_t id, std::string item_name, uint16_t & addr,
uint8_t & size);
bool CheckDxlControlItem(uint8_t comm_id, uint8_t id, std::string item_name);
Comment on lines +84 to +87

@gemini-code-assist gemini-code-assist Bot Nov 26, 2025

Copy link
Copy Markdown
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

medium

For improved performance and to adhere to C++ best practices, std::string parameters should be passed by const std::string& to avoid unnecessary copies. This applies to GetDxlControlItem, CheckDxlControlItem, and other methods in this class like GetDxlUnitValue, ConvertValueToUnit, etc.

  bool GetDxlControlItem(
    uint8_t comm_id, uint8_t id, const std::string & item_name, uint16_t & addr,
    uint8_t & size);
  bool CheckDxlControlItem(uint8_t comm_id, uint8_t id, const std::string & item_name);


bool GetDxlUnitValue(uint8_t id, std::string data_name, double & unit_value);
bool GetDxlSignType(uint8_t id, std::string data_name, bool & is_signed);
bool GetDxlUnitValue(uint8_t comm_id, uint8_t id, std::string data_name, double & unit_value);
bool GetDxlSignType(uint8_t comm_id, uint8_t id, std::string data_name, bool & is_signed);
bool GetDxlOffsetValue(uint8_t comm_id, uint8_t id, std::string data_name, double & offset_value);

// Template-based conversion methods
template<typename T>
double ConvertValueToUnit(uint8_t id, std::string data_name, T value);
double ConvertValueToUnit(uint8_t comm_id, uint8_t id, std::string data_name, T value);

template<typename T>
T ConvertUnitToValue(uint8_t id, std::string data_name, double unit_value);
T ConvertUnitToValue(uint8_t comm_id, uint8_t id, std::string data_name, double unit_value);

// Helper method for internal use
double GetUnitMultiplier(uint8_t id, std::string data_name);
double GetUnitMultiplier(uint8_t comm_id, uint8_t id, std::string data_name);

int32_t ConvertRadianToValue(uint8_t id, double radian);
double ConvertValueToRadian(uint8_t id, int32_t value);
int32_t ConvertRadianToValue(uint8_t comm_id, uint8_t id, double radian);
double ConvertValueToRadian(uint8_t comm_id, uint8_t id, int32_t value);

std::string GetModelName(uint16_t model_number) const;
};

// Template implementations
template<typename T>
double DynamixelInfo::ConvertValueToUnit(uint8_t id, std::string data_name, T value)
double DynamixelInfo::ConvertValueToUnit(
uint8_t comm_id, uint8_t id, std::string data_name,
T value)
{
auto it = dxl_info_[id].unit_map.find(data_name);
if (it != dxl_info_[id].unit_map.end()) {
return static_cast<double>(value) * it->second;
auto & info = dxl_info_by_comm_[comm_id][id];
auto it = info.unit_map.find(data_name);
if (it != info.unit_map.end()) {
double converted_value = static_cast<double>(value) * it->second;
auto offset_it = info.offset_map.find(data_name);
if (offset_it != info.offset_map.end()) {
converted_value += offset_it->second;
}
return converted_value;
}
return static_cast<double>(value);
}

template<typename T>
T DynamixelInfo::ConvertUnitToValue(uint8_t id, std::string data_name, double unit_value)
T DynamixelInfo::ConvertUnitToValue(
uint8_t comm_id, uint8_t id, std::string data_name,
double unit_value)
{
auto it = dxl_info_[id].unit_map.find(data_name);
if (it != dxl_info_[id].unit_map.end()) {
return static_cast<T>(unit_value / it->second);
auto & info = dxl_info_by_comm_[comm_id][id];
auto it = info.unit_map.find(data_name);
if (it != info.unit_map.end()) {
double adjusted_value = unit_value;
auto offset_it = info.offset_map.find(data_name);
if (offset_it != info.offset_map.end()) {
adjusted_value -= offset_it->second;
}
return static_cast<T>(adjusted_value / it->second);
}
return static_cast<T>(unit_value);
}
Expand Down
36 changes: 12 additions & 24 deletions include/dynamixel_hardware_interface/dynamixel_hardware_interface.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -23,6 +23,7 @@
#include <map>
#include <unordered_map>
#include <functional>
#include <utility>

#include "rclcpp/rclcpp.hpp"
#include "ament_index_cpp/get_package_share_directory.hpp"
Expand Down Expand Up @@ -182,8 +183,8 @@ class DynamixelHardware : public
std::map<uint8_t /*id*/, uint8_t /*err*/> dxl_hw_err_;
std::map<uint8_t /*id*/, uint8_t /*error code*/> dxl_error_code_;
DxlTorqueStatus dxl_torque_status_;
std::map<uint8_t /*id*/, bool /*enable*/> dxl_torque_state_;
std::vector<uint8_t> torque_enabled_ids_;
std::map<std::pair<uint8_t /*comm_id*/, uint8_t /*id*/>, bool /*enable*/> dxl_torque_state_;
std::vector<std::pair<uint8_t, uint8_t>> torque_enabled_comm_id_id_;
double err_timeout_ms_;
rclcpp::Duration read_error_duration_{0, 0};
rclcpp::Duration write_error_duration_{0, 0};
Expand Down Expand Up @@ -228,13 +229,13 @@ class DynamixelHardware : public
///// dxl variable
std::string port_name_;
std::string baud_rate_;
std::vector<uint8_t> dxl_id_;
std::vector<uint8_t> virtual_dxl_id_;
std::vector<std::pair<uint8_t, uint8_t>> dxl_comm_id_id_;
std::vector<std::pair<uint8_t, uint8_t>> virtual_dxl_comm_id_id_;

std::vector<uint8_t> sensor_id_;
std::vector<std::pair<uint8_t, uint8_t>> sensor_comm_id_id_;
std::map<uint8_t /*id*/, std::string /*interface_name*/> sensor_item_;

std::vector<uint8_t> controller_id_;
std::vector<std::pair<uint8_t, uint8_t>> controller_comm_id_id_;
std::map<uint8_t /*id*/, std::string /*interface_name*/> controller_item_;

///// handler variable
Expand All @@ -256,27 +257,14 @@ class DynamixelHardware : public
// joint <-> transmission matrix
size_t num_of_joints_;
size_t num_of_transmissions_;
double ** transmission_to_joint_matrix_;
double ** joint_to_transmission_matrix_;
std::vector<std::vector<double>> transmission_to_joint_matrix_;
std::vector<std::vector<double>> joint_to_transmission_matrix_;

/**
* @brief Helper function to initialize items for a specific type.
* @param type_filter The type of items to initialize ("controller" or "dxl" or "sensor").
* @brief Helper function to initialize items
* @return True if initialization was successful, false otherwise.
*/
bool initItems(const std::string & type_filter);

/**
* @brief Initializes Dynamixel items.
* @return True if initialization was successful, false otherwise.
*/
bool InitDxlItems();

/**
* @brief Initializes the controller items.
* @return True if initialization was successful, false otherwise.
*/
bool InitControllerItems();
bool InitItem(const hardware_interface::ComponentInfo & gpio);

/**
* @brief Initializes the read items for Dynamixel.
Expand Down Expand Up @@ -377,7 +365,7 @@ class DynamixelHardware : public
size_t inner_size,
std::vector<HandlerVarType> & outer_handlers,
std::vector<HandlerVarType> & inner_handlers,
double ** matrix,
const std::vector<std::vector<double>> & matrix,
const std::unordered_map<std::string, std::vector<std::string>> & iface_map,
const std::string & conversion_iface = "",
const std::string & conversion_name = "",
Expand Down
2 changes: 1 addition & 1 deletion package.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -2,7 +2,7 @@
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
<name>dynamixel_hardware_interface</name>
<version>1.4.16</version>
<version>1.5.0</version>
<description>
ROS 2 package providing a hardware interface for controlling Dynamixel motors via the ROS 2 control framework.
</description>
Expand Down
18 changes: 6 additions & 12 deletions param/dxl_model/2xc430_w250.model
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,16 +1,10 @@
[type info]
name value
value_of_zero_radian_position 2048
value_of_max_radian_position 4095
value_of_min_radian_position 0
min_radian -3.14159265
max_radian 3.14159265

[unit info]
Data Name value unit Sign Type
Present Velocity 0.0239691227 rad/s signed
Goal Velocity 0.0239691227 rad/s signed
Present Input Voltage 0.1 V unsigned
Data Name value unit Sign Type Offset
Present Velocity 0.0239691227 rad/s signed 0.0
Goal Velocity 0.0239691227 rad/s signed 0.0
Present Position 0.0015339807878856412 rad signed -3.14159265359
Goal Position 0.0015339807878856412 rad signed -3.14159265359
Present Input Voltage 0.1 V unsigned 0.0

[control table]
Address Size Data Name
Expand Down
18 changes: 6 additions & 12 deletions param/dxl_model/2xl430_w250.model
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -1,16 +1,10 @@
[type info]
name value
value_of_zero_radian_position 2048
value_of_max_radian_position 4095
value_of_min_radian_position 0
min_radian -3.14159265
max_radian 3.14159265

[unit info]
Data Name value unit Sign Type
Present Velocity 0.0239691227 rad/s signed
Goal Velocity 0.0239691227 rad/s signed
Present Input Voltage 0.1 V unsigned
Data Name value unit Sign Type Offset
Present Velocity 0.0239691227 rad/s signed 0.0
Goal Velocity 0.0239691227 rad/s signed 0.0
Present Position 0.0015339807878856412 rad signed -3.14159265359
Goal Position 0.0015339807878856412 rad signed -3.14159265359
Present Input Voltage 0.1 V unsigned 0.0

[control table]
Address Size Data Name
Expand Down
Loading
Loading