Add support for getting PID parameters from loaded parameters

[saikishor]

Hello!

The issue #369 will be fixed once this is merged. It seems that for our Humanoids we will be needing this PID feature to make it work using position.

This PR:

• adds a way to get the PIDs from the loaded parameters as well as the URDF
• If the parameters are found in the URDF, it doesn't check for the parameters in the loaded parameters
• This PR add a way for the PIDs to work also for the position interface, because having the position_pid interface doesn't help to use many standard ros2 controllers, as many controllers tend to check if they belong to a subset (https://github.com/ros-controls/ros2_controllers/blob/master/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml#L20-L29)
• The proposal to add it from the loaded parameters is due to the fact it it overcomplicates the declaration in the URDF. In our case, we have macros for defining the ros2_control tag information(for example: https://github.com/pal-robotics/talos_robot/blob/5848d42f15340e35d672e323d61079d72b2fb13d/talos_description/urdf/arm/arm.ros2_control.xacro#L22-L30). If we have to define for each joint, we can't make use of the macros.

I hope this helps someone. If this PR changes look fine. I will open the following PR in the gz_ros2_control

   <gazebo>
      <plugin filename="libgazebo_ros2_control.so" name="gazebo_ros2_control">
        <ros>
          <argument>--ros-args</argument>
          <argument>--params-file</argument>
          <argument>$(find talos_description)/config/gazebo_controller_manager_cfg.yaml</argument>
        </ros>
        <hold_joints>false</hold_joints>
        <parameters>$(find talos_description)/config/gazebo_controller_manager_cfg.yaml</parameters>
      </plugin>
    </gazebo>

Defining it like this should work

gazebo_ros2_control:
  ros__parameters:
    pid_gains:
      position:
        leg_left_1_joint: {kp:  5000.0, kd: 20.0, ki:  5.0, max_integral_error: 10000.0}
        leg_left_2_joint: {kp:  5000.0, kd: 20.0, ki:  5.0, max_integral_error: 10000.0}
        leg_left_3_joint: {kp:  5000.0, kd: 20.0, ki:  5.0, max_integral_error: 10000.0}
        leg_left_4_joint: {kp:  5000.0, kd: 20.0, ki:  5.0, max_integral_error: 10000.0}
        leg_left_5_joint: {kp:  5000, kd: 20.0, ki:  5.0, max_integral_error: 10000.0}
        leg_left_6_joint: {kp:  5000.0, kd: 20.0, ki:  5.0, max_integral_error: 10000.0}

[ahcorde]

Do you mind to add an example and document how to set up the pid values ?

[ahcorde]

include <cmath>

[saikishor]

Sure. I will add it. I just wanted to leave it for initial review. Thank you for the review. I'll get it done soon

[saikishor]

Do you mind to add an example and document how to set up the pid values ?

@ahcorde I've just added it an example, let me know how it looks

If this ros-simulation/gazebo_ros_pkgs#1546 is merged, we can have parameters tag as how we had in gazebo_ros2_control currently and we can make changes in such a way, we declare once and both same the params file

[christophfroehlich]

Changes look fine for me.

[ahcorde]

https://github.com/Mergifyio backport humble iron

[mergify]

backport humble iron

✅ Backports have been created

• #375 Add support for getting PID parameters from loaded parameters (backport #374) has been created for branch humble
• #376 Add support for getting PID parameters from loaded parameters (backport #374) has been created for branch iron