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 (backport #294) #312

Merged
merged 1 commit into from
May 13, 2024

Conversation

mergify[bot]
Copy link
Contributor

@mergify mergify bot commented May 6, 2024

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](https://mergify.com).

@mergify mergify bot added the conflicts label May 6, 2024
Copy link
Contributor Author

mergify bot commented May 6, 2024

Cherry-pick of f769c6c has failed:

On branch mergify/bp/master/pr-294
Your branch is up to date with 'origin/master'.

You are currently cherry-picking commit f769c6c.
  (fix conflicts and run "git cherry-pick --continue")
  (use "git cherry-pick --skip" to skip this patch)
  (use "git cherry-pick --abort" to cancel the cherry-pick operation)

Changes to be committed:
	modified:   doc/index.rst
	modified:   gazebo_ros2_control/CMakeLists.txt
	modified:   gazebo_ros2_control/include/gazebo_ros2_control/gazebo_system.hpp
	modified:   gazebo_ros2_control/include/gazebo_ros2_control/gazebo_system_interface.hpp
	modified:   gazebo_ros2_control/package.xml
	modified:   gazebo_ros2_control_demos/CMakeLists.txt
	new file:   gazebo_ros2_control_demos/config/cart_controller_position.yaml
	new file:   gazebo_ros2_control_demos/examples/example_position_pid.cpp
	new file:   gazebo_ros2_control_demos/launch/vertical_cart_example_position_pid.launch.py
	new file:   gazebo_ros2_control_demos/launch/vertical_cart_example_velocity_pid.launch.py
	new file:   gazebo_ros2_control_demos/urdf/test_vertical_cart_position_pid.xacro.urdf
	new file:   gazebo_ros2_control_demos/urdf/test_vertical_cart_velocity_pid.xacro.urdf

Unmerged paths:
  (use "git add <file>..." to mark resolution)
	both modified:   gazebo_ros2_control/src/gazebo_system.cpp

To fix up this pull request, you can check it out locally. See documentation: https://docs.github.com/en/pull-requests/collaborating-with-pull-requests/reviewing-changes-in-pull-requests/checking-out-pull-requests-locally

@ahcorde ahcorde merged commit a3f099a into master May 13, 2024
3 of 5 checks passed
@ahcorde ahcorde deleted the mergify/bp/master/pr-294 branch May 13, 2024 08:48
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