Hi, I am using 30 “Lynxmotion Smart Servo (LSS) Motor - High Torque (HT1)” in my Robot. They are connected to a “Lynxmotion Smart Servo (LSS) - Adapter Board”, which is connected via USB to a Raspberry-PI 4 using Ubuntu 64 bit. Each of the six ports of the adapter board has 5 servos daisy chained to it.
Sending commands and queries works fine and very quickly, so all 30 servos react to commands quickly and almost at the same time (e.g. by moving all at once to the desired positions).
The problem is receiving the answers to queries. When asking each servo in turn and waiting for the answer before asking the next servo, it takes several seconds to receive answers from all 30 servos, which is way too long for a robot. When asking all servos (e.g. for their current positions) as fast as these queries can be sent, they all seem to answer almost at once (as soon as they receive the request), resulting in corrupted answers that consist of interleaved answer-parts from all servos.
My current solution is to wait 0.017s after asking several queries to one servo before asking the next one. This way, they answer one after the other, almost without ever colliding answers.
As 30*0.017=0.51s, this way I receive almost 2 full updates from all servos per second, even when asking them not only position but also current and status, as questions to one servo can be sent and received as quickly as possible.
But that still is not enough, 60 answers per second would be way more useful to react to e.g. obstacles. Sending 60 commands per second to the servos is no problem at all.
Am I missing a better solution to this? Can the adapter board hardware somehow handle the servo answers to make them be sent one after the other and not simultaneously, while still sending these answers as quickly as possible?
What you describe is exactly right. That being said, there are a few ways to improve the results:
You can increase the baud rate. The communication was tested with up to 36 servos at 500 kbaud with no errors, so this should be safe unless you use very long cables between the servos. Increasing the baud rate won’t solve bus contention but it can at least reduce a bit the duration of transmission.
Since you are using a RPi4, you should try and use the multiple UART buses available on that board. This has been done already for a humanoid project. You can read more about it here (this is a long topic, you have been warned! ). You may want to contact @cmackenzie as he was the main driving force behind those effort and has done most of the work (including a full library for optimized comms!)
You can also use CL and CH for collision situations. They respectively cause the servo to either go limp or hold when they hit an object by setting a threshold for current consumption, requires some trial & error to find the right value for different situations. That way you don’t have to micro-manage each servo continuously and instead can just read its status during motion to check for early hold/limp (vs motion).
Thanks for the info, good to know that at least I am on the right track.
As for the baud rate, I already tried all available rates with very mixed results:
(With DPS I mean full infos from all servos per second, with delay I mean the necessary delay between asking different servos.)
So the default baud rate 115200 already gives the best results. My guess is that the higher baud rates come with more data corruption even without collisions.
I can speak from experience the latency is not from the LSS. I am running 500kbps with 18 servos and they respond within 1.5ms and I know it’s actually less than this but packets get caught up in the protocol overhead of Linux. The buffering is not great for low latency comms especially with the USB-serial devices we find around. FT232 based usb-serial devs can be put in a low-latency mode and get 8ms round-trip (down from 64ms round trip), and CP2102 based devices are fixed at 1ms round-trip. Nothing usb-serial beats the RPi TTL serial at about 1.5ms. I also tried 32bit arduino devices and had the same speeds as RPi. I wanted my total 18-servo control loop to be 16ms max so I split the 18 servos into 3 buses of 6 servos each. I have my 12-16ms control loop I wanted. I am querying position and current and sending position and about 4 other parameters…so I’m doing a lot.
I am using an LSS library I wrote which is at:
I put a lot of effort into fast communication and there are some tricks I pulled to speed things up. First, you can send many commands like position command to all servos at the same time since they dont reply. It’s only queries that can conflict. However, you can send all query commands to the same servo at once. That one servo will reply in sequence without conflicting with itself obviously. Finally, all the timing is quite predictable, the servo will reply to a single query in just under 1ms, multiple queries usually even return in 1ms. I actually send queries for the next servo before I receive the reply assuming that the reply has made it to the RPI’s receive buffer. With some playing around I just kept reducing my loop until I started getting conflicts then backed off. I also dont wait long for a reply, if I get a failed transaction I just move on and the next iteration 12ms later will get it. I record the statistics, it doesnt happen often, perhaps about once every few minutes or better and not noticable on the robot.
All this is built into the library if you want to use it. See the examples instead of the ReadMe (it needs updating i see). Setup a control loop where you send the commands, then the queries. Order your packets as in the above paragraph for performance. The library times the packet transmission appropriately. This video shows how quickly I can read the servo positions from one arm and mirror that to the other arm:
If you are interested in ROS2, we can get you setup on that with a ROS2 joint controller, etc.
As for the command and query orders, I already do all the tricks you suggest, but I will give the USB-serial alternatives you mention a try.
I had already found your library before, but as I understand it is meant for Arduino and not for RPI?
ROS sounds very good, I currently wrote my own ROS node for those joints, but I do not yet use ROS2. Is there a solution for ROS with RPI already maybe?
Anyways, thanks for the support and I will let you know my further results .
That CompliantServo branch supports both Arduino and Linux/RPi and in Linux has support for direct FTDI port library and the normal posix serial (including RPi TTL). It also puts the port in LOW_LATENCY mode, see the code on line 182 in platform/posix/LssPosixChannel.cpp. I would suggest doing the same in your code which will decrease buffer thresholds within linux.
The ROS2 joint controller node also uses this library to talk to the servo bus. I run the joint controller and IMU on the robot itself on an RPi4 and develop and run other nodes on my desktop linux before moving them over to the Rpi. I like being able to develop the nodes on the desktop first. At the moment I am deep into simulating the humanoid in Gazebo.
I am a big fan of ROS2. I’ve been developing on it for a year and I love the new design and performance. I looked at ROS1 but I wasnt happy with the architecture overhead for embedded devices. ROS1 just had it’s last release so not sure if there will be plans to backport anything to ROS1. If you’re ever interested in ROS2 let me know and I can lend a hand.
So, now after also a lot of work I just wanted to let you know, I am finally OK with my solution reaching around 9.4 full updates for all 24 servos, although it still does not reach the performance of your solution which is in the my case of 24 servos around 12.5 updates per second using USB . The most important trick really was to set the low-latency mode, then even USB connection is OK, while the serial connection over GPIO still is a bit faster of course.
I tried to step by step take the features away from your solution to detect the central magic , surprisingly I could remove a lot, from read/write blocking instead of returning immediately, to even removing the 1600ms waiting time between queries, your solution still works like a charm.
Here are my measurements of how many complete updates for all 24 servos per second your solution delivers after the noted changes, just in case you are interested:
14.2857 loops per second: Serial Cable, Original version
14.0845 loops per second: Serial Cable, without "| O_NOCTTY | O_NDELAY | O_NONBLOCK"
12.1951 loops per second: USB-Mode, only servo 12 switched off because of problems (no longer the whole leg with servo 12 like above)
12.5 loops per second: even removing 1600ms waiting time in /lib/lss/LssTransaction.cpp did not change the timing!
Awesome @jangunhed! I am glad you got your loop figured and I was able to help. I appreciate your analysis. Do I understand correctly that the serial cable is direct TTL serial so without USB?
I am not surprised removing the 1600ms wouldnt have a significant affect on USB since it is already ~8ms for round trip time. IIRC The 1600ms is a scheduled wait and not a block wait so it wouldnt add delay either way, it just enforces the next packet doesnt go on the wire until at least 1600ms later…it is definitely required in TTL mode where usb latency doesnt exist.
I welcome any code/API feedback. Any areas I can improve? From code, docs or examples?
). You may want to contact 
.
. The most important trick really was to set the low-latency mode, then even USB connection is OK, while the serial connection over GPIO still is a bit faster of course.