Reset the PIDs states when controller is at rest

[KmakD]

This feature can help when it is impossible to send commands to hardware but the controller may receive state values.

To justify that let me give an example. You have a robot that has a hardware e-stop. When it is in e-stop state the controller receives a false reading (e.g. someone manually moves the robot). This increases the integral part of the PID, which sets the output to some fixed value. If the robot is smart it won't let you reset the e-stop when it receives non zero command, if not it will move unexpectedly as soon as e-stop is reset. This functionality will reset the PID if the controller is at rest (error and input is 0) for a specified time preventing that.

To send us a pull request, please:

• Fork the repository.
• Modify the source; please focus on the specific change you are contributing. If you also reformat all the code, it will be hard for us to focus on your change.
• Ensure local tests pass. (colcon test and pre-commit run (requires you to install pre-commit by pip3 install pre-commit)
• Commit to your fork using clear commit messages.
• Send a pull request, answering any default questions in the pull request interface.
• Pay attention to any automated CI failures reported in the pull request, and stay involved in the conversation.

[christophfroehlich]

in your example, when should the controller be reset? if the robot is moved manually, the error won't be zero in the end right?

and in the case where the input is zero, and with some integral action the error also converges to zero. If the controller will be reset, the robot will jump/sag?

[bmagyar]

@christophfroehlich @KmakD shouldn't we consider this in the joint_trajectory_controller too?

[codecov]

Codecov Report

Attention: Patch coverage is 96.00000% with 1 line in your changes missing coverage. Please review.

Project coverage is 84.79%. Comparing base (496e39d) to head (436c638).
Report is 6 commits behind head on master.

Files with missing lines	Patch %	Lines
pid_controller/src/pid_controller.cpp	87.50%	0 Missing and 1 partial ⚠️

Additional details and impacted files

@@            Coverage Diff             @@
##           master    #1666      +/-   ##
==========================================
+ Coverage   84.77%   84.79%   +0.02%     
==========================================
  Files         127      127              
  Lines       12096    12121      +25     
  Branches     1036     1038       +2     
==========================================
+ Hits        10254    10278      +24     
  Misses       1503     1503              
- Partials      339      340       +1     

Flag	Coverage Δ
unittests	84.79% <96.00%> (+0.02%)	⬆️

Flags with carried forward coverage won't be shown. Click here to find out more.

Files with missing lines	Coverage Δ
pid_controller/test/test_pid_controller.cpp	100.00% <100.00%> (ø)
pid_controller/test/test_pid_controller.hpp	86.13% <ø> (ø)
pid_controller/src/pid_controller.cpp	72.69% <87.50%> (+0.49%)	⬆️

🚀 New features to boost your workflow:

• ❄️ Test Analytics: Detect flaky tests, report on failures, and find test suite problems.

[christophfroehlich]

I have objections in this approach, see my unanswered question

[KmakD]

Sorry for late respone, I had a few days off.

in your example, when should the controller be reset?

The robot should be reset only when the input is zero and the error is zero.

if the robot is moved manually, the error won't be zero in the end right?

Assuming velocity control, when the robot is no longer moved, the velocity feedback becomes zero and the error will be also zero.

and in the case where the input is zero, and with some integral action the error also converges to zero. If the controller will be reset, the robot will jump/sag?

I'm not sure if I fully understans this, If controller is reset when the input and error are 0, the output will also be 0, so there is no way it will jump/sag

[saikishor]

I agree with @christophfroehlich

Zero itself is a valid reference. IMHO using that to reset the PIDs is not a smart decision

[KmakD]

I agree, using reference values is not a perfect solution, but I can't think of a better way to detect this.

There was once an idea to add emergency-stop feature in ros2_control that can be set in hardware component and propagated to all controllers (as a flag if I remember correctly). That would be perfect in this case. What happened to that idea?

[Juliaj]

is “Configuration/Movement/Safety-critical" interfaces definition ( ros-controls/roadmap#51) relevant for this scenario ?

[KmakD]

If I understand this correctly this will only allow to configure command interfaces depending if hardware component is active/inactive. In controller we would like to be able to read hardware interface state and act if it was deactivated/halted, or have some global flag that indicates hardware e-stop was triggerd

[christophfroehlich]

and in the case where the input is zero, and with some integral action the error also converges to zero. If the controller will be reset, the robot will jump/sag?

I'm not sure if I fully understans this, If controller is reset when the input and error are 0, the output will also be 0, so there is no way it will jump/sag

Let's think of an effort-controlled robot arm, where the reference is velocity. steady-state for zero velocity input (and error) will be a non-zero effort for gravity compensation.

I understand that your approach might make sense for some robots, but not in general. This means that your addition should be optional, and not default on. setting the default value of zero_threshold_duration to .inf should do this.

To justify that let me give an example. You have a robot that has a hardware e-stop. When it is in e-stop state the controller receives a false reading (e.g. someone manually moves the robot).

Shouldn't your hardware component go in inactive state if the e-stop is hit?

emergency-stop feature is discussed again and again, but no one is working on it right now afaik.

[KmakD]

I understand that your approach might make sense for some robots, but not in general. This means that your addition should be optional, and not default on. setting the default value of zero_threshold_duration to .inf should do this.

It's already optional, setting zero_threshold to 0.0 (the default value) disables it.

Shouldn't your hardware component go in inactive state if the e-stop is hit?

It should, but right now both state and command interfaces will be available for controller so it will keep making computations even if the hardware is inactive.

[christophfroehlich]

I understand that your approach might make sense for some robots, but not in general. This means that your addition should be optional, and not default on. setting the default value of zero_threshold_duration to .inf should do this.

It's already optional, setting zero_threshold to 0.0 (the default value) disables it.

Ok, this is implicit included here what I haven't seen in first place:
if (std::abs(reference_interfaces_[i]) < zero_threshold && std::abs(error) < zero_threshold)
As comparisons agains zero might fail, could you please add an additional check against eps, like we do here?

Shouldn't your hardware component go in inactive state if the e-stop is hit?

It should, but right now both state and command interfaces will be available for controller so it will keep making computations even if the hardware is inactive.

This will change soon, see ros-controls/ros2_control#2334

[saikishor]

Hello @KmakD

I'm still not sure if this makes sense. What happens if the reference for my arm is zero, then when the PID is tracking it for the zero threshold duration and then it resets and then the arm might fall right? . How do you foresee to correct this issue?. Moreover, when the zero threshold is a parameter, and how it is integrated currently, if I set it to 1, then any value less than 1 will have this effect.

[saikishor]

Can a service to reset the PIDs solve your issue?

To justify that let me give an example. You have a robot that has a hardware e-stop. When it is in e-stop state the controller receives a false reading (e.g. someone manually moves the robot). This increases the integral part of the PID, which sets the output to some fixed value. If the robot is smart it won't let you reset the e-stop when it receives non zero command, if not it will move unexpectedly as soon as e-stop is reset. This functionality will reset the PID if the controller is at rest (error and input is 0) for a specified time preventing that.

As per your description, I personally don't think this is a good approach. Maybe what you need to check is irrespective of the command you are reproducing, the error keeps increasing or your error is huge and not converging

[mergify]

This pull request is in conflict. Could you fix it @KmakD?