Tutorial 7: Control

Objective: This tutorial session is devoted to understand the ros2_control framework used to implement and manage robot controllers for real robots and in simulation within gazebo.

Contents

After this session you will be able to:

  • Understand the structure of the ros2_control framework.

  • Understand how to interface with real hardware using the hardware abstraction layer.

  • Which are the available controllers and how are they managed.

  • Understand how ros2_control is used within gazebo simulation environment.

  • Use ros2_control to define robot trajectories.

References:

This tutorial is partially based on the following resources:

Provided packages:

This tutorial will use the following package:

Package ros2_control_demo_example_1
Package ros2_control_demo_description
Package ros2_controllers_test_nodes

Prepare the colcon workspace to work with it:

  1. If you have not yet done so, follow the steps in Setup to:

  • clone in your ~/data/git-projects folder the meta-repository containing all the required packages,

  • create the colcon workspace folders (use colcon_ws7 for the current tutorial).

  1. In your colcon_ws7/src folder create a symbolic link to the packages to be used:

ln -s ~/data/git-projects/all_introduction_to_ros2_packages/ros2_control_demo_example_1
ln -s ~/data/git-projects/all_introduction_to_ros2_packages/ros2_control_demo_description
ln -s ~/data/git-projects/all_introduction_to_ros2_packages/ros2_controllers_test_nodes
  1. Build the packages form the colcon_ws7 folder:

colcon build --allow-overriding ros2_controllers_test_nodes

ros2_control overview

The ros2_control is a framework for (real-time) control of robots using ROS 2.

The ros2_control packages are a rewrite of ros_control packages used in ROS 1.

The ros2_control framework:

  1. Implements abstractions on hardware interfaces with minimal assumptions on hardware or operating system.

  2. Provides the capability to implement and manage robot controllers, that mainly consists of a feedback mechanism, most probably a PID loop, which can receive a setpoint, and control the output, typically effort, using the feedback from the actuators. The output is applied to the real robot or to its simulation in Gazebo by using a simple Gazebo plugin adapter.

The following figure shows the ros2_control architecture that illustrates interconnection of the main component of ros2_control:

  1. The hardware abstraction layer (the gray and orange blocks; they will be detailed in Section Hardware Abstraction Layer).

  • The hardware components realize communication to physical hardware and are exported as plugins.

  • The Resource Manager dynamically loads the plugins and manages their lifecycle.

  1. The controllers and the control manager (the blue and yellow blocks; they will be detailed in Section Controllers)

  • The controllers compare the reference value with the measured output and, based on this error, calculate a system’s input.

  • The Controller Manager manages (e.g. loads, activates, deactivates, unloads) controllers and the interfaces they require.

The Controller Manager connects the controllers and hardware-abstraction sides of the ros2_control framework

../_images/components_architecture2.png

Hardware Abstraction Layer

Hardware resources

The hardware resources realize communication to physical hardware and are exported as plugins. Each individual resource hardware is loaded at runtime and thus allows a flexible and dynamic composition of the setup to be controlled.

The resource hardware is composed and configured solely through URDF (see Hardware Description in URDF).

There are three basic types of hardware resources:

  • Actuator: Simple 1-DOF robotic hardware like motors or valves.

  • System: Complex multi-DOF robotic hardware like industrial robots.

  • Sensor: Robotic hardware used for sensing the environment, either related to a joint (e.g., encoder) or a link (e.g., force-torque sensor).

Actuator and System components have reading and writing capabilities (using the joint interface), whereas the Sensor component has only reading capabilities (using the sensor interface).

Hardware interfaces

There are two logical components as hardware interfaces:

Joint: The <joint>-tag groups the interfaces associated with the joints of physical robots and actuators. They have:

  • command interfaces: to set the goal values for hardware,

  • state interfaces: to read its current state.

...
<joint name="my_joint">
  <command_interface name="joint_command_interface">
    <param name="my_joint_command_param">1.5</param>
  </command_interface>
  <state_interface name="joint_state_interface1" />
  <state_interface name="joint_state_interface2" />
</joint>
...

Sensor: <sensor>-tag groups multiple state interfaces describing, e.g., internal states of hardware.

...
<sensor name="my_sensor">
  <state_interface name="sensor_state_interface1" />
  <state_interface name="sensor_state_interface2" />
  <state_interface name="sensor_state_interface3" />
</sensor>
...

A detailed explanation of hardware resources and interfaces is given in the Hardware Access through Controllers design document.

Hardware Description in URDF

The URDF hardware description of the hardware resources include the physical hardware resource (<hardware>-tag) and a logical component (<joint>-tag or <sensor>-tag):

  • The URDF snippet for an actuator might look like the following:

    <ros2_control name="my_simple_servo_motor" type="actuator">
      <hardware>
        <class>simple_servo_motor_pkg/SimpleServoMotor</class>
        <param name="serial_port">/dev/tty0</param>
      ...
      </hardware>
      <joint name="joint1">
        <command_interface name="position">
          <param name="min">-1.57</param>
          <param name="max">1.57</param>
        </command>
    
        <state_interface name="position"/>
        ...
      </joint>
    </ros2_control>
    
  • The URDF snippet for a sensor might look like the following:

    <ros2_control name="my_simple_sensor">
      <hardware type="sensor">
        <class>simple_sensor_pkg/SimpleSensor</class>
        <param name="serial_port">/dev/tty0</param>
        ...
      </hardware>
      <sensor name="my_sensor">
        <state_interface name="roll" />
        <state_interface name="pitch" />
        <state_interface name="yaw" />
      </sensor>
    </ros2_control>
    
  • The URDF snippet for a system might look like the following:

    <ros2_control name="MyComplexRobot" type="system">
      <hardware>
        <class>complex_robot_pkg/ComplexRobot</class>
        ...
        </hardware>
        <joint name="joint1">
          <command_interface name="velocity" />
          <command_interface name="effort" />
          <state_interface name="position" />
          <state_interface name="velocity" />
          <state_interface name="effort" />
        </joint>
        <joint name="joint2">
          <command_interface name="velocity" />
          <command_interface name="effort" />
          <state_interface name="position" />
          <state_interface name="velocity" />
          <state_interface name="effort" />
        </joint>
    </ros2_control>
    

The Resource Manager

The RM loads the hardware components using the pluginlib-library and manages:

  • their lifecycle,

  • the components’ state interfaces, and

  • the components’ command interfaces.

In the control loop execution, the RM’s read() and write() methods actually handle the communication with the hardware components.

The rrbot example

As an example, the URDF for the rrbot 2-DOF robot used later is the following.

The robot is a hardware resource of type system.

Note that:

  • it includes the tag <ros2_control> to access and control the robot interfaces.

  • it specifies the <plugin> for the robot inside the <hardware> tag.

  • it defines <joint> tags including the robot controllers: commands and states.

<ros2_control name="RRBot" type="system">
  <hardware>
    <plugin>ros2_control_demo_example_1/RRBotSystemPositionOnlyHardware</plugin>
    <param name="example_param_hw_start_duration_sec">0</param>
    <param name="example_param_hw_stop_duration_sec">3.0</param>
    <param name="example_param_hw_slowdown">100</param>
  </hardware>

  <joint name="joint1">
    <command_interface name="position">
      <param name="min">-1</param>
      <param name="max">1</param>
    </command_interface>
    <state_interface name="position"/>
  </joint>
  <joint name="joint2">
    <command_interface name="position">
      <param name="min">-1</param>
      <param name="max">1</param>
    </command_interface>
    <state_interface name="position"/>
  </joint>
</ros2_control>

Exercise

Launch the rrbot.launch.py file to load and start the robot hardware, the controllers and open RViz. Then check the hardware interfaces.

ros2 launch ros2_control_demo_example_1 rrbot.launch.py
ros2 control list_hardware_interfaces

Exercise

Take a look at the package ros2_control_demo_example_1.

Note:

  • The plugin RRBotSystemPositionOnlyHardware loaded in the <hardware>-tag above is implemented in the class RRBotSystemPositionOnlyHardware (that derives from the class hardware_interface::SystemInterface) in the file hardware/rrbot.cpp. It includes the read() and write() functions (lines 191 and 211, respectively).

  • The rrbot.urd.xacro file loads the rrbot and rrbot_ros2_control macros, the last one including the URDF ros2_control information shown above.

Controllers

Available controllers for manipulators

The main ROS controllers are grouped according to the commands get passed to the hardware/simulator:

  • position_controllers: Used for position-controlled joints (position commands are used to control joint positions).

    joint_position_controller: just transfers the position input as a position command to the hardware interface (using a simple forward_command_controller).
    joint_group_position_controller: Set multiple joint positions at once.
  • velocity_controllers: Used for velocity-controlled joints (velocity commands are used to control joint positions or velocities).

    joint_velocity_controller: just transfers the velocity input as a velocity command to the hardware interface (using a simple forward_command_controller).
    joint_group_velocity_controller: Set multiple joint velocities at once.
  • effort_controllers: Used for effort-controlled joints (efforts commands are used to control joint positions, velocities or efforts).

    joint_effort_controller: just transfers the effort input as an effort command to the hardware interface (using a simple forward_command_controller).
    joint_group_effort_controller: Set multiple joint efforts at once.
  • joint_trajectory_controllers: Used to control the execution of joint-space trajectories on a group of joints, specified as a set of waypoints to be reached at specific time instants. They use a linear, cubic or quintic spline interpolator depending on how the waypoints are specified (i.e. as joint positions, and optionally velocities and accelerations), and are implemented to work with multiple hardware interface types.

    position_controller: just transfers the position inputs as a position commands to the hardware interfaces.
    velocity_controller: the position+velocity trajectory following error is mapped to velocity commands using a PID controller.
    effort_controller: the position+velocity trajectory following error is mapped to effort commands using a PID controller.

Also, a “controller” is defined to publish joint states:

Other available controllers can be found here.

Controller Manager

The Controller Manager (CM) is the main component of the ros2_control framework. It manages lifecycle of controllers, access to the hardware interfaces and offers services to the ROS-world.

It serves as the entry-point for users via ROS services. The recommended way to work with the CM is using the default node-setup implemented in the ros2_control_node file from the controller_manager package. When using Gazebo, however, this is not necessary because the gazebo_ros2_plugin already contains its own controller manager.

The CM is responsible of:

  1. Managing the controllers, i.e. to load, activate, deactivate and unloads the controllers and the interfaces they require.

  2. Granting controllers access to the hardware by matching the required and the provided hardware interfaces (via the Resource Manager).

  3. Executing the control-loop (using its update() method), that reads data from the hardware components, updates outputs of all active controllers, and writes the result to the components.

Configuring and launching controllers

Controllers are configured using YAML files.

As an example, the file bringup/config/rrbot_controllers.yaml in package ros2_control_demo_example_1 is the following.

Note that it defines:

  • A joint_state_broadcaster

  • A forward_position_controller

  • A joint_trajectory_position_controller

controller_manager:
  ros__parameters:
    update_rate: 10  # Hz

    joint_state_broadcaster:
      type: joint_state_broadcaster/JointStateBroadcaster

    forward_position_controller:
      type: forward_command_controller/ForwardCommandController

    joint_trajectory_position_controller:
      type: joint_trajectory_controller/JointTrajectoryController

forward_position_controller:
  ros__parameters:
    joints:
      - joint1
      - joint2
    interface_name: position

joint_trajectory_position_controller:
  ros__parameters:
    joints:
      - joint1
      - joint2

    command_interfaces:
      - position

    state_interfaces:
      - position

    state_publish_rate: 200.0 # Defaults to 50
    action_monitor_rate: 20.0 # Defaults to 20

    allow_partial_joints_goal: false # Defaults to false
    open_loop_control: true
    allow_integration_in_goal_trajectories: true
    constraints:
      stopped_velocity_tolerance: 0.01 # Defaults to 0.01
      goal_time: 0.0 # Defaults to 0.0 (start immediately)

To launch the controller manager, the launch files should:

  1. load the controllers YAML configuration file,

  2. run the node ros2_control_node of the package controller_manager,

  3. spawn the desired controllers.

robot_controllers = PathJoinSubstitution(
    [
        FindPackageShare("ros2_control_demo_example_1"),
        "config",
        "rrbot_controllers.yaml",
    ]
)

control_node = Node(
    package="controller_manager",
    executable="ros2_control_node",
    parameters=[robot_controllers],
    output="both",
)


joint_state_broadcaster_spawner = Node(
    package="controller_manager",
    executable="spawner",
    arguments=["joint_state_broadcaster", "--controller-manager", "/controller_manager"],
)

robot_controller_spawner = Node(
    package="controller_manager",
    executable="spawner",
    arguments=["forward_position_controller", "--controller-manager", "/controller_manager"],
)

The rrbot example

Exercise

Run again the rrbot example and check the controllers:

ros2 launch ros2_control_demo_example_1 rrbot.launch.py
ros2 control list_controllers

Exercise

Using the ROS 2 Command Line Interface for ros2_control send commands to Forward Command Controller:

ros2 topic pub /forward_position_controller/commands std_msgs/msg/Float64MultiArray "data: [0.5, 0.5]"

Also to move the robot you can start a demo node which sends two goals every 5 seconds in a loop:

ros2 launch ros2_controllers_test_nodes test_forward_position_controller.launch.py

Exercise

Now load the joint_trajectory_position_controller and set to the inactive state:

ros2 control load_controller joint_trajectory_position_controller --set-state inactive
ros2 control list_controllers

Then switch to this new controller:

ros2 control switch_controllers --activate joint_trajectory_position_controller --deactivate forward_position_controller
ros2 control list_controllers

Use the new controller using the demo node, which sends four goals every 6 seconds in a loop:

ros2 launch ros2_controllers_test_nodes test_joint_trajectory_controller.launch.py

Exercise

Open the rqt tool, load the Controller manager plugin and the Joint trajectory controller plugin to:

  • deactivate the forward_position_controller

  • activate the joint_trajectory_position_controller

  • move the robot with the sliders

../_images/rqt_ros2_control.png

ros2_control implementation

The following UML Class Diagram describes the internal implementation of the ros2_control framework.

../_images/uml_class_diagram.png

Preparing Gazebo for ros2_control

The gz_ros2_control package designed to integrate ros2_control with Gazebo simulator.

The gz_ros2_control package provides a Gazebo system plugin which instantiates a ros2_control controller manager and connects it to a Gazebo model.

Adding the gz_ros2_control plugin

In addition to the ros2_control tags detailed above (see Section Hardware Description in URDF), a Gazebo system plugin needs to be added to your URDF that actually parses the ros2_control tags and loads the appropriate hardware interfaces and controller manager.

This <plugin> tag has the following parameters element:

  • <parameters>: YAML file with the configuration of the controllers

<gazebo>
  <plugin filename="ign_ros2_control-system" name="ign_ros2_control::IgnitionROS2ControlPlugin">
    <parameters>$(find gz_ros2_control_demos)/config/cart_controller_position.yaml</parameters>
  </plugin>
</gazebo>

The <plugin> tag also has the following optional child elements (with default values):

  • <robot_param>: The location of the robot_description (URDF) on the parameter server, defaults to robot_description

  • <robot_param_node>: Name of the node where the robot_param is located, defaults to robot_state_publisher

  • <hold_joints>: if set to true (default), it will hold the joints’ position if their interface was not claimed, e.g., the controller hasn’t been activated yet.

  • <controller_manager_name>: Set controller manager name (default: controller_manager)

Exercise

Take a look at the ur_description package. In the urdf folder, the file ur.urdf.xacro includes the Gazebo plugin for the UR robots.

Working with ros2_control using actions

The FollowJointTrajectory action

Despite the possibility of sending commands by advertising their values as done before, the correct way to control the robot joints is using actions, with the FollowJointTrajectory action defined by the control_msgs package.

Mainly, it specifies the goal action as a trajectory composed of list of joint configurations and some information on the tolerance in its following, defines some result states (ranging from SUCCESS to different error situations like GOAL_TOLERANCE_VIOLATED), and sets the action feedback as the desired and the actual trajectory_msgs/JointTrajectoryPoint.

The control_msgs/action/FollowJointTrajectory.action file shows the goal, the result and the feedback sections (separated by ---).

Exercise

Use rqt to browse the control_msgs/FollowJointTrajectoryAction and all the involved messages

../_images/rqt_follow_joint_trajectory_action.png

The UR3 example

The package ur_simulation_gz opens Gazebo and load the UR3 robot (follow the instructions in section ROS 2 Install Instructions if you do not have it installed):

Exercise

ros2 launch ur_simulation_gz ur_sim_control.launch.py ur_type:='ur3e'

Open the rqt tool, load the Controller manager plugin and the Joint trajectory controller plugin and move the robot with the sliders.

../_images/ur_rqt_gazebo.png

Exercise

Explore the ur_simulation_gz package:

Explore the ur_description package:

Exercise

Implement a action client node to move the UR robot along a given trajectory. You can use the pan-tilt action client of tutorial 6 as a template.