diff --git a/include/dynamixel_hardware_interface/dynamixel/dynamixel.hpp b/include/dynamixel_hardware_interface/dynamixel/dynamixel.hpp index 5e6db38..6cf197f 100644 --- a/include/dynamixel_hardware_interface/dynamixel/dynamixel.hpp +++ b/include/dynamixel_hardware_interface/dynamixel/dynamixel.hpp @@ -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,15 @@ 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..85c5d2f 100644 --- a/include/dynamixel_hardware_interface/dynamixel/dynamixel_info.hpp +++ b/include/dynamixel_hardware_interface/dynamixel/dynamixel_info.hpp @@ -38,6 +38,13 @@ typedef struct std::string item_name; } ControlItem; +typedef struct +{ + std::string data_name; + double value; + bool is_signed; +} UnitItem; + typedef struct { double torque_constant; @@ -47,9 +54,10 @@ typedef struct 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 +81,47 @@ 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; + } + // fprintf(stderr, "[WARN] No unit mapping found for '%s' in ID %d, returning raw value\n", data_name.c_str(), id); + 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); + } + // fprintf(stderr, "[WARN] No unit mapping found for '%s' in ID %d, returning raw value\n", data_name.c_str(), id); + return static_cast(unit_value); +} + } // namespace dynamixel_hardware_interface #endif // DYNAMIXEL_HARDWARE_INTERFACE__DYNAMIXEL__DYNAMIXEL_INFO_HPP_ diff --git a/param/dxl_model/2xc430_w250.model b/param/dxl_model/2xc430_w250.model index 6a1f335..2636b8d 100644 --- a/param/dxl_model/2xc430_w250.model +++ b/param/dxl_model/2xc430_w250.model @@ -5,7 +5,12 @@ 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 +Profile Time 0.001 s unsigned [control table] Address Size Data Name @@ -45,7 +50,9 @@ Address Size Data Name 100 2 Goal PWM 104 4 Goal Velocity 108 4 Profile Acceleration +108 4 Profile Acceleration Time 112 4 Profile Velocity +112 4 Profile Time 116 4 Goal Position 120 2 Realtime Tick 122 1 Moving diff --git a/param/dxl_model/2xl430_w250.model b/param/dxl_model/2xl430_w250.model index 6a1f335..2636b8d 100644 --- a/param/dxl_model/2xl430_w250.model +++ b/param/dxl_model/2xl430_w250.model @@ -5,7 +5,12 @@ 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 +Profile Time 0.001 s unsigned [control table] Address Size Data Name @@ -45,7 +50,9 @@ Address Size Data Name 100 2 Goal PWM 104 4 Goal Velocity 108 4 Profile Acceleration +108 4 Profile Acceleration Time 112 4 Profile Velocity +112 4 Profile Time 116 4 Goal Position 120 2 Realtime Tick 122 1 Moving 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..722ad6a 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 +Present Velocity 0.0010471976 +Goal Velocity 0.0010471976 +Velocity Limit 0.0010471976 [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..b649cca 100644 --- a/param/dxl_model/xc330_m181.model +++ b/param/dxl_model/xc330_m181.model @@ -5,7 +5,12 @@ 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 +Profile Time 0.001 s unsigned [control table] Address Size Data Name @@ -48,7 +53,9 @@ Address Size Data Name 102 2 Goal Current 104 4 Goal Velocity 108 4 Profile Acceleration +108 4 Profile Acceleration Time 112 4 Profile Velocity +112 4 Profile Time 116 4 Goal Position 120 2 Realtime Tick 122 1 Moving diff --git a/param/dxl_model/xc330_m288.model b/param/dxl_model/xc330_m288.model index 7fa8ab5..b649cca 100644 --- a/param/dxl_model/xc330_m288.model +++ b/param/dxl_model/xc330_m288.model @@ -5,7 +5,12 @@ 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 +Profile Time 0.001 s unsigned [control table] Address Size Data Name @@ -48,7 +53,9 @@ Address Size Data Name 102 2 Goal Current 104 4 Goal Velocity 108 4 Profile Acceleration +108 4 Profile Acceleration Time 112 4 Profile Velocity +112 4 Profile Time 116 4 Goal Position 120 2 Realtime Tick 122 1 Moving diff --git a/param/dxl_model/xc330_t181.model b/param/dxl_model/xc330_t181.model index 63da0b3..466730e 100644 --- a/param/dxl_model/xc330_t181.model +++ b/param/dxl_model/xc330_t181.model @@ -5,8 +5,14 @@ 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 +Profile Time 0.001 s unsigned +Present Current 0.0006709470296015791 N/m signed +Goal Current 0.0006709470296015791 N/m signed [control table] Address Size Data Name @@ -49,7 +55,9 @@ Address Size Data Name 102 2 Goal Current 104 4 Goal Velocity 108 4 Profile Acceleration +108 4 Profile Acceleration Time 112 4 Profile Velocity +112 4 Profile Time 116 4 Goal Position 120 2 Realtime Tick 122 1 Moving diff --git a/param/dxl_model/xc330_t288.model b/param/dxl_model/xc330_t288.model index 1e0dd24..92b03a1 100644 --- a/param/dxl_model/xc330_t288.model +++ b/param/dxl_model/xc330_t288.model @@ -5,8 +5,14 @@ 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 +Profile Time 0.001 s unsigned +Present Current 0.0009674796739867748 N/m signed +Goal Current 0.0009674796739867748 N/m signed [control table] Address Size Data Name @@ -49,7 +55,9 @@ Address Size Data Name 102 2 Goal Current 104 4 Goal Velocity 108 4 Profile Acceleration +108 4 Profile Acceleration Time 112 4 Profile Velocity +112 4 Profile Time 116 4 Goal Position 120 2 Realtime Tick 122 1 Moving diff --git a/param/dxl_model/xc430_w150.model b/param/dxl_model/xc430_w150.model index 6a1f335..2636b8d 100644 --- a/param/dxl_model/xc430_w150.model +++ b/param/dxl_model/xc430_w150.model @@ -5,7 +5,12 @@ 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 +Profile Time 0.001 s unsigned [control table] Address Size Data Name @@ -45,7 +50,9 @@ Address Size Data Name 100 2 Goal PWM 104 4 Goal Velocity 108 4 Profile Acceleration +108 4 Profile Acceleration Time 112 4 Profile Velocity +112 4 Profile Time 116 4 Goal Position 120 2 Realtime Tick 122 1 Moving diff --git a/param/dxl_model/xc430_w240.model b/param/dxl_model/xc430_w240.model index 6a1f335..2636b8d 100644 --- a/param/dxl_model/xc430_w240.model +++ b/param/dxl_model/xc430_w240.model @@ -5,7 +5,12 @@ 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 +Profile Time 0.001 s unsigned [control table] Address Size Data Name @@ -45,7 +50,9 @@ Address Size Data Name 100 2 Goal PWM 104 4 Goal Velocity 108 4 Profile Acceleration +108 4 Profile Acceleration Time 112 4 Profile Velocity +112 4 Profile Time 116 4 Goal Position 120 2 Realtime Tick 122 1 Moving diff --git a/param/dxl_model/xd430_t210.model b/param/dxl_model/xd430_t210.model index 8b2fb3c..63ba6dc 100644 --- a/param/dxl_model/xd430_t210.model +++ b/param/dxl_model/xd430_t210.model @@ -5,7 +5,12 @@ 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 +Profile Time 0.001 s unsigned [control table] Address Size Data Name @@ -47,7 +52,9 @@ Address Size Data Name 102 2 Goal Current 104 4 Goal Velocity 108 4 Profile Acceleration +108 4 Profile Acceleration Time 112 4 Profile Velocity +112 4 Profile Time 116 4 Goal Position 120 2 Realtime Tick 122 1 Moving diff --git a/param/dxl_model/xd430_t350.model b/param/dxl_model/xd430_t350.model index 8b2fb3c..63ba6dc 100644 --- a/param/dxl_model/xd430_t350.model +++ b/param/dxl_model/xd430_t350.model @@ -5,7 +5,12 @@ 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 +Profile Time 0.001 s unsigned [control table] Address Size Data Name @@ -47,7 +52,9 @@ Address Size Data Name 102 2 Goal Current 104 4 Goal Velocity 108 4 Profile Acceleration +108 4 Profile Acceleration Time 112 4 Profile Velocity +112 4 Profile Time 116 4 Goal Position 120 2 Realtime Tick 122 1 Moving diff --git a/param/dxl_model/xd540_t150.model b/param/dxl_model/xd540_t150.model index 0364415..ac1d130 100644 --- a/param/dxl_model/xd540_t150.model +++ b/param/dxl_model/xd540_t150.model @@ -5,7 +5,12 @@ 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 +Profile Time 0.001 s unsigned [control table] Address Size Data Name @@ -50,7 +55,9 @@ Address Size Data Name 102 2 Goal Current 104 4 Goal Velocity 108 4 Profile Acceleration +108 4 Profile Acceleration Time 112 4 Profile Velocity +112 4 Profile Time 116 4 Goal Position 120 2 Realtime Tick 122 1 Moving diff --git a/param/dxl_model/xd540_t270.model b/param/dxl_model/xd540_t270.model index bfe7dcf..ac1d130 100644 --- a/param/dxl_model/xd540_t270.model +++ b/param/dxl_model/xd540_t270.model @@ -5,7 +5,12 @@ 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 +Profile Time 0.001 s unsigned [control table] Address Size Data Name @@ -50,7 +55,9 @@ Address Size Data Name 102 2 Goal Current 104 4 Goal Velocity 108 4 Profile Acceleration +108 4 Profile Acceleration Time 112 4 Profile Velocity +112 4 Profile Time 116 4 Goal Position 120 2 Realtime Tick 122 1 Moving @@ -71,4 +78,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..63ba6dc 100644 --- a/param/dxl_model/xh430_v210.model +++ b/param/dxl_model/xh430_v210.model @@ -5,7 +5,12 @@ 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 +Profile Time 0.001 s unsigned [control table] Address Size Data Name @@ -47,7 +52,9 @@ Address Size Data Name 102 2 Goal Current 104 4 Goal Velocity 108 4 Profile Acceleration +108 4 Profile Acceleration Time 112 4 Profile Velocity +112 4 Profile Time 116 4 Goal Position 120 2 Realtime Tick 122 1 Moving diff --git a/param/dxl_model/xh430_v350.model b/param/dxl_model/xh430_v350.model index 8b2fb3c..63ba6dc 100644 --- a/param/dxl_model/xh430_v350.model +++ b/param/dxl_model/xh430_v350.model @@ -5,7 +5,12 @@ 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 +Profile Time 0.001 s unsigned [control table] Address Size Data Name @@ -47,7 +52,9 @@ Address Size Data Name 102 2 Goal Current 104 4 Goal Velocity 108 4 Profile Acceleration +108 4 Profile Acceleration Time 112 4 Profile Velocity +112 4 Profile Time 116 4 Goal Position 120 2 Realtime Tick 122 1 Moving diff --git a/param/dxl_model/xh430_w210.model b/param/dxl_model/xh430_w210.model index 8b2fb3c..63ba6dc 100644 --- a/param/dxl_model/xh430_w210.model +++ b/param/dxl_model/xh430_w210.model @@ -5,7 +5,12 @@ 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 +Profile Time 0.001 s unsigned [control table] Address Size Data Name @@ -47,7 +52,9 @@ Address Size Data Name 102 2 Goal Current 104 4 Goal Velocity 108 4 Profile Acceleration +108 4 Profile Acceleration Time 112 4 Profile Velocity +112 4 Profile Time 116 4 Goal Position 120 2 Realtime Tick 122 1 Moving diff --git a/param/dxl_model/xh430_w350.model b/param/dxl_model/xh430_w350.model index 8b2fb3c..63ba6dc 100644 --- a/param/dxl_model/xh430_w350.model +++ b/param/dxl_model/xh430_w350.model @@ -5,7 +5,12 @@ 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 +Profile Time 0.001 s unsigned [control table] Address Size Data Name @@ -47,7 +52,9 @@ Address Size Data Name 102 2 Goal Current 104 4 Goal Velocity 108 4 Profile Acceleration +108 4 Profile Acceleration Time 112 4 Profile Velocity +112 4 Profile Time 116 4 Goal Position 120 2 Realtime Tick 122 1 Moving diff --git a/param/dxl_model/xh540_v150.model b/param/dxl_model/xh540_v150.model index 0364415..ac1d130 100644 --- a/param/dxl_model/xh540_v150.model +++ b/param/dxl_model/xh540_v150.model @@ -5,7 +5,12 @@ 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 +Profile Time 0.001 s unsigned [control table] Address Size Data Name @@ -50,7 +55,9 @@ Address Size Data Name 102 2 Goal Current 104 4 Goal Velocity 108 4 Profile Acceleration +108 4 Profile Acceleration Time 112 4 Profile Velocity +112 4 Profile Time 116 4 Goal Position 120 2 Realtime Tick 122 1 Moving diff --git a/param/dxl_model/xh540_v270.model b/param/dxl_model/xh540_v270.model index 0364415..ac1d130 100644 --- a/param/dxl_model/xh540_v270.model +++ b/param/dxl_model/xh540_v270.model @@ -5,7 +5,12 @@ 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 +Profile Time 0.001 s unsigned [control table] Address Size Data Name @@ -50,7 +55,9 @@ Address Size Data Name 102 2 Goal Current 104 4 Goal Velocity 108 4 Profile Acceleration +108 4 Profile Acceleration Time 112 4 Profile Velocity +112 4 Profile Time 116 4 Goal Position 120 2 Realtime Tick 122 1 Moving diff --git a/param/dxl_model/xh540_w150.model b/param/dxl_model/xh540_w150.model index 4dd5bbe..3b1d82d 100644 --- a/param/dxl_model/xh540_w150.model +++ b/param/dxl_model/xh540_w150.model @@ -5,8 +5,14 @@ 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 +Profile Time 0.001 s unsigned +Present Current 0.0045 N/m signed +Goal Current 0.0045 N/m signed [control table] Address Size Data Name @@ -51,7 +57,9 @@ Address Size Data Name 102 2 Goal Current 104 4 Goal Velocity 108 4 Profile Acceleration +108 4 Profile Acceleration Time 112 4 Profile Velocity +112 4 Profile Time 116 4 Goal Position 120 2 Realtime Tick 122 1 Moving diff --git a/param/dxl_model/xh540_w270.model b/param/dxl_model/xh540_w270.model index 0364415..ac1d130 100644 --- a/param/dxl_model/xh540_w270.model +++ b/param/dxl_model/xh540_w270.model @@ -5,7 +5,12 @@ 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 +Profile Time 0.001 s unsigned [control table] Address Size Data Name @@ -50,7 +55,9 @@ Address Size Data Name 102 2 Goal Current 104 4 Goal Velocity 108 4 Profile Acceleration +108 4 Profile Acceleration Time 112 4 Profile Velocity +112 4 Profile Time 116 4 Goal Position 120 2 Realtime Tick 122 1 Moving 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..b649cca 100644 --- a/param/dxl_model/xl330_m077.model +++ b/param/dxl_model/xl330_m077.model @@ -5,7 +5,12 @@ 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 +Profile Time 0.001 s unsigned [control table] Address Size Data Name @@ -48,7 +53,9 @@ Address Size Data Name 102 2 Goal Current 104 4 Goal Velocity 108 4 Profile Acceleration +108 4 Profile Acceleration Time 112 4 Profile Velocity +112 4 Profile Time 116 4 Goal Position 120 2 Realtime Tick 122 1 Moving diff --git a/param/dxl_model/xl330_m288.model b/param/dxl_model/xl330_m288.model index 7fa8ab5..b649cca 100644 --- a/param/dxl_model/xl330_m288.model +++ b/param/dxl_model/xl330_m288.model @@ -5,7 +5,12 @@ 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 +Profile Time 0.001 s unsigned [control table] Address Size Data Name @@ -48,7 +53,9 @@ Address Size Data Name 102 2 Goal Current 104 4 Goal Velocity 108 4 Profile Acceleration +108 4 Profile Acceleration Time 112 4 Profile Velocity +112 4 Profile Time 116 4 Goal Position 120 2 Realtime Tick 122 1 Moving diff --git a/param/dxl_model/xl430_w250.model b/param/dxl_model/xl430_w250.model index 6a1f335..2636b8d 100644 --- a/param/dxl_model/xl430_w250.model +++ b/param/dxl_model/xl430_w250.model @@ -5,7 +5,12 @@ 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 +Profile Time 0.001 s unsigned [control table] Address Size Data Name @@ -45,7 +50,9 @@ Address Size Data Name 100 2 Goal PWM 104 4 Goal Velocity 108 4 Profile Acceleration +108 4 Profile Acceleration Time 112 4 Profile Velocity +112 4 Profile Time 116 4 Goal Position 120 2 Realtime Tick 122 1 Moving diff --git a/param/dxl_model/xm430_w210.model b/param/dxl_model/xm430_w210.model index 8b2fb3c..63ba6dc 100644 --- a/param/dxl_model/xm430_w210.model +++ b/param/dxl_model/xm430_w210.model @@ -5,7 +5,12 @@ 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 +Profile Time 0.001 s unsigned [control table] Address Size Data Name @@ -47,7 +52,9 @@ Address Size Data Name 102 2 Goal Current 104 4 Goal Velocity 108 4 Profile Acceleration +108 4 Profile Acceleration Time 112 4 Profile Velocity +112 4 Profile Time 116 4 Goal Position 120 2 Realtime Tick 122 1 Moving diff --git a/param/dxl_model/xm430_w350.model b/param/dxl_model/xm430_w350.model index 8b2fb3c..63ba6dc 100644 --- a/param/dxl_model/xm430_w350.model +++ b/param/dxl_model/xm430_w350.model @@ -5,7 +5,12 @@ 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 +Profile Time 0.001 s unsigned [control table] Address Size Data Name @@ -47,7 +52,9 @@ Address Size Data Name 102 2 Goal Current 104 4 Goal Velocity 108 4 Profile Acceleration +108 4 Profile Acceleration Time 112 4 Profile Velocity +112 4 Profile Time 116 4 Goal Position 120 2 Realtime Tick 122 1 Moving diff --git a/param/dxl_model/xm540_w150.model b/param/dxl_model/xm540_w150.model index 0364415..ac1d130 100644 --- a/param/dxl_model/xm540_w150.model +++ b/param/dxl_model/xm540_w150.model @@ -5,7 +5,12 @@ 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 +Profile Time 0.001 s unsigned [control table] Address Size Data Name @@ -50,7 +55,9 @@ Address Size Data Name 102 2 Goal Current 104 4 Goal Velocity 108 4 Profile Acceleration +108 4 Profile Acceleration Time 112 4 Profile Velocity +112 4 Profile Time 116 4 Goal Position 120 2 Realtime Tick 122 1 Moving diff --git a/param/dxl_model/xm540_w270.model b/param/dxl_model/xm540_w270.model index 0364415..ac1d130 100644 --- a/param/dxl_model/xm540_w270.model +++ b/param/dxl_model/xm540_w270.model @@ -5,7 +5,12 @@ 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 +Profile Time 0.001 s unsigned [control table] Address Size Data Name @@ -50,7 +55,9 @@ Address Size Data Name 102 2 Goal Current 104 4 Goal Velocity 108 4 Profile Acceleration +108 4 Profile Acceleration Time 112 4 Profile Velocity +112 4 Profile Time 116 4 Goal Position 120 2 Realtime Tick 122 1 Moving diff --git a/param/dxl_model/xw430_t200.model b/param/dxl_model/xw430_t200.model index 0a9b4a1..d4d4251 100644 --- a/param/dxl_model/xw430_t200.model +++ b/param/dxl_model/xw430_t200.model @@ -5,7 +5,12 @@ 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 +Profile Time 0.001 s unsigned [control table] Address Size Data Name @@ -46,7 +51,9 @@ Address Size Data Name 102 2 Goal Current 104 4 Goal Velocity 108 4 Profile Acceleration +108 4 Profile Acceleration Time 112 4 Profile Velocity +112 4 Profile Time 116 4 Goal Position 120 2 Realtime Tick 122 1 Moving diff --git a/param/dxl_model/xw430_t333.model b/param/dxl_model/xw430_t333.model index 0a9b4a1..d4d4251 100644 --- a/param/dxl_model/xw430_t333.model +++ b/param/dxl_model/xw430_t333.model @@ -5,7 +5,12 @@ 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 +Profile Time 0.001 s unsigned [control table] Address Size Data Name @@ -46,7 +51,9 @@ Address Size Data Name 102 2 Goal Current 104 4 Goal Velocity 108 4 Profile Acceleration +108 4 Profile Acceleration Time 112 4 Profile Velocity +112 4 Profile Time 116 4 Goal Position 120 2 Realtime Tick 122 1 Moving diff --git a/param/dxl_model/xw540_h260.model b/param/dxl_model/xw540_h260.model index 0a9b4a1..d4d4251 100644 --- a/param/dxl_model/xw540_h260.model +++ b/param/dxl_model/xw540_h260.model @@ -5,7 +5,12 @@ 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 +Profile Time 0.001 s unsigned [control table] Address Size Data Name @@ -46,7 +51,9 @@ Address Size Data Name 102 2 Goal Current 104 4 Goal Velocity 108 4 Profile Acceleration +108 4 Profile Acceleration Time 112 4 Profile Velocity +112 4 Profile Time 116 4 Goal Position 120 2 Realtime Tick 122 1 Moving diff --git a/param/dxl_model/xw540_t140.model b/param/dxl_model/xw540_t140.model index 0a9b4a1..d4d4251 100644 --- a/param/dxl_model/xw540_t140.model +++ b/param/dxl_model/xw540_t140.model @@ -5,7 +5,12 @@ 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 +Profile Time 0.001 s unsigned [control table] Address Size Data Name @@ -46,7 +51,9 @@ Address Size Data Name 102 2 Goal Current 104 4 Goal Velocity 108 4 Profile Acceleration +108 4 Profile Acceleration Time 112 4 Profile Velocity +112 4 Profile Time 116 4 Goal Position 120 2 Realtime Tick 122 1 Moving diff --git a/param/dxl_model/xw540_t260.model b/param/dxl_model/xw540_t260.model index 0a9b4a1..d4d4251 100644 --- a/param/dxl_model/xw540_t260.model +++ b/param/dxl_model/xw540_t260.model @@ -5,7 +5,12 @@ 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 +Profile Time 0.001 s unsigned [control table] Address Size Data Name @@ -46,7 +51,9 @@ Address Size Data Name 102 2 Goal Current 104 4 Goal Velocity 108 4 Profile Acceleration +108 4 Profile Acceleration Time 112 4 Profile Velocity +112 4 Profile Time 116 4 Goal Position 120 2 Realtime Tick 122 1 Moving 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..4021328 100644 --- a/src/dynamixel/dynamixel.cpp +++ b/src/dynamixel/dynamixel.cpp @@ -610,7 +610,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; } @@ -1449,6 +1449,7 @@ 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) { @@ -1514,6 +1515,7 @@ 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) { @@ -1637,20 +1639,22 @@ 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 +1663,35 @@ 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; } @@ -1821,38 +1843,27 @@ 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) { @@ -2021,19 +2032,26 @@ 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 +2072,27 @@ 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( @@ -2163,4 +2170,88 @@ 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..44e0c84 100644 --- a/src/dynamixel/dynamixel_info.cpp +++ b/src/dynamixel/dynamixel_info.cpp @@ -74,58 +74,109 @@ void DynamixelInfo::ReadDxlModelFile(uint8_t id, uint16_t model_num) 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; + } + } 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); + } } + } - 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; + 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); } - } 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) { + if (type_info_found && !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 (!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 +241,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 +274,37 @@ 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; + } + // fprintf(stderr, "[WARN] No unit mapping found for '%s' in ID %d\n", data_name.c_str(), id); + 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; + } + // fprintf(stderr, "[WARN] No sign type mapping found for '%s' in ID %d\n", data_name.c_str(), id); + 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; + } + // fprintf(stderr, "[WARN] No unit mapping found for '%s' in ID %d, returning 1.0\n", data_name.c_str(), id); + return 1.0; +} + } // namespace dynamixel_hardware_interface diff --git a/src/dynamixel_hardware_interface.cpp b/src/dynamixel_hardware_interface.cpp index 3295951..04f1f56 100644 --- a/src/dynamixel_hardware_interface.cpp +++ b/src/dynamixel_hardware_interface.cpp @@ -943,12 +943,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 +974,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)); } @@ -1111,7 +1111,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;