diff --git a/CHANGELOG.rst b/CHANGELOG.rst index 3bac156..af358b0 100644 --- a/CHANGELOG.rst +++ b/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package dynamixel_hardware_interface ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.4.10 (2025-07-18) +------------------ +* Added unit system to model files +* Added support for Dynamixel Y Error Code handling +* Contributors: Woojin Wie + 1.4.9 (2025-06-24) ------------------ * Support ffw sensor model diff --git a/CMakeLists.txt b/CMakeLists.txt index 828e9d6..069b27e 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -39,9 +39,8 @@ add_library( target_include_directories( ${PROJECT_NAME} - PRIVATE - include - $ + PUBLIC + $ $ ${dynamixel_sdk_INCLUDE_DIRS} ${hardware_interface_INCLUDE_DIRS} @@ -67,7 +66,10 @@ pluginlib_export_plugin_description_file(hardware_interface dynamixel_hardware_i # Install ################################################################################ install(TARGETS ${PROJECT_NAME} - DESTINATION lib + EXPORT export_${PROJECT_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin ) install(DIRECTORY include/ @@ -98,6 +100,7 @@ endif() ################################################################################ ament_export_include_directories(include) ament_export_libraries(${PROJECT_NAME}) +ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET) ament_export_dependencies( rclcpp rclcpp_lifecycle diff --git a/include/dynamixel_hardware_interface/dynamixel/dynamixel.hpp b/include/dynamixel_hardware_interface/dynamixel/dynamixel.hpp index 5e6db38..dbbc8f7 100644 --- a/include/dynamixel_hardware_interface/dynamixel/dynamixel.hpp +++ b/include/dynamixel_hardware_interface/dynamixel/dynamixel.hpp @@ -67,7 +67,7 @@ enum DxlError SET_BULK_READ_FAIL = -13, ///< Failed to configure bulk read. SET_READ_ITEM_FAIL = -14, ///< Failed to set read item. SET_WRITE_ITEM_FAIL = -15, ///< Failed to set write item. - DLX_HARDWARE_ERROR = -16, ///< Hardware error detected. + DXL_HARDWARE_ERROR = -16, ///< Hardware error detected. DXL_REBOOT_FAIL = -17 ///< Reboot failed. }; @@ -260,6 +260,7 @@ class Dynamixel DxlError ProcessDirectReadData( uint8_t id, const std::vector & item_addrs, + const std::vector & item_names, const std::vector & item_sizes, const std::vector> & data_ptrs, std::function get_data_func); @@ -292,6 +293,25 @@ class Dynamixel std::string item_name, uint16_t item_addr, uint8_t item_size); + + // Helper function for value conversion with unit info + double ConvertValueWithUnitInfo( + uint8_t id, + std::string item_name, + uint32_t raw_value, + uint8_t size, + bool is_signed); + + // Helper function for converting unit values to raw values + uint32_t ConvertUnitValueToRawValue( + uint8_t id, + std::string item_name, + double unit_value, + uint8_t size, + bool is_signed); + + // Helper function for writing values to buffer + void WriteValueToBuffer(uint8_t * buffer, uint8_t offset, uint32_t value, uint8_t size); }; } // namespace dynamixel_hardware_interface diff --git a/include/dynamixel_hardware_interface/dynamixel/dynamixel_info.hpp b/include/dynamixel_hardware_interface/dynamixel/dynamixel_info.hpp index 9770e83..122e1e8 100644 --- a/include/dynamixel_hardware_interface/dynamixel/dynamixel_info.hpp +++ b/include/dynamixel_hardware_interface/dynamixel/dynamixel_info.hpp @@ -40,16 +40,16 @@ typedef struct typedef struct { - double torque_constant; double min_radian; double max_radian; int32_t value_of_zero_radian_position; int32_t value_of_max_radian_position; int32_t value_of_min_radian_position; uint16_t model_num; - double velocity_unit; std::vector item; + std::map unit_map; + std::map sign_type_map; } DxlInfo; class DynamixelInfo @@ -73,27 +73,45 @@ class DynamixelInfo void ReadDxlModelFile(uint8_t id, uint16_t model_num); bool GetDxlControlItem(uint8_t id, std::string item_name, uint16_t & addr, uint8_t & size); bool CheckDxlControlItem(uint8_t id, std::string item_name); - // bool GetDxlTypeInfo( - // uint8_t id, - // int32_t & value_of_zero_radian_position, - // int32_t & value_of_max_radian_position, - // int32_t & value_of_min_radian_position, - // double & min_radian, - // double & max_radian, - // double & torque_constant, - // double & velocity_unit); + + bool GetDxlUnitValue(uint8_t id, std::string data_name, double & unit_value); + bool GetDxlSignType(uint8_t id, std::string data_name, bool & is_signed); + + // Template-based conversion methods + template + double ConvertValueToUnit(uint8_t id, std::string data_name, T value); + + template + T ConvertUnitToValue(uint8_t id, std::string data_name, double unit_value); + + // Helper method for internal use + double GetUnitMultiplier(uint8_t id, std::string data_name); + 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) - {return static_cast(effort / dxl_info_[id].torque_constant);} - inline double ConvertCurrentToEffort(uint8_t id, int16_t current) - {return static_cast(current * dxl_info_[id].torque_constant);} - inline double ConvertValueRPMToVelocityRPS(uint8_t id, int32_t value_rpm) - {return static_cast(value_rpm * dxl_info_[id].velocity_unit / 60.0 * 2.0 * M_PI);} - inline int32_t ConvertVelocityRPSToValueRPM(uint8_t id, double vel_rps) - {return static_cast(vel_rps / dxl_info_[id].velocity_unit * 60.0 / 2.0 / M_PI);} }; +// Template implementations +template +double DynamixelInfo::ConvertValueToUnit(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(value) * it->second; + } + return static_cast(value); +} + +template +T DynamixelInfo::ConvertUnitToValue(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(unit_value / it->second); + } + return static_cast(unit_value); +} + } // namespace dynamixel_hardware_interface #endif // DYNAMIXEL_HARDWARE_INTERFACE__DYNAMIXEL__DYNAMIXEL_INFO_HPP_ diff --git a/include/dynamixel_hardware_interface/dynamixel_hardware_interface.hpp b/include/dynamixel_hardware_interface/dynamixel_hardware_interface.hpp index 9006dad..77b16b8 100644 --- a/include/dynamixel_hardware_interface/dynamixel_hardware_interface.hpp +++ b/include/dynamixel_hardware_interface/dynamixel_hardware_interface.hpp @@ -50,6 +50,80 @@ namespace dynamixel_hardware_interface { +// Hardware Error Status bit definitions +struct HardwareErrorStatusBitInfo +{ + int bit; + const char * label; + const char * description; +}; + +static constexpr HardwareErrorStatusBitInfo HardwareErrorStatusTable[] = { + {0, "Input Voltage Error", "Detects that input voltage exceeds the configured operating voltage"}, + {1, "Motor Hall Sensor Error", "Detects that Motor hall sensor value exceeds normal range"}, + {2, "Overheating Error", + "Detects that internal temperature exceeds the configured operating temperature"}, + {3, "Motor Encoder Error", "Detects malfunction of the motor encoder"}, + {4, + "Electrical Shock Error", + "Detects electric shock on the circuit or insufficient power to operate the motor"}, + {5, "Overload Error", "Detects that persistent load exceeds maximum output"}, + {6, "Not used", "Always 0"}, + {7, "Not used", "Always 0"} +}; + +inline const HardwareErrorStatusBitInfo * get_hardware_error_status_bit_info(int bit) +{ + for (const auto & entry : HardwareErrorStatusTable) { + if (entry.bit == bit) {return &entry;} + } + return nullptr; +} + +// Error Code (153) definitions +struct ErrorCodeInfo +{ + int value; + const char * label; + const char * description; +}; + +static constexpr ErrorCodeInfo ErrorCodeTable[] = { + {0x00, "No Error", "No error"}, + {0x01, "Over Voltage Error", "Device supply voltage exceeds the Max Voltage Limit(60)"}, + {0x02, "Low Voltage Error", "Device supply voltage exceeds the Min Voltage Limit(62)"}, + {0x03, + "Inverter Overheating Error", + "The inverter temperature has exceeded the Inverter Temperature Limit(56)"}, + {0x04, + "Motor Overheating Error", + "The motor temperature has exceeded the Motor Temperature Limit(57)"}, + {0x05, "Overload Error", "Operating current exceeding rated current for an extended duration"}, + {0x07, "Inverter Error", "An issue has occurred with the inverter"}, + {0x09, "Battery Warning", "Low Multi-turn battery voltage. Replacement recommended"}, + {0x0A, "Battery Error", "Multi-turn battery voltage is too low, causing issues"}, + {0x0B, "Magnet Error", "Multi-turn position lost. Multi-turn reset required"}, + {0x0C, "Multi-turn Error", "An issue has occurred with the Multi-turn IC"}, + {0x0D, "Encoder Error", "An issue has occurred with the Encoder IC"}, + {0x0E, "Hall Sensor Error", "An issue has occurred with the Hall Sensor"}, + {0x0F, "Calibration Error", "Cannot find calibration Data"}, + {0x11, "Following Error", "Position control error exceeds the Following Error Threshold(44)"}, + {0x12, "Bus Watchdog Error", "An issue has occurred with the Bus Watchdog"}, + {0x13, "Over Speed Error", "Rotates at a speed of 120% or more than the Velocity Limit(72)"}, + {0x14, + "Position Limit Reached Error", + "In position control mode, the current position has moved beyond the Max/Min Position Limit" + " + Position Limit Threshold(38) range."} +}; + +inline const ErrorCodeInfo * get_error_code_info(int value) +{ + for (const auto & entry : ErrorCodeTable) { + if (entry.value == value) {return &entry;} + } + return nullptr; +} + /** * @brief Constants for hardware state interface names. */ @@ -175,14 +249,16 @@ class DynamixelHardware : public private: ///// ros rclcpp::Logger logger_; + rclcpp::Clock clock_; ///// dxl error DxlStatus dxl_status_; DxlError dxl_comm_err_; std::map dxl_hw_err_; + std::map dxl_error_code_; DxlTorqueStatus dxl_torque_status_; std::map dxl_torque_state_; - std::map dxl_torque_enable_; + std::vector torque_enabled_ids_; double err_timeout_ms_; rclcpp::Duration read_error_duration_{0, 0}; rclcpp::Duration write_error_duration_{0, 0}; @@ -298,8 +374,9 @@ class DynamixelHardware : public ///// function /** * @brief Sets up the joint-to-transmission and transmission-to-joint matrices. + * @return True if matrices were set up successfully, false otherwise. */ - void SetMatrix(); + bool SetMatrix(); /** * @brief Calculates the joint states from transmission states. @@ -346,12 +423,30 @@ class DynamixelHardware : public const std::shared_ptr request, std::shared_ptr response); + rclcpp::Service::SharedPtr get_dxl_error_summary_srv_; + void get_dxl_error_summary_srv_callback( + const std::shared_ptr request, + std::shared_ptr response); + void initRevoluteToPrismaticParam(); double revoluteToPrismatic(double revolute_value); double prismaticToRevolute(double prismatic_value); + /** + * @brief Get a formatted error summary for a specific Dynamixel ID + * @param id The Dynamixel ID + * @return A string containing the error summary + */ + std::string getErrorSummary(uint8_t id) const; + + /** + * @brief Get error summaries for all Dynamixel IDs + * @return A string containing error summaries for all Dynamixels + */ + std::string getAllErrorSummaries() const; + void MapInterfaces( size_t outer_size, size_t inner_size, diff --git a/package.xml b/package.xml index f277dad..c264961 100644 --- a/package.xml +++ b/package.xml @@ -2,7 +2,7 @@ dynamixel_hardware_interface - 1.4.9 + 1.4.10 ROS 2 package providing a hardware interface for controlling Dynamixel motors via the ROS 2 control framework. @@ -12,7 +12,9 @@ Sungho Woo Woojin Wie Wonho Yun + ament_cmake + rclcpp hardware_interface pluginlib @@ -20,8 +22,10 @@ dynamixel_sdk std_srvs dynamixel_interfaces + ament_lint_auto ament_lint_common + ament_cmake diff --git a/param/dxl_model/2xc430_w250.model b/param/dxl_model/2xc430_w250.model index 6a1f335..deb48f6 100644 --- a/param/dxl_model/2xc430_w250.model +++ b/param/dxl_model/2xc430_w250.model @@ -5,7 +5,11 @@ value_of_max_radian_position 4095 value_of_min_radian_position 0 min_radian -3.14159265 max_radian 3.14159265 -velocity_unit 0.229 + +[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 diff --git a/param/dxl_model/2xl430_w250.model b/param/dxl_model/2xl430_w250.model index 6a1f335..deb48f6 100644 --- a/param/dxl_model/2xl430_w250.model +++ b/param/dxl_model/2xl430_w250.model @@ -5,7 +5,11 @@ value_of_max_radian_position 4095 value_of_min_radian_position 0 min_radian -3.14159265 max_radian 3.14159265 -velocity_unit 0.229 + +[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 diff --git a/param/dxl_model/ffw_sg2_drive_1.model b/param/dxl_model/ffw_sg2_drive_1.model index 1d84d3d..bb72934 100644 --- a/param/dxl_model/ffw_sg2_drive_1.model +++ b/param/dxl_model/ffw_sg2_drive_1.model @@ -5,7 +5,11 @@ value_of_max_radian_position 262144 value_of_min_radian_position -262144 min_radian -3.14159265 max_radian 3.14159265 -velocity_unit 0.01 + +[unit info] +Data Name value unit Sign Type +Present Velocity 0.0010471976 rad/s signed +Goal Velocity 0.0010471976 rad/s signed [control table] Address Size Data Name diff --git a/param/dxl_model/ffw_sg2_drive_2.model b/param/dxl_model/ffw_sg2_drive_2.model index dedb967..8ef4d6c 100644 --- a/param/dxl_model/ffw_sg2_drive_2.model +++ b/param/dxl_model/ffw_sg2_drive_2.model @@ -5,7 +5,11 @@ value_of_max_radian_position 262144 value_of_min_radian_position -262144 min_radian -3.14159265 max_radian 3.14159265 -velocity_unit 0.01 + +[unit info] +Data Name value unit Sign Type +Present Velocity 0.0010471976 rad/s signed +Goal Velocity 0.0010471976 rad/s signed [control table] Address Size Data Name diff --git a/param/dxl_model/ffw_sg2_drive_3.model b/param/dxl_model/ffw_sg2_drive_3.model index 28c276e..7b4735b 100644 --- a/param/dxl_model/ffw_sg2_drive_3.model +++ b/param/dxl_model/ffw_sg2_drive_3.model @@ -5,7 +5,11 @@ value_of_max_radian_position 262144 value_of_min_radian_position -262144 min_radian -3.14159265 max_radian 3.14159265 -velocity_unit 0.01 + +[unit info] +Data Name value unit Sign Type +Present Velocity 0.0010471976 rad/s signed +Goal Velocity 0.0010471976 rad/s signed [control table] Address Size Data Name diff --git a/param/dxl_model/ffw_sg2_steer_1.model b/param/dxl_model/ffw_sg2_steer_1.model index c05793c..b1fba86 100644 --- a/param/dxl_model/ffw_sg2_steer_1.model +++ b/param/dxl_model/ffw_sg2_steer_1.model @@ -5,7 +5,11 @@ value_of_max_radian_position 262144 value_of_min_radian_position -262144 min_radian -3.14159265 max_radian 3.14159265 -velocity_unit 0.01 + +[unit info] +Data Name value unit Sign Type +Present Velocity 0.0010471976 rad/s signed +Goal Velocity 0.0010471976 rad/s signed [control table] Address Size Data Name diff --git a/param/dxl_model/ffw_sg2_steer_2.model b/param/dxl_model/ffw_sg2_steer_2.model index f591c13..9d28882 100644 --- a/param/dxl_model/ffw_sg2_steer_2.model +++ b/param/dxl_model/ffw_sg2_steer_2.model @@ -5,7 +5,11 @@ value_of_max_radian_position 262144 value_of_min_radian_position -262144 min_radian -3.14159265 max_radian 3.14159265 -velocity_unit 0.01 + +[unit info] +Data Name value unit Sign Type +Present Velocity 0.0010471976 rad/s signed +Goal Velocity 0.0010471976 rad/s signed [control table] Address Size Data Name diff --git a/param/dxl_model/ffw_sg2_steer_3.model b/param/dxl_model/ffw_sg2_steer_3.model index b0c8e8c..d582e28 100644 --- a/param/dxl_model/ffw_sg2_steer_3.model +++ b/param/dxl_model/ffw_sg2_steer_3.model @@ -5,7 +5,11 @@ value_of_max_radian_position 262144 value_of_min_radian_position -262144 min_radian -3.14159265 max_radian 3.14159265 -velocity_unit 0.01 + +[unit info] +Data Name value unit Sign Type +Present Velocity 0.0010471976 rad/s signed +Goal Velocity 0.0010471976 rad/s signed [control table] Address Size Data Name diff --git a/param/dxl_model/omy_end_rh_p12_rn.model b/param/dxl_model/omy_end_rh_p12_rn.model index c013203..3a173a0 100644 --- a/param/dxl_model/omy_end_rh_p12_rn.model +++ b/param/dxl_model/omy_end_rh_p12_rn.model @@ -5,7 +5,11 @@ value_of_max_radian_position 740 value_of_min_radian_position -740 min_radian -1.099 max_radian 1.099 -velocity_unit 0.114 + +[unit info] +Data Name value unit Sign Type +Present Velocity 0.0119380521 rad/s signed +Goal Velocity 0.0119380521 rad/s signed [control table] Address Size Data Name diff --git a/param/dxl_model/ph42_020_s300.model b/param/dxl_model/ph42_020_s300.model index d21a411..df851f3 100644 --- a/param/dxl_model/ph42_020_s300.model +++ b/param/dxl_model/ph42_020_s300.model @@ -5,7 +5,12 @@ value_of_max_radian_position 303750 value_of_min_radian_position -303750 min_radian -3.14159265 max_radian 3.14159265 -velocity_unit 0.01 + +[unit info] +Data Name value unit Sign Type +Present Velocity 0.0010471976 rad/s signed +Goal Velocity 0.0010471976 rad/s signed +Velocity Limit 0.0010471976 rad/s signed [control table] Address Size Data Name diff --git a/param/dxl_model/rh_p12_rn.model b/param/dxl_model/rh_p12_rn.model index 9396d01..87e28e2 100644 --- a/param/dxl_model/rh_p12_rn.model +++ b/param/dxl_model/rh_p12_rn.model @@ -5,7 +5,11 @@ value_of_max_radian_position 740 value_of_min_radian_position -740 min_radian -1.099 max_radian 1.099 -velocity_unit 0.114 + +[unit info] +Data Name value unit Sign Type +Present Velocity 0.0119380521 rad/s signed +Goal Velocity 0.0119380521 rad/s signed [control table] Address Size Data Name diff --git a/param/dxl_model/sensorxel_joy.model b/param/dxl_model/sensorxel_joy.model index 9bd9f54..3d67fbb 100644 --- a/param/dxl_model/sensorxel_joy.model +++ b/param/dxl_model/sensorxel_joy.model @@ -1,5 +1,5 @@ [control table] -Address Size Data Name +Address Size Data Name 0 2 Model Number 2 1 ROBOT_Generation 3 1 Board_Number diff --git a/param/dxl_model/xc330_m181.model b/param/dxl_model/xc330_m181.model index 7fa8ab5..fb8d6ef 100644 --- a/param/dxl_model/xc330_m181.model +++ b/param/dxl_model/xc330_m181.model @@ -5,7 +5,11 @@ value_of_max_radian_position 4095 value_of_min_radian_position 0 min_radian -3.14159265 max_radian 3.14159265 -velocity_unit 0.229 + +[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 diff --git a/param/dxl_model/xc330_m288.model b/param/dxl_model/xc330_m288.model index 7fa8ab5..fb8d6ef 100644 --- a/param/dxl_model/xc330_m288.model +++ b/param/dxl_model/xc330_m288.model @@ -5,7 +5,11 @@ value_of_max_radian_position 4095 value_of_min_radian_position 0 min_radian -3.14159265 max_radian 3.14159265 -velocity_unit 0.229 + +[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 diff --git a/param/dxl_model/xc330_t181.model b/param/dxl_model/xc330_t181.model index 63da0b3..7d397de 100644 --- a/param/dxl_model/xc330_t181.model +++ b/param/dxl_model/xc330_t181.model @@ -5,8 +5,13 @@ value_of_max_radian_position 4095 value_of_min_radian_position 0 min_radian -3.14159265 max_radian 3.14159265 -torque_constant 0.0006709470296015791 -velocity_unit 0.229 + +[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 diff --git a/param/dxl_model/xc330_t288.model b/param/dxl_model/xc330_t288.model index 1e0dd24..f01d3fe 100644 --- a/param/dxl_model/xc330_t288.model +++ b/param/dxl_model/xc330_t288.model @@ -5,8 +5,13 @@ value_of_max_radian_position 4095 value_of_min_radian_position 0 min_radian -3.14159265 max_radian 3.14159265 -torque_constant 0.0009674796739867748 -velocity_unit 0.229 + +[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 diff --git a/param/dxl_model/xc430_w150.model b/param/dxl_model/xc430_w150.model index 6a1f335..deb48f6 100644 --- a/param/dxl_model/xc430_w150.model +++ b/param/dxl_model/xc430_w150.model @@ -5,7 +5,11 @@ value_of_max_radian_position 4095 value_of_min_radian_position 0 min_radian -3.14159265 max_radian 3.14159265 -velocity_unit 0.229 + +[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 diff --git a/param/dxl_model/xc430_w240.model b/param/dxl_model/xc430_w240.model index 6a1f335..deb48f6 100644 --- a/param/dxl_model/xc430_w240.model +++ b/param/dxl_model/xc430_w240.model @@ -5,7 +5,11 @@ value_of_max_radian_position 4095 value_of_min_radian_position 0 min_radian -3.14159265 max_radian 3.14159265 -velocity_unit 0.229 + +[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 diff --git a/param/dxl_model/xd430_t210.model b/param/dxl_model/xd430_t210.model index 8b2fb3c..8b09bc6 100644 --- a/param/dxl_model/xd430_t210.model +++ b/param/dxl_model/xd430_t210.model @@ -5,7 +5,11 @@ value_of_max_radian_position 4095 value_of_min_radian_position 0 min_radian -3.14159265 max_radian 3.14159265 -velocity_unit 0.229 + +[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 diff --git a/param/dxl_model/xd430_t350.model b/param/dxl_model/xd430_t350.model index 8b2fb3c..8b09bc6 100644 --- a/param/dxl_model/xd430_t350.model +++ b/param/dxl_model/xd430_t350.model @@ -5,7 +5,11 @@ value_of_max_radian_position 4095 value_of_min_radian_position 0 min_radian -3.14159265 max_radian 3.14159265 -velocity_unit 0.229 + +[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 diff --git a/param/dxl_model/xd540_t150.model b/param/dxl_model/xd540_t150.model index 0364415..dc54c51 100644 --- a/param/dxl_model/xd540_t150.model +++ b/param/dxl_model/xd540_t150.model @@ -5,7 +5,11 @@ value_of_max_radian_position 4095 value_of_min_radian_position 0 min_radian -3.14159265 max_radian 3.14159265 -velocity_unit 0.229 + +[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 diff --git a/param/dxl_model/xd540_t270.model b/param/dxl_model/xd540_t270.model index bfe7dcf..dc54c51 100644 --- a/param/dxl_model/xd540_t270.model +++ b/param/dxl_model/xd540_t270.model @@ -5,7 +5,11 @@ value_of_max_radian_position 4095 value_of_min_radian_position 0 min_radian -3.14159265 max_radian 3.14159265 -velocity_unit 0.229 + +[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 @@ -71,4 +75,4 @@ Address Size Data Name 168 2 Indirect Address Write 224 1 Indirect Data Write 578 2 Indirect Address Read -634 1 Indirect Data Read \ No newline at end of file +634 1 Indirect Data Read diff --git a/param/dxl_model/xh430_v210.model b/param/dxl_model/xh430_v210.model index 8b2fb3c..8b09bc6 100644 --- a/param/dxl_model/xh430_v210.model +++ b/param/dxl_model/xh430_v210.model @@ -5,7 +5,11 @@ value_of_max_radian_position 4095 value_of_min_radian_position 0 min_radian -3.14159265 max_radian 3.14159265 -velocity_unit 0.229 + +[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 diff --git a/param/dxl_model/xh430_v350.model b/param/dxl_model/xh430_v350.model index 8b2fb3c..8b09bc6 100644 --- a/param/dxl_model/xh430_v350.model +++ b/param/dxl_model/xh430_v350.model @@ -5,7 +5,11 @@ value_of_max_radian_position 4095 value_of_min_radian_position 0 min_radian -3.14159265 max_radian 3.14159265 -velocity_unit 0.229 + +[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 diff --git a/param/dxl_model/xh430_w210.model b/param/dxl_model/xh430_w210.model index 8b2fb3c..8b09bc6 100644 --- a/param/dxl_model/xh430_w210.model +++ b/param/dxl_model/xh430_w210.model @@ -5,7 +5,11 @@ value_of_max_radian_position 4095 value_of_min_radian_position 0 min_radian -3.14159265 max_radian 3.14159265 -velocity_unit 0.229 + +[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 diff --git a/param/dxl_model/xh430_w350.model b/param/dxl_model/xh430_w350.model index 8b2fb3c..8b09bc6 100644 --- a/param/dxl_model/xh430_w350.model +++ b/param/dxl_model/xh430_w350.model @@ -5,7 +5,11 @@ value_of_max_radian_position 4095 value_of_min_radian_position 0 min_radian -3.14159265 max_radian 3.14159265 -velocity_unit 0.229 + +[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 diff --git a/param/dxl_model/xh540_v150.model b/param/dxl_model/xh540_v150.model index 0364415..dc54c51 100644 --- a/param/dxl_model/xh540_v150.model +++ b/param/dxl_model/xh540_v150.model @@ -5,7 +5,11 @@ value_of_max_radian_position 4095 value_of_min_radian_position 0 min_radian -3.14159265 max_radian 3.14159265 -velocity_unit 0.229 + +[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 diff --git a/param/dxl_model/xh540_v270.model b/param/dxl_model/xh540_v270.model index 0364415..dc54c51 100644 --- a/param/dxl_model/xh540_v270.model +++ b/param/dxl_model/xh540_v270.model @@ -5,7 +5,11 @@ value_of_max_radian_position 4095 value_of_min_radian_position 0 min_radian -3.14159265 max_radian 3.14159265 -velocity_unit 0.229 + +[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 diff --git a/param/dxl_model/xh540_w150.model b/param/dxl_model/xh540_w150.model index 4dd5bbe..3487436 100644 --- a/param/dxl_model/xh540_w150.model +++ b/param/dxl_model/xh540_w150.model @@ -5,8 +5,13 @@ value_of_max_radian_position 4095 value_of_min_radian_position 0 min_radian -3.14159265 max_radian 3.14159265 -torque_constant 0.0045 -velocity_unit 0.229 + +[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.0045 N/m signed +Goal Current 0.0045 N/m signed [control table] Address Size Data Name diff --git a/param/dxl_model/xh540_w270.model b/param/dxl_model/xh540_w270.model index 0364415..dc54c51 100644 --- a/param/dxl_model/xh540_w270.model +++ b/param/dxl_model/xh540_w270.model @@ -5,7 +5,11 @@ value_of_max_radian_position 4095 value_of_min_radian_position 0 min_radian -3.14159265 max_radian 3.14159265 -velocity_unit 0.229 + +[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 diff --git a/param/dxl_model/xl320.model b/param/dxl_model/xl320.model index 339130b..df5914a 100644 --- a/param/dxl_model/xl320.model +++ b/param/dxl_model/xl320.model @@ -5,7 +5,11 @@ value_of_max_radian_position 4095 value_of_min_radian_position 0 min_radian -3.14159265 max_radian 3.14159265 -velocity_unit 0.111 + +[unit info] +Data Name value unit Sign Type +Present Velocity 0.0116228475 rad/s signed +Goal Velocity 0.0116228475 rad/s signed [control table] Address Size Data Name diff --git a/param/dxl_model/xl330_m077.model b/param/dxl_model/xl330_m077.model index 7fa8ab5..fb8d6ef 100644 --- a/param/dxl_model/xl330_m077.model +++ b/param/dxl_model/xl330_m077.model @@ -5,7 +5,11 @@ value_of_max_radian_position 4095 value_of_min_radian_position 0 min_radian -3.14159265 max_radian 3.14159265 -velocity_unit 0.229 + +[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 diff --git a/param/dxl_model/xl330_m288.model b/param/dxl_model/xl330_m288.model index 7fa8ab5..fb8d6ef 100644 --- a/param/dxl_model/xl330_m288.model +++ b/param/dxl_model/xl330_m288.model @@ -5,7 +5,11 @@ value_of_max_radian_position 4095 value_of_min_radian_position 0 min_radian -3.14159265 max_radian 3.14159265 -velocity_unit 0.229 + +[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 diff --git a/param/dxl_model/xl430_w250.model b/param/dxl_model/xl430_w250.model index 6a1f335..deb48f6 100644 --- a/param/dxl_model/xl430_w250.model +++ b/param/dxl_model/xl430_w250.model @@ -5,7 +5,11 @@ value_of_max_radian_position 4095 value_of_min_radian_position 0 min_radian -3.14159265 max_radian 3.14159265 -velocity_unit 0.229 + +[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 diff --git a/param/dxl_model/xm430_w210.model b/param/dxl_model/xm430_w210.model index 8b2fb3c..8b09bc6 100644 --- a/param/dxl_model/xm430_w210.model +++ b/param/dxl_model/xm430_w210.model @@ -5,7 +5,11 @@ value_of_max_radian_position 4095 value_of_min_radian_position 0 min_radian -3.14159265 max_radian 3.14159265 -velocity_unit 0.229 + +[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 diff --git a/param/dxl_model/xm430_w350.model b/param/dxl_model/xm430_w350.model index 8b2fb3c..8b09bc6 100644 --- a/param/dxl_model/xm430_w350.model +++ b/param/dxl_model/xm430_w350.model @@ -5,7 +5,11 @@ value_of_max_radian_position 4095 value_of_min_radian_position 0 min_radian -3.14159265 max_radian 3.14159265 -velocity_unit 0.229 + +[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 diff --git a/param/dxl_model/xm540_w150.model b/param/dxl_model/xm540_w150.model index 0364415..dc54c51 100644 --- a/param/dxl_model/xm540_w150.model +++ b/param/dxl_model/xm540_w150.model @@ -5,7 +5,11 @@ value_of_max_radian_position 4095 value_of_min_radian_position 0 min_radian -3.14159265 max_radian 3.14159265 -velocity_unit 0.229 + +[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 diff --git a/param/dxl_model/xm540_w270.model b/param/dxl_model/xm540_w270.model index 0364415..dc54c51 100644 --- a/param/dxl_model/xm540_w270.model +++ b/param/dxl_model/xm540_w270.model @@ -5,7 +5,11 @@ value_of_max_radian_position 4095 value_of_min_radian_position 0 min_radian -3.14159265 max_radian 3.14159265 -velocity_unit 0.229 + +[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 diff --git a/param/dxl_model/xw430_t200.model b/param/dxl_model/xw430_t200.model index 0a9b4a1..7a9e163 100644 --- a/param/dxl_model/xw430_t200.model +++ b/param/dxl_model/xw430_t200.model @@ -5,7 +5,11 @@ value_of_max_radian_position 4095 value_of_min_radian_position 0 min_radian -3.14159265 max_radian 3.14159265 -velocity_unit 0.229 + +[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 diff --git a/param/dxl_model/xw430_t333.model b/param/dxl_model/xw430_t333.model index 0a9b4a1..7a9e163 100644 --- a/param/dxl_model/xw430_t333.model +++ b/param/dxl_model/xw430_t333.model @@ -5,7 +5,11 @@ value_of_max_radian_position 4095 value_of_min_radian_position 0 min_radian -3.14159265 max_radian 3.14159265 -velocity_unit 0.229 + +[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 diff --git a/param/dxl_model/xw540_h260.model b/param/dxl_model/xw540_h260.model index 0a9b4a1..7a9e163 100644 --- a/param/dxl_model/xw540_h260.model +++ b/param/dxl_model/xw540_h260.model @@ -5,7 +5,11 @@ value_of_max_radian_position 4095 value_of_min_radian_position 0 min_radian -3.14159265 max_radian 3.14159265 -velocity_unit 0.229 + +[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 diff --git a/param/dxl_model/xw540_t140.model b/param/dxl_model/xw540_t140.model index 0a9b4a1..7a9e163 100644 --- a/param/dxl_model/xw540_t140.model +++ b/param/dxl_model/xw540_t140.model @@ -5,7 +5,11 @@ value_of_max_radian_position 4095 value_of_min_radian_position 0 min_radian -3.14159265 max_radian 3.14159265 -velocity_unit 0.229 + +[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 diff --git a/param/dxl_model/xw540_t260.model b/param/dxl_model/xw540_t260.model index 0a9b4a1..7a9e163 100644 --- a/param/dxl_model/xw540_t260.model +++ b/param/dxl_model/xw540_t260.model @@ -5,7 +5,11 @@ value_of_max_radian_position 4095 value_of_min_radian_position 0 min_radian -3.14159265 max_radian 3.14159265 -velocity_unit 0.229 + +[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 diff --git a/param/dxl_model/ym070_210_a099.model b/param/dxl_model/ym070_210_a099.model index 82e1ae7..561782f 100644 --- a/param/dxl_model/ym070_210_a099.model +++ b/param/dxl_model/ym070_210_a099.model @@ -5,7 +5,11 @@ value_of_max_radian_position 262144 value_of_min_radian_position -262144 min_radian -3.14159265 max_radian 3.14159265 -velocity_unit 0.01 + +[unit info] +Data Name value unit Sign Type +Present Velocity 0.0010471976 rad/s signed +Goal Velocity 0.0010471976 rad/s signed [control table] Address Size Data Name diff --git a/param/dxl_model/ym070_210_m001.model b/param/dxl_model/ym070_210_m001.model index 82e1ae7..561782f 100644 --- a/param/dxl_model/ym070_210_m001.model +++ b/param/dxl_model/ym070_210_m001.model @@ -5,7 +5,11 @@ value_of_max_radian_position 262144 value_of_min_radian_position -262144 min_radian -3.14159265 max_radian 3.14159265 -velocity_unit 0.01 + +[unit info] +Data Name value unit Sign Type +Present Velocity 0.0010471976 rad/s signed +Goal Velocity 0.0010471976 rad/s signed [control table] Address Size Data Name diff --git a/param/dxl_model/ym070_210_r051.model b/param/dxl_model/ym070_210_r051.model index 82e1ae7..561782f 100644 --- a/param/dxl_model/ym070_210_r051.model +++ b/param/dxl_model/ym070_210_r051.model @@ -5,7 +5,11 @@ value_of_max_radian_position 262144 value_of_min_radian_position -262144 min_radian -3.14159265 max_radian 3.14159265 -velocity_unit 0.01 + +[unit info] +Data Name value unit Sign Type +Present Velocity 0.0010471976 rad/s signed +Goal Velocity 0.0010471976 rad/s signed [control table] Address Size Data Name diff --git a/param/dxl_model/ym070_210_r099.model b/param/dxl_model/ym070_210_r099.model index 82e1ae7..561782f 100644 --- a/param/dxl_model/ym070_210_r099.model +++ b/param/dxl_model/ym070_210_r099.model @@ -5,7 +5,11 @@ value_of_max_radian_position 262144 value_of_min_radian_position -262144 min_radian -3.14159265 max_radian 3.14159265 -velocity_unit 0.01 + +[unit info] +Data Name value unit Sign Type +Present Velocity 0.0010471976 rad/s signed +Goal Velocity 0.0010471976 rad/s signed [control table] Address Size Data Name diff --git a/param/dxl_model/ym080_220_m001.model b/param/dxl_model/ym080_220_m001.model index 82e1ae7..561782f 100644 --- a/param/dxl_model/ym080_220_m001.model +++ b/param/dxl_model/ym080_220_m001.model @@ -5,7 +5,11 @@ value_of_max_radian_position 262144 value_of_min_radian_position -262144 min_radian -3.14159265 max_radian 3.14159265 -velocity_unit 0.01 + +[unit info] +Data Name value unit Sign Type +Present Velocity 0.0010471976 rad/s signed +Goal Velocity 0.0010471976 rad/s signed [control table] Address Size Data Name diff --git a/param/dxl_model/ym080_230_a099.model b/param/dxl_model/ym080_230_a099.model index 82e1ae7..561782f 100644 --- a/param/dxl_model/ym080_230_a099.model +++ b/param/dxl_model/ym080_230_a099.model @@ -5,7 +5,11 @@ value_of_max_radian_position 262144 value_of_min_radian_position -262144 min_radian -3.14159265 max_radian 3.14159265 -velocity_unit 0.01 + +[unit info] +Data Name value unit Sign Type +Present Velocity 0.0010471976 rad/s signed +Goal Velocity 0.0010471976 rad/s signed [control table] Address Size Data Name diff --git a/param/dxl_model/ym080_230_b001.model b/param/dxl_model/ym080_230_b001.model index 4660f52..5246050 100644 --- a/param/dxl_model/ym080_230_b001.model +++ b/param/dxl_model/ym080_230_b001.model @@ -5,7 +5,11 @@ value_of_max_radian_position 262144 value_of_min_radian_position -262144 min_radian -3.14159265 max_radian 3.14159265 -velocity_unit 0.01 + +[unit info] +Data Name value unit Sign Type +Present Velocity 0.0010471976 rad/s signed +Goal Velocity 0.0010471976 rad/s signed [control table] Address Size Data Name diff --git a/param/dxl_model/ym080_230_r051.model b/param/dxl_model/ym080_230_r051.model index 82e1ae7..561782f 100644 --- a/param/dxl_model/ym080_230_r051.model +++ b/param/dxl_model/ym080_230_r051.model @@ -5,7 +5,11 @@ value_of_max_radian_position 262144 value_of_min_radian_position -262144 min_radian -3.14159265 max_radian 3.14159265 -velocity_unit 0.01 + +[unit info] +Data Name value unit Sign Type +Present Velocity 0.0010471976 rad/s signed +Goal Velocity 0.0010471976 rad/s signed [control table] Address Size Data Name diff --git a/param/dxl_model/ym080_230_r099.model b/param/dxl_model/ym080_230_r099.model index 82e1ae7..561782f 100644 --- a/param/dxl_model/ym080_230_r099.model +++ b/param/dxl_model/ym080_230_r099.model @@ -5,7 +5,11 @@ value_of_max_radian_position 262144 value_of_min_radian_position -262144 min_radian -3.14159265 max_radian 3.14159265 -velocity_unit 0.01 + +[unit info] +Data Name value unit Sign Type +Present Velocity 0.0010471976 rad/s signed +Goal Velocity 0.0010471976 rad/s signed [control table] Address Size Data Name diff --git a/src/dynamixel/dynamixel.cpp b/src/dynamixel/dynamixel.cpp index 1157d12..71820ce 100644 --- a/src/dynamixel/dynamixel.cpp +++ b/src/dynamixel/dynamixel.cpp @@ -124,7 +124,7 @@ DxlError Dynamixel::InitTorqueStates(std::vector id_arr, bool disable_t "'disable_torque_at_init' parameter to 'true' to disable torque at initialization " "or disable torque manually.\n", it_id); - return DxlError::DLX_HARDWARE_ERROR; + return DxlError::DXL_HARDWARE_ERROR; } } @@ -182,7 +182,7 @@ DxlError Dynamixel::InitDxlComm( } fprintf(stderr, "[ID:%03d] Hardware Error detected, rebooting...\n", it_id); Reboot(it_id); - return DxlError::DLX_HARDWARE_ERROR; + return DxlError::DXL_HARDWARE_ERROR; } else { fprintf(stderr, " - Ping succeeded. Dynamixel model number : %d\n", dxl_model_number); } @@ -449,6 +449,9 @@ DxlError Dynamixel::SetMultiDxlWrite() DxlError Dynamixel::DynamixelEnable(std::vector id_arr) { for (auto it_id : id_arr) { + if (dxl_info_.CheckDxlControlItem(it_id, "Torque Enable") == false) { + continue; + } if (torque_state_[it_id] == TORQUE_OFF) { if (WriteItem(it_id, "Torque Enable", TORQUE_ON) < 0) { fprintf(stderr, "[ID:%03d] Cannot write \"Torque On\" command!\n", it_id); @@ -465,6 +468,9 @@ DxlError Dynamixel::DynamixelDisable(std::vector id_arr) { DxlError result = DxlError::OK; for (auto it_id : id_arr) { + if (dxl_info_.CheckDxlControlItem(it_id, "Torque Enable") == false) { + continue; + } if (torque_state_[it_id] == TORQUE_ON) { if (WriteItem(it_id, "Torque Enable", TORQUE_OFF) < 0) { fprintf(stderr, "[ID:%03d] Cannot write \"Torque Off\" command!\n", it_id); @@ -549,15 +555,11 @@ DxlError Dynamixel::WriteItem(uint8_t id, uint16_t addr, uint8_t size, uint32_t } else if (dxl_error != 0) { fprintf( stderr, - "[WriteItem][ID:%03d][comm_id:%03d] RX_PACKET_ERROR : %s (retry %d/%d)\n", + "[WriteItem][ID:%03d][comm_id:%03d] RX_PACKET_ERROR : %s\n", id, comm_id, - packet_handler_->getRxPacketError(dxl_error), - i + 1, - MAX_COMM_RETRIES); - if (i == MAX_COMM_RETRIES - 1) { - return DxlError::ITEM_WRITE_FAIL; - } + packet_handler_->getRxPacketError(dxl_error)); + return DxlError::ITEM_WRITE_FAIL; } else { return DxlError::OK; } @@ -610,7 +612,7 @@ DxlError Dynamixel::WriteItemBuf() if (WriteItem(it_write_item.id, "Torque Enable", TORQUE_OFF) < 0) { fprintf( stderr, - "[ID:%03d] Cannot write \"Torque Off\" command! Cannot wirte a Item.\n", + "[ID:%03d] Cannot write \"Torque Off\" command! Cannot write a Item.\n", it_write_item.id); return DxlError::ITEM_WRITE_FAIL; } @@ -827,8 +829,8 @@ std::string Dynamixel::DxlErrorToString(DxlError error_num) return "SET_READ_ITEM_FAIL"; case SET_WRITE_ITEM_FAIL: return "SET_WRITE_ITEM_FAIL"; - case DLX_HARDWARE_ERROR: - return "DLX_HARDWARE_ERROR"; + case DXL_HARDWARE_ERROR: + return "DXL_HARDWARE_ERROR"; case DXL_REBOOT_FAIL: return "DXL_REBOOT_FAIL"; default: @@ -892,19 +894,15 @@ bool Dynamixel::checkReadType() return BULK; } - if (read_data_list_.at(dxl_index).item_name.size() != - read_data_list_.at(dxl_index - 1).item_name.size()) + if (read_data_list_.at(dxl_index).item_size.size() != + read_data_list_.at(dxl_index - 1).item_size.size()) { return BULK; } - for (size_t item_index = 0; item_index < read_data_list_.at(dxl_index).item_name.size(); + for (size_t item_index = 0; item_index < read_data_list_.at(dxl_index).item_size.size(); item_index++) { - if (read_data_list_.at(dxl_index).item_name.at(item_index) != - read_data_list_.at(dxl_index - 1).item_name.at(item_index) || - read_data_list_.at(dxl_index).item_addr.at(item_index) != - read_data_list_.at(dxl_index - 1).item_addr.at(item_index) || - read_data_list_.at(dxl_index).item_size.at(item_index) != + if (read_data_list_.at(dxl_index).item_size.at(item_index) != read_data_list_.at(dxl_index - 1).item_size.at(item_index)) { return BULK; @@ -944,19 +942,15 @@ bool Dynamixel::checkWriteType() return BULK; } - if (write_data_list_.at(dxl_index).item_name.size() != - write_data_list_.at(dxl_index - 1).item_name.size()) + if (write_data_list_.at(dxl_index).item_size.size() != + write_data_list_.at(dxl_index - 1).item_size.size()) { return BULK; } - for (size_t item_index = 0; item_index < write_data_list_.at(dxl_index).item_name.size(); + for (size_t item_index = 0; item_index < write_data_list_.at(dxl_index).item_size.size(); item_index++) { - if (write_data_list_.at(dxl_index).item_name.at(item_index) != - write_data_list_.at(dxl_index - 1).item_name.at(item_index) || - write_data_list_.at(dxl_index).item_addr.at(item_index) != - write_data_list_.at(dxl_index - 1).item_addr.at(item_index) || - write_data_list_.at(dxl_index).item_size.at(item_index) != + if (write_data_list_.at(dxl_index).item_size.at(item_index) != write_data_list_.at(dxl_index - 1).item_size.at(item_index)) { return BULK; @@ -1139,8 +1133,8 @@ DxlError Dynamixel::GetDxlValueFromSyncRead(double period_ms) indirect_info_read_[id].item_name, indirect_info_read_[id].item_size, it_read_data.item_data_ptr_vec, - [this](uint8_t id, uint16_t addr, uint8_t size) { - return group_fast_sync_read_->getData(id, addr, size); + [this](uint8_t lambda_id, uint16_t addr, uint8_t size) { + return group_fast_sync_read_->getData(lambda_id, addr, size); }); } // Mark as permanently using fast sync read after first success @@ -1185,8 +1179,8 @@ DxlError Dynamixel::GetDxlValueFromSyncRead(double period_ms) indirect_info_read_[id].item_name, indirect_info_read_[id].item_size, it_read_data.item_data_ptr_vec, - [this](uint8_t id, uint16_t addr, uint8_t size) { - return group_sync_read_->getData(id, addr, size); + [this](uint8_t lambda_id, uint16_t addr, uint8_t size) { + return group_sync_read_->getData(lambda_id, addr, size); }); } return DxlError::OK; @@ -1235,7 +1229,7 @@ DxlError Dynamixel::SetBulkReadItemAndHandler() if (addr < min_addr) {min_addr = addr;} if (addr + size > max_end_addr) {max_end_addr = addr + size;} } - uint8_t total_size = max_end_addr - min_addr; + uint8_t total_size = static_cast(max_end_addr - min_addr); // Concatenate all item names with '+' std::string group_item_names; for (size_t i = 0; i < it_read_data.item_name.size(); ++i) { @@ -1449,10 +1443,11 @@ DxlError Dynamixel::GetDxlValueFromBulkRead(double period_ms) ProcessDirectReadData( id, it_read_data.item_addr, + it_read_data.item_name, it_read_data.item_size, it_read_data.item_data_ptr_vec, - [this](uint8_t id, uint16_t addr, uint8_t size) { - return group_fast_bulk_read_->getData(id, addr, size); + [this](uint8_t lambda_id, uint16_t addr, uint8_t size) { + return group_fast_bulk_read_->getData(lambda_id, addr, size); }); } else { ProcessReadData( @@ -1462,8 +1457,8 @@ DxlError Dynamixel::GetDxlValueFromBulkRead(double period_ms) indirect_info_read_[id].item_name, indirect_info_read_[id].item_size, it_read_data.item_data_ptr_vec, - [this](uint8_t id, uint16_t addr, uint8_t size) { - return group_fast_bulk_read_->getData(id, addr, size); + [this](uint8_t lambda_id, uint16_t addr, uint8_t size) { + return group_fast_bulk_read_->getData(lambda_id, addr, size); }); } } @@ -1514,10 +1509,11 @@ DxlError Dynamixel::GetDxlValueFromBulkRead(double period_ms) ProcessDirectReadData( id, it_read_data.item_addr, + it_read_data.item_name, it_read_data.item_size, it_read_data.item_data_ptr_vec, - [this](uint8_t id, uint16_t addr, uint8_t size) { - return group_bulk_read_->getData(id, addr, size); + [this](uint8_t lambda_id, uint16_t addr, uint8_t size) { + return group_bulk_read_->getData(lambda_id, addr, size); }); } else { ProcessReadData( @@ -1527,8 +1523,8 @@ DxlError Dynamixel::GetDxlValueFromBulkRead(double period_ms) indirect_info_read_[id].item_name, indirect_info_read_[id].item_size, it_read_data.item_data_ptr_vec, - [this](uint8_t id, uint16_t addr, uint8_t size) { - return group_bulk_read_->getData(id, addr, size); + [this](uint8_t lambda_id, uint16_t addr, uint8_t size) { + return group_bulk_read_->getData(lambda_id, addr, size); }); } } @@ -1637,20 +1633,25 @@ DxlError Dynamixel::ProcessReadData( uint32_t dxl_getdata = get_data_func(comm_id, current_addr, size); - if (item_names[item_index] == "Present Position") { - *data_ptrs[item_index] = dxl_info_.ConvertValueToRadian( - ID, - static_cast(dxl_getdata)); - } else if (item_names[item_index] == "Present Velocity") { - *data_ptrs[item_index] = dxl_info_.ConvertValueRPMToVelocityRPS( - ID, - static_cast(dxl_getdata)); - } else if (item_names[item_index] == "Present Current") { - *data_ptrs[item_index] = dxl_info_.ConvertCurrentToEffort( - ID, - static_cast(dxl_getdata)); + // Check if there is unit info for this item + double unit_value; + bool is_signed; + if (dxl_info_.GetDxlUnitValue(ID, item_names[item_index], unit_value) && + dxl_info_.GetDxlSignType(ID, item_names[item_index], is_signed)) + { + // Use unit info and sign type to properly convert the value + *data_ptrs[item_index] = ConvertValueWithUnitInfo( + ID, item_names[item_index], dxl_getdata, + size, is_signed); } else { - *data_ptrs[item_index] = static_cast(dxl_getdata); + // Fallback to existing logic for compatibility + if (item_names[item_index] == "Present Position") { + *data_ptrs[item_index] = dxl_info_.ConvertValueToRadian( + ID, + static_cast(dxl_getdata)); + } else { + *data_ptrs[item_index] = static_cast(dxl_getdata); + } } } return DxlError::OK; @@ -1659,17 +1660,38 @@ DxlError Dynamixel::ProcessReadData( DxlError Dynamixel::ProcessDirectReadData( uint8_t comm_id, const std::vector & item_addrs, + const std::vector & item_names, const std::vector & item_sizes, const std::vector> & data_ptrs, std::function get_data_func) { for (size_t item_index = 0; item_index < item_addrs.size(); item_index++) { + uint8_t ID = comm_id; uint16_t current_addr = item_addrs[item_index]; uint8_t size = item_sizes[item_index]; uint32_t dxl_getdata = get_data_func(comm_id, current_addr, size); - *data_ptrs[item_index] = static_cast(dxl_getdata); + // Check if there is unit info for this item + double unit_value; + bool is_signed; + if (dxl_info_.GetDxlUnitValue(ID, item_names[item_index], unit_value) && + dxl_info_.GetDxlSignType(ID, item_names[item_index], is_signed)) + { + // Use unit info and sign type to properly convert the value + *data_ptrs[item_index] = ConvertValueWithUnitInfo( + ID, item_names[item_index], dxl_getdata, + size, is_signed); + } else { + // Fallback to existing logic for compatibility + if (item_names[item_index] == "Present Position") { + *data_ptrs[item_index] = dxl_info_.ConvertValueToRadian( + ID, + static_cast(dxl_getdata)); + } else { + *data_ptrs[item_index] = static_cast(dxl_getdata); + } + } } return DxlError::OK; } @@ -1717,7 +1739,7 @@ DxlError Dynamixel::AddIndirectRead( for (uint16_t i = 0; i < item_size; i++) { DxlError write_result = DxlError::INDIRECT_ADDR_FAIL; - uint16_t addr = INDIRECT_ADDR + (using_size * 2); + uint16_t addr = static_cast(INDIRECT_ADDR + (using_size * 2)); uint16_t item_addr_i = item_addr + i; write_result = WriteItem(id, addr, 2, item_addr_i); @@ -1821,38 +1843,30 @@ DxlError Dynamixel::SetDxlValueToSyncWrite() for (uint16_t item_index = 0; item_index < indirect_info_write_[comm_id].cnt; item_index++) { double data = *it_write_data.item_data_ptr_vec.at(item_index); uint8_t ID = it_write_data.id_arr.at(item_index); - if (indirect_info_write_[comm_id].item_name.at(item_index) == "Goal Position") { - int32_t goal_position = dxl_info_.ConvertRadianToValue(ID, data); - param_write_value[added_byte + 0] = DXL_LOBYTE(DXL_LOWORD(goal_position)); - param_write_value[added_byte + 1] = DXL_HIBYTE(DXL_LOWORD(goal_position)); - param_write_value[added_byte + 2] = DXL_LOBYTE(DXL_HIWORD(goal_position)); - param_write_value[added_byte + 3] = DXL_HIBYTE(DXL_HIWORD(goal_position)); - } else if (indirect_info_write_[comm_id].item_name.at(item_index) == "Goal Velocity") { - int32_t goal_velocity = dxl_info_.ConvertVelocityRPSToValueRPM(ID, data); - param_write_value[added_byte + 0] = DXL_LOBYTE(DXL_LOWORD(goal_velocity)); - param_write_value[added_byte + 1] = DXL_HIBYTE(DXL_LOWORD(goal_velocity)); - param_write_value[added_byte + 2] = DXL_LOBYTE(DXL_HIWORD(goal_velocity)); - param_write_value[added_byte + 3] = DXL_HIBYTE(DXL_HIWORD(goal_velocity)); - } else if (indirect_info_write_[comm_id].item_name.at(item_index) == "Goal Current") { - int16_t goal_current = dxl_info_.ConvertEffortToCurrent(ID, data); - param_write_value[added_byte + 0] = DXL_LOBYTE(goal_current); - param_write_value[added_byte + 1] = DXL_HIBYTE(goal_current); + std::string item_name = indirect_info_write_[comm_id].item_name.at(item_index); + uint8_t size = indirect_info_write_[comm_id].item_size.at(item_index); + + // Check if there is unit info for this item + double unit_value; + bool is_signed; + if (dxl_info_.GetDxlUnitValue(ID, item_name, unit_value) && + dxl_info_.GetDxlSignType(ID, item_name, is_signed)) + { + // Use unit info and sign type to properly convert the value + uint32_t raw_value = ConvertUnitValueToRawValue(ID, item_name, data, size, is_signed); + WriteValueToBuffer(param_write_value, added_byte, raw_value, size); } else { - if (indirect_info_write_[comm_id].item_size.at(item_index) == 4) { - int32_t value = static_cast(data); - param_write_value[added_byte + 0] = DXL_LOBYTE(DXL_LOWORD(value)); - param_write_value[added_byte + 1] = DXL_HIBYTE(DXL_LOWORD(value)); - param_write_value[added_byte + 2] = DXL_LOBYTE(DXL_HIWORD(value)); - param_write_value[added_byte + 3] = DXL_HIBYTE(DXL_HIWORD(value)); - } else if (indirect_info_write_[comm_id].item_size.at(item_index) == 2) { - int16_t value = static_cast(data); - param_write_value[added_byte + 0] = DXL_LOBYTE(value); - param_write_value[added_byte + 1] = DXL_HIBYTE(value); - } else if (indirect_info_write_[comm_id].item_size.at(item_index) == 1) { - param_write_value[added_byte] = static_cast(data); + // Fallback to existing logic for compatibility + if (item_name == "Goal Position") { + int32_t goal_position = dxl_info_.ConvertRadianToValue(ID, data); + WriteValueToBuffer( + param_write_value, added_byte, static_cast(goal_position), + 4); + } else { + WriteValueToBuffer(param_write_value, added_byte, static_cast(data), size); } } - added_byte += indirect_info_write_[comm_id].item_size.at(item_index); + added_byte += size; } if (group_sync_write_->addParam(comm_id, param_write_value) != true) { @@ -1913,7 +1927,7 @@ DxlError Dynamixel::SetBulkWriteItemAndHandler() if (addr < min_addr) {min_addr = addr;} if (addr + size > max_end_addr) {max_end_addr = addr + size;} } - uint8_t total_size = max_end_addr - min_addr; + uint8_t total_size = static_cast(max_end_addr - min_addr); // Check for gaps between items std::vector> addr_ranges; @@ -1938,7 +1952,8 @@ DxlError Dynamixel::SetBulkWriteItemAndHandler() // Store direct write info direct_info_write_[it_write_data.comm_id].indirect_data_addr = min_addr; direct_info_write_[it_write_data.comm_id].size = total_size; - direct_info_write_[it_write_data.comm_id].cnt = it_write_data.item_name.size(); + direct_info_write_[it_write_data.comm_id].cnt = + static_cast(it_write_data.item_name.size()); direct_info_write_[it_write_data.comm_id].item_name = it_write_data.item_name; direct_info_write_[it_write_data.comm_id].item_size = it_write_data.item_size; @@ -2021,19 +2036,29 @@ DxlError Dynamixel::SetDxlValueToBulkWrite() for (uint16_t item_index = 0; item_index < direct_info_write_[comm_id].cnt; item_index++) { double data = *it_write_data.item_data_ptr_vec.at(item_index); + uint8_t ID = comm_id; + std::string item_name = direct_info_write_[comm_id].item_name.at(item_index); uint8_t size = direct_info_write_[comm_id].item_size.at(item_index); - if (size == 4) { - int32_t value = static_cast(data); - param_write_value[added_byte + 0] = DXL_LOBYTE(DXL_LOWORD(value)); - param_write_value[added_byte + 1] = DXL_HIBYTE(DXL_LOWORD(value)); - param_write_value[added_byte + 2] = DXL_LOBYTE(DXL_HIWORD(value)); - param_write_value[added_byte + 3] = DXL_HIBYTE(DXL_HIWORD(value)); - } else if (size == 2) { - int16_t value = static_cast(data); - param_write_value[added_byte + 0] = DXL_LOBYTE(value); - param_write_value[added_byte + 1] = DXL_HIBYTE(value); - } else if (size == 1) { - param_write_value[added_byte] = static_cast(data); + + // Check if there is unit info for this item + double unit_value; + bool is_signed; + if (dxl_info_.GetDxlUnitValue(ID, item_name, unit_value) && + dxl_info_.GetDxlSignType(ID, item_name, is_signed)) + { + // Use unit info and sign type to properly convert the value + uint32_t raw_value = ConvertUnitValueToRawValue(ID, item_name, data, size, is_signed); + WriteValueToBuffer(param_write_value, added_byte, raw_value, size); + } else { + // Fallback to existing logic for compatibility + if (item_name == "Goal Position") { + int32_t goal_position = dxl_info_.ConvertRadianToValue(ID, data); + WriteValueToBuffer( + param_write_value, added_byte, static_cast(goal_position), + 4); + } else { + WriteValueToBuffer(param_write_value, added_byte, static_cast(data), size); + } } added_byte += size; } @@ -2054,38 +2079,30 @@ DxlError Dynamixel::SetDxlValueToBulkWrite() for (uint16_t item_index = 0; item_index < indirect_info_write_[comm_id].cnt; item_index++) { double data = *it_write_data.item_data_ptr_vec.at(item_index); uint8_t ID = it_write_data.id_arr.at(item_index); - if (indirect_info_write_[comm_id].item_name.at(item_index) == "Goal Position") { - int32_t goal_position = dxl_info_.ConvertRadianToValue(ID, data); - param_write_value[added_byte + 0] = DXL_LOBYTE(DXL_LOWORD(goal_position)); - param_write_value[added_byte + 1] = DXL_HIBYTE(DXL_LOWORD(goal_position)); - param_write_value[added_byte + 2] = DXL_LOBYTE(DXL_HIWORD(goal_position)); - param_write_value[added_byte + 3] = DXL_HIBYTE(DXL_HIWORD(goal_position)); - } else if (indirect_info_write_[comm_id].item_name.at(item_index) == "Goal Velocity") { - int32_t goal_velocity = dxl_info_.ConvertVelocityRPSToValueRPM(ID, data); - param_write_value[added_byte + 0] = DXL_LOBYTE(DXL_LOWORD(goal_velocity)); - param_write_value[added_byte + 1] = DXL_HIBYTE(DXL_LOWORD(goal_velocity)); - param_write_value[added_byte + 2] = DXL_LOBYTE(DXL_HIWORD(goal_velocity)); - param_write_value[added_byte + 3] = DXL_HIBYTE(DXL_HIWORD(goal_velocity)); - } else if (indirect_info_write_[comm_id].item_name.at(item_index) == "Goal Current") { - int16_t goal_current = dxl_info_.ConvertEffortToCurrent(ID, data); - param_write_value[added_byte + 0] = DXL_LOBYTE(goal_current); - param_write_value[added_byte + 1] = DXL_HIBYTE(goal_current); + std::string item_name = indirect_info_write_[comm_id].item_name.at(item_index); + uint8_t size = indirect_info_write_[comm_id].item_size.at(item_index); + + // Check if there is unit info for this item + double unit_value; + bool is_signed; + if (dxl_info_.GetDxlUnitValue(ID, item_name, unit_value) && + dxl_info_.GetDxlSignType(ID, item_name, is_signed)) + { + // Use unit info and sign type to properly convert the value + uint32_t raw_value = ConvertUnitValueToRawValue(ID, item_name, data, size, is_signed); + WriteValueToBuffer(param_write_value, added_byte, raw_value, size); } else { - if (indirect_info_write_[comm_id].item_size.at(item_index) == 4) { - int32_t value = static_cast(data); - param_write_value[added_byte + 0] = DXL_LOBYTE(DXL_LOWORD(value)); - param_write_value[added_byte + 1] = DXL_HIBYTE(DXL_LOWORD(value)); - param_write_value[added_byte + 2] = DXL_LOBYTE(DXL_HIWORD(value)); - param_write_value[added_byte + 3] = DXL_HIBYTE(DXL_HIWORD(value)); - } else if (indirect_info_write_[comm_id].item_size.at(item_index) == 2) { - int16_t value = static_cast(data); - param_write_value[added_byte + 0] = DXL_LOBYTE(value); - param_write_value[added_byte + 1] = DXL_HIBYTE(value); - } else if (indirect_info_write_[comm_id].item_size.at(item_index) == 1) { - param_write_value[added_byte] = static_cast(data); + // Fallback to existing logic for compatibility + if (item_name == "Goal Position") { + int32_t goal_position = dxl_info_.ConvertRadianToValue(ID, data); + WriteValueToBuffer( + param_write_value, added_byte, static_cast(goal_position), + 4); + } else { + WriteValueToBuffer(param_write_value, added_byte, static_cast(data), size); } } - added_byte += indirect_info_write_[comm_id].item_size.at(item_index); + added_byte += size; } if (group_bulk_write_->addParam( @@ -2138,7 +2155,10 @@ DxlError Dynamixel::AddIndirectWrite( uint8_t using_size = indirect_info_write_[id].size; for (uint16_t i = 0; i < item_size; i++) { - if (WriteItem(id, INDIRECT_ADDR + (using_size * 2), 2, item_addr + i) != DxlError::OK) { + if (WriteItem( + id, static_cast(INDIRECT_ADDR + (using_size * 2)), 2, + item_addr + i) != DxlError::OK) + { return DxlError::SET_BULK_WRITE_FAIL; } using_size++; @@ -2163,4 +2183,92 @@ void Dynamixel::ResetDirectWrite(std::vector id_arr) direct_info_write_[it_id] = temp; } } + +double Dynamixel::ConvertValueWithUnitInfo( + uint8_t id, std::string item_name, uint32_t raw_value, + uint8_t size, bool is_signed) +{ + if (size == 1) { + if (is_signed) { + int8_t signed_value = static_cast(raw_value); + return dxl_info_.ConvertValueToUnit(id, item_name, signed_value); + } else { + uint8_t unsigned_value = static_cast(raw_value); + return dxl_info_.ConvertValueToUnit(id, item_name, unsigned_value); + } + } else if (size == 2) { + if (is_signed) { + int16_t signed_value = static_cast(raw_value); + return dxl_info_.ConvertValueToUnit(id, item_name, signed_value); + } else { + uint16_t unsigned_value = static_cast(raw_value); + return dxl_info_.ConvertValueToUnit(id, item_name, unsigned_value); + } + } else if (size == 4) { + if (is_signed) { + int32_t signed_value = static_cast(raw_value); + return dxl_info_.ConvertValueToUnit(id, item_name, signed_value); + } else { + uint32_t unsigned_value = static_cast(raw_value); + return dxl_info_.ConvertValueToUnit(id, item_name, unsigned_value); + } + } + + // Throw error for unknown sizes + std::string error_msg = "Unknown data size " + std::to_string(size) + + " for item '" + item_name + "' in ID " + std::to_string(id); + throw std::runtime_error(error_msg); +} + +uint32_t Dynamixel::ConvertUnitValueToRawValue( + uint8_t id, std::string item_name, double unit_value, + uint8_t size, bool is_signed) +{ + if (size == 1) { + if (is_signed) { + int8_t signed_value = dxl_info_.ConvertUnitToValue(id, item_name, unit_value); + return static_cast(signed_value); + } else { + uint8_t unsigned_value = dxl_info_.ConvertUnitToValue(id, item_name, unit_value); + return static_cast(unsigned_value); + } + } else if (size == 2) { + if (is_signed) { + int16_t signed_value = dxl_info_.ConvertUnitToValue(id, item_name, unit_value); + return static_cast(signed_value); + } else { + uint16_t unsigned_value = dxl_info_.ConvertUnitToValue(id, item_name, unit_value); + return static_cast(unsigned_value); + } + } else if (size == 4) { + if (is_signed) { + int32_t signed_value = dxl_info_.ConvertUnitToValue(id, item_name, unit_value); + return static_cast(signed_value); + } else { + uint32_t unsigned_value = dxl_info_.ConvertUnitToValue(id, item_name, unit_value); + return unsigned_value; + } + } + + // Throw error for unknown sizes + std::string error_msg = "Unknown data size " + std::to_string(size) + + " for item '" + item_name + "' in ID " + std::to_string(id); + throw std::runtime_error(error_msg); +} + +void Dynamixel::WriteValueToBuffer(uint8_t * buffer, uint8_t offset, uint32_t value, uint8_t size) +{ + if (size == 1) { + buffer[offset] = static_cast(value); + } else if (size == 2) { + uint16_t value_16 = static_cast(value); + buffer[offset + 0] = DXL_LOBYTE(value_16); + buffer[offset + 1] = DXL_HIBYTE(value_16); + } else if (size == 4) { + buffer[offset + 0] = DXL_LOBYTE(DXL_LOWORD(value)); + buffer[offset + 1] = DXL_HIBYTE(DXL_LOWORD(value)); + buffer[offset + 2] = DXL_LOBYTE(DXL_HIWORD(value)); + buffer[offset + 3] = DXL_HIBYTE(DXL_HIWORD(value)); + } +} } // namespace dynamixel_hardware_interface diff --git a/src/dynamixel/dynamixel_info.cpp b/src/dynamixel/dynamixel_info.cpp index 7ce0c8c..4ee7b61 100644 --- a/src/dynamixel/dynamixel_info.cpp +++ b/src/dynamixel/dynamixel_info.cpp @@ -73,59 +73,99 @@ void DynamixelInfo::ReadDxlModelFile(uint8_t id, uint16_t model_num) std::string line; temp_dxl_info.model_num = model_num; - bool torque_constant_set = false; - bool velocity_unit_set = false; + + // Check if [type info] section exists + bool type_info_found = false; + bool unit_info_found = false; + bool control_table_found = false; while (!open_file.eof() ) { getline(open_file, line); - if (strcmp(line.c_str(), "[control table]") == 0) { + if (strcmp(line.c_str(), "[type info]") == 0) { + type_info_found = true; + break; + } else if (strcmp(line.c_str(), "[unit info]") == 0) { + unit_info_found = true; + break; + } else if (strcmp(line.c_str(), "[control table]") == 0) { + control_table_found = true; break; } + } + if (type_info_found) { + // Parse type info section + while (!open_file.eof() ) { + getline(open_file, line); + if (strcmp(line.c_str(), "[unit info]") == 0) { + unit_info_found = true; + break; + } else if (strcmp(line.c_str(), "[control table]") == 0) { + control_table_found = true; + break; + } - std::vector strs; - boost::split(strs, line, boost::is_any_of("\t")); + std::vector strs; + boost::split(strs, line, boost::is_any_of("\t")); - if (strs.size() < 2) { - continue; - } + if (strs.size() < 2) { + continue; + } - try { - if (strs.at(0) == "value_of_zero_radian_position") { - temp_dxl_info.value_of_zero_radian_position = static_cast(stoi(strs.at(1))); - } else if (strs.at(0) == "value_of_max_radian_position") { - temp_dxl_info.value_of_max_radian_position = static_cast(stoi(strs.at(1))); - } else if (strs.at(0) == "value_of_min_radian_position") { - temp_dxl_info.value_of_min_radian_position = static_cast(stoi(strs.at(1))); - } else if (strs.at(0) == "min_radian") { - temp_dxl_info.min_radian = static_cast(stod(strs.at(1))); - } else if (strs.at(0) == "max_radian") { - temp_dxl_info.max_radian = static_cast(stod(strs.at(1))); - } else if (strs.at(0) == "torque_constant") { - temp_dxl_info.torque_constant = static_cast(stod(strs.at(1))); - torque_constant_set = true; - } else if (strs.at(0) == "velocity_unit") { - temp_dxl_info.velocity_unit = static_cast(stod(strs.at(1))); - velocity_unit_set = true; + try { + if (strs.at(0) == "value_of_zero_radian_position") { + temp_dxl_info.value_of_zero_radian_position = static_cast(stoi(strs.at(1))); + } else if (strs.at(0) == "value_of_max_radian_position") { + temp_dxl_info.value_of_max_radian_position = static_cast(stoi(strs.at(1))); + } else if (strs.at(0) == "value_of_min_radian_position") { + temp_dxl_info.value_of_min_radian_position = static_cast(stoi(strs.at(1))); + } else if (strs.at(0) == "min_radian") { + temp_dxl_info.min_radian = static_cast(stod(strs.at(1))); + } else if (strs.at(0) == "max_radian") { + temp_dxl_info.max_radian = static_cast(stod(strs.at(1))); + } + } catch (const std::exception & e) { + std::string error_msg = "Error processing line in model file: " + line + + "\nError: " + e.what(); + throw std::runtime_error(error_msg); } - } catch (const std::exception & e) { - std::string error_msg = "Error processing line in model file: " + line + - "\nError: " + e.what(); - throw std::runtime_error(error_msg); } } - // Set default values and warn if parameters are missing - if (!torque_constant_set) { - fprintf( - stderr, "[WARN] Model file '%s' doesn't contain torque_constant parameter. " - "Using default value: 1.0\n", path.c_str()); - temp_dxl_info.torque_constant = 1.0; + if (unit_info_found) { + getline(open_file, line); // Skip header line "Data Name value unit Sign Type" + while (!open_file.eof() ) { + getline(open_file, line); + if (strcmp(line.c_str(), "[control table]") == 0) { + control_table_found = true; + break; + } + + std::vector strs; + boost::split(strs, line, boost::is_any_of("\t")); + + if (strs.size() < 4) { + continue; + } + + try { + std::string data_name = strs.at(0); + double unit_value = static_cast(stod(strs.at(1))); + std::string sign_type_str = strs.at(3); + bool is_signed = (sign_type_str == "signed"); + temp_dxl_info.unit_map[data_name] = unit_value; + temp_dxl_info.sign_type_map[data_name] = is_signed; + } catch (const std::exception & e) { + std::string error_msg = "Error processing unit info line: " + line + + "\nError: " + e.what(); + throw std::runtime_error(error_msg); + } + } } - if (!velocity_unit_set) { - fprintf( - stderr, "[WARN] Model file '%s' doesn't contain velocity_unit parameter. " - "Using default value: 0.01\n", path.c_str()); - temp_dxl_info.velocity_unit = 0.01; + + if (!control_table_found) { + std::string error_msg = "No [control table] section found in model file for ID " + + std::to_string(id); + throw std::runtime_error(error_msg); } getline(open_file, line); @@ -190,26 +230,6 @@ bool DynamixelInfo::CheckDxlControlItem(uint8_t id, std::string item_name) return false; } -// bool DynamixelInfo::GetDxlTypeInfo( -// uint8_t id, -// int32_t & value_of_zero_radian_position, -// int32_t & value_of_max_radian_position, -// int32_t & value_of_min_radian_position, -// double & min_radian, -// double & max_radian, -// double & torque_constant, -// double & velocity_unit) -// { -// 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; -// velocity_unit = dxl_info_[id].velocity_unit; -// return true; -// } - int32_t DynamixelInfo::ConvertRadianToValue(uint8_t id, double radian) { if (radian > 0) { @@ -243,4 +263,34 @@ double DynamixelInfo::ConvertValueToRadian(uint8_t id, int32_t value) return 0.0; } } + +bool DynamixelInfo::GetDxlUnitValue(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()) { + unit_value = it->second; + return true; + } + return false; +} + +bool DynamixelInfo::GetDxlSignType(uint8_t id, std::string data_name, bool & is_signed) +{ + auto it = dxl_info_[id].sign_type_map.find(data_name); + if (it != dxl_info_[id].sign_type_map.end()) { + is_signed = it->second; + return true; + } + return false; +} + +double DynamixelInfo::GetUnitMultiplier(uint8_t id, std::string data_name) +{ + auto it = dxl_info_[id].unit_map.find(data_name); + if (it != dxl_info_[id].unit_map.end()) { + return it->second; + } + return 1.0; +} + } // namespace dynamixel_hardware_interface diff --git a/src/dynamixel_hardware_interface.cpp b/src/dynamixel_hardware_interface.cpp index 3295951..a1c6aa1 100644 --- a/src/dynamixel_hardware_interface.cpp +++ b/src/dynamixel_hardware_interface.cpp @@ -24,6 +24,8 @@ #include #include #include +#include +#include #include "hardware_interface/types/hardware_interface_type_values.hpp" #include "rclcpp/rclcpp.hpp" @@ -63,19 +65,51 @@ hardware_interface::CallbackReturn DynamixelHardware::on_init( return hardware_interface::CallbackReturn::ERROR; } + if (info_.hardware_parameters.find("number_of_joints") == info_.hardware_parameters.end()) { + RCLCPP_ERROR_STREAM( + logger_, + "Required parameter 'number_of_joints' not found in hardware parameters"); + return hardware_interface::CallbackReturn::ERROR; + } + if (info_.hardware_parameters.find("number_of_transmissions") == + info_.hardware_parameters.end()) + { + RCLCPP_ERROR_STREAM( + logger_, + "Required parameter 'number_of_transmissions' not found in hardware parameters"); + return hardware_interface::CallbackReturn::ERROR; + } + num_of_joints_ = static_cast(stoi(info_.hardware_parameters["number_of_joints"])); num_of_transmissions_ = static_cast(stoi(info_.hardware_parameters["number_of_transmissions"])); - SetMatrix(); + + if (!SetMatrix()) { + RCLCPP_ERROR_STREAM(logger_, "Failed to set transmission matrices"); + return hardware_interface::CallbackReturn::ERROR; + } + + if (info_.hardware_parameters.find("port_name") == info_.hardware_parameters.end()) { + RCLCPP_ERROR_STREAM(logger_, "Required parameter 'port_name' not found in hardware parameters"); + return hardware_interface::CallbackReturn::ERROR; + } + if (info_.hardware_parameters.find("baud_rate") == info_.hardware_parameters.end()) { + RCLCPP_ERROR_STREAM(logger_, "Required parameter 'baud_rate' not found in hardware parameters"); + return hardware_interface::CallbackReturn::ERROR; + } port_name_ = info_.hardware_parameters["port_name"]; baud_rate_ = info_.hardware_parameters["baud_rate"]; - try { - err_timeout_ms_ = stod(info_.hardware_parameters["error_timeout_ms"]); - } catch (const std::exception & e) { - RCLCPP_ERROR( - logger_, "Failed to parse error_timeout_ms parameter: %s, using default value", - e.what()); + if (info_.hardware_parameters.find("error_timeout_ms") != info_.hardware_parameters.end()) { + try { + err_timeout_ms_ = stod(info_.hardware_parameters["error_timeout_ms"]); + } catch (const std::exception & e) { + RCLCPP_ERROR( + logger_, "Failed to parse error_timeout_ms parameter: %s, using default value", + e.what()); + } + } else { + RCLCPP_INFO(logger_, "error_timeout_ms parameter not found, using default value of 500ms"); } // Add new parameter for torque initialization @@ -98,6 +132,12 @@ hardware_interface::CallbackReturn DynamixelHardware::on_init( logger_, "port_name " << port_name_.c_str() << " / baudrate " << baud_rate_.c_str()); + if (info_.hardware_parameters.find("dynamixel_model_folder") == info_.hardware_parameters.end()) { + RCLCPP_ERROR_STREAM( + logger_, + "Required parameter 'dynamixel_model_folder' not found in hardware parameters"); + return hardware_interface::CallbackReturn::ERROR; + } std::string dxl_model_folder = info_.hardware_parameters["dynamixel_model_folder"]; dxl_comm_ = std::unique_ptr( new Dynamixel( @@ -298,8 +338,12 @@ hardware_interface::CallbackReturn DynamixelHardware::on_init( hdl_sensor_states_.push_back(temp_state); } - std::string str_dxl_state_pub_name = - info_.hardware_parameters["dynamixel_state_pub_msg_name"]; + std::string str_dxl_state_pub_name = "dynamixel_hardware_interface/dxl_state"; + if (info_.hardware_parameters.find("dynamixel_state_pub_msg_name") != + info_.hardware_parameters.end()) + { + str_dxl_state_pub_name = info_.hardware_parameters["dynamixel_state_pub_msg_name"]; + } dxl_state_pub_ = this->create_publisher( str_dxl_state_pub_name, rclcpp::SystemDefaultsQoS()); dxl_state_pub_uni_ptr_ = std::make_unique(dxl_state_pub_); @@ -312,26 +356,45 @@ hardware_interface::CallbackReturn DynamixelHardware::on_init( dxl_state_pub_uni_ptr_->unlock(); using namespace std::placeholders; - std::string str_get_dxl_data_srv_name = - info_.hardware_parameters["get_dynamixel_data_srv_name"]; + + // Get Dynamixel data service + std::string str_get_dxl_data_srv_name = "dynamixel_hardware_interface/get_dxl_data"; + if (info_.hardware_parameters.find("get_dynamixel_data_srv_name") != + info_.hardware_parameters.end()) + { + str_get_dxl_data_srv_name = info_.hardware_parameters["get_dynamixel_data_srv_name"]; + } get_dxl_data_srv_ = create_service( str_get_dxl_data_srv_name, std::bind(&DynamixelHardware::get_dxl_data_srv_callback, this, _1, _2)); - std::string str_set_dxl_data_srv_name = - info_.hardware_parameters["set_dynamixel_data_srv_name"]; + // Set Dynamixel data service + std::string str_set_dxl_data_srv_name = "dynamixel_hardware_interface/set_dxl_data"; + if (info_.hardware_parameters.find("set_dynamixel_data_srv_name") != + info_.hardware_parameters.end()) + { + str_set_dxl_data_srv_name = info_.hardware_parameters["set_dynamixel_data_srv_name"]; + } set_dxl_data_srv_ = create_service( str_set_dxl_data_srv_name, std::bind(&DynamixelHardware::set_dxl_data_srv_callback, this, _1, _2)); - std::string str_reboot_dxl_srv_name = - info_.hardware_parameters["reboot_dxl_srv_name"]; + // Reboot Dynamixel service + std::string str_reboot_dxl_srv_name = "dynamixel_hardware_interface/reboot_dxl"; + if (info_.hardware_parameters.find("reboot_dxl_srv_name") != info_.hardware_parameters.end()) { + str_reboot_dxl_srv_name = info_.hardware_parameters["reboot_dxl_srv_name"]; + } reboot_dxl_srv_ = create_service( str_reboot_dxl_srv_name, std::bind(&DynamixelHardware::reboot_dxl_srv_callback, this, _1, _2)); - std::string str_set_dxl_torque_srv_name = - info_.hardware_parameters["set_dxl_torque_srv_name"]; + // Set Dynamixel torque service + std::string str_set_dxl_torque_srv_name = "dynamixel_hardware_interface/set_dxl_torque"; + if (info_.hardware_parameters.find("set_dxl_torque_srv_name") != + info_.hardware_parameters.end()) + { + str_set_dxl_torque_srv_name = info_.hardware_parameters["set_dxl_torque_srv_name"]; + } set_dxl_torque_srv_ = create_service( str_set_dxl_torque_srv_name, std::bind(&DynamixelHardware::set_dxl_torque_srv_callback, this, _1, _2)); @@ -503,18 +566,10 @@ hardware_interface::CallbackReturn DynamixelHardware::start() dxl_comm_->WriteMultiDxlData(); - // Enable torque only for Dynamixels that have torque enabled in their parameters - std::vector torque_enabled_ids; - for (const auto & [id, enabled] : dxl_torque_enable_) { - if (enabled) { - torque_enabled_ids.push_back(id); - } - } - - if (torque_enabled_ids.size() > 0) { + if (torque_enabled_ids_.size() > 0) { RCLCPP_INFO_STREAM(logger_, "Enabling torque for Dynamixels"); for (int i = 0; i < 10; i++) { - if (dxl_comm_->DynamixelEnable(torque_enabled_ids) == DxlError::OK) { + if (dxl_comm_->DynamixelEnable(torque_enabled_ids_) == DxlError::OK) { break; } RCLCPP_ERROR_STREAM(logger_, "Failed to enable torque for Dynamixels, retry..."); @@ -550,9 +605,9 @@ hardware_interface::return_type DynamixelHardware::read( if (dxl_status_ == REBOOTING) { RCLCPP_ERROR_STREAM(logger_, "Dynamixel Read Fail : REBOOTING"); return hardware_interface::return_type::ERROR; - } else if (dxl_status_ == DXL_OK || dxl_status_ == COMM_ERROR) { + } else if (dxl_status_ == DXL_OK || dxl_status_ == COMM_ERROR || dxl_status_ == HW_ERROR) { dxl_comm_err_ = CheckError(dxl_comm_->ReadMultiDxlData(period_ms)); - if (dxl_comm_err_ != DxlError::OK) { + if (dxl_comm_err_ != DxlError::OK && dxl_comm_err_ != DxlError::DXL_HARDWARE_ERROR) { if (!is_read_in_error_) { is_read_in_error_ = true; read_error_duration_ = rclcpp::Duration(0, 0); @@ -571,13 +626,6 @@ hardware_interface::return_type DynamixelHardware::read( } is_read_in_error_ = false; read_error_duration_ = rclcpp::Duration(0, 0); - } else if (dxl_status_ == HW_ERROR) { - dxl_comm_err_ = CheckError(dxl_comm_->ReadMultiDxlData(period_ms)); - if (dxl_comm_err_ != DxlError::OK) { - RCLCPP_ERROR_STREAM( - logger_, - "Dynamixel Read Fail :" << Dynamixel::DxlErrorToString(dxl_comm_err_)); - } } CalcTransmissionToJoint(); @@ -644,42 +692,70 @@ DxlError DynamixelHardware::CheckError(DxlError dxl_comm_err) // check comm error if (dxl_comm_err != DxlError::OK) { - RCLCPP_ERROR_STREAM( - logger_, + RCLCPP_ERROR_STREAM_THROTTLE( + logger_, clock_, 1000, "Communication Fail --> " << Dynamixel::DxlErrorToString(dxl_comm_err)); dxl_status_ = COMM_ERROR; return dxl_comm_err; } + // check hardware error for (size_t i = 0; i < num_of_transmissions_; i++) { for (size_t j = 0; j < hdl_trans_states_.at(i).interface_name_vec.size(); j++) { if (hdl_trans_states_.at(i).interface_name_vec.at(j) == "Hardware Error Status") { - dxl_hw_err_[hdl_trans_states_.at(i).id] = *hdl_trans_states_.at(i).value_ptr_vec.at(j); + dxl_hw_err_[hdl_trans_states_.at(i).id] = + static_cast(*hdl_trans_states_.at(i).value_ptr_vec.at(j)); + uint8_t hw_error_status = static_cast(dxl_hw_err_[hdl_trans_states_.at(i).id]); std::string error_string = ""; - if (dxl_hw_err_[hdl_trans_states_.at(i).id] & 0x01) { - error_string += "input voltage error/ "; - } - if (dxl_hw_err_[hdl_trans_states_.at(i).id] & 0x04) { - error_string += "overheating/ "; - } - if (dxl_hw_err_[hdl_trans_states_.at(i).id] & 0x08) { - error_string += "motor encoder/ "; - } - if (dxl_hw_err_[hdl_trans_states_.at(i).id] & 0x16) { - error_string += "electrical shork/ "; - } - if (dxl_hw_err_[hdl_trans_states_.at(i).id] & 0x32) { - error_string += "Overload/ "; + + // Check each bit in the hardware error status + for (int bit = 0; bit < 8; ++bit) { + if (hw_error_status & (1 << bit)) { + const HardwareErrorStatusBitInfo * bit_info = get_hardware_error_status_bit_info(bit); + if (bit_info) { + error_string += bit_info->label; + error_string += " (" + std::string(bit_info->description) + ")/ "; + } else { + error_string += "Unknown Error Bit " + std::to_string(bit) + "/ "; + } + } } if (!error_string.empty()) { - RCLCPP_WARN_STREAM( - logger_, "Dynamixel Hardware Error States [ ID:" << + RCLCPP_ERROR_STREAM_THROTTLE( + logger_, clock_, 1000, + "Dynamixel Hardware Error States [ ID:" << static_cast(hdl_trans_states_.at(i).id) << "] --> " << - static_cast(dxl_hw_err_[hdl_trans_states_.at(i).id]) << - "/ " << error_string); + "0x" << std::hex << static_cast(hw_error_status) << std::dec << + " (" << static_cast(hw_error_status) << "): " << error_string); dxl_status_ = HW_ERROR; - error_state = DxlError::DLX_HARDWARE_ERROR; + error_state = DxlError::DXL_HARDWARE_ERROR; + } + } + if (hdl_trans_states_.at(i).interface_name_vec.at(j) == "Error Code") { + dxl_error_code_[hdl_trans_states_.at(i).id] = + static_cast(*hdl_trans_states_.at(i).value_ptr_vec.at(j)); + uint8_t error_code = static_cast(dxl_error_code_[hdl_trans_states_.at(i).id]); + + if (error_code != 0x00) { + const ErrorCodeInfo * error_info = get_error_code_info(error_code); + if (error_info) { + RCLCPP_ERROR_STREAM_THROTTLE( + logger_, clock_, 1000, + "Dynamixel Error Code [ ID:" << + static_cast(hdl_trans_states_.at(i).id) << "] --> " << + "0x" << std::hex << static_cast(error_code) << std::dec << + " (" << error_info->label << "): " << error_info->description); + } else { + RCLCPP_ERROR_STREAM_THROTTLE( + logger_, clock_, 1000, + "Dynamixel Error Code [ ID:" << + static_cast(hdl_trans_states_.at(i).id) << "] --> " << + "0x" << std::hex << static_cast(error_code) << std::dec << + " (Unknown Error Code)"); + } + dxl_status_ = HW_ERROR; + error_state = DxlError::DXL_HARDWARE_ERROR; } } } @@ -748,7 +824,9 @@ bool DynamixelHardware::initItems(const std::string & type_filter) if (gpio.parameters.find("Torque Enable") != gpio.parameters.end()) { torque_enabled = std::stoi(gpio.parameters.at("Torque Enable")) != 0; } - dxl_torque_enable_[id] = torque_enabled; + if (torque_enabled) { + torque_enabled_ids_.push_back(id); + } // 1. First pass: Write Operating Mode parameters for (const auto & param : gpio.parameters) { @@ -943,12 +1021,12 @@ bool DynamixelHardware::InitDxlWriteItems() temp_write.name = gpio.name; for (auto it : gpio.command_interfaces) { - if (it.name != "Goal Position" && - it.name != "Goal Velocity" && - it.name != "Goal Current") - { - continue; - } + // if (it.name != "Goal Position" && + // it.name != "Goal Velocity" && + // it.name != "Goal Current") + // { + // continue; + // } temp_write.interface_name_vec.push_back(it.name); temp_write.value_ptr_vec.push_back(std::make_shared(0.0)); } @@ -974,12 +1052,12 @@ bool DynamixelHardware::InitDxlWriteItems() temp_write.name = gpio.name; for (auto it : gpio.command_interfaces) { - if (it.name != "Goal Position" && - it.name != "Goal Velocity" && - it.name != "Goal Current") - { - continue; - } + // if (it.name != "Goal Position" && + // it.name != "Goal Velocity" && + // it.name != "Goal Current") + // { + // continue; + // } temp_write.interface_name_vec.push_back(it.name); temp_write.value_ptr_vec.push_back(std::make_shared(0.0)); } @@ -1028,7 +1106,7 @@ void DynamixelHardware::ReadSensorData(const HandlerVarType & sensor) } } -void DynamixelHardware::SetMatrix() +bool DynamixelHardware::SetMatrix() { std::string str; std::vector d_vec; @@ -1040,6 +1118,14 @@ void DynamixelHardware::SetMatrix() } d_vec.clear(); + if (info_.hardware_parameters.find("transmission_to_joint_matrix") == + info_.hardware_parameters.end()) + { + RCLCPP_ERROR_STREAM( + logger_, + "Required parameter 'transmission_to_joint_matrix' not found in hardware parameters"); + return false; + } std::stringstream ss_tj(info_.hardware_parameters["transmission_to_joint_matrix"]); while (std::getline(ss_tj, str, ',')) { d_vec.push_back(stod(str)); @@ -1064,6 +1150,14 @@ void DynamixelHardware::SetMatrix() } d_vec.clear(); + if (info_.hardware_parameters.find("joint_to_transmission_matrix") == + info_.hardware_parameters.end()) + { + RCLCPP_ERROR_STREAM( + logger_, + "Required parameter 'joint_to_transmission_matrix' not found in hardware parameters"); + return false; + } std::stringstream ss_jt(info_.hardware_parameters["joint_to_transmission_matrix"]); while (std::getline(ss_jt, str, ',')) { d_vec.push_back(stod(str)); @@ -1082,6 +1176,8 @@ void DynamixelHardware::SetMatrix() } fprintf(stderr, "\n"); } + + return true; } void DynamixelHardware::MapInterfaces( @@ -1111,7 +1207,7 @@ void DynamixelHardware::MapInterfaces( if (++key_count < iface_map.size()) {oss << ", ";} } oss << "]"; - RCLCPP_WARN_STREAM(logger_, oss.str()); + RCLCPP_DEBUG_STREAM(logger_, oss.str()); continue; } const std::vector & mapped_ifaces = map_it->second; @@ -1225,21 +1321,23 @@ void DynamixelHardware::SyncJointCommandWithStates() void DynamixelHardware::ChangeDxlTorqueState() { + if (torque_enabled_ids_.size() == 0) { + return; + } + if (dxl_torque_status_ == REQUESTED_TO_ENABLE) { - std::cout << "torque enable" << std::endl; - dxl_comm_->DynamixelEnable(dxl_id_); - dxl_comm_->DynamixelEnable(virtual_dxl_id_); + RCLCPP_WARN_STREAM(logger_, "Requested to enable torque, Enabling torque for all Dynamixels"); + dxl_comm_->DynamixelEnable(torque_enabled_ids_); SyncJointCommandWithStates(); } else if (dxl_torque_status_ == REQUESTED_TO_DISABLE) { - std::cout << "torque disable" << std::endl; - dxl_comm_->DynamixelDisable(dxl_id_); - dxl_comm_->DynamixelDisable(virtual_dxl_id_); + RCLCPP_WARN_STREAM(logger_, "Requested to disable torque, Disabling torque for all Dynamixels"); + dxl_comm_->DynamixelDisable(torque_enabled_ids_); SyncJointCommandWithStates(); } dxl_torque_state_ = dxl_comm_->GetDxlTorqueState(); for (auto single_torque_state : dxl_torque_state_) { - if (single_torque_state.second == false) { + if (single_torque_state.second == TORQUE_OFF) { dxl_torque_status_ = TORQUE_DISABLED; return; } @@ -1313,6 +1411,7 @@ void DynamixelHardware::set_dxl_torque_srv_callback( if (dxl_torque_status_ == TORQUE_ENABLED) { response->success = true; response->message = "Already enabled."; + RCLCPP_INFO_STREAM(logger_, "Requested to enable torque, but already enabled."); return; } else { dxl_torque_status_ = REQUESTED_TO_ENABLE; @@ -1321,37 +1420,40 @@ void DynamixelHardware::set_dxl_torque_srv_callback( if (dxl_torque_status_ == TORQUE_DISABLED) { response->success = true; response->message = "Already disabled."; + RCLCPP_INFO_STREAM(logger_, "Requested to disable torque, but already disabled."); return; } else { dxl_torque_status_ = REQUESTED_TO_DISABLE; } } - - auto start = std::chrono::steady_clock::now(); - while (std::chrono::steady_clock::now() - start < std::chrono::seconds(1)) { - if (dxl_torque_status_ == TORQUE_ENABLED) { - if (request->data) { - response->success = true; - response->message = "Success to enable."; - } else { - response->success = false; - response->message = "Fail to enable."; - } - return; - } else if (dxl_torque_status_ == TORQUE_DISABLED) { - if (!request->data) { - response->success = true; - response->message = "Success to disable."; - } else { - response->success = false; - response->message = "Fail to disable."; - } - return; - } - std::this_thread::sleep_for(std::chrono::milliseconds(50)); - } - response->success = false; - response->message = "Fail to write requeset. main thread is not running."; + response->success = true; + response->message = "Success to write request."; + + // auto start = std::chrono::steady_clock::now(); + // while (std::chrono::steady_clock::now() - start < std::chrono::seconds(1)) { + // if (dxl_torque_status_ == TORQUE_ENABLED) { + // if (request->data) { + // response->success = true; + // response->message = "Success to enable."; + // } else { + // response->success = false; + // response->message = "Fail to enable."; + // } + // return; + // } else if (dxl_torque_status_ == TORQUE_DISABLED) { + // if (!request->data) { + // response->success = true; + // response->message = "Success to disable."; + // } else { + // response->success = false; + // response->message = "Fail to disable."; + // } + // return; + // } + // // std::this_thread::sleep_for(std::chrono::milliseconds(50)); + // } + // response->success = false; + // response->message = "Fail to write requeset. main thread is not running."; } void DynamixelHardware::initRevoluteToPrismaticParam() @@ -1398,6 +1500,82 @@ double DynamixelHardware::prismaticToRevolute(double prismatic_value) return (prismatic_value - conversion_intercept_) / conversion_slope_; } +std::string DynamixelHardware::getErrorSummary(uint8_t id) const +{ + std::ostringstream summary; + summary << "Dynamixel ID " << static_cast(id) << " Error Summary:\n"; + + // Check hardware error status + auto hw_err_it = dxl_hw_err_.find(id); + if (hw_err_it != dxl_hw_err_.end() && hw_err_it->second != 0) { + uint8_t hw_error_status = static_cast(hw_err_it->second); + summary << " Hardware Error Status: 0x" << std::hex << static_cast(hw_error_status) + << std::dec << " (" << static_cast(hw_error_status) << ")\n"; + + for (int bit = 0; bit < 8; ++bit) { + if (hw_error_status & (1 << bit)) { + const HardwareErrorStatusBitInfo * bit_info = get_hardware_error_status_bit_info(bit); + if (bit_info) { + summary << " Bit " << bit << ": " << bit_info->label + << " - " << bit_info->description << "\n"; + } else { + summary << " Bit " << bit << ": Unknown Error\n"; + } + } + } + } + + // Check error code + auto error_code_it = dxl_error_code_.find(id); + if (error_code_it != dxl_error_code_.end() && error_code_it->second != 0x00) { + uint8_t error_code = static_cast(error_code_it->second); + summary << " Error Code: 0x" << std::hex << static_cast(error_code) + << std::dec << " (" << static_cast(error_code) << ")\n"; + + const ErrorCodeInfo * error_info = get_error_code_info(error_code); + if (error_info) { + summary << " " << error_info->label << " - " << error_info->description << "\n"; + } else { + summary << " Unknown Error Code\n"; + } + } + + // Check if no errors + if ((hw_err_it == dxl_hw_err_.end() || hw_err_it->second == 0) && + (error_code_it == dxl_error_code_.end() || error_code_it->second == 0x00)) + { + summary << " No errors detected\n"; + } + + return summary.str(); +} + +std::string DynamixelHardware::getAllErrorSummaries() const +{ + std::ostringstream all_summaries; + all_summaries << "=== All Dynamixel Error Summaries ===\n"; + + // Get all unique IDs from both error maps + std::set all_ids; + for (const auto & pair : dxl_hw_err_) { + all_ids.insert(pair.first); + } + for (const auto & pair : dxl_error_code_) { + all_ids.insert(pair.first); + } + + if (all_ids.empty()) { + all_summaries << "No Dynamixel IDs found in error maps.\n"; + } else { + for (uint8_t id : all_ids) { + all_summaries << getErrorSummary(id) << "\n"; + } + } + + all_summaries << "=====================================\n"; + return all_summaries.str(); +} + } // namespace dynamixel_hardware_interface #include "pluginlib/class_list_macros.hpp"