Merge pull request #28 from ROBOTIS-GIT/feature-omy

Update Torque Disable

Author: robotpilot

.github/workflows/ros-ci.yml

@@ -4,9 +4,9 @@ name: CI
 # Specifies the events that trigger the workflow
 on:
   push:
-    branches: [ main, humble, jazzy ]
+    branches: [ humble, jazzy, main]
   pull_request:
-    branches: [ main, humble, jazzy ]
+    branches: [ humble, jazzy, main]
 
 # Defines a set of jobs to be run as part of the workflow
 jobs:

.gitignore

@@ -505,6 +505,9 @@ CATKIN_IGNORE
 AMENT_IGNORE
 COLCON_IGNORE
 
+## Dynamixel model files
+!*.model
+
 
 ##############################
 # ETC.

CHANGELOG.rst

@@ -2,6 +2,13 @@
 Changelog for package dynamixel_hardware_interface
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+1.4.2 (2025-04-05)
+------------------
+* Added OM-Y dynamixel model files
+* Added a function to enable torque
+* Fixed the configuration for OM-Y robots
+* Contributors: Woojin Wie, Wonho Yun
+
 1.4.1 (2025-03-31)
 ------------------
 * Modified the Model File

include/dynamixel_hardware_interface/dynamixel/dynamixel.hpp

@@ -27,6 +27,7 @@
 #include <iostream>
 #include <cstdarg>
 #include <memory>
+#include <functional>
 
 namespace dynamixel_hardware_interface
 {
@@ -166,7 +167,7 @@ class Dynamixel
   DxlError SetMultiDxlWrite();
 
   // Read Item (sync or bulk)
-  DxlError ReadMultiDxlData();
+  DxlError ReadMultiDxlData(double period_ms);
   // Write Item (sync or bulk)
   DxlError WriteMultiDxlData();
 
@@ -200,12 +201,12 @@ class Dynamixel
   // SyncRead
   DxlError SetSyncReadItemAndHandler();
   DxlError SetSyncReadHandler(std::vector<uint8_t> id_arr);
-  DxlError GetDxlValueFromSyncRead();
+  DxlError GetDxlValueFromSyncRead(double period_ms);
 
   // BulkRead
   DxlError SetBulkReadItemAndHandler();
   DxlError SetBulkReadHandler(std::vector<uint8_t> id_arr);
-  DxlError GetDxlValueFromBulkRead();
+  DxlError GetDxlValueFromBulkRead(double period_ms);
 
   // Read - Indirect Address
   void ResetIndirectRead(std::vector<uint8_t> id_arr);
@@ -215,6 +216,23 @@ class Dynamixel
     uint16_t item_addr,
     uint8_t item_size);
 
+  // Read - Data Processing
+  DxlError ProcessReadData(
+    uint8_t id,
+    uint16_t indirect_addr,
+    const std::vector<std::string> & item_names,
+    const std::vector<uint8_t> & item_sizes,
+    const std::vector<std::shared_ptr<double>> & data_ptrs,
+    std::function<uint32_t(uint8_t, uint16_t, uint8_t)> get_data_func);
+
+  // Read - Communication
+  DxlError ProcessReadCommunication(
+    dynamixel::GroupFastSyncRead * group_sync_read,
+    dynamixel::GroupFastBulkRead * group_bulk_read,
+    dynamixel::PortHandler * port_handler,
+    double period_ms,
+    bool is_sync);
+
   // SyncWrite
   DxlError SetSyncWriteItemAndHandler();
   DxlError SetSyncWriteHandler(std::vector<uint8_t> id_arr);

include/dynamixel_hardware_interface/dynamixel_hardware_interface.hpp

@@ -191,6 +191,8 @@ class DynamixelHardware : public
   bool is_read_in_error_{false};
   bool is_write_in_error_{false};
 
+  bool global_torque_enable_{true};
+
   bool use_revolute_to_prismatic_{false};
   std::string conversion_dxl_name_{""};
   std::string conversion_joint_name_{""};
@@ -235,6 +237,9 @@ class DynamixelHardware : public
   std::vector<uint8_t> sensor_id_;
   std::map<uint8_t /*id*/, std::string /*interface_name*/> sensor_item_;
 
+  std::vector<uint8_t> controller_id_;
+  std::map<uint8_t /*id*/, std::string /*interface_name*/> controller_item_;
+
   ///// handler variable
   std::vector<HandlerVarType> hdl_trans_states_;
   std::vector<HandlerVarType> hdl_trans_commands_;
@@ -245,18 +250,42 @@ class DynamixelHardware : public
   std::vector<HandlerVarType> hdl_gpio_sensor_states_;
   std::vector<HandlerVarType> hdl_sensor_states_;
 
+  bool is_set_hdl_{false};
+
   // joint <-> transmission matrix
   size_t num_of_joints_;
   size_t num_of_transmissions_;
   double ** transmission_to_joint_matrix_;
   double ** joint_to_transmission_matrix_;
 
   /**
-   * @brief Initializes the Dynamixel items.
+   * @brief Helper function to initialize items for a specific type.
+   * @param type_filter The type of items to initialize ("controller" or "dxl" or "sensor").
+   * @return True if initialization was successful, false otherwise.
+   */
+  bool initItems(const std::string & type_filter);
+
+  /**
+   * @brief Helper function to retry writing an item to a Dynamixel device.
+   * @param id The ID of the Dynamixel device.
+   * @param item_name The name of the item to write.
+   * @param value The value to write.
+   * @return True if write was successful, false if timeout occurred.
+   */
+  bool retryWriteItem(uint8_t id, const std::string & item_name, uint32_t value);
+
+  /**
+   * @brief Initializes Dynamixel items.
    * @return True if initialization was successful, false otherwise.
    */
   bool InitDxlItems();
 
+  /**
+   * @brief Initializes the controller items.
+   * @return True if initialization was successful, false otherwise.
+   */
+  bool InitControllerItems();
+
   /**
    * @brief Initializes the read items for Dynamixel.
    * @return True if initialization was successful, false otherwise.

package.xml

@@ -1,7 +1,8 @@
-<?xml version='1.0' encoding='UTF-8'?>
+<?xml version="1.0"?>
+<?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.1</version>
+  <version>1.4.2</version>
   <description>
     ROS 2 package providing a hardware interface for controlling Dynamixel motors via the ROS 2 control framework.
   </description>
@@ -10,6 +11,7 @@
   <author email="[email protected]">Hye-Jong KIM</author>
   <author email="[email protected]">Sungho Woo</author>
   <author email="[email protected]">Woojin Wie</author>
+  <author email="[email protected]">Wonho Yun</author>
   <buildtool_depend>ament_cmake</buildtool_depend>
   <depend>rclcpp</depend>
   <depend>hardware_interface</depend>

param/dxl_model/dynamixel.model

@@ -44,3 +44,5 @@ Number	Name
 4150	ym080_230_r099.model
 4170	ym080_230_a099.model
 35074	rh_p12_rn.model
+220	omy_hat.model
+230	omy_end.model

param/dxl_model/omy_end.model

@@ -0,0 +1,52 @@
+[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
+
+[control table]
+Address	Size	Data Name
+0	2	Model Number
+2	4	Model Information
+6	1	Firmware Version
+7	1	ID
+8	1	Baud Rate (Bus)
+10	1	Tool Analog Enable
+11	1	Tool Baud Rate
+12	1	Tool Protocol
+13	1	Tool ID
+14	1	Tool Status
+30	1	Button Status
+32	1	R LED
+33	1	G LED
+34	1	B LED
+36	2	Realtime Tick
+40	1	Digital Input
+41	1	Digital Output
+42	2	Analog Input1
+44	2	Analog Input2
+46	1	SyncTable Enable
+48	2	SyncTable Read Address
+52	2	SyncTable Read Size
+56	2	SyncTable Write Address
+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
+634	1	Indirect Data Write
+402	2	Indirect Address Read
+646	1	Indirect Data Read

param/dxl_model/omy_hat.model

@@ -0,0 +1,35 @@
+[control table]
+Address	Size	Data Name
+0	2	Model Number
+2	4	Model Information
+6	1	Firmware Version
+7	1	ID
+8	1	Baud Rate (Bus)
+9	1	DXL Baud Rate
+10	2	DB Voltage
+12	1	DB Voltage Thresh
+14	2	DB Voltage D Gain
+16	2	DB Voltage I Gain
+18	2	DB Voltage P Gain
+20	1	SyncTable ID
+30	2	SyncTable Read Address
+50	2	SyncTable Read Size
+70	2	SyncTable Write Address
+90	2	SyncTable Write Size
+110	2	SyncTable Period
+112	2	Indirect Address
+512	1	Power Enable
+513	1	Voltage Control Enable
+514	1	Table Sync Enable
+515	1	R LED
+516	1	G LED
+517	1	B LED
+518	1	Status
+519	1	EMG Power Ctrl
+520	2	Realtime Tick
+522	2	Present Input Voltage
+524	2	Present Input Current
+550	1	Status Return Level
+560	1	SyncTable Read Data
+688	1	SyncTable Write Data
+816	1	Indirect Data

param/dxl_model/rh_p12_rn.model

@@ -2,8 +2,8 @@
 name	value
 value_of_zero_radian_position	0
 value_of_max_radian_position	740
-value_of_min_radian_position	0
-min_radian	0.0
+value_of_min_radian_position	-740
+min_radian	-1.099
 max_radian	1.099
 
 [control table]