Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Add PID controller to control joint using effort #294

Conversation

chameau5050
Copy link
Contributor

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>

- add PID Implementation
- add effort base Position Controller
- add effort base Velocity Controller
@chameau5050 chameau5050 changed the title add PID controller to control joint using effort Add PID controller to control joint using effort Mar 22, 2024
Copy link
Collaborator

@ahcorde ahcorde left a comment

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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

@chameau5050
Copy link
Contributor Author

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
Copy link
Contributor

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
Copy link
Contributor Author

@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.

Copy link
Contributor

@christophfroehlich christophfroehlich left a comment

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Looks good so far. Waiting for examples and documentation.

with gz_ros2_control there is a position_proportional_gain_ factor for a position-controlled joint with velocity interface. This would be easy to include here, is there a use-case for this?

gazebo_ros2_control/package.xml Outdated Show resolved Hide resolved
gazebo_ros2_control/CMakeLists.txt Outdated Show resolved Hide resolved
gazebo_ros2_control/CMakeLists.txt Outdated Show resolved Hide resolved
gazebo_ros2_control/CMakeLists.txt Outdated Show resolved Hide resolved
gazebo_ros2_control/src/gazebo_system.cpp Outdated Show resolved Hide resolved
gazebo_ros2_control/src/gazebo_system.cpp Outdated Show resolved Hide resolved
gazebo_ros2_control/src/gazebo_system.cpp Outdated Show resolved Hide resolved
gazebo_ros2_control/src/gazebo_system.cpp Outdated Show resolved Hide resolved
@chameau5050
Copy link
Contributor Author

chameau5050 commented Apr 5, 2024

@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.

Copy link
Contributor

@christophfroehlich christophfroehlich left a comment

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Please run pre-commit locally and also colcon test (see flake8 errors).

@chameau5050
Copy link
Contributor Author

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

Copy link
Collaborator

@ahcorde ahcorde left a comment

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

format CI is also failing

@chameau5050
Copy link
Contributor Author

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
Copy link
Contributor

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
Copy link
Contributor Author

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

Copy link
Contributor

@christophfroehlich christophfroehlich left a comment

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

LGTM

- ``pos_max_integral_error``: Maximum summation of the error

The same definitions apply to the ``vel_*`` parameters.

Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

do you mind to add here the command to run the examples ? Then it's good to go

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

@ahcorde done

@ahcorde ahcorde merged commit f769c6c into ros-controls:humble May 6, 2024
4 checks passed
@ahcorde
Copy link
Collaborator

ahcorde commented May 6, 2024

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

Copy link
Contributor

mergify bot commented May 6, 2024

backport iron master

✅ Backports have been created

@ahcorde
Copy link
Collaborator

ahcorde commented May 6, 2024

@chameau5050 any plan to include these changes in gz_ros2_control?

mergify bot pushed a commit that referenced this pull request May 6, 2024
Co-authored-by: Alejandro Hernández Cordero <[email protected]>
Co-authored-by: Christoph Fröhlich <[email protected]>
(cherry picked from commit f769c6c)
mergify bot pushed a commit that referenced this pull request May 6, 2024
Co-authored-by: Alejandro Hernández Cordero <[email protected]>
Co-authored-by: Christoph Fröhlich <[email protected]>
(cherry picked from commit f769c6c)

# Conflicts:
#	gazebo_ros2_control/src/gazebo_system.cpp
ahcorde pushed a commit that referenced this pull request May 6, 2024
Co-authored-by: Alejandro Hernández Cordero <[email protected]>
Co-authored-by: Christoph Fröhlich <[email protected]>
(cherry picked from commit f769c6c)

Co-authored-by: chameau5050 <[email protected]>
@chameau5050
Copy link
Contributor Author

@chameau5050 any plan to include these changes in gz_ros2_control?

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

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Labels
None yet
Projects
None yet
Development

Successfully merging this pull request may close these issues.

3 participants