Add PID controller to control joint using effort (#294)

Co-authored-by: Alejandro Hernández Cordero <[email protected]>
Co-authored-by: Christoph Fröhlich <[email protected]>

doc/index.rst

@@ -131,6 +131,58 @@ We should include:
   </joint>
 
 
+Using PID control joints
+-----------------------------------------------------------
+
+To use PID control joints in gazebo_ros2_control, you should define their parameters inside the ``<joint>`` tag
+within the ``<ros2_control>`` tag. These PID joints can be controlled either in position or velocity.
+
+- To control a joint with velocity PID, simply set its ``command_interface`` to ``velocity_PID``.
+- To control a joint with position PID, set its ``command_interface`` to ``position_PID``.
+
+.. note::
+    You cannot have both command interfaces set to position and position_PID for the same joint. The same restriction applies to velocity (and velocity_PID).
+
+To create a system with one joint that can be controlled using both position_PID and velocity_PID, follow this example:
+
+.. code-block:: xml
+
+ <ros2_control name="GazeboSystem" type="system">
+    <hardware>
+      <plugin>gazebo_ros2_control/GazeboSystem</plugin>
+    </hardware>
+    <joint name="slider_to_cart">
+
+      <param name="pos_kp">10</param>
+      <param name="pos_ki">1</param>
+      <param name="pos_kd">2</param>
+      <param name="pos_max_integral_error">10000</param>
+
+      <param name="vel_kp">10</param>
+      <param name="vel_ki">5</param>
+      <param name="vel_kd">2</param>
+      <param name="vel_max_integral_error">10000</param>
+
+      <command_interface name="position_PID"/>
+      <command_interface name="velocity_PID"/>
+
+      <state_interface name="position">
+        <param name="initial_value">1.0</param>
+      </state_interface>
+      <state_interface name="velocity"/>
+      <state_interface name="effort"/>
+    </joint>
+  </ros2_control>
+
+Where the parameters are as follows:
+
+- ``pos_kp``: Proportional gain
+- ``pos_ki``: Integral gain
+- ``pos_kd``: Derivative gain
+- ``pos_max_integral_error``: Maximum summation of the error
+
+The same definitions apply to the ``vel_*`` parameters.
+
 Add the gazebo_ros2_control plugin
 ==========================================
 
@@ -243,6 +295,7 @@ When the Gazebo world is launched you can run some of the following commands to
 .. code-block:: shell
 
   ros2 run gazebo_ros2_control_demos example_position
+  ros2 run gazebo_ros2_control_demos example_position_pid
   ros2 run gazebo_ros2_control_demos example_velocity
   ros2 run gazebo_ros2_control_demos example_effort
 
@@ -310,3 +363,20 @@ degree of freedom on the rail, and the physics of the passive joint of the pendu
 
     ros2 launch gazebo_ros2_control_demos pendulum_example_position.launch.py
     ros2 run gazebo_ros2_control_demos example_position
+
+
+
+PID control joints
+-----------------------------------------------------------
+
+The following examples shows a vertical cart control by a PID joint using position and velocity cmd.
+
+.. code-block:: shell
+
+  ros2 launch gazebo_ros2_control_demos vertical_cart_example_position_pid.launch.py
+  ros2 launch gazebo_ros2_control_demos vertical_cart_example_velocity_pid.launch.py
+
+.. code-block:: shell
+
+  ros2 run gazebo_ros2_control_demos example_position_pid
+  ros2 run gazebo_ros2_control_demos example_velocity

gazebo_ros2_control/CMakeLists.txt

@@ -4,6 +4,7 @@ project(gazebo_ros2_control)
 find_package(ament_cmake REQUIRED)
 find_package(angles REQUIRED)
 find_package(controller_manager REQUIRED)
+find_package(control_toolbox REQUIRED)
 find_package(gazebo_dev REQUIRED)
 find_package(gazebo_ros REQUIRED)
 find_package(hardware_interface REQUIRED)
@@ -33,6 +34,7 @@ add_library(${PROJECT_NAME} SHARED
 ament_target_dependencies(${PROJECT_NAME}
   angles
   controller_manager
+  control_toolbox
   gazebo_dev
   gazebo_ros
   hardware_interface
@@ -46,6 +48,7 @@ add_library(gazebo_hardware_plugins SHARED
 )
 ament_target_dependencies(gazebo_hardware_plugins
   angles
+  control_toolbox
   gazebo_dev
   hardware_interface
   rclcpp

gazebo_ros2_control/include/gazebo_ros2_control/gazebo_system.hpp

@@ -16,12 +16,16 @@
 #ifndef GAZEBO_ROS2_CONTROL__GAZEBO_SYSTEM_HPP_
 #define GAZEBO_ROS2_CONTROL__GAZEBO_SYSTEM_HPP_
 
+#define VELOCITY_PID_PARAMS_PREFIX "vel_"
+#define POSITION_PID_PARAMS_PREFIX "pos_"
+
 #include <memory>
 #include <string>
 #include <vector>
 
 #include "angles/angles.h"
 
+#include "control_toolbox/pid.hpp"
 #include "gazebo_ros2_control/gazebo_system_interface.hpp"
 
 #include "std_msgs/msg/bool.hpp"
@@ -86,6 +90,10 @@ class GazeboSystem : public GazeboSystemInterface
     const hardware_interface::HardwareInfo & hardware_info,
     gazebo::physics::ModelPtr parent_model);
 
+  control_toolbox::Pid extractPID(
+    std::string prefix,
+    hardware_interface::ComponentInfo joint_info);
+
   /// \brief Private data class
   std::unique_ptr<GazeboSystemPrivate> dataPtr;
 };

gazebo_ros2_control/include/gazebo_ros2_control/gazebo_system_interface.hpp

@@ -79,6 +79,8 @@ class GazeboSystemInterface
     POSITION  = (1 << 0),
     VELOCITY  = (1 << 1),
     EFFORT    = (1 << 2),
+    VELOCITY_PID    = (1 << 3),
+    POSITION_PID    = (1 << 4),
   };
 
   typedef SafeEnum<enum ControlMethod_> ControlMethod;

gazebo_ros2_control/package.xml

@@ -19,9 +19,10 @@
   <buildtool_depend>ament_cmake</buildtool_depend>
 
   <depend>angles</depend>
+  <depend>controller_manager</depend>
+  <depend>control_toolbox</depend>
   <depend>gazebo_dev</depend>
   <depend>gazebo_ros</depend>
-  <depend>controller_manager</depend>
   <depend>hardware_interface</depend>
   <depend>pluginlib</depend>
   <depend>rclcpp</depend>