{{ message }}

Instantly share code, notes, and snippets.

# Zach417/blog.md Secret

Created Oct 9, 2020

## Introduction

ROS has a ton of built-in packages to make it so that you don't have to do math. When I was first studying robotics, I was blown away by how much math was required to just control a two-wheel differential drive robot. Kinematics, PID control, and everything in-between meant that I spent a lot of time solving problems that were already solved. This is the beauty of ROS.

ros_control is another one of those packages that allows you to focus your time on problems specific to your robot--the problems that haven't solved. Well, sort of. Unfortunately, I found that the tutorials on ros.org for ros_control were not terribly decipherable. Only after I had spent a week going through source code could I really look back on these and understand what they meant.

This is a blog post to help those of you who might be struggling like I was. The example code used throughout the tutorial is boilerplate code that won't build. It's to give you a better idea for how to setup your own robot-specific package. I'll also assume a basic understanding of ROS: creating your own packages, rostopics, rosservice, roscpp, etc.

## Overview

If you're building a custom robot, using ros_control gives you three basic advantages:

• Standardization of APIs for controllers and hardware interfaces, making it easier to integrate with other packages, such as MoveIt
• Built-in control loop feedback mechanisms like PID controllers
• Easily enforce joint limits at a low level in the stack

The most helpful image I've found of understanding how it works is this:

Other packages send certain high-level desired goals to the controllers, and ros_control utilizes its controllers to work with the hardware to move joints based on those specifications.

Let's say you want to move a joint from 0 radians to 1.57 radians. The robot's state before you do anything is 0 radians. You (or a high level package) sends 1.57 to that joint's pre-specified Joint Position Controller as the desired goal. The Joint Position Controller works with an even lower level package to actually send current to the actuators. The Joint Position Controller has its own PID that continuously reads the joint's state and adjusts the current sent to the motors based on the error between the current state and the goal state.

ros_control handles two things from that process:

• receiving the goals (effort, position, velocity, trajectory, etc.)
• running the PID controllers

ros_control doesn't know or handle:

• implementing hardware control (sending current to motors)

You have to control the hardware (actuators, servos, motors, etc.) using your own code by listening to what ros_control says it should do. You also must provide ros_control the current state of the robot and its joints if you want to control position, velocity, trajectory, etc.

## Installation

On Ubuntu, you can install `ros_control` from debian packages. Depending on your ROS distribution, you will need to change the below command. See the ros_control wiki for a list of supported distributions. https://gist.github.com/f6aca9507cfa4fcf1176545c52dafb3d

One of the most confusing things for me was that ros_control isn't really it's own node. Well, the controller manager is. But it's really just a C++ library. The thing you actually build to get it to work is the middleware between ros_control and your hardware. This is called hardware_interface.

So, in order to get ros_control on our robot, we'll need to create our own package called something like ROBOT_hardware_intereface. At Slate Robotics, we built tr1_hardware_interface to control the TR1 with ros_control, which is the basis for most of this tutorial. I recommend you check out that repository if you're confused at any point.

### Package directory

The directory for our ROBOT_hardware_interface package is simple and follows the standard guidelines. We'll need a `config/` directory where you have `.yaml` files to define controllers, hardware (the joints available to the controllers), and joint limits. We'll make some launch files in `launch/` to make it easy to fire-up certain controllers. There are only two cpp files we'll need to create in our `src/` directory, and two header files in our `include/ROBOT_hardware_interface/` directory. Here's what that looks like:

``````launch/
ROBOT_position_controllers.launch
config/
controllers.yaml
hardware.yaml
joint_limits.yaml
include/
ROBOT_hardware_interface/
ROBOT_hardware.h
ROBOT_hardware_interface.h
src/
ROBOT_hardware_interface.cpp
ROBOT_hardware_interface_node.cpp
CMakeLists.txt
package.xml
``````

### launch/ROBOT_position_controllers.launch

We'll start by looking at the launch file and work our way through its dependencies. https://gist.github.com/bdb82500150f48f2c30721ab95045dbd

All we're doing here is:

• firing up our ROBOT_hardware_interface node
• firing up controller_spawner node, which is part of ros_control stack

Take note of "args" in the controller_spawner node. We're specifying which controllers from controllers.yaml that we want to use. Speaking of which, controllers.yaml follows this structure:

### config/controllers.yaml

`controllers.yaml` is where we define the overall available controllers to the controller_spawner node in our `.launch` file. Take notice of the fact that the args listed above pulls from the controllers defined here. You can define many more controllers in this file, but the `.launch` file ultimately gets to pick which are spawned. https://gist.github.com/c1631263b4cf203f268aec9ea52a6e70

The `type` element can be anything from the ros_controllers repository. Something confusing about this is that a controller is seemingly both an effort and position controller at the same time. For instance, there's both an `effort_controllers/JointPositionController` and `position_controllers/JointPositionController` option.

The difference is what commands get passed to your hardware. `effort_controllers/` means that the controller is using an effort command (the amount of current to the motors, in most cases) to control position, and `position_controllers/` means that the controller is using position itself to control position, which might make sense for controlling servos, for instance.

Further, each controller will require its own parameters. You can view the comments of the header files in the ros_controllers repository to see what's required. `JointPositionController`, for example, requires you define a `joint` and `pid` parameter. As you can see, the joint is something in rosparam, which came from our `config/hardware.yaml` file.

### config/hardware.yaml

This is probably the simplest of the configuration files. https://gist.github.com/d1ec0d5a7abe0d12c3a59131a2ab2227

We have defined `loop_hz`, which is a parameter we've set here for convenience. We'll use it in our `src/ROBOT_hardware_interface.cpp` file. And of course, the joints are defined here as well.

You can list as many joints as your robot has to offer here. You'll have access to the names of these joints as we loop through to control them in the `src/` files, so you can use that to your advantage when actuating the motors.

### config/joint_limits.yaml

The required parameters and schema for the joint_limits can be found at ros_control/joint_limits_interface. I've inserted some sample data below, but you will need to change the values depending on the specification of your robot. https://gist.github.com/d67fafa2fc9ce09b95065021f29322d1

### include/ROBOT_hardware_interface/ROBOT_hardware.h

The `ROBOT_hardware` class will be a base class for our `ROBOT_hardware_interface` class that we will tackle in the next subsection. Here, we will store many of the interfaces to `ros_control` as well as variables for joint information that get initialized by the class deriving `ROBOT_hardware`. This is a fairly abstract class that can be shared by various robots or if you wish to make multiple `ROBOT_hardware_interface` classes. Also note that there is no `.cpp` file corresponding with this header file. https://gist.github.com/eeedf69252eb73aa3d1e3d6ed595cad6

### include/ROBOT_hardware_interface/ROBOT_hardware_interface.h

`ROBOT_hardware_interface.h` defines our list of available variables and class members. In the next file, we'll define the init, update, read, and write methods, which will make up the bulk of the work for getting ros_control installed. https://gist.github.com/20566f783fddb1138c0b2856c78b43f9

### src/ROBOT_hardware_interface.cpp

Here, we define the classes that were stated in `ROBOT_hardware_interface.h`. Our goals are to:

1. init(): Register joint-specific handles to each type of controller interfaces (joint state, position, effort, etc.)
2. read(): Read joint positions from the robot's hardware and set the `joint_position_` array with that data.
3. write(): Actuate the robot's joints using the `joint_effort_command_` variable. Based on our position controllers defined in `config/controllers.yaml`, this will be set by `ros_control` by using the desired joint position (`joint_position_command_`), the error between `joint_position_` and `joint_position_command_`, and the PID parameter for that controller in `config/controllers.yaml`--all of which gets calculated when we call `update()`.
4. update(): simply read joint state, call `update()` on our controller manager, and write/actuate the joints based on what's calculated.

https://gist.github.com/11195beee8736d26cfdff9ab4ff356d2

### src/ROBOT_hardware_interface_node.cpp

Finally, we'll wrap everything up in a nice little node that can be executed by our `.launch` file. The only thing of note here is that we're setting up the ROS node, a node handle, and passing that node handle to our ROBOT_hardware_interface class. https://gist.github.com/c0948d16df809724c53b966730fc4da8

You may have noticed in a few of the files references to ROBOTcpp or a ROBOT object, such as this line in `src/ROBOT_hardware_interface.cpp`: https://gist.github.com/654a6b94d27702faaab7f2542399b8da

What we've done in our implementation of `ros_control` on the TR1 is create a separate C++ library for simple hardware functionality called tr1cpp. This allows us to decouple the `ros_control` `TR1_hardware_interface` package from actual implementation of sending current to motors and reading bits of data from joint sensors.

Ultimately, something is sending a PWM signal to a motor driver that sends current to the motor. So, when you call `write()` and `read()` in your `src/ROBOT_hardware_interface.cpp` file, you need to connect the dots between the `ros_control` output/input and your actual hardware--something that synchronously executes the `joint_effort_command_` variable on the hardware.

`tr1cpp` has an `actuate()` method on the `joint` class, so we just pass that value to that method. Of course, you can set this up anyway you like. This is just what we found to be a solution.

## Conclusion

This guide has shown you an example for how to setup `ros_control` on your custom robot. `ros_control` was one of those things that took a long time to figure out, but we've been so happy with how easily we can spin up high-level packages on the TR1 now that we have it. Anybody interested in building complex robots really need to think about configuring `ros_control`. If that's you, we hope this has been a helpful resource on that journey.

Best wishes!