Robot Control

Objective: This tutorial session is devoted to understanding the ros_control framework used to implement and manage robot controllers for simulation within the gazebo.

Tutorial install package:

Package rrbot_description, rrbot_gazebo, rrbot_control

cd ~/catkin_ws/src
git clone https://github.com/ros-simulation/gazebo_ros_demos.git

Package mastering_ros_robot_description_pkg

Package seven_dof_arm_gazebo_simple

Package gazebo_control_tutorial

Package actions_tutorial

Package universal_robot

Package UR3-IK

1. Ros control overview

The ros_control framework provides the capability to implement and manage robot controllers, which 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 primary motivation of ros_control is the lack of realtime-safe communication layer in ROS.

The framework implements abstractions on hardware interfaces with minimal assumptions on hardware or operating system. 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 interconnection of the ROS controller, robot hardware interface, and simulator/real hardware:

More details on all the components of ros_control are shown in the next figure. They can be grouped into two main blocks:

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

  Several controllers may be used (some are already available) and are managed by the Control Manager that is responsible of loading, unloading and switching between them.

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

  The ROS controllers do not directly communicate with the hardware, but do it through the Hardware Abstraction Layer, enabling controllers to be hardware-agnostic. It is composed of:

  • the ros_control interfaces found in the figure within the Hardware Resource Interface Layer

  • the hardware interfaces that encapsulate the hardware drivers. They are modeled with classes that inherit from the hardware_interface::RobotHW class (this class also acts as resource manager by performing resource conflict checking for a given set of controllers, maintaining a map of hardware interfaces). In the case of simulation, the Gazebo plugin version hardware_interface::RobotHWSim is used.

2. Controllers Block

Available controllers

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

• effort_controller: efforts commands are used to control joint positions, velocities or efforts.

  • joint_effort_controller

  • joint_position_controller

  • joint_velocity_controller

• position_controllers: position commands are used to control joint positions.

  • joint_position_controller

  • joint_group_position_controller

• velocity_controllers: velocity commands are used to control joint velocities.

  • joint_velocity_controller

  • joint_group_velocity_controller

To set an entire trajectory, the following controllers are defined:

• joint_trajectory_controllers:

  • position_controller

  • velocity_controller

  • effort_controller

  • position_velocity_controller

  • position_velocity_acceleration_controller

Also, a controller is defined to publish joint states:

• joint_state_controller

All available controllers can be found here.

3. Controller manager

The controller_manager provides the infrastructure to interact with controllers, i.e. load, unload, start and stop controllers.

This interaction can be done:

• Using the command line tools:

$ rosrun controller_manager controller_manager <command> <controller_name>

where the available commands are load, unload, start, stop, spawn (load+start), kill (stop+unload).

Also, several controllers can be loaded and started at once with the spawner tool:

$ rosrun controller_manager spawner [--stopped] name1 name2 name3

Using launch files (which is the most practical and common way):

<launch>
    <node pkg="controller_manager"
          type="spawner"
          args="controller_name1 controller_name2" />
<launch>

Using rqt:

$ rosrun rqt_controller_manager rqt_controller_manager

Using service calls to interact with controllers from another ROS node, e.g.:

• controller_manager/load_controller has as request the name of the controller to be loaded and returns success or failure.

• controller_manager/switch_controller has as request a list (which can be empty) of controller names to start, a list (which can be empty) of controller names to stop, and the switching policy.

More information can be found in the controller manager wiki page.

4. Configuring and launching controllers

Controllers are usually defined with yaml files (stored in a config folder), like those shown here.

Another example is shown in the following figure, that corresponds to the rrbot_control.yaml configuration file of the rrbot_control package where the controllers for the rrbot robot are defined. In this case there is:

• the joint state controller to publish the joint states of the arm (with a publishing rate set to 50 Hz),

• two joint position controllers to move each of the two joints upon receiving a goal position.

rrbot_control.yaml

rrbot:
 # Publish all joint states -----------------------------------
 joint_state_controller:
   type: joint_state_controller/JointStateController
   publish_rate: 50

 # Position Controllers ---------------------------------------
 joint1_position_controller:
   type: effort_controllers/JointPositionController
   joint: joint1
   pid: {p: 100.0, i: 0.01, d: 10.0}
 joint2_position_controller:
   type: effort_controllers/JointPositionController
   joint: joint2
   pid: {p: 100.0, i: 0.01, d: 10.0}

We can see that all the controllers are inside the namespace rrbot. This configuration file can be loaded in a launch file, prior to the spawning of the controller:

rrbot_control.launch

<launch>
 <!-- Load joint controller configurations from YAML file to parameter server -->
 <rosparam file="$(find rrbot_control)/config/rrbot_control.yaml" command="load"/>

 <!-- load the controllers -->
 <node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false"
       output="screen" ns="/rrbot" args="joint_state_controller
                                         joint1_position_controller
                                         joint2_position_controller"/>

 <!-- convert joint states to TF transforms for rviz, etc -->
 <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher"
       respawn="false" output="screen">
   <remap from="/joint_states" to="/rrbot/joint_states" />
 </node>
</launch>

If order to simulate the robot in Gazebo with those controllers, the corresponding plugin has to be added to the URDF model, as detailed in Section Preparing Gazebo for ros_control.

Important: Note that the controllers have to be spawned within the same namespace where the controls have been defined, rrbot in this case, using the ns= argument in the controller_spawner node tag. Also, the gazebo plugin will need to be within this namespace.

Working with rrbot using ros_control

Advertising joint values: The joint position controllers of the rrbot robot of Section Configuring and launching controllers can be tested by advertising the desired joint values:

$ roslaunch rrbot_gazebo rrbot_world.launch
$ roslaunch rrbot_control rrbot_control.launch
$ rostopic pub -1 /rrbot/joint1_position_controller/command std_msgs/Float64 "data: 1.5"
$ rostopic pub -1 /rrbot/joint2_position_controller/command std_msgs/Float64 "data: 1.0"

5. Hardware Abstraction Layer

ros_control intefaces

The connection of the controllers with the hardware interface (that encapsulates the hardware) is done through the ros_control interfaces. Some of the commonly used are:

• Joint Command Interfaces:

  • EffortJointInterface: to send the effort command.

  • VelocityJointInterface: to send the velocity command.

  • PositionJointInterface: to send the position command.

• Joint State Interfaces: to retrieve the joint states from the actuators encoder.

Hardware interface

To control a given robot using ros_control, a class derived from the hardware_interface::RobotHW class should be implemented. This class should support one or more of the standard interfaces like e.g. the EffortJointInterface or VelocityJointInterface:

For instance, the following code shows the class implemented for a robot with two position-controlled joints. It uses the JointPositionInterface and the JointStateInterface:

#include <hardware_interface/joint_command_interface.h>
#include <hardware_interface/joint_state_interface.h>
#include <hardware_interface/robot_hw.h>

class MyRobot : public hardware_interface::RobotHW
{
public:
  MyRobot()
 {
   // connect and register the joint state interface
   hardware_interface::JointStateHandle state_handle_a("A", &pos[0], &vel[0], &eff[0]);
   jnt_state_interface.registerHandle(state_handle_a);

   hardware_interface::JointStateHandle state_handle_b("B", &pos[1], &vel[1], &eff[1]);
   jnt_state_interface.registerHandle(state_handle_b);

   registerInterface(&jnt_state_interface);

   // connect and register the joint position interface
   hardware_interface::JointHandle pos_handle_a(jnt_state_interface.getHandle("A"), &cmd[0]);
   jnt_pos_interface.registerHandle(pos_handle_a);

   hardware_interface::JointHandle pos_handle_b(jnt_state_interface.getHandle("B"), &cmd[1]);
   jnt_pos_interface.registerHandle(pos_handle_b);

   registerInterface(&jnt_pos_interface);
  }

private:
  hardware_interface::JointStateInterface jnt_state_interface;
  hardware_interface::PositionJointInterface jnt_pos_interface;
  double cmd[2];
  double pos[2];
  double vel[2];
  double eff[2];
};

In this example, the controller manager (and the controllers inside the controller manager) will get read access to the joint state of the robot (pos, vel and eff variables) through the hardware_interface::JointStateInterface and write access to the command to be sent to the robot (the cmd variable) through the hardware_interface::PositionJointInterface. Two functions should be added to this class, namely read() and write(), to fill the pos, vel and eff with the latest available values from the robot and to run the command available at cmd on the robot. These functions will then be called in the control loop before and after the computation of the control command as shown next:

More information can be found in the ros_controls git repository.

Joint Limits

The joint_limits_interface package contains data structures for representing joint limits, methods to populate them through URDF or yaml files, and methods to enforce these limits. An example of joint limits specification in URDF and YAML formats is:

<joint name="$foo_joint" type="revolute">
  <!-- other joint description elements -->

  <!-- Joint limits -->
  <limit lower="0.0"
         upper="1.0"
         effort="10.0"
         velocity="5.0" />

  <!-- Soft limits -->
  <safety_controller k_position="100"
                     k_velocity="10"
                     soft_lower_limit="0.1"
                     soft_upper_limit="0.9" />
</joint>

A joint limits specification in YAML format that can be loaded to the ROS parameter server looks like:

joint_limits:
  foo_joint:
    has_position_limits: true
    min_position: 0.0
    max_position: 1.0
    has_velocity_limits: true
    max_velocity: 2.0
    has_acceleration_limits: true
    max_acceleration: 5.0
    has_jerk_limits: true
    max_jerk: 100.0
    has_effort_limits: true
    max_effort: 5.0
  bar_joint:
    has_position_limits: false # Continuous joint
    has_velocity_limits: true
    max_velocity: 4.0

An example of how to read these values from the URDF and populate the joint_limits_interface data structures can be found in the GitHub wiki.

Transmissions

Transmission interfaces implement mechanical transmissions, like a mechanical reducer with a given ratio n, and therefore maps effort/flow variables to output effort/flow variables while preserving power.

Transmissions are defined in the URDF robot file as detailed in the ROS wiki urdf transmission page, e.g.:

<transmission name="simple_trans">
  <type>transmission_interface/SimpleTransmission</type>
  <joint name="foo_joint">
    <hardwareInterface>EffortJointInterface</hardwareInterface>
  </joint>
  <actuator name="foo_motor">
    <mechanicalReduction>50</mechanicalReduction>
    <hardwareInterface>EffortJointInterface</hardwareInterface>
  </actuator>
</transmission>

The complete control loop when considering joint limits and the existence of transmissions is shown in the next figure:

6.ROS Control and Gazebo

Data flow of Ros control.

Simulation a ros controllers in gazebo shown in the picture below

Following the example of gazebo_control_tutorial, to moves the seven_dof_arm robot along a sinusoidal trajectory by advertising the desired joint values.

$ roslaunch gazebo_control_tutorial aruco_seven_dof_arm_with_rgbd_world_position_controllers.launch
$ rosrun gazebo_control_tutorial pubcontrols
$ roslaunch gazebo_control_tutorial kinect_arm_aruco_cube_rviz_b.launch