Controller restart

[mhubii]

Hi All, and thanks for the great work you put into this project. I have a question regarding restarting the controllers. In the previous ros_control, the controller manager was capable of restarting the controllers, see https://github.com/ros-controls/ros_control/blob/83a392bafa1ec01dc77ec2a592f3b74a783f3faa/controller_manager/src/controller_manager.cpp#L70.

So if the hardware signals the SystemInterface it was restarted, how would one reload/reset the controllers, ie how would the ControllerManager be notified? Is there a simple way to achieve this?

[mhubii]

Closing this and asking in the forum..

[mhubii]

following a discussion here https://discourse.ros.org/t/ros2-control-controller-restart/24662, a temporary solution to restarting the controllers can be achieve through #677.

I am now facing the following issue: On re-loading the controllers, it appears to me that the commanded states are not updated to the current state.

Let me elaborate. I am controlling a KUKA LBR robot through the Fast Robot Interface. The robot may change the position without the user being able to send commands. When sending commands becomes possible again, the current state is not equal to the previously commanded state. ros2_control then tries to immediately bring the robot back to the previously commanded state.

Is this the case, or do I have some cached variable somewhere else? Thank you for your help, kind regards

[mhubii]

in the SystemInterface, one may set the command interfaces to the state interfaces after re-loading the controllers to mitigate this behaviour. But not sure if this is the SystemInterface's task

[destogl]

in the SystemInterface, one may set the command interfaces to the state interfaces after re-loading the controllers to mitigate this behaviour. But not sure if this is the SystemInterface's task

IMO, this is the correct solution. Your hardware interface has the storage of values for interface, and only hardware interfaces know exact moment when interfaces (commands) should be reseted. Everything else could cause race conditions. When hardware is re-activated, I would at least invalidate command interfaces, e.g., set them to NaN (std::limits::quiet_nan()).

Ok, thinking more about this. which controller are you using and in which configuration? Most of the controllers are resetting command interfaces to states when activated. Maybe we have some bug.

[mhubii]

might definitely be some bug on my side. I am using the JointTrajectoryController, but through the MoveItSimpleControllerManager. So chances are Moveit2 causes this behavior. I'll strip away the Moveit2 stuff and check if the issue persists. Thanks for thinking it through

[destogl]

might definitely be some bug on my side. I am using the JointTrajectoryController, but through the MoveItSimpleControllerManager. So chances are Moveit2 causes this behavior. I'll strip away the Moveit2 stuff and check if the issue persists. Thanks for thinking it through

This should not actually cause this behavior. Let us know if you are still struggling.

[Tuebel]

I have struggled with a similar Problem, specifically with a KUKA iiwa, too. However I have only used ros_control in ROS 1 yet. In my case the error was, that I reset the controller but forgot to reset the safety (saturation & soft) limits. These would override the controller commands and sent old commands. This caused very unsafe and jerky motions which triggered the limits on the KUKA controller and caused it to stop with an error.

[nilp-dromeda]

I am also having the same jump in position occurring on ros2 control for humble. I am even setting the command interfaces to the state interfaces internally in on_configure method after the hardware component has been restarted. However, the robot still jumps to the last state interface that the software controllers last held. For context I am using JointTrajectoryController.

[christophfroehlich]

@nilp-dromeda can you please give more information, which lifecycle changes of which components (hardware, controller) are you performing and what is the expected result?

[mhubii]

think the problem here is that the joint trajectory controller buffers some state that deviates from the hardware.

When leaving "commanding active", the KUKAs can move back to when they initially connected without a commanding controller.

I am assuming the JTC might be open loop, causing this deviation?

[saikishor]

@mhubii Did you do the whole lifecycle transition using the controller manager services? Or just did it internally?

[mhubii]

this has been a long time ago. Currently, whenever the KUKA leaves a commanding mode, I force shut the connection so this bug cannot appear.

[nilp-dromeda]

Hi @christophfroehlich,

Basically I have written the hardware components to interface with a JTC. On the read state in the hardware component I throw a return_type::ERROR to signal a hardware error has occurred. This moves the hardware component into an unconfigured state. Now, the software side (JTC) continues to run in the background (keeps following trajectories). Now, as soon as I reactivate the hardware component side of the control stack the joints of the robot move to wherever the JTC is currently commanding in the trajectory. For example let us say there is a trajectory running that is 10 seconds long and the controller is running at 100 Hz and the trajectory has 10 points in it each that take 1 second to reach. So everything is running fine at time 0seconds, at time 4 seconds there is a hardware error and the hardware component moves into a unconfigured state. But the JTC continues to execute the trajectory as if the hardware is still "working" (which is fine and good to have), however, now my software commands the hardware component to restart - e.g. move into the active state again, and this occurs at time 10 seconds for example. The joints of my robot jump to the joint trajectory point at 10 seconds (e.g. the 10th trajectory point since the JTC continues to run and has now finished executing the trajectory).

Now the problem here is that in the on_configure method of the hardware component I am setting the command interface to be equal to the state interface, hence, the robot should stay exactly where it should be, but actually it jumps to the positions commanded by the 10th trajectory point? It seems that the JTC publishes the last command it has to any late joining subscribers maybe?

Also, as a side note, I realised that in the hardware components, there is a race condition between on_activate method and read method. I have noticed that whilst on_activate is running, the read method also runs async. However, in the on_configure method no other method is called by the controller manager - is this intentional?

[saikishor]

Hello @nilp-dromeda !

This is very strange, if you return ERROR on the hardware read cycle, the controllers that use the interfaces are supposed to be deactivated automatically. This should be the desired behaviour as per design. It is very weird if that is not the case. Can you recheck this with the current rolling version? You can compile ros2_control rolling stack without issues on older distros. Please keep us posted.

Thank you!

[nilp-dromeda]

No this is with humble. Is this a known issue in humble? Also, are you saying that you can run rolling version of ros2_control with the rest of the stack working off humble?

[saikishor]

No this is with humble. Is this a known issue in humble? Also, are you saying that you can run rolling version of ros2_control with the rest of the stack working off humble?

@mhubii you are right. In Humble, this part is not implemented: https://github.com/ros-controls/ros2_control/blob/humble/controller_manager%2Fsrc%2Fcontroller_manager.cpp#L2000-L2003. In rolling, this is handled in such a way that if the hardware reports error in read or write cycle it stops the corresponding controllers that use its interfaces.

Yes you can compile ros2_control and ros2_contorllers of rolling on humble as well to get new features. Check control.ros.org on what packages to compile

[nilp-dromeda]

Understood, thankyou @saikishor.