🚀 Add PID controller 🎉

[destogl]

Proposal for a new controller that provides PID functionality.

The controller can be used stand-alone, in a chain. Measured states can be provided through the topic or state interface, depening on the concrete control arch. For example, if you are having an external estimator, using topic might be eaiser at first then full integration into ros2_control.

The PR depends on changes in ros-controls/control_msgs#69.

Looking forward for your feedback!

[codecov-commenter]

Codecov Report

Merging #434 (d0cb6e2) into master (23f944f) will increase coverage by 0.60%. The diff coverage is n/a.

:exclamation: Current head d0cb6e2 differs from pull request most recent head 40b9360. Consider uploading reports for the commit 40b9360 to get more accurate results

Additional details and impacted files

@@            Coverage Diff             @@
##           master     #434      +/-   ##
==========================================
+ Coverage   46.73%   47.33%   +0.60%     
==========================================
  Files          40       40              
  Lines        3640     3640              
  Branches     1718     1718              
==========================================
+ Hits         1701     1723      +22     
+ Misses        749      725      -24     
- Partials     1190     1192       +2     

Flag	Coverage Δ
unittests	47.33% <ø> (+0.60%)	:arrow_up:

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

see 2 files with indirect coverage changes

[mergify[bot]]

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

[destogl]

It does not compile how it is now, see comments in code.

Could you please add a proper documentation of this controller in docs/userdoc.rst for our sphinx docs? ;)

I see that the proposed pid_controller has some different options, but there is a subset of problems which can be identically solved by pid_controller and JTC if I'm right (the JTC could also be used as a simple PID controller with a single reference point coming from command topic). If so, please indicate this also in the docs and in which case to use what, otherwise this possibly will confuse our users.

done

[wfetter]

Dears:

I used the pid_controller (standalone) for some weeks in a mobile robot application. It worked without any problem, but I have some comments:

1 - I don't think that the comments on the beginning of the source code about when to use PI, PD, or PID are correct from the theory of control systems point of view. I would say that the best type of controller depends on the dynamic characteristics I want to give to the closed loop system. For example, If I want null steady state error or a compliant controller. I would remove those comments as they are misleading.

2 - Most of the controller parameters are defined as read-only. That means that they need to be set when loading the controller manager. Usually, I set up my systems so that the loading of the controller manager and the loading of the controllers are done in two different launch files. Hence, the configuration of the controller manager is separate from the configuration of the controllers themselves, so that each controller has its own configuration and launch files. This way, I can implement and configure new controllers without having to mess with the already configured system and controller manager. Unfortunately that does not work with pid_controller, because the read-only parameters should be loaded with the controller-manager. Perhaps it is problem with the controller spawner. I think that even the read-only parameters should configured when the controller is loaded by the spawner.

3 - The feedforward input could be more generic. Instead of tying it to the reference it could be left as another generic input, which is multiplied by its gain and added to he output. This way it could be used to implement, for example, a PID with gravity compensation.

[christophfroehlich]

1 - I don't think that the comments on the beginning of the source code about when to use PI, PD, or PID are correct from the theory of control systems point of view. I would say that the best type of controller depends on the dynamic characteristics I want to give to the closed loop system. For example, If I want null steady state error or a compliant controller. I would remove those comments as they are misleading.

I totally agree with you!

3 - The feedforward input could be more generic. Instead of tying it to the reference it could be left as another generic input, which is multiplied by its gain and added to he output. This way it could be used to implement, for example, a PID with gravity compensation.

This would be a nice feature. Maybe it would make sense to extend the controller chains with parallel paths for creating such two-degrees-of-freedom control architectures. But I have no glue if this is feasible to implement.

[destogl]

2 - Most of the controller parameters are defined as read-only. That means that they need to be set when loading the controller manager. Usually, I set up my systems so that the loading of the controller manager and the loading of the controllers are done in two different launch files. Hence, the configuration of the controller manager is separate from the configuration of the controllers themselves, so that each controller has its own configuration and launch files. This way, I can implement and configure new controllers without having to mess with the already configured system and controller manager. Unfortunately that does not work with pid_controller, because the read-only parameters should be loaded with the controller-manager. Perhaps it is problem with the controller spawner. I think that even the read-only parameters should configured when the controller is loaded by the spawner.

Your problem is easy to solve. Why don't you put your controller configuration in a separate YAML file and give a path to this file to the spawner. Would this work? It sounds to me that is even better approach on how you are setting up and using stuff.

[destogl]

3 - The feedforward input could be more generic. Instead of tying it to the reference it could be left as another generic input, which is multiplied by its gain and added to he output. This way it could be used to implement, for example, a PID with gravity compensation.

~~I am adding this to the reference? This was not the goal. It was the goal to do exactly what you are describing.~~

Actually, this is like a “school” example of feed-forward. I am not certain that I understand what you are trying to do, and would you like to have – I can think of another better approach.

[gwalck]

I tried to use PIDControllers as following controller of a modified DiffDrive (to enable to send commands to an exported interface of the PID controller) in a gazebo classic environment.

The activation of the set of diffDrive+PID controllers kept leading to a robot "breaking" (robot is set at origin of gazebo world). It took a while to debug (because I could not find a way to pass --ros-args --log-level debug to gazebo) but I found that the culprit is the setting of quiet_Nan to the command_interface https://github.com/StoglRobotics-forks/ros2_controllers/blob/add-pid-controller/pid_controller/src/pid_controller.cpp#L363

Here is the chain of events:

1. gazebo starts with ros2_control, all hardware_interfaces are ready
2. ros2_control broadcaster for joint_state reports clean joints, no NaN values
3. PIDController starts, has NaN as reference and hence sets NO command_interface values yet
4. DiffDrive controller starts, and because it is chained to PID controller, 4.1. makes the ControllerManager deactivate the PIDController, 4.2 set it in chained _mode, 4.3 re-activate it. => At that point the command_interface of the PIDController is a NaN,
5. Unfortunately NaN is then sent to gazebo making the robot break.
6. gazebo sends back an NaN to the state_interfaces
7. state_interface has a NaN, which makes the DiffDrive also fail to compute anything clean for the chain to be recovered.

We are indefinitely in NaN state everywhere.

I personally don't find correct that a controller sets its command_interface with anything invalid. The fact that it checks for its input to be valid is fair, but setting output values to NaN is strange to me.

I am used to have a on_activate (starting in ros_control ) either set the command to a sensible value (example 0.0) or read the feedback state and set the command to the feed_back.

some other discussions that had the word quiet_NaN (https://github.com/ros-controls/ros2_control/issues/674#issuecomment-1077610485) tends to suggest that invalidating a command_interface should be done with a NaN.

Then gazebo_ros2_control should be protected from setting physics state with NaN and allow the control loop to "settle" to something valid as long as gazebo has valid readings sent back to ros2_control

So globally something is wrong either in the PIDController or gazebo_ros2_control is not adapted to the desired behavior of controllers.

[destogl]

PID controller should not set values on NaN on deactivate. We should change this here. Fixed in ff17e8a – please check.

[gwalck]

@destogl thanks for the fix, but I still wonder what is the correct procedure in on_activate. Should the controller initialize the command to the current state (and ignore any unset reference) ?

[destogl]

@destogl thanks for the fix, but I still wonder what is the correct procedure in on_activate. Should the controller initialize the command to the current state (and ignore any unset reference) ?

I think this is the safest. We should move like this and seeing this cases any issues - then we know better how to optimize.

[AndyZe]

It sounds like this would be a good usage of std::optional. std::optional can easily flag if a valid value exists or not

[xy-23]

Hi, I'm interested in this controller but failed to build it.

parallels@ubuntu:~/Desktop/pid_ws$ colcon build --symlink-install --allow-overriding control_msgs control_toolbox
Starting >>> control_msgs
Finished <<< control_msgs [1.11s]                     
Starting >>> control_toolbox
Finished <<< control_toolbox [0.23s]                    
Starting >>> pid_controller
--- stderr: pid_controller                             
In file included from /home/parallels/Desktop/pid_ws/src/pid_controller/src/pid_controller.cpp:18:
/home/parallels/Desktop/pid_ws/src/pid_controller/include/pid_controller/pid_controller.hpp:80:37: error: ‘controller_interface::return_type pid_controller::PidController::update_reference_from_subscribers(const rclcpp::Time&, const rclcpp::Duration&)’ marked ‘override’, but does not override
   80 |   controller_interface::return_type update_reference_from_subscribers(
      |                                     ^~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
/home/parallels/Desktop/pid_ws/src/pid_controller/src/pid_controller.cpp: In member function ‘virtual controller_interface::CallbackReturn pid_controller::PidController::on_configure(const rclcpp_lifecycle::State&)’:
/home/parallels/Desktop/pid_ws/src/pid_controller/src/pid_controller.cpp:187:87: error: no matching function for call to ‘rclcpp_lifecycle::LifecycleNode::create_service<pid_controller::PidController::ControllerModeSrvType>(const char [26], pid_controller::PidController::on_configure(const rclcpp_lifecycle::State&)::<lambda(std::shared_ptr<std_srvs::srv::SetBool_Request_<std::allocator<void> > >, std::shared_ptr<std_srvs::srv::SetBool_Response_<std::allocator<void> > >)>&, rclcpp::QoS&)’
  187 |   set_feedforward_control_service_ = get_node()->create_service<ControllerModeSrvType>(
      |                                      ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~^
  188 |     "~/set_feedforward_control", set_feedforward_control_callback, qos_services);
      |     ~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~       
In file included from /opt/ros/humble/include/controller_interface/controller_interface_base.hpp:29,
                 from /opt/ros/humble/include/controller_interface/chainable_controller_interface.hpp:20,
                 from /home/parallels/Desktop/pid_ws/src/pid_controller/include/pid_controller/pid_controller.hpp:28,
                 from /home/parallels/Desktop/pid_ws/src/pid_controller/src/pid_controller.cpp:18:
/opt/ros/humble/include/rclcpp_lifecycle/rclcpp_lifecycle/lifecycle_node.hpp:275:3: note: candidate: ‘template<class ServiceT, class CallbackT> typename rclcpp::Service<ServiceT>::SharedPtr rclcpp_lifecycle::LifecycleNode::create_service(const string&, CallbackT&&, const rmw_qos_profile_t&, rclcpp::CallbackGroup::SharedPtr)’
  275 |   create_service(
      |   ^~~~~~~~~~~~~~
/opt/ros/humble/include/rclcpp_lifecycle/rclcpp_lifecycle/lifecycle_node.hpp:275:3: note:   template argument deduction/substitution failed:
/home/parallels/Desktop/pid_ws/src/pid_controller/src/pid_controller.cpp:188:68: note:   cannot convert ‘{anonymous}::qos_services’ (type ‘rclcpp::QoS’) to type ‘const rmw_qos_profile_t&’ {aka ‘const rmw_qos_profile_s&’}
  188 |     "~/set_feedforward_control", set_feedforward_control_callback, qos_services);
      |                                                                    ^~~~~~~~~~~~
In file included from /opt/ros/humble/include/class_loader/class_loader/class_loader_core.hpp:57,
                 from /opt/ros/humble/include/class_loader/class_loader/class_loader.hpp:55,
                 from /opt/ros/humble/include/pluginlib/pluginlib/class_list_macros.hpp:40,
                 from /home/parallels/Desktop/pid_ws/src/pid_controller/src/pid_controller.cpp:487:
/opt/ros/humble/include/class_loader/class_loader/meta_object.hpp: In instantiation of ‘B* class_loader::impl::MetaObject<C, B>::create() const [with C = pid_controller::PidController; B = controller_interface::ChainableControllerInterface]’:
/opt/ros/humble/include/class_loader/class_loader/meta_object.hpp:216:7:   required from here
/opt/ros/humble/include/class_loader/class_loader/meta_object.hpp:218:12: error: invalid new-expression of abstract class type ‘pid_controller::PidController’
  218 |     return new C;
      |            ^~~~~
In file included from /home/parallels/Desktop/pid_ws/src/pid_controller/src/pid_controller.cpp:18:
/home/parallels/Desktop/pid_ws/src/pid_controller/include/pid_controller/pid_controller.hpp:52:7: note:   because the following virtual functions are pure within ‘pid_controller::PidController’:
   52 | class PidController : public controller_interface::ChainableControllerInterface
      |       ^~~~~~~~~~~~~
In file included from /home/parallels/Desktop/pid_ws/src/pid_controller/include/pid_controller/pid_controller.hpp:28,
                 from /home/parallels/Desktop/pid_ws/src/pid_controller/src/pid_controller.cpp:18:
/opt/ros/humble/include/controller_interface/chainable_controller_interface.hpp:100:23: note:     ‘virtual controller_interface::return_type controller_interface::ChainableControllerInterface::update_reference_from_subscribers()’
  100 |   virtual return_type update_reference_from_subscribers() = 0;
      |                       ^~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
gmake[2]: *** [CMakeFiles/pid_controller.dir/build.make:76: CMakeFiles/pid_controller.dir/src/pid_controller.cpp.o] Error 1
gmake[1]: *** [CMakeFiles/Makefile2:208: CMakeFiles/pid_controller.dir/all] Error 2
gmake: *** [Makefile:146: all] Error 2
---
Failed   <<< pid_controller [2.77s, exited with code 2]

Summary: 2 packages finished [4.22s]
  1 package failed: pid_controller
  1 package had stderr output: pid_controller

[christophfroehlich]

Hi, I'm interested in this controller but failed to build it.

you have to install/build rolling version of ros2_control, but you have humble.

[xy-23]

you have to install/build rolling version of ros2_control, but you have humble.

Thanks! I wonder if the pid controller will be available on humble after this PR get merged.

[xy-23]

Hi, I'm trying to use it on a continuous joint with reference position value [0, 2π). It works fine such as 0.5π to π or vice versa. However, it behaves badly such as 0.5π to 0, because it is possible to suddenly jump from 0 to a value near 2π and causes large value from the error calculation. I guess the fix would be having some params in the controller like continuous mode so we can use another way to calculate the error?

[christophfroehlich]

Hi, I'm trying to use it on a continuous joint with reference position value [0, 2π). It works fine such as 0.5π to π or vice versa. However, it behaves badly such as 0.5π to 0, because it is possible to suddenly jump from 0 to a value near 2π and causes large value from the error calculation. I guess the fix would be having some params in the controller like continuous mode so we can use another way to calculate the error?

I'm not sure what you are testing having a look at your legend (reference is constant?). But it is true that the feature does not exist here. I added the option you are looking for to the joint trajectory controller, called normalize_error.

[xy-23]

I'm not sure what you are testing having a look at your legend (reference is constant?).

Oh, the forward_position_controller is actually a pid controller (reference is position, state is velocity and command is velocity). I ported it from another file and forgot to change its name. And yes, reference is a constant at position 0.

But it is true that the feature does not exist here. I added the option you are looking for to the joint trajectory controller, called normalize_error.

Thanks, I'll look it.

[christophfroehlich]

I'm not sure what you are testing having a look at your legend (reference is constant?).

Oh, the forward_position_controller is actually a pid controller (reference is position, state is velocity and command is velocity). I ported it from another file and forgot to change its name. And yes, reference is a constant at position 0.

why is it moving if you have a constant reference?

[xy-23]

why is it moving if you have a constant reference?

According to this draft Design proposals for controllers, the reference is the input goal of the pid controller and the feedback is measured by the sensor(encoder on my motor). While the reference is constant, the feedback are varying due to the random noise or overshoot by the poor pid gains, causing the change of the error.

[christophfroehlich]

why is it moving if you have a constant reference?

According to this draft Design proposals for controllers, the reference is the input goal of the pid controller and the feedback is measured by the sensor(encoder on my motor).

This is correct.

While the reference is constant, the feedback are varying due to the random noise or overshoot by the poor pid gains, causing the change of the error.

You mean it should maintain a constant position (reference is constantly zero) but gets instable? Your initial description was different.

[xy-23]

You mean it should maintain a constant position (reference is constantly zero) but gets instable?

Yes, its starting position is 0.5π, after sending the reference 0, it should rotate to position 0 and stay at there. However, as the plot shows, the reference is 0 but the position is unstable, and is amplified by the wrong error.

Your initial description was different.

Maybe my description is not clear.

[christophfroehlich]

You mean it should maintain a constant position (reference is constantly zero) but gets instable?

Yes, its starting position is 0.5π, after sending the reference 0, it should rotate to position 0 and stay at there. However, as the plot shows, the reference is 0 but the position is unstable, and is amplified by the wrong error.

Your initial description was different.

Maybe my description is not clear.

Thanks for clarification. Then the problem is, that your position encoder is limited to [0, 2π), and the PID position controller turns in the wrong direction if it ever turns left of 0. I think this is what you were describing anyways -> one has to normalize the position error as we did it with the JTC.

[xy-23]

Then the problem is, that your position encoder is limited to [0, 2π), and the PID position controller turns in the wrong direction if it ever turns left of 0.

Exactly, thanks your solution, I'll try it. But I do hope to see this normalize error option in the pid controller, because I think the pid controller is more easy for me to understand and tackle with (JTC bring somethings like interpolation feel too heavy for me). Maybe I can help to implement, not sure though.

[xy-23]

The param runtime_param_update seems not working? I set it to true and activated the pid controller first with p equals 1 and changed p to 10, but not seeing any response change. If I reactivate the pid controller (p=10), I can do see the response change.

[xfiderek]

Hello! Is there any progress on this functionality? I would find it very handy for my current project.

If there are some points left I am happy to contribute. Just let me know what is the status and I can help.

[christophfroehlich]

Can we backport this to iron and humble?