Port PID controller to contro joint using effort

[ahcorde]

Related with this PR on gazebo_ros2_control ros-controls/gazebo_ros2_control#294

[christophfroehlich]

There is a stale PR which already implemented something similar #122

[peci1]

As a workaround, pid_controller/PidController can be used explicitly (not sure about mimic joints, though).

  controller_manager:
    ros__parameters:
      use_sim_time: True
      joint_state_broadcaster:
        type: joint_state_broadcaster/JointStateBroadcaster
      diffdrive_controller:
        type: diff_drive_controller/DiffDriveController
      wheel_pids:
        type: pid_controller/PidController

  joint_state_broadcaster:
    ros__parameters:
      use_sim_time: True
      update_rate: 100  # Hz

  diffdrive_controller:
    ros__parameters:
      use_sim_time: True
      left_wheel_names: ["wheel_pids/left_wheel_j"]
      right_wheel_names: ["wheel_pids/right_wheel_j"]
      # other options

  wheel_pids:
    ros__parameters:
      use_sim_time: True
      dof_names: ["left_wheel_j", "right_wheel_j"]
      command_interface: effort
      reference_and_state_interfaces: ["velocity", "effort"]
      gains:
        left_wheel_j: { p: 100.0, i: 0.1, d: 0.0, i_clamp_max: 5.0, i_clamp_min: -5.0, feedforward_gain: 1.0 }
        right_wheel_j: { p: 100.0, i: 0.1, d: 0.0, i_clamp_max: 5.0, i_clamp_min: -5.0, feedforward_gain: 1.0 }

and the launch file:

def generate_launch_description():
    description_package = ...  # your robot description package
    control_params_file = PathJoinSubstitution([description_package, 'config', 'control.yaml'])

    diffdrive_controller_node = Node(
        package='controller_manager',
        executable='spawner',
        namespace="",  # Namespace is not pushed when used in EventHandler
        parameters=[control_params_file],
        arguments=[
            'diffdrive_controller',
            '-c',
            'controller_manager',
            '--controller-manager-timeout',
            '120'
        ],
        output='screen',
    )

    wheel_pids = Node(
        package='controller_manager',
        executable='spawner',
        namespace="",  # Namespace is not pushed when used in EventHandler
        parameters=[control_params_file],
        arguments=[
            'wheel_pids',
            '-c',
            'controller_manager',
            '--controller-manager-timeout',
            '120'
        ],
        output='screen',
    )

    joint_state_broadcaster_spawner = Node(
        package='controller_manager',
        executable='spawner',
        arguments=[
            'joint_state_broadcaster',
            '-c',
            'controller_manager',
            '--controller-manager-timeout',
            '120'
        ],
        output='screen',
    )

    diffdrive_controller_callback = RegisterEventHandler(
        event_handler=OnProcessExit(
            target_action=wheel_pids,
            on_exit=[diffdrive_controller_node],
        )
    )

    wheel_pids_callback = RegisterEventHandler(
        event_handler=OnProcessExit(
            target_action=joint_state_broadcaster_spawner,
            on_exit=[wheel_pids],
        )
    )

    ld = LaunchDescription(ARGUMENTS)
    ld.add_action(joint_state_broadcaster_spawner)
    ld.add_action(wheel_pids_callback)
    ld.add_action(diffdrive_controller_callback)
    return ld

[peci1]

I've put up a complete example here: ros-controls/ros2_control_demos#786 .

[annaborn]

Hello!

I'm facing a similar issue: we need effort-based joint position control, which is currently missing in gz_ros2_control.

My initial approach was to add an additional effort controller within ros2_control. However, this would result in different controller setups for the real and simulated robots.

That said, Gazebo already provides a built-in effort-based joint position controller:
https://github.com/gazebosim/gz-sim/tree/ign-gazebo6/src/systems/joint_position_controller
So the question is: why not use Gazebo's native controller directly and connect it to a ROS 2 position controller?

My idea is to use a topic-based hardware interface—similar to this project:
https://github.com/PickNikRobotics/topic_based_ros2_control
This would connect the ROS 2 controller with Gazebo's internal controller over the topics.

I see several advantages in this approach:

• No need for gz_ros2_control, avoiding the reimplementation of PID functionality that already exists in Gazebo.
• The controller manager remains on the ROS 2 side, which simplifies simulation setup—no special handling is needed.
• The simulation model can be configured once and reused with or without the ROS 2 control stack. Also, the robot can be controlled via the Gazebo GUI widget.
• The controller setup remains consistent between the real and simulated robots, avoiding the need to switch between position control (for the real robot) and effort control (for the simulation).

I would love to hear your thoughts on using a topic-based hardware interface for this.

@ahcorde @christophfroehlich

[peci1]

I've created an alternative approach using ROS 2 control to provide effort-based velocity control of Gazebo joints in ros-controls/ros2_control_demos#786 . However, maintainers of ROS 2 control do not think it is a good idea. So I'm also curious what would be the correct approach.

In Gazebo classic, there were the "velocity_pid" and "position_pid" interfaces to joints which provided the effort-based control. And that is what #122 adds. However, the PR is very stale...

[annaborn]

Hello @peci1 ,

I think what you’ve proposed is a good approach, especially since there is currently no solution for this in gz_ros2_control.

I just took a look at the existing Gazebo control plugins. By using the topic-based hardware interface, you could add the JointControl Gazebo plugin in force mode (according to the documentation, this means joint velocity control using a PID controller). On the ROS 2 side, you could use only the diff drive velocity controller, without the need for a chainable controller. But this is just a rough idea of how I imagine it might work. :)

One remark: the JointControl Gazebo plugin is a single-joint plugin and not model-based. So, the hardware interface would need to split the joint velocity commands into separate topics—one topic per JointControl Gazebo plugin instance. This could have some drawbacks.

Additionally, the JointControl Gazebo plugin could potentially be extended to support model-based control. However, I’m not sure about the potential side effects or limitations of doing so.

[christophfroehlich]

Can you elaborate why you really need effort interface in the physics engine? For me it seemed that gazebo works better than classic did, see the pendulum examples.

If there is a built-in controller and go in the direction of using topic_based_ros2_control, this is fine. People are doing the same with Isaac Sim for example.

My argument remains the same, I don't feel the need to move anything related to actuator dynamics or physics engine to a ros2_controller, this should be done by the provided or a custom gazebo plugin.

Slightly related to
#87 (comment)

[peci1]

Can you elaborate why you really need effort interface in the physics engine? For me it seemed that gazebo works better than classic did, see the pendulum examples.

Try to spawn a larger diff drive into https://app.gazebosim.org/ats/fuel/models/Rubicon and you'll see. With the direct velocity control, it's almost impossible to drive anywhere. With PID velocity control of an effort interface, the driving behaves as it should.

[Juliaj]

Try to spawn a larger diff drive into https://app.gazebosim.org/ats/fuel/models/Rubicon and you'll see. With the direct velocity control, it's almost impossible to drive anywhere. With PID velocity control of an effort interface, the driving behaves as it should.

@peci1, I created an example to spawn a COSTAR Husky bot in Rubicon based on your suggestion above. ATM, it does direct velocity control . Would you mind giving this example a quick look to see whether this is what you had in mind ?

Based on my experiment, it was possible to drive somewhere though it was difficult because of the rugged terrain. See result in attached video clip.

output.mp4

[Juliaj]

I’ve updated the example above to support simulation with Gazebo’s built-in gz::sim::systems::DiffDrive plugin in addition to the existing gz_ros2_control::GazeboSimROS2ControlPlugin.

Based on my testing (see instructions), there are some differences between the gz_ros2_control::GazeboSimROS2ControlPlugin and Gazebo’s built-in gz::sim::systems::DiffDrive plugin when moving around the diffbot. However, IMO, the overall experience remains quite similar. If anyone else has time to give a spin, I'd love to hear your comments and experience.

@ahcorde and @christophfroehlich, what are your thoughts on the next steps for this issue? After reviewing previous PRs related to this topic, I noticed that the scope extends beyond just effort-based velocity control, for example to support the use case of MimicJointSystem. Your guidance on how to proceed is appreciated.