Add PID controller to control joint using effort

[chameau5050]

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:

<!-- define control interface -->
    <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>


        <!-- define command interface -->
        <command_interface name="velocity">
          <param name="min">0</param>
          <param name="max">2</param>
        </command_interface>

        <!-- define state 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 :

<!-- define control interface -->
    <ros2_control name="my_GazeboSystem" type="system">
    
      <hardware>
        <plugin>gazebo_ros2_control/GazeboSystem</plugin>
      </hardware>
      
      <joint name="my_joint">
        
        <!-- define command interface -->
        <command_interface name="velocity">
          <param name="min">0</param>
          <param name="max">2</param>
        </command_interface>

        <!-- define state interface -->
        <state_interface name="position">
          <param name="initial_value">0</param>
        </state_interface>

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

      </joint>

    </ros2_control>

[chameau5050]

can you use the PID from https://github.com/ros-controls/control_toolbox ? then I suggest you to add new control modes: POSITION_PID, VELOCITY_PID and EFFORT_PID, and keep the behaviour of the current controllers. This is important just in case we want to backport this feature to an older version

Sure, I can use the control_toolbox PID. I am not sure how you want me to add new control modes in terms of usage. Maybe something like that ?

<!-- define control interface -->
    <ros2_control name="my_GazeboSystem" type="system">
    
      <hardware>
        <plugin>gazebo_ros2_control/GazeboSystem</plugin>
      </hardware>
      
      <joint name="my_joint">
    
        <!-- define command interface -->
        <command_interface name="velocity_PID">
          <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>

        <!-- define state interface -->
        <state_interface name="position">
          <param name="initial_value">0</param>
        </state_interface>

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

      </joint>

    </ros2_control>

[christophfroehlich]

Yes I think this is what @ahcorde suggests. Don't forget to register the handles in registerJoints and the control bits in perform_command_mode_switch for the new tags.

Please also add a section to the documentation, and maybe also a demo to the demos package using this new feature.

[chameau5050]

@ahcorde @christophfroehlich, I have updated the PID implementation and added the new control mode. Please let me know if you notice any issues with this implementation. I will also update the documentation and provide examples during this week.

[chameau5050]

@christophfroehlich @ahcorde I added the doc and example and fix all your comment. I let you review everything. Let me know if you got and problem with the new feature.

[chameau5050]

Sorry I forgot to run colcon test before commit. The current version should pass all colcon test @christophfroehlich

[chameau5050]

Sorry for the confusion with the CI I was not running pre-commit with the arg --all-files so I was not seeing all error @ahcorde

[christophfroehlich]

Sorry for the confusion with the CI I was not running pre-commit with the arg --all-files so I was not seeing all error @ahcorde

Fyi: If you use the github UI to apply the suggested changes, it would be clearer for reviewer to see if this was applied or not.

[chameau5050]

Sorry for the confusion with the CI I was not running pre-commit with the arg --all-files so I was not seeing all error @ahcorde

Fyi: If you use the github UI to apply the suggested changes, it would be clearer for reviewer to see if this was applied or not.

Sorry for that, is there any other change needed in this PR ? @christophfroehlich @ahcorde

[ahcorde]

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

[mergify[bot]]

backport iron master

✅ Backports have been created

• #311 Add PID controller to control joint using effort (backport #294) has been created for branch iron
• #312 Add PID controller to control joint using effort (backport #294) has been created for branch master but encountered conflicts

[ahcorde]

@chameau5050 any plan to include these changes in gz_ros2_control?

[chameau5050]

@chameau5050 any plan to include these changes in gz_ros2_control?

@ahcorde maybe in a few months but not in the short term