In the last tutorial we set up our UR5 robot in Gazebo and realized that we need some controllers in order to move our robot. Ros_control is a package that helps with controller implementation as well as hardware abstraction. It is a handy way to easily set up low level controls for our joints. In this tutorial we will see how to install ros_control, extend the URDF description with position controllers for every joint and parametrize them with a configuration file. We will also look at a simple technique to tune the parameters of our position controller. Let’s go!

Note: If you just want to run the UR5 robot in Gazebo together with ros_control to command the joint positions without learning about how to set up and configure the joint position controllers with ros_control and tune them for good control performance then you can jump directly to the Quickstart section. There you find all the necessary commands to get started quickly. Otherwise, let’s start setting up our joint controllers!

Setting up ros_control

To follow along, please clone the git repository with the modified URDF, the configuration file and the launch files into your catkin workspace and build the workspace.

~/catkin_ws/src$ git clone https://github.com/dairal/ur5-joint-position-control.git
~/catkin_ws/src$ cd ..
~/catkin_ws$ catkin_make
~/catkin_ws$ source devel/setup.bash

If you are not familiar with what a workspace is and how to build ROS packages, please start with this tutorial. If you don’t use git, you can also download the .zip file from github (link) and unzip it in your workspace folder. Then you can build the workspace.

Note that if you don’t add the source command to your ./bashrc file, you have to repeat the last command for every new linux terminal you launch such that ROS finds the package locations.

To install ros_control and some controllers run the following command in your terminal:

$ sudo apt-get install ros-$ROS_DISTRO-ros-control ros-$ROS_DISTRO-ros-controllers

Modifying the URDF

I will now describe the parts that have to be added to the URDF in oder to use it together with ros_control. If you downloaded the repository, these modifications are already done in the ur5_jnt_pos_ctrl.urdf file inside the urdf folder. First, we need to insert a ros_control plugin that parses the URDF directly after the opening <robot> tag.

    <plugin name="gazebo_ros_control" filename="libgazebo_ros_control.so">
    </plugin>

Second, we have to add the position controllers after the <link> and <joint> specifications. This is done inside the <transmission> tag which is presented below for the soulder lift joint:

  <transmission name="shoulder_lift_trans">
    <type>transmission_interface/SimpleTransmission</type>
    <joint name="shoulder_lift_joint">
      <hardwareInterface>EffortJointInterface</hardwareInterface>
    </joint>
    <actuator name="shoulder_lift_motor">
      <mechanicalReduction>1</mechanicalReduction>
    </actuator>
  </transmission>

The <type> tag lets us define the type of actuator. We choose SimpleTransmission as transmission interface which represents a simple reducer transmission. That is exactly what we have between our actuator side (the motor) and joint side (output shaft to which the link is attached), where the reduction is defined by the gearbox.

In order to use our controllers in Gazebo we define an EffortJointInterface, because in the end, our position controller output and the value that is commanded on the joint motors is the effort (or force). We pretend the transmission between our actuator and the joint to be 1:1 so we set the <mechanicalReduction> property to 1.

Adding the configuration file

Apart from modifying the URDF, we have to provide a configuration file that loads the controller parameters to the parameter server. The position controller we will use is a PID controller which means that it consists of one proportional, one integral and one differential term. If these things are unfamiliar to you don’t worry about it, we will discuss them in more detail in the last section of this article when we are gonna tune our controller.

For now the important thing to understand is that we need a configuration file that tells ros_control the parameters of our joint controllers. The configuration file stored stored in a separate sub folder named config in our project folder. This is what the file looks like for our UR5 robot:

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

  # Position Controllers ---------------------------------------
  shoulder_pan_joint_position_controller:
    type: effort_controllers/JointPositionController
    joint: shoulder_pan_joint
    pid: {p: 500.0, i: 0.01, d: 50.0, i_clamp_min: -100.0, i_clamp_max: 100.0}
  shoulder_lift_joint_position_controller:
    type: effort_controllers/JointPositionController
    joint: shoulder_lift_joint
    pid: {p: 500.0, i: 100.0, d: 30.0, i_clamp_min: -400.0, i_clamp_max: 400.0}
  elbow_joint_position_controller:
    type: effort_controllers/JointPositionController
    joint: elbow_joint
    pid: {p: 10000.0, i: 0.01, d: 50.0, i_clamp_min: -100.0, i_clamp_max: 100.0}
  wrist_1_joint_position_controller:
    type: effort_controllers/JointPositionController
    joint: wrist_1_joint
    pid: {p: 200.0, i: 10.0, d: 20.0, i_clamp_min: -400.0, i_clamp_max: 400.0}
  wrist_2_joint_position_controller:
    type: effort_controllers/JointPositionController
    joint: wrist_2_joint
    pid: {p: 100.0, i: 0.1, d: 10.0, i_clamp_min: -100.0, i_clamp_max: 100.0}
  wrist_3_joint_position_controller:
    type: effort_controllers/JointPositionController
    joint: wrist_3_joint
    pid: {p: 100.0, i: 0.1, d: 10.0, i_clamp_min: -100.0, i_clamp_max: 100.0}

There is p,i, and d values defined for every joint controller. At the top there is also a robot_state_publisher. That one publishes the joint states and is not important for us at the moment.

The launch file

Before we start everything up, let’s take a quick look at the launch file of our project.

<launch>

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

  <param name="robot_description" textfile="$(find ur5-joint-position-control)/urdf/ur5_jnt_pos_ctrl.urdf"/>
  
  <!-- load the controllers -->
  <node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false"
    output="screen" args="shoulder_pan_joint_position_controller shoulder_lift_joint_position_controller elbow_joint_position_controller wrist_1_joint_position_controller wrist_2_joint_position_controller wrist_3_joint_position_controller joint_state_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="/ur5/joint_states" />
  </node>

</launch>

First, we load the controller parameters from our ur5_config.yaml and our URDF to the parameter server. Second, we spawn the joint controllers with ros_control’s controller_spawner. Then we start the robot_state_publisher that publishes the joint state topic of the joint_state_controller transformer to /tf for other programs like Rviz (not important atm.).

The last thing we have to do before we can start is get the universal_robot package from the ros industrial project which contains the visual and geometric description files of the ur5 robot. If you did one of the previous tutorials you might have it already installed. Otherwise, please execute the following commands and replace <distro> with your ROS version:

~/catkin_ws/src$ git clone -b <distro>-devel https://github.com/ros-industrial/universal_robot.git $ cd $HOME/catkin_ws
$ rosdep update
$ rosdep install --rosdistro $ROS_DISTRO --ignore-src --from-paths src

Then build your workspace:

~/catkin_ws$ catkin_make
~/catkin_ws$ source devel/setup.bash

Launching the UR5 in Gazebo with ros_control

Now let’s start up gazebo:

$ roslaunch gazebo_ros empty_world.launch

In a new terminal, spawn the robot and launch the controllers:

$ rosrun gazebo_ros spawn_model -file `rospack find ur5-joint-position-control`/urdf/ur5_jnt_pos_ctrl.urdf -urdf -x 0 -y 0 -z 0.1 -model ur5
$ roslaunch ur5-joint-position-control ur5_joint_position_control.launch

The robot spawns in it’s zero configuration which, for the UR5, means that it is “lying” on the ground. However, starting to tune the controllers from there might be a bit awkward. You first have to get the robot into a suitable position in which you can command the positions necessary for tuning. With non-tuned controllers, this can be annoying. To avoid this, we can define the initial pose of the robot together with our spawn command. However, there still seem to be some bugs in this feature of Gazebo. Let me describe the steps of spawning the robot in a certain joint configuration that works for me:

First, launch Gazebo as before:

$ roslaunch gazebo_ros empty_world.launch

Then press the pause button in the lower bar to pause the simulation. After that, spawn the robot with the following arguments:

$ rosrun gazebo_ros spawn_model -file `rospack find ur5-joint-position-control`/urdf/ur5_jnt_pos_ctrl.urdf -urdf -x 0 -y 0 -z 0.1 -model ur5 -J shoulder_lift_joint -1.5 -J elbow_joint 1.0

The additional arguments -J shoulder_lift_joint -1.5 and -J elbow_joint 1.0 set the initial angles of the shoulder lift and the elbow joint. Then we launch the controllers just as before:

$ roslaunch ur5-joint-position-control ur5_joint_position_control.launch

After that, we can unpause the simulation by clicking the play button in the lower bar. Now the robot is in a more practical position and the controllers try to hold the robot in that position. You can also run the following launch file that does the former described steps for you (except for unpausing Gazebo after start up):

$ roslaunch ur5-joint-position-control ur5_gazebo_position_control.launch

Commanding the joint positions

Now we can send position commands to our position controllers in order to move the robot. If you don’t know the concept of publishing topics and subscribing to them, please read the tutorials in the ROS wiki. The position controllers listen to the topic ur5/controller_name/command/. The easiest way to send commands is to start up the rqt_gui:

$ rosrun rqt_gui rqt_gui

In the menu bar under Plugins we choose the Message Publisher from the Topics folder.

Now we can choose topics from the list and publish on them. In the upper selection bar next to Topic you can simply type in the topic name and then press the green plus button to add them. We will go through the tuning process for the wrist_1 joint so please add the /wrist_1_joint_position_controller/command topic. We can set a static value by extending the topic we want to publish on and put a value into the expression column under the data property of the topic. Another possibility is to define functions and let rqt compute a sequence for us. Instead of a static value we would write something like sin(0.1*i) as an expression in which i is a time variable provided by rqt. Another property we can configure is the rate at which the topic is published. We will set it to 100 for now. As soon as you put a tick into the box to the left of the topic, you should see the joint reacting to your command.

The performance of the controllers may vary significantly between different joints because we have set default controller parameters for different joints with different weights to move.

Tuning a PID Position Controller

Below we can see the difference between one badly and one well tuned controller that both react to the same position command. You can see that we need tuned control parameters In order to move the robot nice and smoothly. Here, once more, rqt_gui comes in handy.

As before with the Message Publisher, add the Plot plugin from the plugin menu to your rqt window.

Similar to the topic publisher you can add topics that you want to visualize to the graph by searching for them in the selection bar and pressing the green plus. For tuning, we want to look at the controller command and the system response to it. As an example, add the topics /wrist_1_joint_position_controller/state/process_value which is the (virtually) measured joint position and the /wrist_1_joint_position_controller/command/data to the plot. One common way to tune controller parameters is to look at the step response, meaning, how does the controller and the system behave when we set a sudden step in the command value. We can simply do that by changing our value for /wrist_1_joint_position_controller/command e.g. from 0 to -1 which, in case of the wrist_1 joint, means moving upwards. Now we can observe how the real position of the robot follows our command. Let’s go through the tuning process to enhance the controller performance

Rqt lets us directly control the controller parameters. Just add Dynamic Reconfigure from the Plugins menu under the Configuration folder just like the Plot and Message Publisher and choose the wrist_1_joint_positon_controller from the list on the left by clicking on its pid.

The Proportional Part

The proportional part of our controller is simply multiplying the error \(p_{error}\) between the desired position and the current position with the parameter \(K_p\). So if we increase \(K_p\), the controller reaches the desired position quicker and more accurately, but the oscillations are increasing. Additionally, if there is only a \(K_p\) gain, we will need a very high gain to be able to reach the exact goal position. In order to demonstrate the influence of the different gains let’s set p = 20, i = 0.0 and d = 0.0 in our Dynamic Reconfigure plugin. Now we command a position step from 0 to -1.

The Differential Part

The differential part (d) is used for damping of the system. It multiplies the velocity \(v\) of our joints (i.e. the change of position over time) with the \(K_d\) parameter and subtracts it from our controller output. When increasing \(K_d\) to e.g. 5, we see that the controller takes more time to get to the target position but has close to no overshoot. A rule of thumb is that up to 10% overshoot is acceptable in many control applications. That would mean for it is okay to have an overshoot of 0.1 rad.

The Integral Part

In the integral term we simply add up the measured errors over time (\(\int_{0}^{t} p_{error}(t)\)) such that even a small error adds up to a significant contribution to the control output over time. That way we get rid of the lasting position errors, that the proportional part can’t handle. However, the integral can also lead to more overshoot.

The other parameters, namely i_clamp_min and i_clamp_max limit the magnitude that our integral term can reach such that it doesn’t command unwanted high torques. Let’s set i = 0.1, i_clamp_min = -200 and i_clamp_min = 200.

You can continue to play with the different controllers until you found good settings for our UR5 robot. This can be quite challenging for some of the joints but it’s a good way to get a feeling for the influence that robot properties and control parameters have on the controller performance. We will use the tuned parameters in the next tutorial where we want to command the tcp (tool center point i.e. the tip of the robot arm) of our robot in cartesian space by using forward and inverse kinematics. This is important for nearly every robot arm application to control the position of the tool that is attached to the robot arm. If you are up for the challenge, then let’s continue with the next tutorial. If you want make your life easier and directly control the robot with a motion planning framework then checkout the Moveit! tutorial. Here you can find an overview of all tutorials.

Quickstart

In order to spawn the UR5 robot in Gazebo together with ros_control, first clone the following git repository into your catkin workspace:

~/catkin_ws/src$ git clone https://github.com/dairal/ur5-joint-position-control.git

or download the zip-file from github (link) and unzip it into your workspace.

Then install ros_control if you have not already done so:

$ sudo apt-get install ros-$ROS_DISTRO-ros-control ros-$ROS_DISTRO-ros-controllers

Make sure you have the universal_robot package installed:

~/catkin_ws/src$ git clone -b <distro>-devel https://github.com/ros-industrial/universal_robot.git
$ rosdep update
$ rosdep install --rosdistro $ROS_DISTRO --ignore-src --from-paths src

Build the workspace:

~/catkin_ws/src$ cd ..
~/catkin_ws$ catkin_make
~/catkin_ws$ source devel/setup.bash

Then you can start up an empty world in gazebo

$ roslaunch gazebo_ros empty_world.launch

In a new terminal, spawn the UR5 robot and start the joint position controllers with ros_control:

$ rosrun gazebo_ros spawn_model -file `rospack find ur5-joint-position-control`/urdf/ur5_jnt_pos_ctrl.urdf -urdf -x 0 -y 0 -z 0.1 -model ur5
$ roslaunch ur5-joint-position-control ur5_joint_position_control.launch

Now you can publish position commands e.g. for the shoulder joint on the /shoulder_lift_joint_controller/command topic. The easiest way to do that is with the rqt_gui. Or you learn how to do it with code in the next tutorial on controlling the tcp of the UR5 robot. You can also find and overview of all tutorials here.