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

Change :

Added PID Implementation
Added effort-based Position Controller
Added effort-based Velocity Controller

Motivation :

The primary goal behind these changes is to model the behavior of a low-level joint controller more accurately, rather than relying on workarounds within the physics engine. Additionally, addressing some limitations related to closed-loop kinematic chains using Position and Velocity control interfaces is another objective. The PR could maybe also solve this issue #240

Change in usage :

The usage remains largely the same as before, with one key difference. If you declare parameters kp, ki, or kd using the command_interface for Position or Velocity, the controller will apply effort to the joint based on the specified PID gains instead of directly setting the joint position or velocity.

Usage exemple :

Here’s an example of the new usage for controlling the velocity of a joint using an Effort PID:


    <ros2_control name="my_GazeboSystem" type="system">

      <hardware>
        <plugin>gazebo_ros2_control/GazeboSystem</plugin>
      </hardware>

      <joint name="my_joint">

        <param name="kp">10</param>
        <param name="ki">2</param>
        <param name="kd">0.1</param>
        <param name="max_integral_error">1000</param>

        <command_interface name="velocity">
          <param name="min">0</param>
          <param name="max">2</param>
        </command_interface>

        <state_interface name="position">
          <param name="initial_value">0</param>
        </state_interface>

        <state_interface name="velocity"/>
        <state_interface name="effort"/>

      </joint>

    </ros2_control>

The second example retains the original behavior, where a Velocity controller sets the velocity of the joint in the physics engine :


    <ros2_control name="my_GazeboSystem" type="system">

      <hardware>
        <plugin>gazebo_ros2_control/GazeboSystem</plugin>
      </hardware>

      <joint name="my_joint">

        <command_interface name="velocity">
          <param name="min">0</param>
          <param name="max">2</param>
        </command_interface>

        <state_interface name="position">
          <param name="initial_value">0</param>
        </state_interface>

        <state_interface name="velocity"/>
        <state_interface name="effort"/>

      </joint>

    </ros2_control>

This is an automatic backport of pull request #294 done by Mergify.

ros-controls / gazebo_ros2_control