Skip to content

Commit 038a547

Browse files
destogldestogl
authored
🚀 Add PID controller 🎉 (ros-controls#434)
1 parent 23f944f commit 038a547

17 files changed

+2028
-0
lines changed

‎doc/controllers_index.rst‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -55,6 +55,7 @@ The controllers are using `common hardware interface definitions`_, and may use
5555
Forward Command Controller <../forward_command_controller/doc/userdoc.rst>
5656
Gripper Controller <../gripper_controllers/doc/userdoc.rst>
5757
Joint Trajectory Controller <../joint_trajectory_controller/doc/userdoc.rst>
58+
PID Controller <../pid_controller/doc/userdoc.rst>
5859
Position Controllers <../position_controllers/doc/userdoc.rst>
5960
Velocity Controllers <../velocity_controllers/doc/userdoc.rst>
6061

‎pid_controller/CMakeLists.txt‎

Lines changed: 108 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,108 @@
1+
cmake_minimum_required(VERSION 3.16)
2+
project(pid_controller LANGUAGES CXX)
3+
4+
if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)")
5+
add_compile_options(-Wall -Wextra)
6+
endif()
7+
8+
set(THIS_PACKAGE_INCLUDE_DEPENDS
9+
angles
10+
control_msgs
11+
control_toolbox
12+
controller_interface
13+
generate_parameter_library
14+
hardware_interface
15+
parameter_traits
16+
pluginlib
17+
rclcpp
18+
rclcpp_lifecycle
19+
realtime_tools
20+
std_srvs
21+
)
22+
23+
find_package(ament_cmake REQUIRED)
24+
find_package(backward_ros REQUIRED)
25+
foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS})
26+
find_package(${Dependency} REQUIRED)
27+
endforeach()
28+
29+
generate_parameter_library(pid_controller_parameters
30+
src/pid_controller.yaml
31+
)
32+
33+
add_library(pid_controller SHARED
34+
src/pid_controller.cpp
35+
)
36+
target_compile_features(pid_controller PUBLIC cxx_std_17)
37+
target_include_directories(pid_controller PUBLIC
38+
$<BUILD_INTERFACE:${PROJECT_SOURCE_DIR}/include>
39+
$<INSTALL_INTERFACE:include/pid_controller>
40+
)
41+
target_link_libraries(pid_controller PUBLIC
42+
pid_controller_parameters
43+
)
44+
ament_target_dependencies(pid_controller PUBLIC ${THIS_PACKAGE_INCLUDE_DEPENDS})
45+
46+
# Causes the visibility macros to use dllexport rather than dllimport,
47+
# which is appropriate when building the dll but not consuming it.
48+
target_compile_definitions(pid_controller PRIVATE "PID_CONTROLLER_BUILDING_DLL")
49+
50+
pluginlib_export_plugin_description_file(controller_interface pid_controller.xml)
51+
52+
if(BUILD_TESTING)
53+
find_package(ament_cmake_gmock REQUIRED)
54+
find_package(controller_manager REQUIRED)
55+
find_package(ros2_control_test_assets REQUIRED)
56+
57+
add_rostest_with_parameters_gmock(
58+
test_pid_controller
59+
test/test_pid_controller.cpp
60+
${CMAKE_CURRENT_SOURCE_DIR}/test/pid_controller_params.yaml
61+
)
62+
target_include_directories(test_pid_controller PRIVATE include)
63+
target_link_libraries(test_pid_controller pid_controller)
64+
ament_target_dependencies(
65+
test_pid_controller
66+
controller_interface
67+
hardware_interface
68+
)
69+
70+
add_rostest_with_parameters_gmock(
71+
test_pid_controller_preceding
72+
test/test_pid_controller_preceding.cpp
73+
${CMAKE_CURRENT_SOURCE_DIR}/test/pid_controller_preceding_params.yaml
74+
)
75+
target_include_directories(test_pid_controller_preceding PRIVATE include)
76+
target_link_libraries(test_pid_controller_preceding pid_controller)
77+
ament_target_dependencies(
78+
test_pid_controller_preceding
79+
controller_interface
80+
hardware_interface
81+
)
82+
83+
ament_add_gmock(test_load_pid_controller test/test_load_pid_controller.cpp)
84+
target_include_directories(test_load_pid_controller PRIVATE include)
85+
ament_target_dependencies(
86+
test_load_pid_controller
87+
controller_manager
88+
ros2_control_test_assets
89+
)
90+
endif()
91+
92+
install(
93+
DIRECTORY include/
94+
DESTINATION include/pid_controller
95+
)
96+
97+
install(TARGETS
98+
pid_controller
99+
pid_controller_parameters
100+
EXPORT export_pid_controller
101+
RUNTIME DESTINATION bin
102+
ARCHIVE DESTINATION lib
103+
LIBRARY DESTINATION lib
104+
)
105+
106+
ament_export_targets(export_pid_controller HAS_LIBRARY_TARGET)
107+
ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS})
108+
ament_package()

‎pid_controller/README.md‎

Lines changed: 7 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,7 @@
1+
pid_controller
2+
==========================================
3+
4+
Controller based on PID implementation from control_toolbox package.
5+
6+
Pluginlib-Library: pid_controller
7+
Plugin: pid_controller/PidController (controller_interface::ControllerInterface)

‎pid_controller/doc/userdoc.rst‎

Lines changed: 86 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,86 @@
1+
:github_url: https://github.com/ros-controls/ros2_controllers/blob/{REPOS_FILE_BRANCH}/pid_controller/doc/userdoc.rst
2+
3+
.. _pid_controller_userdoc:
4+
5+
PID Controller
6+
--------------------------------
7+
8+
PID Controller implementation that uses PidROS implementation from `control_toolbox <https://github.com/ros-controls/control_toolbox/>`_ package.
9+
The controller can be used directly by sending references through a topic or in a chain having preceding or following controllers.
10+
It also enables to use the first derivative of the reference and its feedback to have second-order PID control.
11+
12+
Depending on the reference/state and command interface of the hardware a different parameter setup of PidROS should be used as for example:
13+
14+
- reference/state POSITION; command VELOCITY --> PI CONTROLLER
15+
- reference/state VELOCITY; command ACCELERATION --> PI CONTROLLER
16+
17+
- reference/state VELOCITY; command POSITION --> PD CONTROLLER
18+
- reference/state ACCELERATION; command VELOCITY --> PD CONTROLLER
19+
20+
- reference/state POSITION; command POSITION --> PID CONTROLLER
21+
- reference/state VELOCITY; command VELOCITY --> PID CONTROLLER
22+
- reference/state ACCELERATION; command ACCELERATION --> PID CONTROLLER
23+
- reference/state EFFORT; command EFFORT --> PID CONTROLLER
24+
25+
.. note::
26+
27+
Theoretically one can misuse :ref:`Joint Trajectory Controller (JTC)<joint_trajectory_controller_userdoc>` for the same purpose by sending only one reference point into it.
28+
Nevertheless, this is not recommended. JTC should be used if you need to interpolate between trajectory points using linear, cubic or quintic interpolation. PID Controller doesn't do that.
29+
PID term of JTC has different purpose - it enables commanding only ``velocity`` or ``effort`` interfaces to hardware.
30+
31+
Execution logic of the controller
32+
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
33+
34+
The controller can be also used in "feed-forward" mode where feed-forward gain is used to increase controllers dynamics.
35+
If one type of the reference and state interfaces is used, only immediate error is used. If there are two, then the second interface type is considered to be the first derivative of the first type.
36+
For example a valid combination would be ``position`` and ``velocity`` interface types.
37+
38+
Using the controller
39+
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
40+
41+
Pluginlib-Library: pid_controller
42+
Plugin name: pid_controller/PidController
43+
44+
Description of controller's interfaces
45+
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
46+
47+
References (from a preceding controller)
48+
,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,
49+
- <reference_and_state_dof_names[i]>/<reference_and_state_interfaces[j]> [double]
50+
**NOTE**: ``reference_and_state_dof_names[i]`` can be from ``reference_and_state_dof_names`` parameter, or if it is empty then ``dof_names``.
51+
52+
Commands
53+
,,,,,,,,,
54+
- <dof_names[i]>/<command_interface> [double]
55+
56+
States
57+
,,,,,,,
58+
- <reference_and_state_dof_names[i]>/<reference_and_state_interfaces[j]> [double]
59+
**NOTE**: ``reference_and_state_dof_names[i]`` can be from ``reference_and_state_dof_names`` parameter, or if it is empty then ``dof_names``.
60+
61+
62+
Subscribers
63+
,,,,,,,,,,,,
64+
If controller is not in chained mode (``in_chained_mode == false``):
65+
66+
- <controller_name>/reference [control_msgs/msg/MultiDOFCommand]
67+
68+
If controller parameter ``use_external_measured_states`` is true:
69+
70+
- <controller_name>/measured_state [control_msgs/msg/MultiDOFCommand]
71+
72+
Services
73+
,,,,,,,,,,,
74+
75+
- <controller_name>/set_feedforward_control [std_srvs/srv/SetBool]
76+
77+
Publishers
78+
,,,,,,,,,,,
79+
- <controller_name>/controller_state [control_msgs/msg/MultiDOFStateStamped]
80+
81+
Parameters
82+
,,,,,,,,,,,
83+
84+
The PID controller uses the `generate_parameter_library <https://github.com/PickNikRobotics/generate_parameter_library>`_ to handle its parameters.
85+
86+
.. generate_parameter_library_details:: ../src/pid_controller.yaml

‎pid_controller/include/pid_controller/pid_controller.hpp‎

Lines changed: 141 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,141 @@
1+
// Copyright (c) 2023, Stogl Robotics Consulting UG (haftungsbeschränkt)
2+
//
3+
// Licensed under the Apache License, Version 2.0 (the "License");
4+
// you may not use this file except in compliance with the License.
5+
// You may obtain a copy of the License at
6+
//
7+
// http://www.apache.org/licenses/LICENSE-2.0
8+
//
9+
// Unless required by applicable law or agreed to in writing, software
10+
// distributed under the License is distributed on an "AS IS" BASIS,
11+
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12+
// See the License for the specific language governing permissions and
13+
// limitations under the License.
14+
//
15+
// Authors: Daniel Azanov, Dr. Denis
16+
//
17+
18+
#ifndef PID_CONTROLLER__PID_CONTROLLER_HPP_
19+
#define PID_CONTROLLER__PID_CONTROLLER_HPP_
20+
21+
#include <memory>
22+
#include <string>
23+
#include <vector>
24+
25+
#include "control_msgs/msg/multi_dof_command.hpp"
26+
#include "control_msgs/msg/multi_dof_state_stamped.hpp"
27+
#include "control_toolbox/pid_ros.hpp"
28+
#include "controller_interface/chainable_controller_interface.hpp"
29+
#include "pid_controller/visibility_control.h"
30+
#include "pid_controller_parameters.hpp"
31+
#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp"
32+
#include "rclcpp_lifecycle/state.hpp"
33+
#include "realtime_tools/realtime_buffer.h"
34+
#include "realtime_tools/realtime_publisher.h"
35+
#include "std_srvs/srv/set_bool.hpp"
36+
37+
#include "control_msgs/msg/joint_controller_state.hpp"
38+
#include "control_msgs/msg/joint_jog.hpp"
39+
40+
#include "control_msgs/msg/pid_state.hpp"
41+
#include "trajectory_msgs/msg/joint_trajectory_point.hpp"
42+
43+
namespace pid_controller
44+
{
45+
46+
enum class feedforward_mode_type : std::uint8_t
47+
{
48+
OFF = 0,
49+
ON = 1,
50+
};
51+
52+
class PidController : public controller_interface::ChainableControllerInterface
53+
{
54+
public:
55+
PID_CONTROLLER__VISIBILITY_PUBLIC
56+
PidController();
57+
58+
PID_CONTROLLER__VISIBILITY_PUBLIC
59+
controller_interface::CallbackReturn on_init() override;
60+
61+
PID_CONTROLLER__VISIBILITY_PUBLIC
62+
controller_interface::InterfaceConfiguration command_interface_configuration() const override;
63+
64+
PID_CONTROLLER__VISIBILITY_PUBLIC
65+
controller_interface::InterfaceConfiguration state_interface_configuration() const override;
66+
67+
PID_CONTROLLER__VISIBILITY_PUBLIC
68+
controller_interface::CallbackReturn on_cleanup(
69+
const rclcpp_lifecycle::State & previous_state) override;
70+
71+
PID_CONTROLLER__VISIBILITY_PUBLIC
72+
controller_interface::CallbackReturn on_configure(
73+
const rclcpp_lifecycle::State & previous_state) override;
74+
75+
PID_CONTROLLER__VISIBILITY_PUBLIC
76+
controller_interface::CallbackReturn on_activate(
77+
const rclcpp_lifecycle::State & previous_state) override;
78+
79+
PID_CONTROLLER__VISIBILITY_PUBLIC
80+
controller_interface::CallbackReturn on_deactivate(
81+
const rclcpp_lifecycle::State & previous_state) override;
82+
83+
PID_CONTROLLER__VISIBILITY_PUBLIC
84+
controller_interface::return_type update_reference_from_subscribers(
85+
const rclcpp::Time & time, const rclcpp::Duration & period) override;
86+
87+
PID_CONTROLLER__VISIBILITY_PUBLIC
88+
controller_interface::return_type update_and_write_commands(
89+
const rclcpp::Time & time, const rclcpp::Duration & period) override;
90+
91+
using ControllerReferenceMsg = control_msgs::msg::MultiDOFCommand;
92+
using ControllerMeasuredStateMsg = control_msgs::msg::MultiDOFCommand;
93+
using ControllerModeSrvType = std_srvs::srv::SetBool;
94+
using ControllerStateMsg = control_msgs::msg::MultiDOFStateStamped;
95+
96+
protected:
97+
std::shared_ptr<pid_controller::ParamListener> param_listener_;
98+
pid_controller::Params params_;
99+
100+
std::vector<std::string> reference_and_state_dof_names_;
101+
size_t dof_;
102+
std::vector<double> measured_state_values_;
103+
104+
using PidPtr = std::shared_ptr<control_toolbox::PidROS>;
105+
std::vector<PidPtr> pids_;
106+
// Feed-forward velocity weight factor when calculating closed loop pid adapter's command
107+
std::vector<double> feedforward_gain_;
108+
109+
// Command subscribers and Controller State publisher
110+
rclcpp::Subscription<ControllerReferenceMsg>::SharedPtr ref_subscriber_ = nullptr;
111+
realtime_tools::RealtimeBuffer<std::shared_ptr<ControllerReferenceMsg>> input_ref_;
112+
113+
rclcpp::Subscription<ControllerMeasuredStateMsg>::SharedPtr measured_state_subscriber_ = nullptr;
114+
realtime_tools::RealtimeBuffer<std::shared_ptr<ControllerMeasuredStateMsg>> measured_state_;
115+
116+
rclcpp::Service<ControllerModeSrvType>::SharedPtr set_feedforward_control_service_;
117+
realtime_tools::RealtimeBuffer<feedforward_mode_type> control_mode_;
118+
119+
using ControllerStatePublisher = realtime_tools::RealtimePublisher<ControllerStateMsg>;
120+
121+
rclcpp::Publisher<ControllerStateMsg>::SharedPtr s_publisher_;
122+
std::unique_ptr<ControllerStatePublisher> state_publisher_;
123+
124+
// override methods from ChainableControllerInterface
125+
std::vector<hardware_interface::CommandInterface> on_export_reference_interfaces() override;
126+
127+
bool on_set_chained_mode(bool chained_mode) override;
128+
129+
// internal methods
130+
void update_parameters();
131+
controller_interface::CallbackReturn configure_parameters();
132+
133+
private:
134+
// callback for topic interface
135+
PID_CONTROLLER__VISIBILITY_LOCAL
136+
void reference_callback(const std::shared_ptr<ControllerReferenceMsg> msg);
137+
};
138+
139+
} // namespace pid_controller
140+
141+
#endif // PID_CONTROLLER__PID_CONTROLLER_HPP_

0 commit comments

Comments
 (0)