Skip to content

Commit d88ff3b

Browse files
Woojin-CriveWoojin-Crive
committed
feat: Add velocity unit handling to Dynamixel model processing
1 parent ba7957c commit d88ff3b

File tree

3 files changed

+57
-35
lines changed

3 files changed

+57
-35
lines changed

‎include/dynamixel_hardware_interface/dynamixel/dynamixel_info.hpp‎

Lines changed: 14 additions & 12 deletions
Original file line numberDiff line numberDiff line change
@@ -47,6 +47,7 @@ typedef struct
4747
int32_t value_of_max_radian_position;
4848
int32_t value_of_min_radian_position;
4949
uint16_t model_num;
50+
double velocity_unit;
5051

5152
std::vector<ControlItem> item;
5253
} DxlInfo;
@@ -72,24 +73,25 @@ class DynamixelInfo
7273
void ReadDxlModelFile(uint8_t id, uint16_t model_num);
7374
bool GetDxlControlItem(uint8_t id, std::string item_name, uint16_t & addr, uint8_t & size);
7475
bool CheckDxlControlItem(uint8_t id, std::string item_name);
75-
bool GetDxlTypeInfo(
76-
uint8_t id,
77-
int32_t & value_of_zero_radian_position,
78-
int32_t & value_of_max_radian_position,
79-
int32_t & value_of_min_radian_position,
80-
double & min_radian,
81-
double & max_radian,
82-
double & torque_constant);
76+
// bool GetDxlTypeInfo(
77+
// uint8_t id,
78+
// int32_t & value_of_zero_radian_position,
79+
// int32_t & value_of_max_radian_position,
80+
// int32_t & value_of_min_radian_position,
81+
// double & min_radian,
82+
// double & max_radian,
83+
// double & torque_constant,
84+
// double & velocity_unit);
8385
int32_t ConvertRadianToValue(uint8_t id, double radian);
8486
double ConvertValueToRadian(uint8_t id, int32_t value);
8587
inline int16_t ConvertEffortToCurrent(uint8_t id, double effort)
8688
{return static_cast<int16_t>(effort / dxl_info_[id].torque_constant);}
8789
inline double ConvertCurrentToEffort(uint8_t id, int16_t current)
8890
{return static_cast<double>(current * dxl_info_[id].torque_constant);}
89-
inline double ConvertValueRPMToVelocityRPS(int32_t value_rpm)
90-
{return static_cast<double>(value_rpm * 0.01 / 60.0 * 2.0 * M_PI);}
91-
inline int32_t ConvertVelocityRPSToValueRPM(double vel_rps)
92-
{return static_cast<int32_t>(vel_rps * 100.0 * 60.0 / 2.0 / M_PI);}
91+
inline double ConvertValueRPMToVelocityRPS(uint8_t id, int32_t value_rpm)
92+
{return static_cast<double>(value_rpm * dxl_info_[id].velocity_unit / 60.0 * 2.0 * M_PI);}
93+
inline int32_t ConvertVelocityRPSToValueRPM(uint8_t id, double vel_rps)
94+
{return static_cast<int32_t>(vel_rps / dxl_info_[id].velocity_unit * 60.0 / 2.0 / M_PI);}
9395
};
9496

9597
} // namespace dynamixel_hardware_interface

‎src/dynamixel/dynamixel.cpp‎

Lines changed: 8 additions & 6 deletions
Original file line numberDiff line numberDiff line change
@@ -1608,21 +1608,23 @@ DxlError Dynamixel::ProcessReadData(
16081608
uint16_t current_addr = indirect_addr;
16091609

16101610
for (size_t item_index = 0; item_index < item_names.size(); item_index++) {
1611+
uint8_t ID = id_arr[item_index];
16111612
uint8_t size = item_sizes[item_index];
16121613
if (item_index > 0) {current_addr += item_sizes[item_index - 1];}
16131614

16141615
uint32_t dxl_getdata = get_data_func(comm_id, current_addr, size);
16151616

16161617
if (item_names[item_index] == "Present Position") {
16171618
*data_ptrs[item_index] = dxl_info_.ConvertValueToRadian(
1618-
id_arr[item_index],
1619+
ID,
16191620
static_cast<int32_t>(dxl_getdata));
16201621
} else if (item_names[item_index] == "Present Velocity") {
1621-
*data_ptrs[item_index] =
1622-
dxl_info_.ConvertValueRPMToVelocityRPS(static_cast<int32_t>(dxl_getdata));
1622+
*data_ptrs[item_index] = dxl_info_.ConvertValueRPMToVelocityRPS(
1623+
ID,
1624+
static_cast<int32_t>(dxl_getdata));
16231625
} else if (item_names[item_index] == "Present Current") {
16241626
*data_ptrs[item_index] = dxl_info_.ConvertCurrentToEffort(
1625-
id_arr[item_index],
1627+
ID,
16261628
static_cast<int16_t>(dxl_getdata));
16271629
} else {
16281630
*data_ptrs[item_index] = static_cast<double>(dxl_getdata);
@@ -1807,7 +1809,7 @@ DxlError Dynamixel::SetDxlValueToSyncWrite()
18071809
param_write_value[added_byte + 2] = DXL_LOBYTE(DXL_HIWORD(goal_position));
18081810
param_write_value[added_byte + 3] = DXL_HIBYTE(DXL_HIWORD(goal_position));
18091811
} else if (indirect_info_write_[comm_id].item_name.at(item_index) == "Goal Velocity") {
1810-
int32_t goal_velocity = dxl_info_.ConvertVelocityRPSToValueRPM(data);
1812+
int32_t goal_velocity = dxl_info_.ConvertVelocityRPSToValueRPM(ID, data);
18111813
param_write_value[added_byte + 0] = DXL_LOBYTE(DXL_LOWORD(goal_velocity));
18121814
param_write_value[added_byte + 1] = DXL_HIBYTE(DXL_LOWORD(goal_velocity));
18131815
param_write_value[added_byte + 2] = DXL_LOBYTE(DXL_HIWORD(goal_velocity));
@@ -2030,7 +2032,7 @@ DxlError Dynamixel::SetDxlValueToBulkWrite()
20302032
param_write_value[added_byte + 2] = DXL_LOBYTE(DXL_HIWORD(goal_position));
20312033
param_write_value[added_byte + 3] = DXL_HIBYTE(DXL_HIWORD(goal_position));
20322034
} else if (indirect_info_write_[comm_id].item_name.at(item_index) == "Goal Velocity") {
2033-
int32_t goal_velocity = dxl_info_.ConvertVelocityRPSToValueRPM(data);
2035+
int32_t goal_velocity = dxl_info_.ConvertVelocityRPSToValueRPM(ID, data);
20342036
param_write_value[added_byte + 0] = DXL_LOBYTE(DXL_LOWORD(goal_velocity));
20352037
param_write_value[added_byte + 1] = DXL_HIBYTE(DXL_LOWORD(goal_velocity));
20362038
param_write_value[added_byte + 2] = DXL_LOBYTE(DXL_HIWORD(goal_velocity));

‎src/dynamixel/dynamixel_info.cpp‎

Lines changed: 35 additions & 17 deletions
Original file line numberDiff line numberDiff line change
@@ -73,6 +73,8 @@ void DynamixelInfo::ReadDxlModelFile(uint8_t id, uint16_t model_num)
7373
std::string line;
7474

7575
temp_dxl_info.model_num = model_num;
76+
bool torque_constant_set = false;
77+
bool velocity_unit_set = false;
7678

7779
while (!open_file.eof() ) {
7880
getline(open_file, line);
@@ -100,6 +102,10 @@ void DynamixelInfo::ReadDxlModelFile(uint8_t id, uint16_t model_num)
100102
temp_dxl_info.max_radian = static_cast<double>(stod(strs.at(1)));
101103
} else if (strs.at(0) == "torque_constant") {
102104
temp_dxl_info.torque_constant = static_cast<double>(stod(strs.at(1)));
105+
torque_constant_set = true;
106+
} else if (strs.at(0) == "velocity_unit") {
107+
temp_dxl_info.velocity_unit = static_cast<double>(stod(strs.at(1)));
108+
velocity_unit_set = true;
103109
}
104110
} catch (const std::exception & e) {
105111
std::string error_msg = "Error processing line in model file: " + line +
@@ -108,6 +114,16 @@ void DynamixelInfo::ReadDxlModelFile(uint8_t id, uint16_t model_num)
108114
}
109115
}
110116

117+
// Set default values and warn if parameters are missing
118+
if (!torque_constant_set) {
119+
fprintf(stderr, "[WARN] Model file doesn't contain torque_constant parameter. Using default value: 1.0\n");
120+
temp_dxl_info.torque_constant = 1.0;
121+
}
122+
if (!velocity_unit_set) {
123+
fprintf(stderr, "[WARN] Model file doesn't contain velocity_unit parameter. Using default value: 0.01\n");
124+
temp_dxl_info.velocity_unit = 0.01;
125+
}
126+
111127
getline(open_file, line);
112128
while (!open_file.eof() ) {
113129
getline(open_file, line);
@@ -170,23 +186,25 @@ bool DynamixelInfo::CheckDxlControlItem(uint8_t id, std::string item_name)
170186
return false;
171187
}
172188

173-
bool DynamixelInfo::GetDxlTypeInfo(
174-
uint8_t id,
175-
int32_t & value_of_zero_radian_position,
176-
int32_t & value_of_max_radian_position,
177-
int32_t & value_of_min_radian_position,
178-
double & min_radian,
179-
double & max_radian,
180-
double & torque_constant)
181-
{
182-
value_of_zero_radian_position = dxl_info_[id].value_of_zero_radian_position;
183-
value_of_max_radian_position = dxl_info_[id].value_of_max_radian_position;
184-
value_of_min_radian_position = dxl_info_[id].value_of_min_radian_position;
185-
min_radian = dxl_info_[id].min_radian;
186-
max_radian = dxl_info_[id].max_radian;
187-
torque_constant = dxl_info_[id].torque_constant;
188-
return true;
189-
}
189+
// bool DynamixelInfo::GetDxlTypeInfo(
190+
// uint8_t id,
191+
// int32_t & value_of_zero_radian_position,
192+
// int32_t & value_of_max_radian_position,
193+
// int32_t & value_of_min_radian_position,
194+
// double & min_radian,
195+
// double & max_radian,
196+
// double & torque_constant,
197+
// double & velocity_unit)
198+
// {
199+
// value_of_zero_radian_position = dxl_info_[id].value_of_zero_radian_position;
200+
// value_of_max_radian_position = dxl_info_[id].value_of_max_radian_position;
201+
// value_of_min_radian_position = dxl_info_[id].value_of_min_radian_position;
202+
// min_radian = dxl_info_[id].min_radian;
203+
// max_radian = dxl_info_[id].max_radian;
204+
// torque_constant = dxl_info_[id].torque_constant;
205+
// velocity_unit = dxl_info_[id].velocity_unit;
206+
// return true;
207+
// }
190208

191209
int32_t DynamixelInfo::ConvertRadianToValue(uint8_t id, double radian)
192210
{

0 commit comments

Comments
 (0)