Running ROS2 Controller Manager at high loop rate - groupSyncRead error

[nilp-dromeda]

I have been trying to use this package to interface with our custom robot. The robot contains the below hardware:

• XL330-M288-T (x3)
• XM430-W350-T (x9)
• XC330-T288-T (x3)
• U2D2 serial converter

The U2D2 is running at a baud rate of 57600 and all Dynamixels are running in Position Control Mode and running everything on Humble.

The problem I am having is that when running the ROS2 controllers at faster than 10-20 Hz (using the config parameters in the .yaml file for ros2_controllers I get alot of the below errors:

[ros2_control_node-3] [ERROR] [1719906084.814933792] [DynamixelHardware]: groupSyncRead getdata failed
[ros2_control_node-3] [ERROR] [1719906084.814955616] [DynamixelHardware]: groupSyncRead getdata failed
[ros2_control_node-3] [ERROR] [1719906084.814971969] [DynamixelHardware]: groupSyncRead getdata failed
[ros2_control_node-3] [ERROR] [1719906084.914753622] [DynamixelHardware]: [TxRxResult] There is no status packet!

Reducing the controller loop rate to 10Hz seems to fix this issue. But then, when joints are under mechanical load the above errors start to appear again (maybe because internal PID controllers of the Dynamixels are busy reading the registers when under load alot more frequently?). When these errors occur the /joint_states topic publishes garbage data, hence, causing the controller to cause massive jumps in motion randomly. I would like to run the controllers at around 100Hz and get rid of the above groupSyncRead issue. Does anyone have any ideas on possible fixes to this issue at all or has seen something similar?

I have used this library as well and it runs the control loop at around 500Hz and the above error does not occur at all. This makes me wonder if it is an issue with this repo's implementation?

[BenKlee]

I am running into a similar problem using 18x RX-64 motors, also in ROS Humble.
Writing to the motors works fine but reading fails. Your workaround (lowering the rate to 10 Hz) did not work for me.

The major problem this causes for me is that since the actual positions of the motors cannot be read, it will assume they are at 0.
This causes the write method to write a goal position of 0 to all motors until my ROS node starts publishing the actual goal positions.
I have not found a way to prevent this and it leads to my robot colliding with itself until ROS has properly started up.

I also assume that the trajectory controller does work as well as it should if its is not getting any position and velocity feedback from the motors.

[brennand]

I fixed this bug in my own branch. and running at 1khz for a single motor.

https://github.com/brennand/dynamixel_hardware

There is some other modifications, for example gear ratio and motor offset as well.

[Tacha-S]

@brennand
I could only achieve 60Hz with a single motor, which led me here.
The time taken for read was the dominant factor, preventing high-speed control.
However, in your branch and execution environment, it doesn't seem like read has been significantly modified.
How did you achieve 1kHz?