Merge pull request #54 from ROBOTIS-GIT/feature-omy-end

Add new model configuration for OMY end tool

Author: Woojin-Crive

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

package.xml

@@ -2,7 +2,7 @@
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>dynamixel_hardware_interface</name>
-  <version>1.4.7</version>
+  <version>1.4.8</version>
   <description>
     ROS 2 package providing a hardware interface for controlling Dynamixel motors via the ROS 2 control framework.
   </description>

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

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

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

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

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<int32_t>(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<int16_t>(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<uint8_t>(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<int32_t>(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<int16_t>(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<uint8_t>(data);
+          }
         }
         added_byte += indirect_info_write_[comm_id].item_size.at(item_index);
       }

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<uint8_t> torque_enabled_ids;