diff --git a/CHANGELOG.rst b/CHANGELOG.rst index 86fcd07..b6a60eb 100644 --- a/CHANGELOG.rst +++ b/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package dynamixel_hardware_interface ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +1.4.8 (2025-06-23) +------------------ +* Added new model for OMY to use virtual_dxl +* Added goal position synchronization before torque enable for OMY sync table feature +* Contributors: Woojin Wie + 1.4.7 (2025-06-19) ------------------ * Added virtual_dxl and support for rcu diff --git a/package.xml b/package.xml index a30a5cd..17a40a8 100644 --- a/package.xml +++ b/package.xml @@ -2,7 +2,7 @@ dynamixel_hardware_interface - 1.4.7 + 1.4.8 ROS 2 package providing a hardware interface for controlling Dynamixel motors via the ROS 2 control framework. diff --git a/param/dxl_model/dynamixel.model b/param/dxl_model/dynamixel.model index 3cdc33b..66d68ad 100644 --- a/param/dxl_model/dynamixel.model +++ b/param/dxl_model/dynamixel.model @@ -46,6 +46,7 @@ Number Name 35074 rh_p12_rn.model 220 omy_hat.model 230 omy_end.model +231 omy_end_rh_p12_rn.model 536 sensorxel_joy.model 600 sensorxel_joy.model 602 ffw_g10_rcu.model diff --git a/param/dxl_model/omy_end.model b/param/dxl_model/omy_end.model index ad05924..3f02165 100644 --- a/param/dxl_model/omy_end.model +++ b/param/dxl_model/omy_end.model @@ -1,12 +1,3 @@ -[type info] -name value -value_of_zero_radian_position 0 -value_of_max_radian_position 740 -value_of_min_radian_position -740 -min_radian -1.099 -max_radian 1.099 -velocity_unit 0.114 - [control table] Address Size Data Name 0 2 Model Number @@ -35,16 +26,6 @@ Address Size Data Name 60 2 SyncTable Write Size 378 2 Indirect Address 634 1 Indirect Data -72 1 Hardware Error Status -73 1 Moving -74 2 Present Current -76 4 Present Velocity -80 4 Present Position -84 4 Position Trajectory -200 1 Torque Enable -202 2 Goal Current -204 4 Goal Velocity -208 4 Goal Position 378 2 Indirect Address 1 634 1 Indirect Data 1 378 2 Indirect Address Write diff --git a/param/dxl_model/omy_end_rh_p12_rn.model b/param/dxl_model/omy_end_rh_p12_rn.model new file mode 100644 index 0000000..c013203 --- /dev/null +++ b/param/dxl_model/omy_end_rh_p12_rn.model @@ -0,0 +1,21 @@ +[type info] +name value +value_of_zero_radian_position 0 +value_of_max_radian_position 740 +value_of_min_radian_position -740 +min_radian -1.099 +max_radian 1.099 +velocity_unit 0.114 + +[control table] +Address Size Data Name +72 1 Hardware Error Status +73 1 Moving +74 2 Present Current +76 4 Present Velocity +80 4 Present Position +84 4 Position Trajectory +200 1 Torque Enable +202 2 Goal Current +204 4 Goal Velocity +208 4 Goal Position diff --git a/param/dxl_model/omy_hat.model b/param/dxl_model/omy_hat.model index e51bd9f..3d2113a 100644 --- a/param/dxl_model/omy_hat.model +++ b/param/dxl_model/omy_hat.model @@ -33,3 +33,7 @@ Address Size Data Name 560 1 SyncTable Read Data 688 1 SyncTable Write Data 816 1 Indirect Data +112 2 Indirect Address Write +816 1 Indirect Data Write +176 2 Indirect Address Read +848 1 Indirect Data Read diff --git a/src/dynamixel/dynamixel.cpp b/src/dynamixel/dynamixel.cpp index aad50ed..ec3116c 100644 --- a/src/dynamixel/dynamixel.cpp +++ b/src/dynamixel/dynamixel.cpp @@ -1834,6 +1834,20 @@ DxlError Dynamixel::SetDxlValueToSyncWrite() 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); + } 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); + } } added_byte += indirect_info_write_[comm_id].item_size.at(item_index); } @@ -2053,6 +2067,20 @@ DxlError Dynamixel::SetDxlValueToBulkWrite() 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); + } 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); + } } added_byte += indirect_info_write_[comm_id].item_size.at(item_index); } diff --git a/src/dynamixel_hardware_interface.cpp b/src/dynamixel_hardware_interface.cpp index 06a2a68..3295951 100644 --- a/src/dynamixel_hardware_interface.cpp +++ b/src/dynamixel_hardware_interface.cpp @@ -497,9 +497,11 @@ hardware_interface::CallbackReturn DynamixelHardware::start() CalcTransmissionToJoint(); - // sync commands = states joint SyncJointCommandWithStates(); - std::this_thread::sleep_for(std::chrono::milliseconds(500)); + + CalcJointToTransmission(); + + dxl_comm_->WriteMultiDxlData(); // Enable torque only for Dynamixels that have torque enabled in their parameters std::vector torque_enabled_ids;