Needed Help with Dynamixel Hardware Interface and unstable motor values

[Antuvan]

Issue 1)

Hello , I have setup Dynamixel Hardware Interface with ros2 control ,I created a custom node to publish /joint_trajectory_controller/joint_trajectory messages and subscribed to /joint_states . The Issue is that the motors doesn't move when i specify positions in radiansand only works when i use raw dynamixel values . I would like some to help to figure out what to do here . Thank you

Issue 2)

I also noticed that my /joint_states values are very choppy and jittery when visualized in rviz .while i am using a good update rate and good latency value it doesnt appear to be stable eventhough the motor doesnt physically move maybe its because of conversion of radians to raw dynamixel values. I am not sure though.

header:                                                                                                                  
  stamp:                                                                                                                    
    sec: 1751376289                                                                                                      
    nanosec: 967814945
  frame_id: ''
name:
- motor_1
- motor_2
- motor_3
position:
- 2045.0
- 2045.0
- 2046.0

header:
  stamp:
    sec: 1751376290
    nanosec: 567781283
  frame_id: ''
joint_names:
- motor_1
- motor_2
- motor_3
interface_values:
- interface_names:
  - Present Current
  - Present Velocity
  - Present Position
  - position
  - velocity
  values:
  - 5.0
  - 0.0
  - 2045.0
  - 2045.0
  - 0.0

[Woojin-Crive]

Hi @Antuvan,

Would you mind sharing your ros2_control xacro file?

Also, could you let me know which Dynamixel model you're using?

Thanks in advance!

[Antuvan]

@Woojin-Crive I am using Three MX106 motors with Protocol V2.0 in which two are coupled in Dual Mode and also one Mx28AT motor with protocol version 2.0 and here is the ros2 control urdf i also tried without Dual Mode still the same problem is encountered.

<robot name="manipulator_mx" xmlns:xacro="http://www.ros.org/wiki/xacro">

    <ros2_control name="manipulator_mx_control" type="system">
      <hardware>

        <plugin>dynamixel_hardware_interface/DynamixelHardware</plugin>
    

        <param name="port_name">/dev/dynamixel</param>
        <param name="baud_rate">3000000</param>
        <param name="error_timeout_ms">500</param>
        <param name="torque_enable">0</param>
        <param name="dynamixel_model_folder">/param/dxl_model</param> 
    

        <param name="number_of_joints">3</param>
        <param name="number_of_transmissions">3</param>

        <param name="transmission_to_joint_matrix">1,0,0,0,1,0,0,0,1</param>
        <param name="joint_to_transmission_matrix">1,0,0,0,1,0,0,0,1</param>

        <param name="dynamixel_state_pub_msg_name">dynamixel_hardware_interface/dxl_state</param>
        <param name="get_dynamixel_data_srv_name">dynamixel_hardware_interface/get_dxl_data</param>
        <param name="set_dynamixel_data_srv_name">dynamixel_hardware_interface/set_dxl_data</param>
        <param name="reboot_dxl_srv_name">dynamixel_hardware_interface/reboot_dxl</param>
        <param name="set_dxl_torque_srv_name">dynamixel_hardware_interface/set_dxl_torque</param>

      </hardware>


      <joint name="motor_1">
        <command_interface name="position"/>
        <state_interface name="position"/>
        <state_interface name="velocity"/>
      </joint>

      <joint name="motor_2">
        <command_interface name="position"/>
        <state_interface name="position"/>
        <state_interface name="velocity"/>
      </joint>

      <joint name="motor_3">
        <command_interface name="position"/>
        <state_interface name="position"/>
        <state_interface name="velocity"/>
      </joint>

  
      <!-- declare each Dynamixel under a “gpio” tag, so the C++ can find ID, type, etc. -->
      <gpio name="motor_1">
        <param name="type">dxl</param>
        <param name="ID">1</param>
        <command_interface name="Goal Position"/>
        <state_interface name="Present Current"/>
        <state_interface name="Present Velocity"/>
        <state_interface name="Present Position"/>
        <param name="Position P Gain">400</param> 
        <param name="Position I Gain">0</param> 
        <param name="Position D Gain">200</param> 
        <param name="Profile Acceleration">30</param>
        <param name="Profile Velocity">20</param>        
        <param name="Return Delay Time">0</param>
        <param name="Drive Mode">0</param> 
      </gpio>
    
      <gpio name="motor_2">
        <param name="type">dxl</param>
        <param name="ID">2</param>
        <command_interface name="Goal Position"/>
        <state_interface name="Present Current"/>
        <state_interface name="Present Velocity"/>
        <state_interface name="Present Position"/>
        <param name="Position P Gain">400</param> 
        <param name="Position I Gain">0</param> 
        <param name="Position D Gain">200</param> 
        <param name="Profile Acceleration">30</param>
        <param name="Profile Velocity">20</param>        
        <param name="Return Delay Time">0</param>
        <param name="Drive Mode">0</param>
      </gpio>
    
      <gpio name="motor_3">
        <param name="type">dxl</param>
        <param name="ID">3</param>
        <command_interface name="Goal Position"/>
        <state_interface name="Present Load"/>
        <state_interface name="Present Velocity"/>
        <state_interface name="Present Position"/>
        <param name="Position P Gain">400</param> 
        <param name="Position I Gain">0</param> 
        <param name="Position D Gain">200</param> 
        <param name="Profile Acceleration">30</param>
        <param name="Profile Velocity">20</param>        
        <param name="Return Delay Time">0</param>
        <param name="Drive Mode">0</param> 
      </gpio> 

    </ros2_control>

</robot>

[Woojin-Crive]

Hi @Antuvan,

Could you please share your Dynamixel model file?

Also, I recommend renaming the gpio elements to avoid using the same names as the joint elements. For example, you could use names like dxl1, dxl2, dxl3, etc.

[Antuvan]

@Woojin-Crive This is the model file for mx106 and i am also using a slightly modified model file for mx28AT.

[type info]
name	value
value_of_zero_radian_position	2048
value_of_max_radian_position	4095
value_of_min_radian_position	0
min_radian	-3.14159265
max_radian	3.14159265

[control table]
Address	Size	Data Name
0	2	Model Number
6	1	Firmware Version
7	1	ID
8	1	Baud Rate
9	1	Return Delay Time
10	1	Drive Mode
11	1	Operating Mode
12	1	Secondary (Shadow) ID
13	1	Protocol Type
20	4	Homing Offset
24	4	Moving Threshold
31	1	Temperature Limit
32	2	Max Voltage Limit
34	2	Min Voltage Limit
36	2	PWM Limit
38	2	Current Limit
40	4	Acceleration Limit
44	4	Velocity Limit
48	4	Max Position Limit
52	4	Min Position Limit
63	1	Shutdown
64	1	Torque Enable
65	1	LED
68	1	Status Return Level
69	1	Registered Instruction
70	1	Hardware Error Status
76	2	Velocity I Gain
78	2	Velocity P Gain
80	2	Position D Gain
82	2	Position I Gain
84	2	Position P Gain
88	2	Feedforward 2nd Gain
90	2	Feedforward 1st Gain
98	1	BUS Watchdog
100	2	Goal PWM
102	2	Goal Current
104	4	Goal Velocity
108	4	Profile Acceleration
112	4	Profile Velocity
116	4	Goal Position
120	2	Realtime Tick
122	1	Moving
123	1	Moving Status
124	2	Present PWM
126	2	Present Current
128	4	Present Velocity
132	4	Present Position
136	4	Velocity Trajectory
140	4	Position Trajectory
144	2	Present Input Voltage
146	1	Present Temperature
168	2	Indirect Address Write 1
224	1	Indirect Data Write 1
578	2	Indirect Address Read 1
634	1	Indirect Data Read 1
170	2	Indirect Address Write 2
172	2	Indirect Address Write 3
174	2	Indirect Address Write 4
225	1	Indirect Data Write 2
226	1	Indirect Data Write 3
227	1	Indirect Data Write 4
580	2	Indirect Address Read 2
582	2	Indirect Address Read 3
584	2	Indirect Address Read 4
586	2	Indirect Address Read 5
588	2	Indirect Address Read 6
590	2	Indirect Address Read 7
592	2	Indirect Address Read 8
594	2	Indirect Address Read 9
596	2	Indirect Address Read 10
635	1	Indirect Data Read 2
636	1	Indirect Data Read 3
637	1	Indirect Data Read 4
638	1	Indirect Data Read 5
639	1	Indirect Data Read 6
640	1	Indirect Data Read 7
641	1	Indirect Data Read 8
642	1	Indirect Data Read 9
643	1	Indirect Data Read 10

[Woojin-Crive]

Hi @Antuvan,

I've checked the model file, and it looks fine.
Just to confirm—have you ever changed the gpio element's name?

Thanks,

[Antuvan]

@Woojin-Crive ,I modified the control URDF to use different joint names and different GPIO names as you mentioned above, but apart from that, I didn’t change much.
However, I noticed some strange behavior during real-time visualization in RViz. The robot appears to make jittery movements. Interestingly, when I manually held the motor shaft in place, the jittering stopped — but I also couldn’t observe any noticeable motion from the motor itself.
Could this be due to the absence of a threshold on the commanded or measured values? Would adding a threshold help mitigate this behavior?
Additionally, I see that the /joint_states topic is currently being published with raw Dynamixel tick values instead of radians. Could this also be contributing to the issue?

[Woojin-Crive]

Hi @Antuvan ,

I was able to reproduce your issue using my own Dynamixel.
It turns out there were two main problems:

1. Your model file was corrupted.
   It should follow this structure:

   [control table]
   Address	Size	Data Name
   0	2	Model Number
   6	1	Firmware Version
   ...
   168	2	Indirect Address Write
   224	1	Indirect Data Write
   578	2	Indirect Address Read
   634	1	Indirect Data Read
   

   The indirect address section was not properly written, which caused the Dynamixel Hardware Interface to fall back on using direct read/write with bulk instructions.

2. Lack of unit conversion in direct read/write mode.
   The direct method is primarily designed for devices that do not support indirect addressing. As a result, it skips unit conversion logic, which led to inconsistent behavior.

Thanks to your report, we plan to update the code so that unit conversion logic is applied even in direct read/write mode.

That said, for optimal performance and compatibility, we strongly recommend using the indirect method wherever possible.

Thank you again for your valuable contribution!

Best regards,
Woojin Wie.

[Woojin-Crive]

This fix is included in #64

[Antuvan]

Hi @Woojin-Crive ,

I tried applying the model file correction you suggested, but I’m still encountering errors. That’s why I reverted to my original model file, which directly defines all of the indirect addresses for the interfaces I’m using—only with that file can I successfully load the controllers.

For reference, I’m using MX-28 AT and MX-106 servos running Protocol 2.0 firmware V45 on ROS 2 Jazzy with a U2D2 interface.

Is there a way to apply your fix #64 to my existing model file with direct definition of indirect addresses so that it works under Jazzy?

Also, could you help by providing updated model files for the MX series?

Thank you!

[ros2_control_node-2] Fail to set indirect address fast bulk read. the dxl unincluding indirect address in control table are being used.
[ros2_control_node-2] FastBulkRead handler failed, falling back to normal BulkRead.
[ros2_control_node-2] Fail to set indirect address bulk read. the dxl unincluding indirect address in control table are being used.
[ros2_control_node-2] Cannot set the BulkRead handler.
[ros2_control_node-2] [ERROR] [1752224810.047790883] [dynamixel_hardware_interface]: Error: InitDxlReadItems

[Woojin-Crive]

Hi @Antuvan ,

Based on the model file you provided, I’ve fixed the indirect address section.

[type info]
name	value
value_of_zero_radian_position	2048
value_of_max_radian_position	4095
value_of_min_radian_position	0
min_radian	-3.14159265
max_radian	3.14159265

[control table]
Address	Size	Data Name
0	2	Model Number
6	1	Firmware Version
7	1	ID
8	1	Baud Rate
9	1	Return Delay Time
10	1	Drive Mode
11	1	Operating Mode
12	1	Secondary (Shadow) ID
13	1	Protocol Type
20	4	Homing Offset
24	4	Moving Threshold
31	1	Temperature Limit
32	2	Max Voltage Limit
34	2	Min Voltage Limit
36	2	PWM Limit
38	2	Current Limit
40	4	Acceleration Limit
44	4	Velocity Limit
48	4	Max Position Limit
52	4	Min Position Limit
63	1	Shutdown
64	1	Torque Enable
65	1	LED
68	1	Status Return Level
69	1	Registered Instruction
70	1	Hardware Error Status
76	2	Velocity I Gain
78	2	Velocity P Gain
80	2	Position D Gain
82	2	Position I Gain
84	2	Position P Gain
88	2	Feedforward 2nd Gain
90	2	Feedforward 1st Gain
98	1	BUS Watchdog
100	2	Goal PWM
102	2	Goal Current
104	4	Goal Velocity
108	4	Profile Acceleration
112	4	Profile Velocity
116	4	Goal Position
120	2	Realtime Tick
122	1	Moving
123	1	Moving Status
124	2	Present PWM
126	2	Present Current
128	4	Present Velocity
132	4	Present Position
136	4	Velocity Trajectory
140	4	Position Trajectory
144	2	Present Input Voltage
146	1	Present Temperature
168	2	Indirect Address Write
224	1	Indirect Data Write
578	2	Indirect Address Read
634	1	Indirect Data Read

This is the updated model file I used for testing.
The error message you’re encountering is typically printed when “Indirect Data Read” is missing from the model file.
I’ve updated that section in the model file above, so please give it a try.

Also, I’ll do my best to add support for the MX-series Dynamixel in the dynamixel_hardware_interface — maybe it can be included in the next release.
Thanks!