Skip to content

Commit 28c35b6

Browse files
Woojin-CriveWoojin-Crive
committed
refactor: Update Dynamixel interface for improved handler management and model definitions
1 parent 013ee93 commit 28c35b6

File tree

6 files changed

+53
-9
lines changed

6 files changed

+53
-9
lines changed

‎include/dynamixel_hardware_interface/dynamixel_hardware_interface.hpp‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -365,7 +365,7 @@ class DynamixelHardware : public
365365
size_t outer_size,
366366
size_t inner_size,
367367
std::vector<HandlerVarType> & outer_handlers,
368-
const std::vector<HandlerVarType> & inner_handlers,
368+
std::vector<HandlerVarType> & inner_handlers,
369369
double ** matrix,
370370
const std::unordered_map<std::string, std::vector<std::string>> & iface_map,
371371
const std::string & conversion_iface = "",

‎param/dxl_model/ffw_sg2_drive_1.model‎

Lines changed: 15 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -44,6 +44,21 @@ Address Size Data Name
4444
182 4 Velocity Limit
4545
186 4 Max Position Limit
4646
190 4 Min Position Limit
47+
206 4 GearRatio Num
48+
210 4 GearRatio Den
49+
214 2 Safe Stop Time
50+
216 2 Brake Delay
51+
218 2 Goal Update Delay
52+
220 1 Overexcitation Voltage
53+
221 1 Normal Excitation Voltage
54+
222 2 Overexcitation Time
55+
224 2 Goal Current BSF Freqency
56+
226 2 Goal Current BSF Bandwith.
57+
228 2 Goal Current BSF Depth.
58+
230 2 Present Velocity LPF Freqency
59+
232 2 Goal Current LPF Freqency
60+
234 2 Position FF LPF Freqency
61+
236 2 Velocity FF LPF Freqency
4762
249 1 Controller State
4863
292 1 Error Code
4964
293 1 Error Code History 1

‎param/dxl_model/ffw_sg2_drive_2.model‎

Lines changed: 15 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -44,6 +44,21 @@ Address Size Data Name
4444
1206 4 Velocity Limit
4545
1210 4 Max Position Limit
4646
1214 4 Min Position Limit
47+
1230 4 GearRatio Num
48+
1234 4 GearRatio Den
49+
1238 2 Safe Stop Time
50+
1240 2 Brake Delay
51+
1242 2 Goal Update Delay
52+
1244 1 Overexcitation Voltage
53+
1245 1 Normal Excitation Voltage
54+
1246 2 Overexcitation Time
55+
1248 2 Goal Current BSF Freqency
56+
1250 2 Goal Current BSF Bandwith.
57+
1252 2 Goal Current BSF Depth.
58+
1254 2 Present Velocity LPF Freqency
59+
1256 2 Goal Current LPF Freqency
60+
1258 2 Position FF LPF Freqency
61+
1260 2 Velocity FF LPF Freqency
4762
1273 1 Controller State
4863
1316 1 Error Code
4964
1317 1 Error Code History 1

‎param/dxl_model/ffw_sg2_drive_3.model‎

Lines changed: 15 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -44,6 +44,21 @@ Address Size Data Name
4444
2230 4 Velocity Limit
4545
2234 4 Max Position Limit
4646
2238 4 Min Position Limit
47+
2254 4 GearRatio Num
48+
2258 4 GearRatio Den
49+
2262 2 Safe Stop Time
50+
2264 2 Brake Delay
51+
2266 2 Goal Update Delay
52+
2268 1 Overexcitation Voltage
53+
2269 1 Normal Excitation Voltage
54+
2270 2 Overexcitation Time
55+
2272 2 Goal Current BSF Freqency
56+
2274 2 Goal Current BSF Bandwith.
57+
2276 2 Goal Current BSF Depth.
58+
2278 2 Present Velocity LPF Freqency
59+
2280 2 Goal Current LPF Freqency
60+
2282 2 Position FF LPF Freqency
61+
2284 2 Velocity FF LPF Freqency
4762
2297 1 Controller State
4863
2340 1 Error Code
4964
2341 1 Error Code History 1

‎src/dynamixel/dynamixel.cpp‎

Lines changed: 0 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -2012,7 +2012,6 @@ DxlError Dynamixel::AddIndirectWrite(
20122012
uint8_t using_size = indirect_info_write_[id].size;
20132013

20142014
for (uint16_t i = 0; i < item_size; i++) {
2015-
fprintf(stderr, "[ID:%03d] addr : %d, data : %d\n", id, INDIRECT_ADDR + (using_size * 2), item_addr + i);
20162015
if (WriteItem(id, INDIRECT_ADDR + (using_size * 2), 2, item_addr + i) != DxlError::OK) {
20172016
return DxlError::SET_BULK_WRITE_FAIL;
20182017
}

‎src/dynamixel_hardware_interface.cpp‎

Lines changed: 7 additions & 7 deletions
Original file line numberDiff line numberDiff line change
@@ -823,7 +823,7 @@ bool DynamixelHardware::InitDxlReadItems()
823823
hdl_gpio_sensor_states_.clear();
824824
hdl_gpio_controller_states_.clear();
825825
for (const hardware_interface::ComponentInfo & gpio : info_.gpios) {
826-
if (gpio.state_interfaces.size() && gpio.parameters.at("type") == "dxl") {
826+
if (gpio.parameters.at("type") == "dxl") {
827827
uint8_t id = static_cast<uint8_t>(stoi(gpio.parameters.at("ID")));
828828
HandlerVarType temp_read;
829829
temp_read.id = id;
@@ -839,7 +839,7 @@ bool DynamixelHardware::InitDxlReadItems()
839839
}
840840
}
841841
hdl_trans_states_.push_back(temp_read);
842-
} else if (gpio.state_interfaces.size() && gpio.parameters.at("type") == "sensor") {
842+
} else if (gpio.parameters.at("type") == "sensor") {
843843
uint8_t id = static_cast<uint8_t>(stoi(gpio.parameters.at("ID")));
844844
HandlerVarType temp_sensor;
845845
temp_sensor.id = id;
@@ -863,7 +863,7 @@ bool DynamixelHardware::InitDxlReadItems()
863863
temp_controller.value_ptr_vec.push_back(std::make_shared<double>(0.0));
864864
}
865865
hdl_gpio_controller_states_.push_back(temp_controller);
866-
} else if (gpio.state_interfaces.size() && gpio.parameters.at("type") == "virtual_dxl") {
866+
} else if (gpio.parameters.at("type") == "virtual_dxl") {
867867
uint8_t id = static_cast<uint8_t>(stoi(gpio.parameters.at("ID")));
868868
uint8_t comm_id = static_cast<uint8_t>(stoi(gpio.parameters.at("comm_id")));
869869
HandlerVarType temp_read;
@@ -923,7 +923,7 @@ bool DynamixelHardware::InitDxlWriteItems()
923923
hdl_trans_commands_.clear();
924924
hdl_gpio_controller_commands_.clear();
925925
for (const hardware_interface::ComponentInfo & gpio : info_.gpios) {
926-
if (gpio.command_interfaces.size() && gpio.parameters.at("type") == "dxl") {
926+
if (gpio.parameters.at("type") == "dxl") {
927927
uint8_t id = static_cast<uint8_t>(stoi(gpio.parameters.at("ID")));
928928
HandlerVarType temp_write;
929929
temp_write.id = id;
@@ -941,7 +941,7 @@ bool DynamixelHardware::InitDxlWriteItems()
941941
temp_write.value_ptr_vec.push_back(std::make_shared<double>(0.0));
942942
}
943943
hdl_trans_commands_.push_back(temp_write);
944-
} else if (gpio.command_interfaces.size() && gpio.parameters.at("type") == "controller") {
944+
} else if (gpio.parameters.at("type") == "controller") {
945945
uint8_t id = static_cast<uint8_t>(stoi(gpio.parameters.at("ID")));
946946
HandlerVarType temp_controller;
947947
temp_controller.id = id;
@@ -953,7 +953,7 @@ bool DynamixelHardware::InitDxlWriteItems()
953953
temp_controller.value_ptr_vec.push_back(std::make_shared<double>(0.0));
954954
}
955955
hdl_gpio_controller_commands_.push_back(temp_controller);
956-
} else if (gpio.command_interfaces.size() && gpio.parameters.at("type") == "virtual_dxl") {
956+
} else if (gpio.parameters.at("type") == "virtual_dxl") {
957957
uint8_t id = static_cast<uint8_t>(stoi(gpio.parameters.at("ID")));
958958
uint8_t comm_id = static_cast<uint8_t>(stoi(gpio.parameters.at("comm_id")));
959959
HandlerVarType temp_write;
@@ -1076,7 +1076,7 @@ void DynamixelHardware::MapInterfaces(
10761076
size_t outer_size,
10771077
size_t inner_size,
10781078
std::vector<HandlerVarType> & outer_handlers,
1079-
const std::vector<HandlerVarType> & inner_handlers,
1079+
std::vector<HandlerVarType> & inner_handlers,
10801080
double ** matrix,
10811081
const std::unordered_map<std::string, std::vector<std::string>> & iface_map,
10821082
const std::string & conversion_iface,

0 commit comments

Comments
 (0)