diff --git a/CHANGELOG.rst b/CHANGELOG.rst index af358b0..371c7a7 100644 --- a/CHANGELOG.rst +++ b/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package dynamixel_hardware_interface ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.4.11 (2025-07-21) +------------------ +* Added support for firmware version-aware model file selection +* Contributors: Woojin Wie + 1.4.10 (2025-07-18) ------------------ * Added unit system to model files diff --git a/include/dynamixel_hardware_interface/dynamixel/dynamixel.hpp b/include/dynamixel_hardware_interface/dynamixel/dynamixel.hpp index dbbc8f7..36f63bd 100644 --- a/include/dynamixel_hardware_interface/dynamixel/dynamixel.hpp +++ b/include/dynamixel_hardware_interface/dynamixel/dynamixel.hpp @@ -212,6 +212,8 @@ class Dynamixel 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); void SetCommId(uint8_t id, uint8_t comm_id) {comm_id_[id] = comm_id;} diff --git a/include/dynamixel_hardware_interface/dynamixel/dynamixel_info.hpp b/include/dynamixel_hardware_interface/dynamixel/dynamixel_info.hpp index 122e1e8..282c1d9 100644 --- a/include/dynamixel_hardware_interface/dynamixel/dynamixel_info.hpp +++ b/include/dynamixel_hardware_interface/dynamixel/dynamixel_info.hpp @@ -25,6 +25,7 @@ #include #include #include +#include #include @@ -46,6 +47,7 @@ typedef struct int32_t value_of_max_radian_position; int32_t value_of_min_radian_position; uint16_t model_num; + uint8_t firmware_version; std::vector item; std::map unit_map; @@ -60,6 +62,12 @@ class DynamixelInfo std::string dxl_model_file_dir; + // Firmware version-aware model file selection + std::string SelectModelFileByFirmwareVersion( + const std::string & base_model_name, + uint8_t firmware_version); + uint8_t ExtractFirmwareVersionFromFilename(const std::string & filename); + public: // Id, Control table std::map dxl_info_; @@ -71,6 +79,7 @@ class DynamixelInfo 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); @@ -89,6 +98,8 @@ class DynamixelInfo int32_t ConvertRadianToValue(uint8_t id, double radian); double ConvertValueToRadian(uint8_t id, int32_t value); + + std::string GetModelName(uint16_t model_number) const; }; // Template implementations diff --git a/package.xml b/package.xml index c264961..383b20c 100644 --- a/package.xml +++ b/package.xml @@ -2,7 +2,7 @@ dynamixel_hardware_interface - 1.4.10 + 1.4.11 ROS 2 package providing a hardware interface for controlling Dynamixel motors via the ROS 2 control framework. diff --git a/param/dxl_model/xc330_m181.model b/param/dxl_model/xc330_m181.model index fb8d6ef..0ff1069 100644 --- a/param/dxl_model/xc330_m181.model +++ b/param/dxl_model/xc330_m181.model @@ -66,8 +66,8 @@ Address Size Data Name 144 2 Present Input Voltage 146 1 Present Temperature 168 2 Indirect Address 1 -208 1 Indirect Data 1 +224 1 Indirect Data 1 168 2 Indirect Address Write -208 1 Indirect Data Write +224 1 Indirect Data Write 180 2 Indirect Address Read -214 1 Indirect Data Read +230 1 Indirect Data Read diff --git a/param/dxl_model/xc330_m181_fw52.model b/param/dxl_model/xc330_m181_fw52.model new file mode 100644 index 0000000..fb8d6ef --- /dev/null +++ b/param/dxl_model/xc330_m181_fw52.model @@ -0,0 +1,73 @@ +[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 + +[control table] +Address Size Data Name +0 2 Model Number +2 4 Model Information +6 1 Firmware Version +7 1 ID +8 1 Baud Rate +9 1 Return Delay Time +10 1 Drive Mode +11 1 Operating Mode +12 1 Secondary(Shadow) ID +13 1 Protocol Type +20 4 Homing Offset +24 4 Moving Threshold +31 1 Temperature Limit +32 2 Max Voltage Limit +34 2 Min Voltage Limit +36 2 PWM Limit +38 2 Current Limit +44 4 Velocity Limit +48 4 Max Position Limit +52 4 Min Position Limit +62 1 PWM Slope +63 1 Shutdown +64 1 Torque Enable +65 1 LED +68 1 Status Return Level +69 1 Registered Instruction +70 1 Hardware Error Status +76 2 Velocity I Gain +78 2 Velocity P Gain +80 2 Position D Gain +82 2 Position I Gain +84 2 Position P Gain +88 2 Feedforward 2nd Gain +90 2 Feedforward 1st Gain +98 1 Bus Watchdog +100 2 Goal PWM +102 2 Goal Current +104 4 Goal Velocity +108 4 Profile Acceleration +112 4 Profile Velocity +116 4 Goal Position +120 2 Realtime Tick +122 1 Moving +123 1 Moving Status +124 2 Present PWM +126 2 Present Current +128 4 Present Velocity +132 4 Present Position +136 4 Velocity Trajectory +140 4 Position Trajectory +144 2 Present Input Voltage +146 1 Present Temperature +168 2 Indirect Address 1 +208 1 Indirect Data 1 +168 2 Indirect Address Write +208 1 Indirect Data Write +180 2 Indirect Address Read +214 1 Indirect Data Read diff --git a/param/dxl_model/xc330_m288.model b/param/dxl_model/xc330_m288.model index fb8d6ef..0ff1069 100644 --- a/param/dxl_model/xc330_m288.model +++ b/param/dxl_model/xc330_m288.model @@ -66,8 +66,8 @@ Address Size Data Name 144 2 Present Input Voltage 146 1 Present Temperature 168 2 Indirect Address 1 -208 1 Indirect Data 1 +224 1 Indirect Data 1 168 2 Indirect Address Write -208 1 Indirect Data Write +224 1 Indirect Data Write 180 2 Indirect Address Read -214 1 Indirect Data Read +230 1 Indirect Data Read diff --git a/param/dxl_model/xc330_m288_fw52.model b/param/dxl_model/xc330_m288_fw52.model new file mode 100644 index 0000000..fb8d6ef --- /dev/null +++ b/param/dxl_model/xc330_m288_fw52.model @@ -0,0 +1,73 @@ +[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 + +[control table] +Address Size Data Name +0 2 Model Number +2 4 Model Information +6 1 Firmware Version +7 1 ID +8 1 Baud Rate +9 1 Return Delay Time +10 1 Drive Mode +11 1 Operating Mode +12 1 Secondary(Shadow) ID +13 1 Protocol Type +20 4 Homing Offset +24 4 Moving Threshold +31 1 Temperature Limit +32 2 Max Voltage Limit +34 2 Min Voltage Limit +36 2 PWM Limit +38 2 Current Limit +44 4 Velocity Limit +48 4 Max Position Limit +52 4 Min Position Limit +62 1 PWM Slope +63 1 Shutdown +64 1 Torque Enable +65 1 LED +68 1 Status Return Level +69 1 Registered Instruction +70 1 Hardware Error Status +76 2 Velocity I Gain +78 2 Velocity P Gain +80 2 Position D Gain +82 2 Position I Gain +84 2 Position P Gain +88 2 Feedforward 2nd Gain +90 2 Feedforward 1st Gain +98 1 Bus Watchdog +100 2 Goal PWM +102 2 Goal Current +104 4 Goal Velocity +108 4 Profile Acceleration +112 4 Profile Velocity +116 4 Goal Position +120 2 Realtime Tick +122 1 Moving +123 1 Moving Status +124 2 Present PWM +126 2 Present Current +128 4 Present Velocity +132 4 Present Position +136 4 Velocity Trajectory +140 4 Position Trajectory +144 2 Present Input Voltage +146 1 Present Temperature +168 2 Indirect Address 1 +208 1 Indirect Data 1 +168 2 Indirect Address Write +208 1 Indirect Data Write +180 2 Indirect Address Read +214 1 Indirect Data Read diff --git a/param/dxl_model/xc330_t181.model b/param/dxl_model/xc330_t181.model index 7d397de..80ca68d 100644 --- a/param/dxl_model/xc330_t181.model +++ b/param/dxl_model/xc330_t181.model @@ -68,8 +68,8 @@ Address Size Data Name 144 2 Present Input Voltage 146 1 Present Temperature 168 2 Indirect Address 1 -208 1 Indirect Data 1 +224 1 Indirect Data 1 168 2 Indirect Address Write -208 1 Indirect Data Write +224 1 Indirect Data Write 180 2 Indirect Address Read -214 1 Indirect Data Read +230 1 Indirect Data Read diff --git a/param/dxl_model/xc330_t181_fw52.model b/param/dxl_model/xc330_t181_fw52.model new file mode 100644 index 0000000..7d397de --- /dev/null +++ b/param/dxl_model/xc330_t181_fw52.model @@ -0,0 +1,75 @@ +[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 Current 0.0006709470296015791 N/m signed +Goal Current 0.0006709470296015791 N/m signed + +[control table] +Address Size Data Name +0 2 Model Number +2 4 Model Information +6 1 Firmware Version +7 1 ID +8 1 Baud Rate +9 1 Return Delay Time +10 1 Drive Mode +11 1 Operating Mode +12 1 Secondary(Shadow) ID +13 1 Protocol Type +20 4 Homing Offset +24 4 Moving Threshold +31 1 Temperature Limit +32 2 Max Voltage Limit +34 2 Min Voltage Limit +36 2 PWM Limit +38 2 Current Limit +44 4 Velocity Limit +48 4 Max Position Limit +52 4 Min Position Limit +62 1 PWM Slope +63 1 Shutdown +64 1 Torque Enable +65 1 LED +68 1 Status Return Level +69 1 Registered Instruction +70 1 Hardware Error Status +76 2 Velocity I Gain +78 2 Velocity P Gain +80 2 Position D Gain +82 2 Position I Gain +84 2 Position P Gain +88 2 Feedforward 2nd Gain +90 2 Feedforward 1st Gain +98 1 Bus Watchdog +100 2 Goal PWM +102 2 Goal Current +104 4 Goal Velocity +108 4 Profile Acceleration +112 4 Profile Velocity +116 4 Goal Position +120 2 Realtime Tick +122 1 Moving +123 1 Moving Status +124 2 Present PWM +126 2 Present Current +128 4 Present Velocity +132 4 Present Position +136 4 Velocity Trajectory +140 4 Position Trajectory +144 2 Present Input Voltage +146 1 Present Temperature +168 2 Indirect Address 1 +208 1 Indirect Data 1 +168 2 Indirect Address Write +208 1 Indirect Data Write +180 2 Indirect Address Read +214 1 Indirect Data Read diff --git a/param/dxl_model/xc330_t288.model b/param/dxl_model/xc330_t288.model index f01d3fe..a3ba279 100644 --- a/param/dxl_model/xc330_t288.model +++ b/param/dxl_model/xc330_t288.model @@ -68,8 +68,8 @@ Address Size Data Name 144 2 Present Input Voltage 146 1 Present Temperature 168 2 Indirect Address 1 -208 1 Indirect Data 1 +224 1 Indirect Data 1 168 2 Indirect Address Write -208 1 Indirect Data Write +224 1 Indirect Data Write 180 2 Indirect Address Read -214 1 Indirect Data Read +230 1 Indirect Data Read diff --git a/param/dxl_model/xc330_t288_fw52.model b/param/dxl_model/xc330_t288_fw52.model new file mode 100644 index 0000000..f01d3fe --- /dev/null +++ b/param/dxl_model/xc330_t288_fw52.model @@ -0,0 +1,75 @@ +[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 Current 0.0009674796739867748 N/m signed +Goal Current 0.0009674796739867748 N/m signed + +[control table] +Address Size Data Name +0 2 Model Number +2 4 Model Information +6 1 Firmware Version +7 1 ID +8 1 Baud Rate +9 1 Return Delay Time +10 1 Drive Mode +11 1 Operating Mode +12 1 Secondary(Shadow) ID +13 1 Protocol Type +20 4 Homing Offset +24 4 Moving Threshold +31 1 Temperature Limit +32 2 Max Voltage Limit +34 2 Min Voltage Limit +36 2 PWM Limit +38 2 Current Limit +44 4 Velocity Limit +48 4 Max Position Limit +52 4 Min Position Limit +62 1 PWM Slope +63 1 Shutdown +64 1 Torque Enable +65 1 LED +68 1 Status Return Level +69 1 Registered Instruction +70 1 Hardware Error Status +76 2 Velocity I Gain +78 2 Velocity P Gain +80 2 Position D Gain +82 2 Position I Gain +84 2 Position P Gain +88 2 Feedforward 2nd Gain +90 2 Feedforward 1st Gain +98 1 Bus Watchdog +100 2 Goal PWM +102 2 Goal Current +104 4 Goal Velocity +108 4 Profile Acceleration +112 4 Profile Velocity +116 4 Goal Position +120 2 Realtime Tick +122 1 Moving +123 1 Moving Status +124 2 Present PWM +126 2 Present Current +128 4 Present Velocity +132 4 Present Position +136 4 Velocity Trajectory +140 4 Position Trajectory +144 2 Present Input Voltage +146 1 Present Temperature +168 2 Indirect Address 1 +208 1 Indirect Data 1 +168 2 Indirect Address Write +208 1 Indirect Data Write +180 2 Indirect Address Read +214 1 Indirect Data Read diff --git a/param/dxl_model/xl330_m077.model b/param/dxl_model/xl330_m077.model index fb8d6ef..0ff1069 100644 --- a/param/dxl_model/xl330_m077.model +++ b/param/dxl_model/xl330_m077.model @@ -66,8 +66,8 @@ Address Size Data Name 144 2 Present Input Voltage 146 1 Present Temperature 168 2 Indirect Address 1 -208 1 Indirect Data 1 +224 1 Indirect Data 1 168 2 Indirect Address Write -208 1 Indirect Data Write +224 1 Indirect Data Write 180 2 Indirect Address Read -214 1 Indirect Data Read +230 1 Indirect Data Read diff --git a/param/dxl_model/xl330_m077_fw52.model b/param/dxl_model/xl330_m077_fw52.model new file mode 100644 index 0000000..fb8d6ef --- /dev/null +++ b/param/dxl_model/xl330_m077_fw52.model @@ -0,0 +1,73 @@ +[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 + +[control table] +Address Size Data Name +0 2 Model Number +2 4 Model Information +6 1 Firmware Version +7 1 ID +8 1 Baud Rate +9 1 Return Delay Time +10 1 Drive Mode +11 1 Operating Mode +12 1 Secondary(Shadow) ID +13 1 Protocol Type +20 4 Homing Offset +24 4 Moving Threshold +31 1 Temperature Limit +32 2 Max Voltage Limit +34 2 Min Voltage Limit +36 2 PWM Limit +38 2 Current Limit +44 4 Velocity Limit +48 4 Max Position Limit +52 4 Min Position Limit +62 1 PWM Slope +63 1 Shutdown +64 1 Torque Enable +65 1 LED +68 1 Status Return Level +69 1 Registered Instruction +70 1 Hardware Error Status +76 2 Velocity I Gain +78 2 Velocity P Gain +80 2 Position D Gain +82 2 Position I Gain +84 2 Position P Gain +88 2 Feedforward 2nd Gain +90 2 Feedforward 1st Gain +98 1 Bus Watchdog +100 2 Goal PWM +102 2 Goal Current +104 4 Goal Velocity +108 4 Profile Acceleration +112 4 Profile Velocity +116 4 Goal Position +120 2 Realtime Tick +122 1 Moving +123 1 Moving Status +124 2 Present PWM +126 2 Present Current +128 4 Present Velocity +132 4 Present Position +136 4 Velocity Trajectory +140 4 Position Trajectory +144 2 Present Input Voltage +146 1 Present Temperature +168 2 Indirect Address 1 +208 1 Indirect Data 1 +168 2 Indirect Address Write +208 1 Indirect Data Write +180 2 Indirect Address Read +214 1 Indirect Data Read diff --git a/param/dxl_model/xl330_m288.model b/param/dxl_model/xl330_m288.model index fb8d6ef..0ff1069 100644 --- a/param/dxl_model/xl330_m288.model +++ b/param/dxl_model/xl330_m288.model @@ -66,8 +66,8 @@ Address Size Data Name 144 2 Present Input Voltage 146 1 Present Temperature 168 2 Indirect Address 1 -208 1 Indirect Data 1 +224 1 Indirect Data 1 168 2 Indirect Address Write -208 1 Indirect Data Write +224 1 Indirect Data Write 180 2 Indirect Address Read -214 1 Indirect Data Read +230 1 Indirect Data Read diff --git a/param/dxl_model/xl330_m288_fw52.model b/param/dxl_model/xl330_m288_fw52.model new file mode 100644 index 0000000..fb8d6ef --- /dev/null +++ b/param/dxl_model/xl330_m288_fw52.model @@ -0,0 +1,73 @@ +[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 + +[control table] +Address Size Data Name +0 2 Model Number +2 4 Model Information +6 1 Firmware Version +7 1 ID +8 1 Baud Rate +9 1 Return Delay Time +10 1 Drive Mode +11 1 Operating Mode +12 1 Secondary(Shadow) ID +13 1 Protocol Type +20 4 Homing Offset +24 4 Moving Threshold +31 1 Temperature Limit +32 2 Max Voltage Limit +34 2 Min Voltage Limit +36 2 PWM Limit +38 2 Current Limit +44 4 Velocity Limit +48 4 Max Position Limit +52 4 Min Position Limit +62 1 PWM Slope +63 1 Shutdown +64 1 Torque Enable +65 1 LED +68 1 Status Return Level +69 1 Registered Instruction +70 1 Hardware Error Status +76 2 Velocity I Gain +78 2 Velocity P Gain +80 2 Position D Gain +82 2 Position I Gain +84 2 Position P Gain +88 2 Feedforward 2nd Gain +90 2 Feedforward 1st Gain +98 1 Bus Watchdog +100 2 Goal PWM +102 2 Goal Current +104 4 Goal Velocity +108 4 Profile Acceleration +112 4 Profile Velocity +116 4 Goal Position +120 2 Realtime Tick +122 1 Moving +123 1 Moving Status +124 2 Present PWM +126 2 Present Current +128 4 Present Velocity +132 4 Present Position +136 4 Velocity Trajectory +140 4 Position Trajectory +144 2 Present Input Voltage +146 1 Present Temperature +168 2 Indirect Address 1 +208 1 Indirect Data 1 +168 2 Indirect Address Write +208 1 Indirect Data Write +180 2 Indirect Address Read +214 1 Indirect Data Read diff --git a/param/dxl_model/ym070_210_a099.model b/param/dxl_model/ym070_210_a099.model index 561782f..5dfcf10 100644 --- a/param/dxl_model/ym070_210_a099.model +++ b/param/dxl_model/ym070_210_a099.model @@ -72,7 +72,7 @@ Address Size Data Name 167 1 Error Code History 14 168 1 Error Code History 15 169 1 Error Code History 16 -170 1 Hybrid Saveve +170 1 Hybrid Save 212 4 Velocity I Gain 216 4 Velocity P Gain 220 4 Velocity FF Gain diff --git a/param/dxl_model/ym070_210_m001.model b/param/dxl_model/ym070_210_m001.model index 561782f..5dfcf10 100644 --- a/param/dxl_model/ym070_210_m001.model +++ b/param/dxl_model/ym070_210_m001.model @@ -72,7 +72,7 @@ Address Size Data Name 167 1 Error Code History 14 168 1 Error Code History 15 169 1 Error Code History 16 -170 1 Hybrid Saveve +170 1 Hybrid Save 212 4 Velocity I Gain 216 4 Velocity P Gain 220 4 Velocity FF Gain diff --git a/param/dxl_model/ym070_210_r051.model b/param/dxl_model/ym070_210_r051.model index 561782f..5dfcf10 100644 --- a/param/dxl_model/ym070_210_r051.model +++ b/param/dxl_model/ym070_210_r051.model @@ -72,7 +72,7 @@ Address Size Data Name 167 1 Error Code History 14 168 1 Error Code History 15 169 1 Error Code History 16 -170 1 Hybrid Saveve +170 1 Hybrid Save 212 4 Velocity I Gain 216 4 Velocity P Gain 220 4 Velocity FF Gain diff --git a/param/dxl_model/ym070_210_r099.model b/param/dxl_model/ym070_210_r099.model index 561782f..5dfcf10 100644 --- a/param/dxl_model/ym070_210_r099.model +++ b/param/dxl_model/ym070_210_r099.model @@ -72,7 +72,7 @@ Address Size Data Name 167 1 Error Code History 14 168 1 Error Code History 15 169 1 Error Code History 16 -170 1 Hybrid Saveve +170 1 Hybrid Save 212 4 Velocity I Gain 216 4 Velocity P Gain 220 4 Velocity FF Gain diff --git a/param/dxl_model/ym080_220_m001.model b/param/dxl_model/ym080_220_m001.model index 561782f..5dfcf10 100644 --- a/param/dxl_model/ym080_220_m001.model +++ b/param/dxl_model/ym080_220_m001.model @@ -72,7 +72,7 @@ Address Size Data Name 167 1 Error Code History 14 168 1 Error Code History 15 169 1 Error Code History 16 -170 1 Hybrid Saveve +170 1 Hybrid Save 212 4 Velocity I Gain 216 4 Velocity P Gain 220 4 Velocity FF Gain diff --git a/param/dxl_model/ym080_230_a099.model b/param/dxl_model/ym080_230_a099.model index 561782f..5dfcf10 100644 --- a/param/dxl_model/ym080_230_a099.model +++ b/param/dxl_model/ym080_230_a099.model @@ -72,7 +72,7 @@ Address Size Data Name 167 1 Error Code History 14 168 1 Error Code History 15 169 1 Error Code History 16 -170 1 Hybrid Saveve +170 1 Hybrid Save 212 4 Velocity I Gain 216 4 Velocity P Gain 220 4 Velocity FF Gain diff --git a/param/dxl_model/ym080_230_b001.model b/param/dxl_model/ym080_230_b001.model index 5246050..5dfcf10 100644 --- a/param/dxl_model/ym080_230_b001.model +++ b/param/dxl_model/ym080_230_b001.model @@ -58,6 +58,18 @@ Address Size Data Name 153 1 Error Code 154 1 Error Code History 1 155 1 Error Code History 2 +156 1 Error Code History 3 +157 1 Error Code History 4 +158 1 Error Code History 5 +159 1 Error Code History 6 +160 1 Error Code History 7 +161 1 Error Code History 8 +162 1 Error Code History 9 +163 1 Error Code History 10 +164 1 Error Code History 11 +165 1 Error Code History 12 +166 1 Error Code History 13 +167 1 Error Code History 14 168 1 Error Code History 15 169 1 Error Code History 16 170 1 Hybrid Save diff --git a/param/dxl_model/ym080_230_r051.model b/param/dxl_model/ym080_230_r051.model index 561782f..5dfcf10 100644 --- a/param/dxl_model/ym080_230_r051.model +++ b/param/dxl_model/ym080_230_r051.model @@ -72,7 +72,7 @@ Address Size Data Name 167 1 Error Code History 14 168 1 Error Code History 15 169 1 Error Code History 16 -170 1 Hybrid Saveve +170 1 Hybrid Save 212 4 Velocity I Gain 216 4 Velocity P Gain 220 4 Velocity FF Gain diff --git a/param/dxl_model/ym080_230_r099.model b/param/dxl_model/ym080_230_r099.model index 561782f..5dfcf10 100644 --- a/param/dxl_model/ym080_230_r099.model +++ b/param/dxl_model/ym080_230_r099.model @@ -72,7 +72,7 @@ Address Size Data Name 167 1 Error Code History 14 168 1 Error Code History 15 169 1 Error Code History 16 -170 1 Hybrid Saveve +170 1 Hybrid Save 212 4 Velocity I Gain 216 4 Velocity P Gain 220 4 Velocity FF Gain diff --git a/scripts/check_model_file_formatting.py b/scripts/check_model_file_formatting.py new file mode 100755 index 0000000..973e559 --- /dev/null +++ b/scripts/check_model_file_formatting.py @@ -0,0 +1,242 @@ +#!/usr/bin/env python3 +# +# Copyright 2025 ROBOTIS CO., LTD. +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. +# +# Author: Woojin Wie + +""" +Script to check formatting of Dynamixel model files. + +This script checks for: +1. Trailing spaces on lines +2. Proper EOF (End of File) placement +3. Consistent line endings + +Usage: + python3 check_model_file_formatting.py [--fix] [--verbose] + +Options: + --fix Automatically fix formatting issues + --verbose Show detailed information about each file +""" + +import argparse +import os +import sys + + +def check_file_formatting(file_path, fix=False, verbose=False): + """ + Check and optionally fix formatting issues in a file. + + Args: + file_path (str): Path to the file to check + fix (bool): Whether to automatically fix issues + verbose (bool): Whether to show detailed output + + Returns + ------- + dict: Dictionary with issues found and fixed + + """ + issues = { + 'trailing_spaces': [], + 'no_eof_newline': False, + 'empty_lines_at_end': 0, + 'fixed': [], + } + + try: + with open(file_path, 'r', encoding='utf-8') as f: + lines = f.readlines() + except Exception as e: + print(f'Error reading {file_path}: {e}') + return issues + + if not lines: + if verbose: + print(f' {file_path}: Empty file') + return issues + + # Check for trailing spaces + for i, line in enumerate(lines, 1): + # Remove newline and check if there are trailing spaces + line_content = line.rstrip('\n') + if line_content.rstrip() != line_content: + issues['trailing_spaces'].append(i) + + # Check EOF + if lines and not lines[-1].endswith('\n'): + issues['no_eof_newline'] = True + + # Count empty lines at the end + empty_lines_at_end = 0 + for line in reversed(lines): + if line.strip() == '': + empty_lines_at_end += 1 + else: + break + + if empty_lines_at_end > 1: + issues['empty_lines_at_end'] = empty_lines_at_end + + # Fix issues if requested + if fix and ( + issues['trailing_spaces'] + or issues['no_eof_newline'] + or issues['empty_lines_at_end'] > 1 + ): + fixed_lines = [] + + for i, line in enumerate(lines): + # Remove trailing spaces but preserve newline + if i + 1 in issues['trailing_spaces']: + line_content = line.rstrip('\n') + fixed_lines.append(line_content.rstrip() + '\n') + issues['fixed'].append( + f'Line {i + 1}: Removed trailing spaces') + else: + fixed_lines.append(line) + + # Ensure proper EOF + if issues['no_eof_newline']: + if fixed_lines and not fixed_lines[-1].endswith('\n'): + fixed_lines[-1] = fixed_lines[-1] + '\n' + issues['fixed'].append('Added EOF newline') + + # Remove excessive empty lines at the end + if issues['empty_lines_at_end'] > 1: + # Remove all empty lines at the end, then add one + while fixed_lines and fixed_lines[-1].strip() == '': + fixed_lines.pop() + fixed_lines.append('\n') # Add single newline at end + issues['fixed'].append( + f'Removed {issues["empty_lines_at_end"] - 1} excessive empty lines at end' + ) + + # Write fixed file + try: + with open(file_path, 'w', encoding='utf-8') as f: + f.writelines(fixed_lines) + except Exception as e: + print(f'Error writing {file_path}: {e}') + return issues + + return issues + + +def main(): + """Run the formatting check or fix process for Dynamixel model files.""" + parser = argparse.ArgumentParser( + description='Check formatting of Dynamixel model files', + formatter_class=argparse.RawDescriptionHelpFormatter, + epilog=""" +Examples: + python3 check_model_file_formatting.py + python3 check_model_file_formatting.py --fix + python3 check_model_file_formatting.py --verbose + python3 check_model_file_formatting.py --fix --verbose + """, + ) + + parser.add_argument( + '--fix', action='store_true', help='Automatically fix formatting issues' + ) + parser.add_argument( + '--verbose', + action='store_true', + help='Show detailed information about each file', + ) + + args = parser.parse_args() + + # Find model files + model_dir = 'param/dxl_model' + if not os.path.exists(model_dir): + print(f"Error: Model directory '{model_dir}' not found.") + print('Please run this script from the dynamixel_hardware_interface directory.') + sys.exit(1) + + model_files = [] + for file in os.listdir(model_dir): + if file.endswith('.model'): + model_files.append(os.path.join(model_dir, file)) + + if not model_files: + print('No .model files found.') + sys.exit(0) + + print(f'Checking {len(model_files)} model files...') + if args.fix: + print('Auto-fix mode enabled.') + print() + + total_issues = 0 + files_with_issues = 0 + + for file_path in sorted(model_files): + file_name = os.path.basename(file_path) + issues = check_file_formatting(file_path, args.fix, args.verbose) + + has_issues = ( + issues['trailing_spaces'] + or issues['no_eof_newline'] + or issues['empty_lines_at_end'] > 1 + ) + + if has_issues: + files_with_issues += 1 + print(f'❌ {file_name}') + + if issues['trailing_spaces']: + print( + f' Trailing spaces on lines: \ + {", ".join(map(str, issues["trailing_spaces"]))}' + ) + total_issues += len(issues['trailing_spaces']) + + if issues['no_eof_newline']: + print(' Missing EOF newline') + total_issues += 1 + + if issues['empty_lines_at_end'] > 1: + print( + f' {issues["empty_lines_at_end"]} empty lines at end of file') + total_issues += 1 + + if args.fix and issues['fixed']: + print(' Fixed:') + for fix in issues['fixed']: + print(f' - {fix}') + + print() + elif args.verbose: + print(f'✅ {file_name} - No issues found') + + # Summary + print('=' * 50) + if files_with_issues == 0: + print('🎉 All model files are properly formatted!') + else: + print( + f'Found {total_issues} formatting issues in {files_with_issues} files.') + if not args.fix: + print('Run with --fix to automatically fix these issues.') + + return 0 if files_with_issues == 0 else 1 + + +if __name__ == '__main__': + sys.exit(main()) diff --git a/src/dynamixel/dynamixel.cpp b/src/dynamixel/dynamixel.cpp index 71820ce..6ce8c3e 100644 --- a/src/dynamixel/dynamixel.cpp +++ b/src/dynamixel/dynamixel.cpp @@ -91,6 +91,31 @@ DxlError Dynamixel::ReadDxlModelFile(uint8_t id, uint16_t model_num) } } +DxlError Dynamixel::ReadDxlModelFile(uint8_t id, uint16_t model_num, uint8_t firmware_version) +{ + try { + dxl_info_.ReadDxlModelFile(id, model_num, firmware_version); + return DxlError::OK; + } catch (const std::exception & e) { + fprintf(stderr, "[ReadDxlModelFile][ID:%03d] Error reading model file: %s\n", id, e.what()); + return DxlError::CANNOT_FIND_CONTROL_ITEM; + } +} + +DxlError Dynamixel::ReadFirmwareVersion(uint8_t id, uint8_t & firmware_version) +{ + uint32_t fw_version_data; + DxlError result = ReadItem(id, "Firmware Version", fw_version_data); + if (result == DxlError::OK) { + firmware_version = static_cast(fw_version_data); + // fprintf(stderr, "[ReadFirmwareVersion][ID:%03d] Firmware Version: %d\n", + // id, firmware_version); + } else { + fprintf(stderr, "[ReadFirmwareVersion][ID:%03d] Failed to read firmware version\n", id); + } + return result; +} + DxlError Dynamixel::InitTorqueStates(std::vector id_arr, bool disable_torque) { for (auto it_id : id_arr) { @@ -184,15 +209,35 @@ DxlError Dynamixel::InitDxlComm( Reboot(it_id); return DxlError::DXL_HARDWARE_ERROR; } else { - fprintf(stderr, " - Ping succeeded. Dynamixel model number : %d\n", dxl_model_number); + std::string model_name = dxl_info_.GetModelName(dxl_model_number); + fprintf( + stderr, " - Ping succeeded. Dynamixel model number : %d (%s)\n", dxl_model_number, + model_name.c_str()); } + // First, read the model file to get the control table structure try { dxl_info_.ReadDxlModelFile(it_id, dxl_model_number); } catch (const std::exception & e) { fprintf(stderr, "[InitDxlComm][ID:%03d] Error reading model file: %s\n", it_id, e.what()); return DxlError::CANNOT_FIND_CONTROL_ITEM; } + + // Read firmware version and reload model file with firmware-specific version if available + uint8_t firmware_version = 0; + DxlError fw_result = ReadFirmwareVersion(it_id, firmware_version); + if (fw_result == DxlError::OK && firmware_version > 0) { + // fprintf(stderr, "[InitDxlComm][ID:%03d] Reloading model file with firmware version %d\n", + // it_id, firmware_version); + try { + dxl_info_.ReadDxlModelFile(it_id, dxl_model_number, firmware_version); + } catch (const std::exception & e) { + fprintf( + stderr, "[InitDxlComm][ID:%03d] Error reading firmware-specific model file: %s\n", + it_id, e.what()); + // Continue with the base model file if firmware-specific file fails + } + } } read_data_list_.clear(); diff --git a/src/dynamixel/dynamixel_info.cpp b/src/dynamixel/dynamixel_info.cpp index 4ee7b61..b64e07a 100644 --- a/src/dynamixel/dynamixel_info.cpp +++ b/src/dynamixel/dynamixel_info.cpp @@ -15,9 +15,11 @@ // Authors: Hye-Jong KIM, Sungho Woo, Woojin Wie #include "dynamixel_hardware_interface/dynamixel/dynamixel_info.hpp" +#include #include #include #include +#include namespace dynamixel_hardware_interface { @@ -52,12 +54,21 @@ void DynamixelInfo::InitDxlModelInfo() } void DynamixelInfo::ReadDxlModelFile(uint8_t id, uint16_t model_num) +{ + ReadDxlModelFile(id, model_num, 0); // Default to firmware version 0 (unknown) +} + +void DynamixelInfo::ReadDxlModelFile(uint8_t id, uint16_t model_num, uint8_t firmware_version) { std::string path = dxl_model_file_dir + "/"; auto it = dxl_model_list_.find(model_num); if (it != dxl_model_list_.end()) { - path += it->second; + std::string base_model_name = it->second; + std::string selected_model_name = SelectModelFileByFirmwareVersion( + base_model_name, + firmware_version); + path += selected_model_name; } else { fprintf(stderr, "[ERROR] CANNOT FIND THE DXL MODEL FROM FILE LIST.\n"); throw std::runtime_error("Cannot find the DXL model from file list"); @@ -73,6 +84,7 @@ void DynamixelInfo::ReadDxlModelFile(uint8_t id, uint16_t model_num) std::string line; temp_dxl_info.model_num = model_num; + temp_dxl_info.firmware_version = firmware_version; // Check if [type info] section exists bool type_info_found = false; @@ -206,6 +218,108 @@ void DynamixelInfo::ReadDxlModelFile(uint8_t id, uint16_t model_num) open_file.close(); } +std::string DynamixelInfo::SelectModelFileByFirmwareVersion( + const std::string & base_model_name, + uint8_t firmware_version) +{ + // If firmware version is 0 (unknown), use the base model file + if (firmware_version == 0) { + return base_model_name; + } + + // Extract base name without extension + std::string base_name = base_model_name; + size_t dot_pos = base_name.find_last_of('.'); + if (dot_pos != std::string::npos) { + base_name = base_name.substr(0, dot_pos); + } + + // Scan directory for firmware-specific model files + std::vector available_firmware_versions; + std::string search_path = dxl_model_file_dir + "/"; + + DIR * dir = opendir(search_path.c_str()); + if (dir != nullptr) { + struct dirent * entry; + std::string prefix = base_name + "_fw"; + std::string suffix = ".model"; + + while ((entry = readdir(dir)) != nullptr) { + std::string filename = entry->d_name; + if (filename.find(prefix) == 0 && + filename.find(suffix) == filename.length() - suffix.length()) + { + available_firmware_versions.push_back(filename); + } + } + closedir(dir); + } + + if (available_firmware_versions.empty()) { + // fprintf(stderr, + // "[Firmware Version Selection] No firmware-specific files found for %s, using base model\n", + // base_model_name.c_str()); + return base_model_name; + } + + // fprintf(stderr, "[Firmware Version Selection] Found %zu firmware-specific files for %s\n", + // available_firmware_versions.size(), base_model_name.c_str()); + + // Find the highest firmware version file that is <= device firmware version + std::string selected_file = base_model_name; // Default to base model + int highest_fw_version = -1; + std::string highest_fw_file; + + for (const auto & fw_file : available_firmware_versions) { + uint8_t fw_version = ExtractFirmwareVersionFromFilename(fw_file); + if (fw_version > highest_fw_version) { + highest_fw_version = fw_version; + highest_fw_file = fw_file; + } + if (fw_version <= firmware_version && + fw_version > ExtractFirmwareVersionFromFilename(selected_file)) + { + selected_file = fw_file; + } + } + + // If device FW is greater than the highest available firmware-specific file, use base model + if (firmware_version > highest_fw_version) { + // fprintf( + // stderr, + // "[Firmware Version Selection] Device FW: %d > " + // "highest firmware-specific file FW: %d, using base model.\n", + // firmware_version, highest_fw_version); + return base_model_name; + } + + fprintf( + stderr, + "[NOTICE] Your DYNAMIXEL is not using the latest firmware." + " For full performance, please download the latest DYNAMIXEL Wizard 2.0" + " and update your DYNAMIXEL firmware." + " See: https://emanual.robotis.com/docs/en/software/dynamixel/dynamixel_wizard2/\n"); + + // Otherwise, use the highest firmware-specific file <= device FW + fprintf( + stderr, "[Firmware Version Selection] Device FW: %d, Selected Model: %s (FW: %d)\n", + firmware_version, selected_file.c_str(), + ExtractFirmwareVersionFromFilename(selected_file)); + return selected_file; +} + +uint8_t DynamixelInfo::ExtractFirmwareVersionFromFilename(const std::string & filename) +{ + std::regex fw_pattern(R"(_fw(\d+)\.model$)"); + std::smatch match; + + if (std::regex_search(filename, match, fw_pattern) && match.size() > 1) { + return static_cast(std::stoi(match[1].str())); + } + + return 0; // Return 0 if no firmware version found +} + bool DynamixelInfo::GetDxlControlItem( uint8_t id, std::string item_name, uint16_t & addr, uint8_t & size) @@ -293,4 +407,13 @@ double DynamixelInfo::GetUnitMultiplier(uint8_t id, std::string data_name) return 1.0; } +std::string DynamixelInfo::GetModelName(uint16_t model_number) const +{ + auto it = dxl_model_list_.find(model_number); + if (it != dxl_model_list_.end()) { + return it->second; + } + return "unknown"; +} + } // namespace dynamixel_hardware_interface