Fixed time periods for update method

[AndreasKuhner]

Is your feature request related to a problem? Please describe.
For our Franka Robotics robot FR3 and franka_ros2 package, we want to write an example which is based on the provided time by the update method. However, this time has a varying value - going from 0.8ms to 1.2ms (in our 1kHz loop case). Yet, if we want to 'scale' our commands according to those numbers, we create to big steps in the backend.

E.g. if you operate our robot on the maximal allowed change per ms and you start to scale it by 1.2, you will end up with bigger than allowed steps, throwing an error... which is unnecessary because our control loop in the backend is anyways running with a fixed step time and doesn't depend on the cycle time in ros2_control.

An easy example for the problem is one of our example controller: The position control example uses a sinus function - super easy but depends heavily on the absolut accumulated time. The expectation would be to receive a sinus signal with fixed time steps.

Describe the solution you'd like
Allowing to configure a fixed time per cycle. E.g. for a 1kHz loop it would be a fixed time step of 1ms (aka the period is always 1ms or 2ms+ if for some reasons cycles are skipped/blocked/...).

Describe alternatives you've considered
We could publish a constant fixed timestep with each robot state and use that instead of the provided period/time... which would be a bit sad due to diverging from the intended ros2_control way 😞

[saikishor]

Hello @AndreasKuhner!

If you want to use a fixed period, you can do it internally by using the get_update_rate method. This should be your desired period, and the one you get from the period arg of your update method is the real triggering that's happening and this is limited to the real-time capabilities of your system.

ros2_control/controller_interface/src/controller_interface_base.cpp

Line 203 in 8fa60db

unsigned int ControllerInterfaceBase::get_update_rate() const { return update_rate_; }

[AndreasKuhner]

Thanks for the fast reply!

The returned update there might be e.g. 1000 Hz if we run the controllers with that frequency.. right? This would yield 'only' the fixed step time but is not dynamic. Lets assume that for some reason your read method in the hardware interface is blocked because of a short burst of network trouble. This could lead to 1 or 2 cycles being 'skipped' of the update because the read is blocked during that time... The hope would be that I then get a correct period length for that blocked time. E.g. if the read was blocked for a whole 2 ms, I would get 2 ms or 3 ms. I guess one could also use the current period and do a `floor(period + 0.5) 🤔

[saikishor]

The returned update there might be e.g. 1000 Hz if we run the controllers with that frequency.. right? This would yield 'only' the fixed step time but is not dynamic. Lets assume that for some reason your read method in the hardware interface is blocked because of a short burst of network trouble. This could lead to 1 or 2 cycles being 'skipped' of the update because the read is blocked during that time... The hope would be that I then get a correct period length for that blocked time. E.g. if the read was blocked for a whole 2 ms, I would get 2 ms or 3 ms. I guess one could also use the current period and do a `floor(period + 0.5) 🤔

Before going further, can you tell me which ros2_control version and which DISTRO are you guys using?

[AndreasKuhner]

We are currently on humble and latest released ros2_control .deb - that would be 2.43.1.

[saikishor]

Hello @AndreasKuhner!

I think the issue you are describing might be kinda solved in the rolling version. I recommend you to try out the rolling branch and let us know if the issue persists.

You can compile the rolling branch of ros2_control and it's respective package using the instructions here: https://control.ros.org/humble/doc/getting_started/getting_started.html#building-from-source

Keep us posted if the issue is solved on rolling and maybe we can try to backport the change if it works.

Thank you

[AndreasKuhner]

After checking the changes.. what exactly do you mean that it might have solved the issue?

[saikishor]

After checking the changes.. what exactly do you mean that it might have solved the issue?

It doesn't solve it completely ofcourse, but you can do something like floor(period + 0.5) as you mentioned.

You are guaranteed to get the correct period, due to the following lines

ros2_control/controller_manager/src/controller_manager.cpp

Lines 2368 to 2369 in 30d51cd

	const auto controller_actual_period =
	(current_time - *loaded_controller.last_update_cycle_time);

In Humble, IIRC you only get the period of the controller manager cycle

ros2_control/controller_manager/src/controller_manager.cpp

Lines 2074 to 2077 in dcb958f

	auto controller_ret = loaded_controller.c->update(
	time, (controller_update_factor != 1u)
	? rclcpp::Duration::from_seconds(1.0 / controller_update_rate)
	: period);

After checking the changes.. what exactly do you mean that it might have solved the issue?

Why have you thought this doesn't solve it in the first place?

[AndreasKuhner]

Okey, oversaw that change, sorry 🤔

What we sometimes saw is that cycles were somehow skipped/delayed - this has for sure to do with our own hardware interfaces which block and might eat one or the other cycle - so for us it was a problem that the period didn't show then a '2ms' but was still something around '1ms'. But I guess that could be prevented now by being based on the last update cycle time... we need to investigate 😄