Synchronize Control Loop to Robot

[AndreasKuhner]

Hello all,
I am from the Franka Robotics Team and we offer with libfranka a way to have a very low level control. Of course, this comes with some major limitations: E.g. the user needs to send a response before the robot starts its next computation cycle. Typically, the robot does compute - sleep - compute. While the robot is sleeping, the libfranka user has time to receive the current robot state and send back the next control command. Overall within a 1khz fashion.

With Humble (compared to ROS 1), a 'standalone' control loop was introduced which was running independent from our internal robot loop. In ROS 1, the control was still timing-wise aligned with our internal loop, with Humble not anymore. Yet, it still worked most of the time good enough.
With Jazzy, a new timing behavior was introduced: Instead of having (like Humble)
take current time -> next_iteration_time = current + period -> process -> sleep until next_iteration_time
it is now
take current time -> process -> check for delay -> next_iteration_time = current + period + delay -> sleep until next_iteration_time
What does it mean? The loop can start e.g. at 1 then 2 then 3.1 then 4.1 then 5.3 then ... aka it can drift. With humble it was just 1, 2, 3, 4, 5, ....
The new behavior now causes that the loop of ROS 2 and our robot control loop drift apart - leading eventually to a 'communication_constraint_violation' because the new commands of the user don't arrive in time anymore (aka they arrive not anymore during the sleep but compute time).

Describe the solution you'd like
The perfect solution would be if all of the 'sleep until' behavior is neglected and is just part of the read of the hardware. E.g. our read blocks until a new robot state arrives - naturally aligning the user control loop with our internal one. Of course, the read already blocks but with ROS 2 it is block + sleep until instead of just block.

Describe alternatives you've considered
An alternative is to make the new loop/sleep until logic configurable (aka allowing to choose between old and new humble behavior).

Another option I shortly looked into was to use the use_sim_time... But it would just be a workaround and not nicely integratable with other hardware.

A lot of people ask us to finally offer a Jazzy integration of our FR3 robot. But this behavior proofs now to be some major blocker.

Btw. I also tried to use the is_async for hardware_interfaces. It decouples the control loop from the hardware_interface. However, it also decouples the 'fixed' control cycle between robot and user causing quickly some discontinuities because some commands are send double/not at all/too late and out of sync (I mean.. it is async 😅 )

Cheers to all,
Andreas

[saikishor]

Hello @AndreasKuhner!

Thank you for opening an issue

With Jazzy, a new timing behavior was introduced: Instead of having (like Humble)
take current time -> next_iteration_time = current + period -> process -> sleep until next_iteration_time
it is now
take current time -> process -> check for delay -> next_iteration_time = current + period + delay -> sleep until next_iteration_time

Where is the check_for_delay that you are taking about?

Humble implementation:

ros2_control/controller_manager/src/ros2_control_node.cpp

Lines 127 to 136 in 05bc27e

	// wait until we hit the end of the period
	next_iteration_time += period;
	if (use_sim_time)
	{
	cm->get_clock()->sleep_until(current_time + period);
	}
	else
	{
	std::this_thread::sleep_until(next_iteration_time);
	}

Jazzy and later distros implementation:

ros2_control/controller_manager/src/ros2_control_node.cpp

Lines 154 to 172 in 1845171

	next_iteration_time += period;
	const auto time_now = std::chrono::steady_clock::now();
	if (next_iteration_time < time_now)
	{
	const double time_diff =
	static_cast<double>(
	std::chrono::duration_cast<std::chrono::nanoseconds>(time_now - next_iteration_time)
	.count()) /
	1.e6;
	const double cm_period = 1.e3 / static_cast<double>(cm->get_update_rate());
	const int overrun_count = static_cast<int>(std::ceil(time_diff / cm_period));
	RCLCPP_WARN_THROTTLE(
	cm->get_logger(), *cm->get_clock(), 1000,
	"Overrun detected! The controller manager missed its desired rate of %d Hz. The loop "
	"took %f ms (missed cycles : %d).",
	cm->get_update_rate(), time_diff + cm_period, overrun_count + 1);
	next_iteration_time += (overrun_count * period);
	}
	std::this_thread::sleep_until(next_iteration_time);

If this is not the right part, can you please point to the part of the code?

Thank you!

[AndreasKuhner]

Hi @saikishor ,
yes, you found the part. Especially

ros2_control/controller_manager/src/ros2_control_node.cpp

Line 170 in 45e548c

next_iteration_time += (overrun_count * period);

.

I can understand, that the Jazzy variant makes sense as well because with humble, the next iteration could be less than e.g. 1ms if the last iteration took 1.1ms. However.... :)

(At least I hope I understood it correctly and explains why our robot can run stable with humble but not jazzy...)

Cheers,
Andreas

[saikishor]

I can understand, that the Jazzy variant makes sense as well because with humble, the next iteration could be less than e.g. 1ms if the last iteration took 1.1ms. However.... :)

(At least I hope I understood it correctly and explains why our robot can run stable with humble but not jazzy...)

Hello!

If it is that part, that means overruns are happening on the robot, which shouldn't happen. If you are in the situation where the overruns are ocurring, then you need to find the root cause and fix it over there. The added logic is to protect that scenario.

To give you an idea, if in an iteration it took 10 ms instead of 1 ms (lets say the usual total cycle time is around 0.01ms), that means the next cycles will be triggered at a relatively very high frequency(given it goes as the total cycle time as 0.01ms, then you will have around 900-1000 cycles before you recover your usual periodicity). This is there to exactly protect those cases.

[AndreasKuhner]

The overruns are 'fine'. The robot is running on its own PC (our controller) and the ros2_control hardware_interface wraps libfranka. libfranka is a library to send TCP/UDP packages back-and-forth for receiving the robot states and sending commands.

The robot itself is controlled on its own and is supervised by the safety to uphold its 1khz. libfranka now syncs in with the read/write. Meaning, the hardware_interface (aka libfranka) is not running the hardware but the hardware_interface is part of the control cycle of the hardware.

Long text, more input: The warnings of the overruns I see are in the range of 1 us to 20 us. It is not much but if you let the robot run for more than 5 minutes, it builds up and leads to a desync (aka robot commands don't arrive at the correct time anymore).

Important other info: Our read blocks to receive the latest robot state. Meaning, it could very well be that the control loop of ros2_control mostly blocks due to the read. The rest of the example is super lightweight.. mostly forwarding some simple sinus signal (e.g. https://github.com/frankarobotics/franka_ros2/blob/humble/franka_example_controllers/src/joint_impedance_example_controller.cpp)

[saikishor]

I don't think the overruns are fine at all. In your case, if your blocked read cycle takes already most of the cycle time, then either you reduce the loop rate or run it asynchronously. I only see 2 options here.

[saikishor]

@AndreasKuhner you can also do one more thing, just copy the contents of this ros2_control_node on your packages and remove the fix that is troubling you. This would also work for your case.

[AndreasKuhner]

I think two different concepts on how to do real-time control clash:
For our FR3, we have a fixed scheduled real-time loop running. The special part about our libfranka control interface is that the user is closing the control loop of the robot. Meaning: robot computes -> sends new robot state -> user receives, computes, sends back next command -> robot computes again.
However: The robot computes part is on a fixed schedule. Meaning, it will always send the new robot state at a fixed interval and expects the next user command before a certain deadline (aka before it starts computing again).

Lets assume the fixed control loop of ros2_control starts at timepoint x.0 ms and finishes at x.0 ms + 1 ms. And lets assume the robot control loop finishes at x.7 ms and resumes at x.7 ms + 0.8 ms (aka 2 independently running control loops). Due to many many hardware factors, it means that the ros2_control loop will most of the time finish before reaching x.0 ms + 1 ms but often enough not. With jazzy that means that we lose 1 complete cycle because it will then sleep until x.0 ms + 2 ms. Due to this part:

const int overrun_count = static_cast<int>(std::ceil(time_diff / cm_period));
next_iteration_time += (overrun_count * period);

This 'overrun' has nothing to do with the user not being able to compute all parts or the hardware was not able to send the new robot state in time... it has mostly to do that the robot runs on a fixed schedule and ros2_control as well. And it is arbitrary with what offset they both run. With humble, it 'didn't matter' because there was no penalty for it, with jazzy, the new overrun mechanism removes a whole cycle - which is a no go if you do position or velocity control. With a torque control, it is 'okish' but still bad.

Additional update: We did some further testing and if we remove this overrun mechanism, the performance is similar to the one from humble.

@AndreasKuhner you can also do one more thing, just copy the contents of this ros2_control_node on your packages and remove the fix that is troubling you. This would also work for your case.

I guess it is a workaround but I would very much like to depend on the main jazzy branch and not on our 'own' version - that clashes a bit with a normal installation.

[saikishor]

And lets assume the robot control loop finishes at x.7 ms and resumes at x.7 ms + 0.8 ms (aka 2 independently running control loops). Due to many many hardware factors, it means that the ros2_control loop will most of the time finish before reaching x.0 ms + 1 ms but often enough not.

@AndreasKuhner Do you mind explaining this part again?

This 'overrun' has nothing to do with the user not being able to compute all parts or the hardware was not able to send the new robot state in time... it has mostly to do that the robot runs on a fixed schedule and ros2_control as well

What do you mean it has nothing to do with the user not being able to compute all parts?. For instance, if a user deployed an MPC controller, which runs at a much lower rate, as its execution time is around 5 ms. What will happen?. I understand that your case, as most of the cycle is occupied by (read + update + write), and the sleep in the ros2_control_node is not significant for you, so the effect of overruns is not that visible for you. However, what about the systems that do not occupy much of their time?. Can you share a plot of your total cycle time?. You should be able to plot it with the new /controller_manager/statistics/names and /controller_manager/statistics/values topic in plotjuggler. Post that, we can think about having a way to handle it for you.

Thank you

[AndreasKuhner]

@AndreasKuhner Do you mind explaining this part again?

Our Master Controller as well as the ros2_control cycles are on a fixed schedule. Aka they both always start at the same time point (fixed schedule). Yet, if one is unlucky, the 2 schedules produce and need data at 'bad timepoints'. E.g. in the graph, the internal computations generate a new robot state shortly before the end of the ros2_control cycle. Due to network traffic (and other hardware factors), the new robot state doesn't arrive immediately to the user/ros2_control cycle but needs a bit of time (usually around 200 us). Then the update happens and the write to send the next robot command... which will arrive in-time for the next iteration of the internal computations.
However, with the overrun mechanic, it means that we 'overrun' because we are still updating and writing. Also meaning, with the overrun, it doesn't continue but sleeps another cycle - completely losing a whole cycle.

However, what about the systems that do not occupy much of their time?

I guess we talk here about two concepts -> one control which is in sync with the hardware and one control which is asynchronous to the hardware. We have the first case - hence, it would be optimal if the ros2_control cycle would sync with us (because we can't sync with the ros2_control cycle).

Can you share a plot of your total cycle time?. You should be able to plot it with the new /controller_manager/statistics/names and /controller_manager/statistics/values topic in plotjuggler. Post that, we can think about having a way to handle it for you.

We will check!

[saikishor]

@AndreasKuhner Thank you for the explanation. I understand the need. I have a similar idea to do it along the lines of #2540, I'll try to have it soon. Once you have the plots, please share them here. It would be very useful. Thank you.

[AndreasKuhner]

Do you mean these numbers? (btw. nice to also see now the actual value plotable! Cool feature)

[saikishor]

@AndreasKuhner not that one. That topic has all the internal data published. You will need the new /controller_manager/statistics/names and /controller_manager/statistics/values topic introduced in #2449. This got backported to Jazzy, however, it is not tagged yet. You will need to compile locally and check it

It should look something like below:

[AndreasKuhner]

Okay, got it. If I outcomment the overrun stuff (if not, we go quickly into violation...), we get this: