Improve performance of the control loop

[dogoepp]

The default frequency for this control loop is 10 Hz for us. Let's set cycle_time_error_threshold to 0.1, i.e. the maximum allowed duration of an iteration to ensure 10 Hz iteration frequency. I observe on Positronic that the threshold is regularly exceeded (see bellow). Admittedly, the excess time is very little, but it shows that we are already close to the limit.

[ WARN] [1458293591.557913852]: Cycle time exceeded error threshold by: 3.7721e-05, cycle time: 0.100037721, threshold: 0.1
[ WARN] [1458293592.057749621]: Cycle time exceeded error threshold by: 2.0122e-05, cycle time: 0.100020122, threshold: 0.1
[ WARN] [1458293592.557678208]: Cycle time exceeded error threshold by: 4.261e-05, cycle time: 0.100042610, threshold: 0.1
[ WARN] [1458293593.158254929]: Cycle time exceeded error threshold by: 0.00104895, cycle time: 0.101048953, threshold: 0.1
[ WARN] [1458293593.358195718]: Cycle time exceeded error threshold by: 0.0010338, cycle time: 0.101033797, threshold: 0.1
[ WARN] [1458293595.658286463]: Cycle time exceeded error threshold by: 0.00106704, cycle time: 0.101067042, threshold: 0.1
[ WARN] [1458293595.858179759]: Cycle time exceeded error threshold by: 0.00102926, cycle time: 0.101029257, threshold: 0.1
[ WARN] [1458293596.857821194]: Cycle time exceeded error threshold by: 1.7817e-05, cycle time: 0.100017817, threshold: 0.1
[ WARN] [1458293597.057700941]: Cycle time exceeded error threshold by: 5.8185e-05, cycle time: 0.100058185, threshold: 0.1
[ WARN] [1458293598.257216257]: Cycle time exceeded error threshold by: 2.11e-05, cycle time: 0.100021100, threshold: 0.1
[ WARN] [1458293598.357235889]: Cycle time exceeded error threshold by: 2.033e-05, cycle time: 0.100020330, threshold: 0.1
[ WARN] [1458293598.458147884]: Cycle time exceeded error threshold by: 0.0009539, cycle time: 0.100953900, threshold: 0.1
[ WARN] [1458293600.757213708]: Cycle time exceeded error threshold by: 1.0134e-05, cycle time: 0.100010134, threshold: 0.1
[ WARN] [1458293600.858150496]: Cycle time exceeded error threshold by: 0.000973175, cycle time: 0.100973175, threshold: 0.1
[ WARN] [1458293601.457944227]: Cycle time exceeded error threshold by: 2.1029e-05, cycle time: 0.100021029, threshold: 0.1
[ WARN] [1458293603.157214574]: Cycle time exceeded error threshold by: 3.4369e-05, cycle time: 0.100034369, threshold: 0.1
[ WARN] [1458293603.258169032]: Cycle time exceeded error threshold by: 0.00100335, cycle time: 0.101003347, threshold: 0.1
[ WARN] [1458293603.957935811]: Cycle time exceeded error threshold by: 1.621e-05, cycle time: 0.100016210, threshold: 0.1

One possible axis of improvement is the find operations on _dynamixel_corrections in each read_joints() and write_joints(). Nonetheless, the C++ reference states that it's an operation with log complexity. The impact of these operations should be limited, because until know, we never used more than 18 actuators at the same time.

The alternative choice is to consider that the control loop cannot run at such frequency. I think it would be a sad limitation.

[naoki-mizuno]

I'm also trying to figure out why I'm only getting 5 Hz for the joint_states with 7 servos daisy-chained at 1Mbps baud rate.

average rate: 5.207
        min: 0.189s max: 0.195s std dev: 0.00220s window: 5
average rate: 5.210
        min: 0.189s max: 0.195s std dev: 0.00141s window: 11
average rate: 5.208
        min: 0.189s max: 0.195s std dev: 0.00126s window: 16
average rate: 5.145
        min: 0.188s max: 0.239s std dev: 0.01047s window: 21
average rate: 5.158
        min: 0.188s max: 0.239s std dev: 0.00949s window: 26
average rate: 5.082
        min: 0.188s max: 0.241s std dev: 0.01444s window: 31
average rate: 5.100
        min: 0.188s max: 0.241s std dev: 0.01348s window: 36
average rate: 5.082
        min: 0.188s max: 0.241s std dev: 0.01443s window: 41
average rate: 5.096
        min: 0.185s max: 0.241s std dev: 0.01377s window: 46
average rate: 5.107
        min: 0.185s max: 0.241s std dev: 0.01313s window: 51

It seems like the recv is taking time (0.015 sec/call, twice per servo for position and speed, 7 servos total becomes 0.015 * 2 * 7 = 0.21 which is about 5 Hz). I changed the method to read more bytes at a time rather than byte-by-byte, but didn't see any improvement in the rate. Seems like the guys at dynamixel workbench are getting 30+ Hz so I would think my servos can do faster.

Reference: http://forums.trossenrobotics.com/showthread.php?6690-Dynamixels-communication-speed

[jbmouret]

The find should be really negligible here.

I think this comes more from how we broadcast commands and wait for replies.

Also, do the dynamixel_workbench code query the position of each servos at each time step? This takes a significant time (versus sending commands and that's it).

[naoki-mizuno]

do the dynamixel_workbench code query the position of each servos at each time step?

I believe so. Although, they're also reading data for each servo (and not using "sync read" or "bulk read"), so I'm not sure where the difference comes from :confused: That being said, I haven't actually used dynamixel_workbench myself so I'll need to test it out on my servos to do a comparison.

[dogoepp]

One difference between DynamixelSDK and Libdynamixel is that they use a non-blocking call to read(). We were already considering this option which shall be tested (hopefully soon).

Also, They have slightly different settings for the serial interface, and there is a latency_timer thing.

For further reading, their reading method (for protocol 1) is here..

[gjjab]

Excuse me Is this problem solved? I also have the same problem!! I use the latest dynamixel_workbench to controll 7 motors in ROS kinetic !! How can I solve this problem about low frequency dynamixel state???

[dogoepp]

This problem is under investigation. I plan on going back to it from today on.

If you want to help on this investigation, it seems that most of the time is waisted in read(), in src/dynamixel/controllers/usb2dynamixel.hpp

[naoki-mizuno]

@dogoepp

I plan on going back to it from today on

:+1: Do you think it's not necessary to use bulk_read?

[dogoepp]

I think that it should help but I am not sure that it is the root cause, since it seems that robotis themselves do not use it.

[dogoepp]

I found a few, very interesting, lines in DynamixelSQK. They point to latency_timer which setting we can change.

It appears that by putting it to 1 (ms), we were able to run the control loop at 100 Hz with 5 actuators.

@naoki-mizuno could you confirm this observation ?

Next I'll see how we can use commands to read and write for all joints at once. This is expected to further improve the loop frequency.

[naoki-mizuno]

@dogoepp I gotta say, you just made my day :laughing: I set the latency timer to 1, and I was able to get the joint states at 50 Hz with 7 servos. I'm not sure how I overlooked that parameter when reading through their code, but I'm pretty happy with how smooth our manipulator works now!

[dogoepp]

Great ! I hope that the next step will make it even better.

[SammyRamone]

Hi sorry for digging this thread up. I just stumpled on it randomly We actually achieved 400Hz at 4Mbaud reading (pos, vel, eff) and writing (pos) 20 servos. Setting the latency timer to 0 and using sync reads and writes can increase the frequency a lot. You can find our implementation for a dynamixel ros_control hardware interface here: https://github.com/bit-bots/bitbots_lowlevel/tree/master/bitbots_ros_control

[PedroDesRobots]

Hi @SammyRamone , Thx you for your advice. But which kind of Dynamixel motors did you use? (I guess Dynamixel pro series in your case). Actually, we are working with Dynamixel MX-28T which are limited to 1Mbaud.

[SammyRamone]

In my case MX-64 and MX-106. If you update the firmware you can use up to 4.5Mbaud. In reality 4.5Mbaud doesn't seem to work, but 4Mbaud does. http://emanual.robotis.com/docs/en/dxl/mx/mx-28-2/#baud-rate

[dogoepp]

Sounds great. I think indeed that using sync read and writes could help a lot. If I recall correctly, either of these commands is only available in the version 2 of the protocol however. Good to know that implementing this would allow for much greater loop frequencies.

As for the firmware, it might be that some older versions of MX-28 cannot be upgraded.

Out of curiosity, could I ask why you have implemented your own hardware interface? Was the barrier of entry to use this one too high?

[SammyRamone]

Actually you can also use sync reads in the protocol version 1. Out of some reason its not documented, but it works. We used to do it before we switched to version 2. If you want to do it I can dig out our old code.

Yes thats possible. I remember that we had some problems with updating MX-28 servos but we didn't bother since we stopped using them.

Actually I implemented most of this over a year ago. I think at that point your software was not finished. I just had a look at it today because I thought about switching to yours since it doesn't make sense to have thousands of implementations for the same thing. But currently I'm missing the sync read and write functions.

Btw: Did you set the return delay time in the servos to 0? This is also a common source for low frequency. I don't really know why robotis did set the default value to something else than 0.

Second btw: we just developed a multi-bus controller board solution which enables us to read/write 20 servos with 1.3KHz, if you need even higher frequencies

[dogoepp]

I would like to help, but I do not have access to Dynamixels anymore and am rather busy. If @PedroDesRobots / @dalinel and/or you are willing to go this way, I would be happy to give a hand, at least to find the parts in the code that need modification.

I was not aware of the return delay time. It should be checked.

About your board, how is it connected to the computer? How do you handle the multiple buses?

[PedroDesRobots]

Firstly, I have set the return delay time in the servos to 0. It was the default value 250 ms... as expected it increases the transmission speed. I was plotting time delay in my controller loop (update function) and that divided by two the time.

Concerning updating MX-28 servos I will try it soon. And I will take a look at sync reads and how to add it in our code.

@SammyRamone could you tell us more about your multi-bus controller? Because we are really interesting to increase our frequencies to be able to produce better controllers in velocity mode.

[SammyRamone]

I recommend to always set the return delay time to 0 automatically in all servos when starting up your software. Otherwise you will change a servo or update firmware at some point and wonder why your update rate dropped again. Trust me, I've been through this :D Keep in mind that even when setting delay time to 0, it will still be around 50us (I don't know why, probably just because robotis writes bad firmware).

Regarding updates of the MX-28: I think the older versions had a chip with less memory and the new firmware doesn't fit on it. Therefore they don't work. But I'm not totally sure about this.

Regarding the multi-bus: We just took an multi channel FTDI chip (similar to the robotis U2D2 but with 4 channels). Currently we just let our software run 4 times on the different bus systems for tests. But it should be possible to have 4 hardware interfaces where the controllers connect to, but I didn't try it yet. I'm currently writing a paper about it, hopefully it will be finished next week. I can send you the first version of it then, if you are interested.

[PedroDesRobots]

I have upgraded the MX-28 firmware (v36 to v39) under the software RoboPlus. It was impossible to do it under R+ Manager... I don't know why but R+ never detect my servos but I suspect to use protocol 2 only.

Now I can increase the baudrate at 3 Mbauds (it didn't propose 4MBauds in the soft...) BUT when I'm trying to read address 5, which is the address number in control table for baudrate, it returns 1 (1Mbaud in protocol 1 or 57600 in protocol 2). In the end, the servo responds only at 3Mbauds expected result but only under protocol 1. I set the return delay time at 0 but it responds only for a --scan-timeout of 0,05. While with MX-28 v36 set at 1 Mbaud we are able to reach a scan-timeout of 0,001.

@SammyRamone could tell me which soft do you use to update servos (RoboPlus or R+) and how do you manage to communicate with protocol 2 ? Do you know if we are able to recognize old MX-28 from new version ?

Btw, with our MX-28 servo v36 set at 1 Mbaud we are able to reach 1000Hz in our control loop. when I'm trying to communicate with 18 servos, the best result is around 20Hz.... and normally we should be able to reach 50Hz easily (~1000Hz / 18 servos ) but I guess it's not linear. Of course, our solution to use multi-channel FTDI chip is the best way to carry on to increase speed. Which kind of multi-channel FTDI do you use? If you can send me the paper it will be pleasant.

[jbmouret]

So, 1khz with a single Mx-28, but 20Hz with 18 Mx-28. Right? Which usb2dynamixel did you use? we have three options.

We could also simply use 4 USB adapters with a good USB hub.

Also, @SammyRamone might be using the 4-wires servos.

[dogoepp]

@jbmouret four wires or three wires are the same in term of communication : half-duplex.

@PedroDesRobots could you clarify your second paragraph? It is hard to figure what are your observations, under which circumstances.

In addition, It may be simpler to start by moving to sync read and write before going the way of multiple hardware controllers. This is no modification to existing hardware and has the potential of an immediate improvement, for free, for all users of this software.

[SammyRamone]

Hi, sorry for replying so late. There were just too many deadlines. @PedroDesRobots We're using R+. RoboPlus does not support the newest firmwares if I'm not mistaking. We can communicate and set values without problems using R+. But we basically use this only for flashing the firmware. Otherwise we use our ros_control software that I posted earlier for communication. For debugging we also made a small ROS package with a few hacky scripts: https://github.com/bit-bots/bitbots_lowlevel/tree/master/bitbots_dynamixel_debug

Regarding the MX-28: I'm not totally sure about these, since we stopped using them. You should be able to differentiate the versions by the chip that is installed in them. But again, I'm not totally sure about the 28s.

Regarding your 20Hz : I would guess one of them still has a return delay time. Best way to investigate this is using a logic analyser. I can recommend the one from Saleae. That way you can actually see what is happening on the bus.

@jbmouret I used RS-485 and TTL both (4 and 3 wires). There is no real difference in them, except that RS-485 works better on long distance transmissions and on bad cables.

The paper is unfortunately still in a state where I can't send it around. But I can sum up the most important points. We used a FT4232H chip which is basically 4 times the FT232H (which is used in the Robotis U2D2) in one chip. It's basically the same as pluging in 4 U2D2s, just smaller and more convenient. Since the USB2 baudrate is much higher than the ones used on the dynamixel buses (480 Mbaud), it is no problem to merge 4 buses into one USB bus. Our results showed that it performs equally for a single bus as a single FTDI chip, so there seems no advantage on using multiple single chips. We also compared this to other approaches which use a STM32 chip as a controller board (such as this one https://github.com/bit-bots/DXLBoard ) and found out that the FTDI approach is better, since it has lower latency in processing than STM32 chip (which was what we expected). Overall we reached 1.3kHz with 20 servos reading 10 bytes and writing 4 in each cycle, when using 4 buses. On one bus we reached only 400Hz. So it scales well (but not linear since you have additional header bytes). The theoretical maximum would be much higher (700Hz for one bus) but it can't be reached since the servos need around 50us to respond, even when delay time is 0. This response delay is the same independent from your baudrate, therefore update rates do not scale linear to increasing baudrate.

When the paper is ready, I will be happy to share it with you. Feel free to ask any further question, I will try to answer quicker than the last time ;)

[raess1]

I have some problem getting high loop speeds. I am running 12 x MX-28R in one setup with U2D2 and the servos are updated to protocol 2.0 and running at 4 MBPS. I get around 50hz. The servos are Daisy chained in Groups of 3.

My second setup will be 8 x MX-106R and 4 x MX-64R. Also Daisy chained in Groups of 3. But havent tried the loop speed on this setup yet….

I have set the return delay time in the servos to 0. (as discussed in improve loop speed thread) and latency_timer to (1)ms

Any suggestions ? :)

[jbmouret]

Hi

The specialists are @Aneoshun and @PedroDesRobots (but he is on vacation this week). We definitely get much higher frequencies with MX-28.

We have limited access to the lab right now (covid-19 restrictions), but we can have a look at the end of the week, maybe.

[raess1]

thanks @jbmouret I did some troubleshooting last night an it seems that I forgot to set return delay time to zero for one servo. With that fixed I can now hit around 70hz at the joint states topic for 12 servos. What rates could one expect?

I read that @SammyRamone suggest to set latency_timer to zero for improved freq. Will that have a big impact on the rate?

btw: First time on the legs https://www.youtube.com/watch?v=Ix_kLGgavYM

[dogoepp]

Nice video and congratulations ! It's always nice to get to the point where the robot actually does something.

I guess that @jbmouret or @PedroDesRobots will know the exact loop frequency we can get for our robots (those with 18 servos). I think that it could run at 100 Hz.

If I recall correctly the driver would always make us wait the duration of latency timer before our software could access any new data from the servos. It thus had a serious impact on the loop frequency, as we are systematically requesting servo angular position, thus waiting for data. There is a little information on the matter in the readme of libdynamixel, admittedly too scarce.

I would then advise to put this setting to a low value. I think that we used as low as 0.

[SammyRamone]

The U2D2 should be fast enough to give you a decent update rate with only 12 servos. There are following possible problems limiting your update rate:

1. As it has been said, the latency timer has to be set to 0. There is some command which sets it to "low_latency", but this is still acutally 1ms. I use sudo sh -c "echo '0' > /sys/bus/usb-serial/devices/ttyUSB0/latency_timer" to set it to 0. You may need to change ttyUSB0 to something else.
2. If I understand the code correctly, each value (position, velocity,...) of each servo is read with a single read. This is super slow. Not only do you need to transmit more bytes as in a sync read (due to additional headers), you also have to wait multiple times to for the servos to respond, which takes for the MX-64 and MX-106 around 50us. The MX-28 are maybe even slower as they have a different microcontroller. You should only send one sync read and one sync write per update cycle. You can find more details about this in my paper (https://www.researchgate.net/publication/337716753_High-Frequency_Multi_Bus_Servo_and_Sensor_Communication_Using_the_Dynamixel_Protocol).
3. Check your main computer CPU. Is it fast enough to handle this update rate or is it at 100% workload? Also check if the library your using is not "busy waiting" during reads (which the robotis dxl library did in earlier versions), since your CPU core can do nothing else while waiting for the status package to be returned. This leads to having more other things to do between handling packages, thus a longer latency from your main computer.
4. Check if you have any package losses. If one of your servos does not respond to a package, the master waits till the time out, which is quiet long. Some servos may also just sometimes not respond correctly.
5. Do you have any other devices that you have connected to the bus, e.g an IMU? Speaking with those takes away additional bus time and depending on the model, they may also answer slowly. If you are looking for a fast IMU with dxl bus compatibility, you can have a look at ours https://github.com/bit-bots/bitbots_imu_dxl

Generally speaking, when having such problems, you should investigate what is actually running on your bus, to see what takes which amount of time. The easiest way to do this, is to use a logic analyser. I'm using one from Saleae and it works very well. Also implement in your code, that the ROM registers (especially "return delay time") are set correctly on start up. It happens to easily, that one servo has wrong values and messes up everything.

[raess1]

Thanks @SammyRamone for the detail respons. Will try out some of your suggestions when I get home from work.

Some commments. 2. If I remember correctly I use the libdynamixel DEV Branch and dynamixel_control_hw devel Branch from my understanding have implemented buld_write and sync_read for Protocol 2.0.

3. I am using the Jetson nano at MAXN . Will check the CPU usage later.

4. I am running nothing else on the BUS. no IMU or nothing at the moment. I have adis 16465-a that I think I will use.

best regards Robin

[SammyRamone]

I don't have experience with the libdynamixel, but generally a bulk_write is still slower as a sync_write due to having a larger package length. Still, it is much better than using single writes. For the sync_read, it still makes a difference if you read all registers with one sync read or use multiple ones.

I'm using forks of the DynamixelSDK and Dynamixel Workbench from Robotis. I implemented reading multiple registers with a single sync read there. You can find it here: https://github.com/bit-bots/DynamixelSDK/tree/master https://github.com/bit-bots/dynamixel-workbench/tree/master